Skip to content

Refactor pf400 driver and server #103

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We鈥檒l occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
Alberto024 wants to merge 1 commit into from
Closed

Refactor pf400 driver and server #103

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 11 additions & 15 deletions tools/pf400/driver.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -309,6 +309,7 @@ def initialize(self) -> None:
if self.initializer is not None:
self.initializer.initialize()
self.movement = MovementController(self.communicator, self.state, self.config)
self.set_sys_speed(speed=100)
logging.info("Successfully connected to PF400")
except Exception as e:
logging.error(f"Failed to connect to PF400: {str(e)}")
Expand Down Expand Up @@ -491,19 +492,14 @@ def __del__(self) -> None:
"""Cleanup when driver is destroyed"""
self.close()

# if __name__ == "__main__":
# driver = Pf400Driver(
# tcp_host="192.168.0.1",
# tcp_port=10100,
# joints=6,
# gpl_version="v2"
# )
# driver.initialize()

# logging.info("Getting system speed")
# driver.get_sys_speed()
# logging.info("Getting close width")
# driver.get_gripper_close_position()
# logging.info("Getting open width")
# driver.get_gripper_open_position()
if __name__ == "__main__":
driver = Pf400Driver(tcp_host="192.168.0.2", tcp_port=10100, joints=5, gpl_version="v1")
driver.initialize()

logging.info("Getting system speed")
driver.get_sys_speed()
logging.info("Getting close width")
driver.get_gripper_close_position()
logging.info("Getting open width")
driver.get_gripper_open_position()

147 changes: 74 additions & 73 deletions tools/pf400/server.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,12 +32,12 @@
MotionProfile(
name="default",
id=1,
speed=85,
speed2=80,
acceleration=60,
deceleration=60,
accel_ramp=0.1,
decel_ramp=0.1,
speed=100,
speed2=100,
acceleration=100,
deceleration=100,
accel_ramp=0.5,
decel_ramp=0.5,
inrange=0,
straight=0
),
Expand Down Expand Up @@ -136,72 +136,72 @@ def _getProfileId(self, profile_name:str) -> int:


def LoadWaypoints(self, params: Command.LoadWaypoints) -> None:
logging.info("Loading waypoints")
#Load locations
waypoints_dictionary: dict[str, t.Any] = json_format.MessageToDict(params.waypoints)
locations_list = waypoints_dictionary.get("locations", [])
self.waypoints = Waypoints.parse_obj({"locations": locations_list})
logging.info(f"Loaded {len(self.waypoints.locations)} locations")

#Load grips
grips_list = waypoints_dictionary.get("grip_params")
grip_params : Grips = Grips.parse_obj({"grip_params": grips_list})
logging.info(f"Loaded {len(grip_params.grip_params)} grip parameters")
if len(grip_params.grip_params) == 0:
logging.warning("No grip parameters found. Using default grip parameters.")
self.plate_handling_params = DEFAULT_PLATE_HANDLING_PARAMS
else:
for grip in grip_params.grip_params:
self.plate_handling_params[grip.name.lower()] = {
"grasp": Command.GraspPlate(width=grip.width, force=grip.force, speed=grip.speed),
"release": Command.ReleasePlate(width=grip.width+10, speed=grip.speed)
}

self.grips = grip_params

logging.info(f"Loaded {len(self.plate_handling_params)} plate handling parameters")
logging.info(self.plate_handling_params)
#Load and register profiles
motion_profiles_list = waypoints_dictionary.get("motion_profiles")
if motion_profiles_list:
for i, profile in enumerate(motion_profiles_list):
profile["id"] = i + 1
motion_profiles = MotionProfiles.parse_obj({"profiles": motion_profiles_list})
self.motion_profiles = motion_profiles
logging.info(f"Loaded {len(motion_profiles.profiles)} motion profiles")
if motion_profiles_list and len(motion_profiles_list) > 0:
for motion_profile in motion_profiles.profiles:
logging.info(f"Registering motion profile {motion_profile.name}")
try:
profile_no_name = motion_profile.copy(deep=True)
profile_no_name.name = ""
self.driver.register_motion_profile(str(motion_profile))
except Exception as e:
logging.error(f"Error registering motion profile {motion_profile.name}: {e}")
raise Exception(f"Error registering motion profile {motion_profile.name}: {e}")
else:
#Register default motion profiles
logging.info("No motion profiles loaded. Using default profiles.")
for motion_profile in DEFAULT_MOTION_PROFILES:
logging.info(f"Registering default motion profiles {motion_profile.name}")
try:
#Remove name from the profile
profile_no_name = motion_profile.copy(deep=True)
profile_no_name.name = ""
logging.info(profile_no_name)
self.driver.register_motion_profile(str(motion_profile))
except Exception as e:
logging.error(f"Error registering motion profile {motion_profile.name}: {e}")
raise Exception(f"Error registering motion profile {motion_profile.name}: {e}")
# #Load Sequences
sequences_list = waypoints_dictionary.get("sequences")
sequences = ArmSequences.parse_obj({"sequences":sequences_list})
logging.info(f"Loaded {len(sequences.sequences)} sequences")
self.sequences = sequences
logging.info("Loading waypoints")
#Load locations
waypoints_dictionary: dict[str, t.Any] = json_format.MessageToDict(params.waypoints)
locations_list = waypoints_dictionary.get("locations", [])
self.waypoints = Waypoints.parse_obj({"locations": locations_list})
logging.info(f"Loaded {len(self.waypoints.locations)} locations")

#Load grips
grips_list = waypoints_dictionary.get("grip_params")
grip_params : Grips = Grips.parse_obj({"grip_params": grips_list})
logging.info(f"Loaded {len(grip_params.grip_params)} grip parameters")
if len(grip_params.grip_params) == 0:
logging.warning("No grip parameters found. Using default grip parameters.")
self.plate_handling_params = DEFAULT_PLATE_HANDLING_PARAMS
else:
for grip in grip_params.grip_params:
self.plate_handling_params[grip.name.lower()] = {
"grasp": Command.GraspPlate(width=grip.width, force=grip.force, speed=grip.speed),
"release": Command.ReleasePlate(width=grip.width+10, speed=grip.speed)
}

self.grips = grip_params

logging.info(f"Loaded {len(self.plate_handling_params)} plate handling parameters")
logging.info(self.plate_handling_params)

#Load and register profiles
motion_profiles_list = waypoints_dictionary.get("motion_profiles")
if motion_profiles_list:
for i, profile in enumerate(motion_profiles_list):
profile["id"] = i + 1
motion_profiles = MotionProfiles.parse_obj({"profiles": motion_profiles_list})

self.motion_profiles = motion_profiles
logging.info(f"Loaded {len(motion_profiles.profiles)} motion profiles")

if motion_profiles_list and len(motion_profiles_list) > 0:
for motion_profile in motion_profiles.profiles:
logging.info(f"Registering motion profile {motion_profile.name}")
try:
profile_no_name = motion_profile.copy(deep=True)
profile_no_name.name = ""
self.driver.register_motion_profile(str(motion_profile))
except Exception as e:
logging.error(f"Error registering motion profile {motion_profile.name}: {e}")
raise Exception(f"Error registering motion profile {motion_profile.name}: {e}")
else:
#Register default motion profiles
logging.info("No motion profiles loaded. Using default profiles.")
for motion_profile in DEFAULT_MOTION_PROFILES:
logging.info(f"Registering default motion profiles {motion_profile.name}")
try:
#Remove name from the profile
profile_no_name = motion_profile.copy(deep=True)
profile_no_name.name = ""
logging.info(profile_no_name)
self.driver.register_motion_profile(str(motion_profile))
except Exception as e:
logging.error(f"Error registering motion profile {motion_profile.name}: {e}")
raise Exception(f"Error registering motion profile {motion_profile.name}: {e}")

# #Load Sequences
sequences_list = waypoints_dictionary.get("sequences")
sequences = ArmSequences.parse_obj({"sequences":sequences_list})
logging.info(f"Loaded {len(sequences.sequences)} sequences")
self.sequences = sequences


def LoadLabware(self, params: Command.LoadLabware) -> None:
Expand All @@ -221,7 +221,7 @@ def _unwind(self) -> None:
current_loc_array = self.driver.wherej().split(" ")
#Unwind the arm while keeping the z height, gripper width and rail constant
if self.config.joints == 5:
new_loc = f"{current_loc_array[1]} {waypoint_loc.vec[1]} {waypoint_loc.vec[2]} {waypoint_loc.vec[3]} {current_loc_array[5]}"
new_loc = f"{current_loc_array[1]} {waypoint_loc.vec[1]} {waypoint_loc.vec[2]} {waypoint_loc.vec[3]} {current_loc_array[5]}"
else:
new_loc = f"{current_loc_array[1]} {waypoint_loc.vec[1]} {waypoint_loc.vec[2]} {waypoint_loc.vec[3]} {current_loc_array[5]} {current_loc_array[6]}"
self.driver.movej(new_loc, motion_profile=1)
Expand Down Expand Up @@ -294,6 +294,7 @@ def retrieve_plate(
approach_height = int(approach_height)
grasp: Command.GraspPlate
if not grasp_params or (grasp_params.width == 0):
logging.info(f"Grasp params not provided, using params: {self.plate_handling_params}")
grap_param_exists = self.plate_handling_params.get(source_location.orientation.lower())
if not grap_param_exists:
raise Exception(f"Grasp params for {source_location.orientation.lower()} not found")
Expand Down
Loading