class TorqueExample: def __init__(self, router, router_real_time): self.expected_number_of_actuators = 7 # example works for 7dof Gen3 # self.torque_amplification = 2.0 # Torque measure on 7th actuator is sent as a command to first actuator self.torque_amplification = 20.0 # Torque measure on 7th actuator is sent as a command to first actuator # Create required services device_manager = DeviceManagerClient(router) self.actuator_config = ActuatorConfigClient(router) self.base = BaseClient(router) self.base_cyclic = BaseCyclicClient(router_real_time) self.base_command = BaseCyclic_pb2.Command() self.base_feedback = BaseCyclic_pb2.Feedback() self.base_custom_data = BaseCyclic_pb2.CustomData() # Detect all devices device_handles = device_manager.ReadAllDevices() self.actuator_count = 0 # Only actuators are relevant for this example for handle in device_handles.device_handle: if handle.device_type == Common_pb2.BIG_ACTUATOR or handle.device_type == Common_pb2.SMALL_ACTUATOR: self.base_command.actuators.add() self.base_feedback.actuators.add() self.actuator_count += 1 # Change send option to reduce max timeout at 3ms self.sendOption = RouterClientSendOptions() self.sendOption.andForget = False self.sendOption.delay_ms = 0 self.sendOption.timeout_ms = 3 self.cyclic_t_end = 30 #Total duration of the thread in seconds. 0 means infinite. self.cyclic_thread = {} self.kill_the_thread = False self.already_stopped = False self.cyclic_running = False def MoveToHomePosition(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: print("Can't reach safe position. Exiting") return False self.base.ExecuteActionFromReference(action_handle) time.sleep(20) # Leave time to action to complete return True def InitCyclic(self, sampling_time_cyclic, t_end, print_stats): if self.cyclic_running: return True # Move to Home position first if not self.MoveToHomePosition(): return False print("Init Cyclic") sys.stdout.flush() base_feedback = self.SendCallWithRetry(self.base_cyclic.RefreshFeedback, 3) if base_feedback: self.base_feedback = base_feedback if len(self.base_feedback.actuators) == self.expected_number_of_actuators: # Init command frame for x in range(self.actuator_count): self.base_command.actuators[x].flags = 1 # servoing self.base_command.actuators[x].position = self.base_feedback.actuators[x].position # First actuator is going to be controlled in torque # To ensure continuity, torque command is set to measure self.base_command.actuators[0].torque_joint = self.base_feedback.actuators[0].torque # Set arm in LOW_LEVEL_SERVOING base_servo_mode = Base_pb2.ServoingModeInformation() base_servo_mode.servoing_mode = Base_pb2.LOW_LEVEL_SERVOING self.base.SetServoingMode(base_servo_mode) # Send first frame self.base_feedback = self.base_cyclic.Refresh(self.base_command, 0, self.sendOption) # Set first actuator in torque mode now that the command is equal to measure control_mode_message = ActuatorConfig_pb2.ControlModeInformation() control_mode_message.control_mode = ActuatorConfig_pb2.ControlMode.Value('TORQUE') device_id = 1 # first actuator as id = 1 self.SendCallWithRetry(self.actuator_config.SetControlMode, 3, control_mode_message, device_id) # Init cyclic thread self.cyclic_t_end = t_end self.cyclic_thread = threading.Thread(target=self.RunCyclic, args=(sampling_time_cyclic, print_stats)) self.cyclic_thread.daemon = True self.cyclic_thread.start() return True else: print("InitCyclic: number of actuators in base_feedback does not match expected number") return False else: print("InitCyclic: failed to communicate") return False def RunCyclic(self, t_sample, print_stats): self.cyclic_running = True print("Run Cyclic") sys.stdout.flush() cyclic_count = 0 # Counts refresh stats_count = 0 # Counts stats prints failed_cyclic_count = 0 # Count communication timeouts # Initial delta between first and last actuator init_delta_position = self.base_feedback.actuators[0].position - self.base_feedback.actuators[6].position # Initial first and last actuator torques; avoids unexpected movement due to torque offsets init_last_torque = self.base_feedback.actuators[6].torque init_first_torque = -self.base_feedback.actuators[0].torque # Torque measure is reversed compared to actuator direction t_now = time.time() t_cyclic = t_now # cyclic time t_stats = t_now # print time t_init = t_now # init time print("Running torque control example for {} seconds".format(self.cyclic_t_end)) while not self.kill_the_thread: t_now = time.time() # Cyclic Refresh if (t_now - t_cyclic) >= t_sample: t_cyclic = t_now # Position command to first actuator is set to measured one to avoid following error to trigger # Bonus: When doing this instead of disabling the following error, if communication is lost and first # actuator continue to move under torque command, resulting position error with command will # trigger a following error and switch back the actuator in position command to hold its position self.base_command.actuators[0].position = self.base_feedback.actuators[0].position # First actuator torque command is set to last actuator torque measure times an amplification # self.base_command.actuators[0].torque_joint = init_first_torque + \ # self.torque_amplification * (self.base_feedback.actuators[6].torque - init_last_torque) self.base_command.actuators[0].torque_joint = 5.0 print(self.base_command.actuators[0].torque_joint) # First actuator position is sent as a command to last actuator self.base_command.actuators[6].position = self.base_feedback.actuators[0].position - init_delta_position # Incrementing identifier ensure actuators can reject out of time frames self.base_command.frame_id += 1 if self.base_command.frame_id > 65535: self.base_command.frame_id = 0 for i in range(self.expected_number_of_actuators): self.base_command.actuators[i].command_id = self.base_command.frame_id # Frame is sent try: self.base_feedback = self.base_cyclic.Refresh(self.base_command, 0, self.sendOption) except: failed_cyclic_count = failed_cyclic_count + 1 print("failed") cyclic_count = cyclic_count + 1 # Stats Print if print_stats and ((t_now - t_stats) > 1): t_stats = t_now stats_count = stats_count + 1 cyclic_count = 0 failed_cyclic_count = 0 sys.stdout.flush() if self.cyclic_t_end != 0 and (t_now - t_init > self.cyclic_t_end): print("Cyclic Finished") sys.stdout.flush() break self.cyclic_running = False return True def StopCyclic(self): print ("Stopping the cyclic and putting the arm back in position mode...") if self.already_stopped: return # Kill the thread first if self.cyclic_running: self.kill_the_thread = True self.cyclic_thread.join() # Set first actuator back in position mode control_mode_message = ActuatorConfig_pb2.ControlModeInformation() control_mode_message.control_mode = ActuatorConfig_pb2.ControlMode.Value('POSITION') device_id = 1 # first actuator has id = 1 self.SendCallWithRetry(self.actuator_config.SetControlMode, 3, control_mode_message, device_id) base_servo_mode = Base_pb2.ServoingModeInformation() base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING self.base.SetServoingMode(base_servo_mode) self.cyclic_t_end = 0.1 self.already_stopped = True print('Clean Exit') @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
# Create required services base_client_service = BaseClient(router) device_manager_service = DeviceManagerClient(router) # Find the number of actuator in angular action and trajectory sub_device_info = device_manager_service.ReadAllDevices() act_count = 0 for dev in sub_device_info.device_handle: if dev.device_type is Common_pb2.BIG_ACTUATOR or dev.device_type is Common_pb2.SMALL_ACTUATOR: act_count += 1 # Move arm to ready position print("\nMoving the arm to a safe position before executing example") action_type = Base_pb2.RequestedActionType() action_type.action_type = Base_pb2.REACH_JOINT_ANGLES action_list = base_client_service.ReadAllActions(action_type) action_handle = None # print("Actions:") # print( action_list.action_list) for action in action_list.action_list: if action.name == "Home": action_handle = action.handle if action_handle == None: import sys print("\nCan't reach safe position. Exiting") sys.exit(0) base_client_service.ExecuteActionFromReference(action_handle) time.sleep(5) # Leave time to action to complete
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, )