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
class KinovaRobotEnv: metadata = {'render_modes':['human', 'rbg_array']} def __init__(self, action_scale=0.05, target_range=0.15, distance_threshold=0.05, speed=0.10): self.connection = utilities.DeviceConnection.createTcpConnection() self.router = self.connection.__enter__() self.base = BaseClient(self.router) self.base_cyclic = BaseCyclicClient(self.router) self.webcam = cv2.VideoCapture(0) self.action_scale = action_scale self.target_range = target_range self.distance_threshold = distance_threshold self.speed = speed self.observation_space = gym.spaces.Dict( observation=gym.spaces.Box(-np.inf, np.inf, self._get_obs().shape), achieved_goal=gym.spaces.Box(-np.inf, np.inf, (3,)), desired_goal=gym.spaces.Box(-np.inf, np.inf, (3,)), ) self.action_space = gym.spaces.Box(-1., 1., (3,)) self.reward_range = (-np.inf, 0) self.unwrapped = self self.spec = None self._set_to_home() self.init_xyz = self._get_obs()[:3] self.goal = None self.sample_goal() assert self.goal is not None def seed(self, seed): pass def close(self): self.connection.__exit__(None, None, None) def render(self, mode='human', width=84, height=84): # return np.zeros((width, height, 3)) ret, frame = self.webcam.read() frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) if mode == 'human': cv2.imshow('', frame) elif mode == 'rgb_array': frame = cv2.resize(frame, (width, height)) return frame def sample_goal(self, target_range=None): if target_range is None: target_range = self.target_range self.goal = self.init_xyz + (-1 + 2*np.random.rand(*self.init_xyz.shape))*self.target_range return self.goal def reset(self): self.sample_goal() self._set_to_home() return self._get_obs_dict() def step(self, act): print(f'act: {act}') act = act.copy()*self.action_scale action = Base_pb2.Action() action.name = "Example Cartesian action movement" action.application_data = "" action.reach_pose.constraint.speed.translation = self.speed feedback = self.base_cyclic.RefreshFeedback() cartesian_pose = action.reach_pose.target_pose cartesian_pose.x = feedback.base.tool_pose_x + act[0] # (meters) cartesian_pose.y = feedback.base.tool_pose_y + act[1] # (meters) cartesian_pose.z = feedback.base.tool_pose_z + act[2] # (meters) cartesian_pose.theta_x = feedback.base.tool_pose_theta_x # (degrees) cartesian_pose.theta_y = feedback.base.tool_pose_theta_y # (degrees) cartesian_pose.theta_z = feedback.base.tool_pose_theta_z # (degrees) self.base.ExecuteAction(action) duration = 1. + (np.linalg.norm(act)/self.speed) print(f'step w duration {duration}') time.sleep(duration) obs_dict = self._get_obs_dict() info = dict() reward = self.compute_reward( obs_dict['achieved_goal'], obs_dict['desired_goal'], info, ) done = True if np.abs(reward) < self.distance_threshold else False info['is_success'] = done return obs_dict, reward, done, info def _set_to(self, xyz): action = Base_pb2.Action() action.name = "Example Cartesian action movement" action.application_data = "" action.reach_pose.constraint.speed.translation = self.speed feedback = self.base_cyclic.RefreshFeedback() current_pos = np.asarray([feedback.base.tool_pose_x, feedback.base.tool_pose_y, feedback.base.tool_pose_z]) distance_to_xyz = np.linalg.norm(current_pos - xyz) duration = 1. + (distance_to_xyz/self.speed) print(f'_set_to w duration {duration}') cartesian_pose = action.reach_pose.target_pose cartesian_pose.x = xyz[0] cartesian_pose.y = xyz[1] cartesian_pose.z = xyz[2] cartesian_pose.theta_x = feedback.base.tool_pose_theta_x # (degrees) cartesian_pose.theta_y = feedback.base.tool_pose_theta_y # (degrees) cartesian_pose.theta_z = feedback.base.tool_pose_theta_z # (degrees) self.base.ExecuteAction(action) time.sleep(duration) def _set_to_home(self): # Make sure the arm is in Single Level Servoing mode base_servo_mode = Base_pb2.ServoingModeInformation() base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING self.base.SetServoingMode(base_servo_mode) # Move arm to ready position print("Moving the arm to a safe position") action_type = Base_pb2.RequestedActionType() action_type.action_type = Base_pb2.REACH_JOINT_ANGLES action_list = self.base.ReadAllActions(action_type) action_handle = None for action in action_list.action_list: if action.name == "Home": action_handle = action.handle if action_handle == None: raise Run("Can't reach safe position. Exiting") self.base.ExecuteActionFromReference(action_handle) print('Resetting for duration 6') time.sleep(6) def compute_reward(self, achieved_goal, desired_goal, info): d = np.linalg.norm(achieved_goal - desired_goal, axis=-1) return 0. if d < self.distance_threshold else -1. def _is_success(self, achieved_goal, desired_goal): d = np.linalg.norm(achieved_goal - desired_goal, axis=-1) return True if d < self.distance_threshold else False def _get_obs(self): feedback = self.base_cyclic.RefreshFeedback() observation = np.asarray([ feedback.base.tool_pose_x, feedback.base.tool_pose_y, feedback.base.tool_pose_z, ]) return observation def _get_obs_dict(self): observation = self._get_obs() achieved_goal = observation[:3] desired_goal = self.goal return dict( observation=observation, achieved_goal=achieved_goal, desired_goal=desired_goal, )