class KinovaController: def __init__(self, router, router_real_time): self.camera_mtr = None self.world3D_list = [] self.camera3D_list = [] self.camera3D = None self.camera3D_count = -100 self.world3D_count = 0 self.joint_angle_list = None device_manager = DeviceManagerClient(router) self.actuator_config = ActuatorConfigClient(router) self.base = BaseClient(router) self.base_cyclic = BaseCyclicClient(router_real_time) self.base_feedback = BaseCyclic_pb2.Feedback() self.gripper = GripperController(router, router_real_time) def set_single_level_servoing_mode(self): # Save servoing mode before changing it self.previous_servoing_mode = self.base.GetServoingMode() # Set base in single level servoing mode servoing_mode_info = Base_pb2.ServoingModeInformation() # print("Setting single level servoi print("Current Mode:") print(servoing_mode_info) servoing_mode_info.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING self.base.SetServoingMode(servoing_mode_info) print("Waiting 5 second for changing to single level servoing mode...") time.sleep(5) print("After setting single level") print(servoing_mode_info) def move_arm(self, pos_list): self.set_single_level_servoing_mode() # for count, joint_angle in enumerate(self.joint_angle_list): for count, pos in enumerate(pos_list): print("Moving to ", pos) cartesian_action_movement(self.base, pos) self.base_feedback = self.SendCallWithRetry( self.base_cyclic.RefreshFeedback, 3) joint_angles = [ self.base_feedback.actuators[i].position for i in range(7) ] @staticmethod def SendCallWithRetry(call, retry, *args): i = 0 arg_out = [] while i < retry: try: arg_out = call(*args) break except: i = i + 1 continue if i == retry: print("Failed to communicate") return arg_out
class GripperLowLevelExample: def __init__(self, router, router_real_time, proportional_gain=2.0): """ GripperLowLevelExample class constructor. Inputs: kortex_api.RouterClient router: TCP router kortex_api.RouterClient router_real_time: Real-time UDP router float proportional_gain: Proportional gain used in control loop (default value is 2.0) Outputs: None Notes: - Actuators and gripper initial position are retrieved to set initial positions - Actuator and gripper cyclic command objects are created in constructor. Their references are used to update position and speed. """ self.proportional_gain = proportional_gain ########################################################################################### # UDP and TCP sessions are used in this example. # TCP is used to perform the change of servoing mode # UDP is used for cyclic commands. # # 2 sessions have to be created: 1 for TCP and 1 for UDP ########################################################################################### self.router = router self.router_real_time = router_real_time # Create base client using TCP router self.base = BaseClient(self.router) # Create base cyclic client using UDP router. self.base_cyclic = BaseCyclicClient(self.router_real_time) # Create base cyclic command object. self.base_command = BaseCyclic_pb2.Command() self.base_command.frame_id = 0 self.base_command.interconnect.command_id.identifier = 0 self.base_command.interconnect.gripper_command.command_id.identifier = 0 # Add motor command to interconnect's cyclic self.motorcmd = self.base_command.interconnect.gripper_command.motor_cmd.add( ) # Set gripper's initial position velocity and force base_feedback = self.base_cyclic.RefreshFeedback() self.motorcmd.position = base_feedback.interconnect.gripper_feedback.motor[ 0].position self.motorcmd.velocity = 0 self.motorcmd.force = 100 for actuator in base_feedback.actuators: self.actuator_command = self.base_command.actuators.add() self.actuator_command.position = actuator.position self.actuator_command.velocity = 0.0 self.actuator_command.torque_joint = 0.0 self.actuator_command.command_id = 0 print("Position = ", actuator.position) # Save servoing mode before changing it self.previous_servoing_mode = self.base.GetServoingMode() # Set base in low level servoing mode servoing_mode_info = Base_pb2.ServoingModeInformation() servoing_mode_info.servoing_mode = Base_pb2.LOW_LEVEL_SERVOING self.base.SetServoingMode(servoing_mode_info) def Cleanup(self): """ Restore arm's servoing mode to the one that was effective before running the example. Inputs: None Outputs: None Notes: None """ # Restore servoing mode to the one that was in use before running the example self.base.SetServoingMode(self.previous_servoing_mode) def Goto(self, target_position): """ Position gripper to a requested target position using a simple proportional feedback loop which changes speed according to error between target position and current gripper position Inputs: float target_position: position (0% - 100%) to send gripper to. Outputs: Returns True if gripper was positionned successfully, returns False otherwise. Notes: - This function blocks until position is reached. - If target position exceeds 100.0, its value is changed to 100.0. - If target position is below 0.0, its value is set to 0.0. """ if target_position > 100.0: target_position = 100.0 if target_position < 0.0: target_position = 0.0 while True: try: base_feedback = self.base_cyclic.Refresh(self.base_command) # Calculate speed according to position error (target position VS current position) position_error = target_position - base_feedback.interconnect.gripper_feedback.motor[ 0].position # If positional error is small, stop gripper if abs(position_error) < 1.5: position_error = 0 self.motorcmd.velocity = 0 self.base_cyclic.Refresh(self.base_command) return True else: self.motorcmd.velocity = self.proportional_gain * abs( position_error) if self.motorcmd.velocity > 100.0: self.motorcmd.velocity = 100.0 self.motorcmd.position = 100.0 if position_error < 0.0: self.motorcmd.position = 0.0 except Exception as e: print("Error in refresh: " + str(e)) return False time.sleep(0.001) return True