def execute_trajectory(self, trajectory, joint_names, speed=None, acceleration=None): """ trajectory is a list of points: approach (joint), init (cart), drawing (cart), retreat (cart) """ speed = speed if speed is not None else self.speed acceleration = acceleration if acceleration is not None else self.acceleration if isinstance(trajectory, dict) and "approach" in trajectory: for mtype in ["approach", "init", "drawing", "retreat"]: points = trajectory[mtype] if points["type"] == "joint": self._seed = points["joints"] self.move_to_joint_positions( dict(zip(joint_names, points["joints"]))) elif points["type"] == "cart": wpt_opts = MotionWaypointOptions( max_joint_speed_ratio=speed, max_joint_accel=acceleration) traj = MotionTrajectory(limb=self.limb) waypoint = MotionWaypoint(options=wpt_opts, limb=self.limb) t_opt = TrajectoryOptions( interpolation_type=TrajectoryOptions.CARTESIAN) jv = deepcopy(points["joints"]) jv.reverse() waypoint.set_joint_angles(jv) waypoint.set_cartesian_pose( list_to_pose_stamped(points["pose"], frame_id="base")) traj.append_waypoint(waypoint) jn = [str(j) for j in joint_names] jn.reverse() traj.set_joint_names(jn) traj.set_trajectory_options(t_opt) result = traj.send_trajectory(timeout=10) self.last_activity = rospy.Time.now() if result is None: rospy.logerr("Trajectory failed to send") elif not result.result: rospy.logerr( 'Motion controller failed to complete the trajectory with error %s', result.errorId) else: rospy.logwarn("Unknown type %", mtype) else: rospy.logerr("Incorrect inputs to execute_trajectory") return
def joint_angles_in_contact(input_arg): """ Move the robot arm to the specified configuration with the desired interaction control options. Call using: $ rosrun intera_examples go_to_joint_angles_in_contact.py [arguments: see below] -q 0.0 0.0 0.0 0.0 0.0 0.0 0.0 --> Go to joint pose: 0.0 0.0 0.0 0.0 0.0 0.0 0.0 using default settings -q 0.1 -0.2 0.15 -0.05 -0.08 0.14 -0.04 -s 0.1 --> Go to pose [...] with a speed ratio of 0.1 -q -0.2 0.1 0.1 0.2 -0.3 0.2 0.4 -s 0.9 -a 0.1 --> Go to pose [...] witha speed ratio of 0.9 and an accel ratio of 0.1 --trajType CARTESIAN --> Use a Cartesian interpolated endpoint path to reach the goal === Interaction Mode options === -st 1 --> Set the interaction controller state (1 for True, 0 for False) in the current configuration -k 500.0 500.0 500.0 10.0 10.0 10.0 --> Set K_impedance to [500.0 500.0 500.0 10.0 10.0 10.0] in the current configuration -m 0 --> Set max_impedance to False for all 6 directions in the current configuration -m 1 1 0 1 1 1 --> Set max_impedance to [True True False True True True] in the current configuration -kn 5.0 3.0 5.0 4.0 6.0 4.0 6.0 --> Set K_nullspace to [5.0 3.0 5.0 4.0 6.0 4.0 6.0] in the current configuration -f 0.0 0.0 30.0 0.0 0.0 0.0 --> Set force_command to [0.0 0.0 30.0 0.0 0.0 0.0] in the current configuration -ef --> Set in_endpoint_frame to True in the current configuration -en 'right_hand' --> Specify the desired endpoint frame where impedance and force control behaviors are defined -md 1 --> Set interaction_control_mode to impedance mode for all 6 directions in the current configuration (1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit) -md 1 1 2 1 1 1 --> Set interaction_control_mode to [impedance, impedance, force, impedance, impedance, impedance] in the current configuration (1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit) """ arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser( formatter_class=arg_fmt, description="joint_angles_in_contact.__doc__") parser.add_argument( "-q", "--joint_angles", type=float, nargs='+', default=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0], help="A list of joint angles, one for each of the 7 joints, J0...J6") parser.add_argument( "-s", "--speed_ratio", type=float, default=0.1, help="A value between 0.001 (slow) and 1.0 (maximum joint velocity)") parser.add_argument( "-a", "--accel_ratio", type=float, default=0.5, help="A value between 0.001 (slow) and 1.0 (maximum joint accel)") parser.add_argument("-t", "--trajType", type=str, default='JOINT', choices=['JOINT', 'CARTESIAN'], help="trajectory interpolation type") parser.add_argument( "-st", "--interaction_active", type=int, default=1, choices=[0, 1], help="Activate (1) or Deactivate (0) interaction controller") parser.add_argument( "-k", "--K_impedance", type=float, nargs='+', default=[1300.0, 1300.0, 1300.0, 30.0, 30.0, 30.0], help= "A list of desired stiffnesses, one for each of the 6 directions -- stiffness units are (N/m) for first 3 and (Nm/rad) for second 3 values" ) parser.add_argument( "-m", "--max_impedance", type=int, nargs='+', default=[1, 1, 1, 1, 1, 1], choices=[0, 1], help= "A list of impedance modulation state, one for each of the 6 directions (a single value can be provided to apply the same value to all the directions) -- 0 for False, 1 for True" ) parser.add_argument( "-md", "--interaction_control_mode", type=int, nargs='+', default=[1, 1, 1, 1, 1, 1], choices=[1, 2, 3, 4], help= "A list of desired interaction control mode (1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit), one for each of the 6 directions" ) parser.add_argument( "-fr", "--interaction_frame", type=float, nargs='+', default=[0, 0, 0, 1, 0, 0, 0], help= "Specify the reference frame for the interaction controller -- first 3 values are positions [m] and last 4 values are orientation in quaternion (w, x, y, z)" ) parser.add_argument( "-ef", "--in_endpoint_frame", action='store_true', default=False, help= "Set the desired reference frame to endpoint frame; otherwise, it is base frame by default" ) parser.add_argument( "-en", "--endpoint_name", type=str, default='right_hand', help= "Set the desired endpoint frame by its name; otherwise, it is right_hand frame by default" ) parser.add_argument( "-f", "--force_command", type=float, nargs='+', default=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], help= "A list of desired force commands, one for each of the 6 directions -- in force control mode this is the vector of desired forces/torques to be regulated in (N) and (Nm), in impedance with force limit mode this vector specifies the magnitude of forces/torques (N and Nm) that the command will not exceed" ) parser.add_argument( "-kn", "--K_nullspace", type=float, nargs='+', default=[5.0, 10.0, 5.0, 10.0, 5.0, 10.0, 5.0], help= "A list of desired nullspace stiffnesses, one for each of the 7 joints (a single value can be provided to apply the same value to all the directions) -- units are in (Nm/rad)" ) parser.add_argument("-dd", "--disable_damping_in_force_control", action='store_true', default=False, help="Disable damping in force control") parser.add_argument( "-dr", "--disable_reference_resetting", action='store_true', default=False, help= "The reference signal is reset to actual position to avoid jerks/jumps when interaction parameters are changed. This option allows the user to disable this feature." ) parser.add_argument( "-rc", "--rotations_for_constrained_zeroG", action='store_true', default=False, help= "Allow arbitrary rotational displacements from the current orientation for constrained zero-G (use only for a stationary reference orientation)" ) parser.add_argument( "--timeout", type=float, default=None, help= "Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout." ) args = parser.parse_args(input_arg[1:]) try: #rospy.init_node('go_to_joint_angles_in_contact_py') limb = Limb() traj = MotionTrajectory(limb=limb) wpt_opts = MotionWaypointOptions( max_joint_speed_ratio=args.speed_ratio, max_joint_accel=args.accel_ratio) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) joint_angles = limb.joint_ordered_angles() waypoint.set_joint_angles(joint_angles=joint_angles) traj.append_waypoint(waypoint.to_msg()) if len(args.joint_angles) != len(joint_angles): rospy.logerr('The number of joint_angles must be %d', len(joint_angles)) return None waypoint.set_joint_angles(joint_angles=args.joint_angles) traj.append_waypoint(waypoint.to_msg()) # set the interaction control options in the current configuration interaction_options = InteractionOptions() trajectory_options = TrajectoryOptions() trajectory_options.interaction_control = True trajectory_options.interpolation_type = args.trajType interaction_options.set_interaction_control_active( int2bool(args.interaction_active)) interaction_options.set_K_impedance(args.K_impedance) interaction_options.set_max_impedance(int2bool(args.max_impedance)) interaction_options.set_interaction_control_mode( args.interaction_control_mode) interaction_options.set_in_endpoint_frame( int2bool(args.in_endpoint_frame)) interaction_options.set_force_command(args.force_command) interaction_options.set_K_nullspace(args.K_nullspace) interaction_options.set_endpoint_name(args.endpoint_name) if len(args.interaction_frame) < 7: rospy.logerr('The number of elements must be 7!') elif len(args.interaction_frame) == 7: quat_sum_square = args.interaction_frame[ 3] * args.interaction_frame[3] + args.interaction_frame[ 4] * args.interaction_frame[4] +args.interaction_frame[5] * args.interaction_frame[ 5] + args.interaction_frame[6] * args.interaction_frame[6] if quat_sum_square < 1.0 + 1e-7 and quat_sum_square > 1.0 - 1e-7: interaction_frame = Pose() interaction_frame.position.x = args.interaction_frame[0] interaction_frame.position.y = args.interaction_frame[1] interaction_frame.position.z = args.interaction_frame[2] interaction_frame.orientation.w = args.interaction_frame[3] interaction_frame.orientation.x = args.interaction_frame[4] interaction_frame.orientation.y = args.interaction_frame[5] interaction_frame.orientation.z = args.interaction_frame[6] interaction_options.set_interaction_frame(interaction_frame) else: rospy.logerr( 'Invalid input to quaternion! The quaternion must be a unit quaternion!' ) else: rospy.logerr('Invalid input to interaction_frame!') interaction_options.set_disable_damping_in_force_control( args.disable_damping_in_force_control) interaction_options.set_disable_reference_resetting( args.disable_reference_resetting) interaction_options.set_rotations_for_constrained_zeroG( args.rotations_for_constrained_zeroG) trajectory_options.interaction_params = interaction_options.to_msg() traj.set_trajectory_options(trajectory_options) result = traj.send_trajectory(timeout=args.timeout) if result is None: rospy.logerr('Trajectory FAILED to send!') return if result.result: rospy.loginfo( 'Motion controller successfully finished the trajectory with interaction options set!' ) return True else: rospy.logerr( 'Motion controller failed to complete the trajectory with error %s', result.errorId) # print the resultant interaction options rospy.loginfo('Interaction Options:\n%s', interaction_options.to_msg()) except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user. %s', 'Exiting before trajectory completion.')
class MySawyer(object): # # Init class def __init__(self, name='MySawyer', limb='right', anonymous=True, disable_signals=True, light=True, gripper_reverse=False): rospy.init_node(name, anonymous=anonymous, disable_signals=disable_signals) # rospy.sleep(1) # # self._limb=None self._head=None self._light=None self._head_display=None self._display=None self._cuff=None self._limits=None self._navigator=None self._init_nodes(limb,light) self._get_gripper(gripper_reverse) # # Default Variables self._home_pos=[0.0, -1.178, 0.0, 2.178, 0.0, 0.567, 3.313] self._init_pos=[0.0, -1.178, 0.0, 2.178, 0.0, 0.567, 3.313] self._default_pos=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0] self._motion_trajectory=None #self._joint_names=self._limb.joint_names() #self._velocity_limits=self._limits.joint_velocity_limits() # # for motion controller self._motions={} self._joint_positions={'home':self._home_pos,'init':self._init_pos, 'default':self._default_pos} self._index=0 self._p_index=0 self._is_recording=False self.max_record_time=30 self._accuracy=0.01 # 0.05 this value use velocity control mode and position control mode self._recording_intval=0.5 # # for velicity control mode self._running=True self._target=[0.0, -1.178, 0.0, 2.178, 0.0, 0.567, 3.313] ### initial position self._target_motion=[] self._vmax=0.4 self._vrate=2.0 self._is_moving=False # # for interaction mode self._speed_ratio=0.1 # 0.001 -- 1.0 self._max_speed_ratio=0.5 # 0.001 -- 1.0 self._max_accel_ratio=0.5 # 0.001 -- 1.0 self._trajType='JOINT' # 'JOINT' ot 'CARTESIAN' self._interaction_active=True self._K_impedance=[1300.0,1300.0, 1300.0, 30.0, 30.0, 30.0] self._max_impedance=[1,1,1,1,1,1] self._interaction_control_mode=[1,1,1,1,1,1] self._interaction_frame=[0,0,0,1,0,0,0] self._in_endpoint_frame=False self._endpoint_name='right_hand' self._force_command=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self._K_nullspace=[5.0, 10.0, 5.0, 10.0, 5.0, 10.0, 5.0] self._disable_damping_in_force_control=False self._disable_reference_resetting=False self._rotations_for_constrained_zeroG=False self._timeout=None # for Cartesian Pose base motion self._in_tip_frame=False self._tip_name='right_hand' self._linear_speed=0.6 # m/s self._linear_accel=0.6 # m/s/s self._rotational_speed=1.57 # rad/s self._rotational_accel=1.57 # rad/s/s ## for event handlers self.ok_id=None self.show_id=None self.back_id=None # # for RTC self._is_pause=False # # def _init_nodes(self, limb, light): try: self._limb=intera_interface.Limb(limb) self._head=intera_interface.Head() self._light=SawyerLight(light) #self._head_display=intera_interface.HeadDisplay() self._display=SawyerDisplay() self._cuff=intera_interface.Cuff() self._limits=intera_interface.JointLimits() self._navigator=intera_interface.Navigator() self._joint_names=self._limb.joint_names() self._velocity_limits=self._limits.joint_velocity_limits() self._stop_cmd={} for i,name in enumerate(self._joint_names): self._stop_cmd[name]=0.0 except: print("Warning caught exception...") traceback.print_exc() pass # 3 def _get_gripper(self, gripper_reverse): try: self._gripper=intera_interface.get_current_gripper_interface() self._is_clicksmart = isinstance(self._gripper, intera_interface.SimpleClickSmartGripper) self._gripper_reverse=gripper_reverse if self._is_clicksmart: if self._gripper.needs_init(): self._gripper.initialize() _signals=self._gripper.get_ee_signals() if 'grip' in _signals: self._gripper_type='grip' elif 'vacuumOn' in _signals: self._gripper_type='vacuum' else: self._gripper_type='unknown' else: if not (self._gripper.is_calibrated() or self._gripper.calibrate() == True): raise except: self._gripper=None self._is_clicksmart=False self._gripper_type=None self._gripper_reverse=None # # def activate(self): # # Enable Robot self._rs=intera_interface.RobotEnable(intera_interface.CHECK_VERSION) self._init_state=self._rs.state().enabled self._rs.enable() # # current positions self._angles=self._limb.joint_angles() self._pose=self._limb.endpoint_pose() # # self._limb.set_joint_position_speed(self._speed_ratio) # # LED white ON self._light.head_on() self.mkRosPorts() self.set_record() # # def mkRosPorts(self): self._sub=dict() self._pub=dict() self._sub['target_joint_pos']=rospy.Subscriber('target_joint_pos', String,self.set_target_joint_pos) self._pub['current_joint_pos']=rospy.Publisher('current_joint_pos', String,queue_size=1) self._pub['target_joint_pos']=rospy.Publisher('target_joint_pos', String,queue_size=1) self._pub['image']=rospy.Publisher('/robot/head_display', Image, latch=True, queue_size=10) # # def set_motion_sequencer(self): self.set_subscriber('current_joint_pos', self.set_next_target, String) # # def set_subscriber(self, name, func, arg_type=String): if name in self._sub and self._sub[name]: self._sub[name].unregister() self._sub[name]=rospy.Subscriber(name, arg_type,func) def unset_subscriber(self, name): if name in self._sub and self._sub[name]: self._sub[name].unregister() self._sub[name]=None # # def get_joint_angles(self): return self._limb.joint_ordered_angles() def get_joint_velocities(self): vel=self._limb.joint_velocities() return map(lambda x: vel[x], self._joint_names) # # def enable(self): self._rs.enable() # # def state(self): print(self._rs.state()) # # def reset(self): self._rs.reset() # # def disable(self): self._rs.disable() # # def stop(self): self._rs.stop() # # def exit_control_mode(self): self._limb.exit_control_mode() # # def update_pose(self): self._angles=self._limb.joint_angles() self._pose=self._limb.endpoint_pose() # # def init_pos(self, use_motion_ctrl=True): if use_motion_ctrl: self.move_to([self._init_pos]) else: self._light.head_green() self._limb.move_to_neutral(speed=self._speed_ratio) self.update_pose() self._light.head_on() # # def set_speed(self, rate=0.3): self._speed_ratio=rate self._limb.set_joint_position_speed(rate) # # def print_joiint_pos(self, dtime=5.0, intval=0.1): end_time = rospy.Time.now() + rospy.Duration(dtime) while rospy.Time.now() < end_time: if rospy.is_shutdown() : break print(self._limb.endpoint_pose()) rospy.sleep(intval) ############################################## # Joint Position Control (Depreciated for Intera 5.2 and beyond) def move_joints(self, pos): self._limb.set_joint_position_speed(self._speed_ratio) self._light.head_green() self._limb.move_to_joint_positions(pos) self.update_pose() self._light.head_on() # # def move_cart(self, x_dist, y_dist, z_dist): self._limb.set_joint_position_speed(self._speed_ratio) self._pose=self.endpoint_pose() self._pose.position.x += x_dist self._pose.position.y += y_dist self._pose.position.z += z_dist self.move_joints(self._limb.ik_request(self._pose)) # # def record_motion(self, name=None, dtime=0, intval=1.0): if not name : name=self.mk_motion_name() self._index += 1 if dtime <= 0: dtime=self.max_record_time print ("Start Recording:", name) self._light.head_blue() self._motions[name]=[] self._is_recording=True end_time = rospy.Time.now() + rospy.Duration(dtime) while (rospy.Time.now() < end_time) and self._is_recording : if rospy.is_shutdown() : break self._motions[name].append(self._limb.joint_angles()) rospy.sleep(intval) print ("End Recording: record ", len(self._motions[name]), " points") self._is_recording=False self._light.head_on() # # def mk_motion_name(self): name = 'Motion_' + str(self._index) while name in self._motions: self._index += 1 name = 'Motion_' + str(self._index) return name # # Record positions def record_pos(self,val): if val : self._light.head_yellow() name = 'P_' + str(self._p_index) while name in self._joint_positions: self._p_index += 1 name = 'P_' + str(self._p_index) self._joint_positions[name]=self._limb.joint_ordered_angles() self._light.head_on() # # Motion Recorder Event Handleer(Start) def start_record(self, value): if value: print('Start..') self.record_motion(None, 0, self._recording_intval) # # Motion Recorder Event Handler(Stop) def stop_record(self, value): if value: print('Stop..') self._is_recording=False # # set Event Handlers def set_record(self): print ("Register callbacks") self.ok_id=self._navigator.register_callback(self.start_record, 'right_button_ok') self.back_id=self._navigator.register_callback(self.stop_record, 'right_button_back') self.square_id=self._navigator.register_callback(self.record_pos, 'right_button_square') self.show_id=self._navigator.register_callback(self.unset_record, 'right_button_show') # # unset Event Handlers def unset_record(self, value=0): if value and self.ok_id : print ("Unregister all callbacks") if self._navigator.deregister_callback(self.ok_id) : self.ok_id=None #if self._navigator.deregister_callback(self.show_id) : self.show_id=None if self._navigator.deregister_callback(self.back_id) : self.back_id=None if self._navigator.deregister_callback(self.square_id) : self.square_id=None def gripper_state(self): if self.is_gripping : return 1 else: return 0 ####################################################### # # For Joint Position mode (before SDK-5.2) def play_motion(self, name, intval=0.0): self._limb.set_joint_position_speed(self._speed_ratio) self._light.head_green() for pos in self._motions[name]: if rospy.is_shutdown() : self._light.head_red() return # self._limb.move_to_joint_positions(pos, threshold=self._accuracy) if intval > 0: rospy.sleep(intval) self._light.head_on() # # def play_motion_seq(self, names): self._limb.set_joint_position_speed(self._speed_ratio) self._light.head_green() for name in names: for pos in self._motions[name]: if rospy.is_shutdown() : self._light.head_red() return self._limb.move_to_joint_positions(pos) self._light.head_on() ############################################### # # def list_motions(self): print(self._motions.keys()) # # def joint_pos_d2l(self, pos): return map(lambda x: pos[x], self._joint_names) def convert_motion(self, name): return map(lambda x: self.joint_pos_d2l(x), self._motions[name]) # # def save_motion(self, name): with open("motions/"+name+".jpos", mode="w") as f: for pos in self._motions[name]: f.write(str(pos)) f.write("\n") # # def load_motion(self, name): self._motions[name]=[] with open("motions/"+name+".jpos") as f: motion=f.readlines() for p in motion: self._motions[name].append( eval(p) ) # # def get_joint_positions(self, name): if type(name) == str: if name in self._joint_positions: target_joints=self._joint_positions[name] else: print("Invalid position name") target_joints=None elif len(name) == 7: target_joints=name return target_joints #################################### # # Move Motion def move_to(self, name=None, tout=None, with_in_contact=False, wait_for_result=True): # # for Motion Controller Interface if type(name) == str and name in self._motions: waypoints=self.convert_motion(name) elif type(name) == list: waypoints=name else: print("Invalid motion name") return None self._motion_trajectory=MotionTrajectory(limb=self._limb) _wpt_opts=MotionWaypointOptions(max_joint_speed_ratio=self._max_speed_ratio, max_joint_accel=self._max_accel_ratio) _waypoint=MotionWaypoint(options=_wpt_opts, limb=self._limb) # # set current joint position... _waypoint.set_joint_angles(joint_angles=self._limb.joint_ordered_angles()) self._motion_trajectory.append_waypoint(_waypoint.to_msg()) # # set target joint position... for pos in waypoints: if type(pos) == str: if pos in self._joint_positions: pos=self._joint_positions[pos] _waypoint.set_joint_angles(joint_angles=pos) self._motion_trajectory.append_waypoint(_waypoint.to_msg()) else: _waypoint.set_joint_angles(joint_angles=pos) self._motion_trajectory.append_waypoint(_waypoint.to_msg()) # # if with_in_contact : opts=self.get_in_contact_opts() if opts : self._motion_trajectory.set_trajectory_options(opts) # # run motion... self._light.head_green() result=self._motion_trajectory.send_trajectory(wait_for_result=wait_for_result,timeout=tout) # # if result is None: self._light.head_yellow() print("Trajectory FAILED to send") return None # # if not wait_for_result : return True # if result.result: self._light.head_on() else: self._light.head_red() # # self._motion_trajectory=None return result.result # # Move in Certecian Mode def cart_move_to(self, target_pos, tout=None, relative_mode=False, wait_for_result=True): # # for Motion Controller Interface _trajectory_opts=TrajectoryOptions() _trajectory_opts.interpolation_type=TrajectoryOptions.CARTESIAN # self._motion_trajectory=MotionTrajectory(trajectory_options=_trajectory_opts, limb=self._limb) # # set Waypoint Options _wpt_opts=MotionWaypointOptions(max_linear_speed=self._linear_speed, max_linear_accel=self._linear_accel, max_rotational_speed=self._rotational_speed, max_rotational_accel=self._rotational_accel, max_joint_speed_ratio=1.0) _waypoint=MotionWaypoint(options=_wpt_opts, limb=self._limb) # endpoint_state=self._limb.tip_state(self._tip_name) pose=endpoint_state.pose ######################################## # set target position if relative_mode: # relative position : target_pos -> x, y, z, roll, pitch, yew trans = PyKDL.Vector(target_pos[0],target_pos[1],target_pos[2]) rot = PyKDL.Rotation.RPY(target_pos[3], target_pos[4],target_pos[5]) f2 = PyKDL.Frame(rot, trans) if self._in_tip_frame: # endpoint's cartesian systems pose=posemath.toMsg(posemath.fromMsg(pose) * f2) else: # base's cartesian systems pose=posemath.toMsg(f2 * posemath.fromMsg(pose)) else: # global position : x, y, z, rx, ry, rz, rw pose.position.x=target_pos[0] pose.position.y=target_pos[1] pose.position.z=target_pos[2] pose.orientation.x=target_pos[3] pose.orientation.y=target_pos[4] pose.orientation.z=target_pos[5] pose.orientation.w=target_pos[6] # ########################################### # set target position. poseStamped=PoseStamped() poseStamped.pose=pose _waypoint.set_cartesian_pose(poseStamped, self._tip_name, []) self._motion_trajectory.append_waypoint(_waypoint.to_msg()) # # run motion... self._light.head_green() result=self._motion_trajectory.send_trajectory( wait_for_result=wait_for_result,timeout=tout) # if result is None: self._light.head_yellow() print("Trajectory FAILED to send") return None # if not wait_for_result : return True # if result.result: self._light.head_on() else: self._light.head_red() # self._motion_trajectory=None return result.result # # stop motion... def stop_trajectory(self): if self._motion_trajectory : self._motion_trajectory.stop_trajectory() # # set Interaction control def set_interaction_params(self): interaction_options = InteractionOptions() interaction_options.set_interaction_control_active(self._interaction_active) interaction_options.set_K_impedance(self._K_impedance) interaction_options.set_max_impedance(self._max_impedance) interaction_options.set_interaction_control_mode(self._interaction_control_mode) interaction_options.set_in_endpoint_frame(self._in_endpoint_frame) interaction_options.set_force_command(self._force_command) interaction_options.set_K_nullspace(self._K_nullspace) interaction_options.set_endpoint_name(self._endpoint_name) if len(self._interaction_frame) == 7: quat_sum_square = self._interaction_frame[3]*self._interaction_frame[3] + self._interaction_frame[4]*self._interaction_frame[4] + self._interaction_frame[5]*self._interaction_frame[5] + self._interaction_frame[6]*self._interaction_frame[6] if quat_sum_square < 1.0 + 1e-7 and quat_sum_square > 1.0 - 1e-7: interaction_frame = Pose() interaction_frame.position.x = self._interaction_frame[0] interaction_frame.position.y = self._interaction_frame[1] interaction_frame.position.z = self._interaction_frame[2] interaction_frame.orientation.w = self._interaction_frame[3] interaction_frame.orientation.x = self._interaction_frame[4] interaction_frame.orientation.y = self._interaction_frame[5] interaction_frame.orientation.z = self._interaction_frame[6] interaction_options.set_interaction_frame(interaction_frame) else: print('Invalid input to quaternion! The quaternion must be a unit quaternion!') return None else: print('Invalid input to interaction_frame!') return None interaction_options.set_disable_damping_in_force_control(self._disable_damping_in_force_control) interaction_options.set_disable_reference_resetting(self._disable_reference_resetting) interaction_options.set_rotations_for_constrained_zeroG(self._rotations_for_constrained_zeroG) return interaction_options # # def set_interaction_mode(self): pub = rospy.Publisher('/robot/limb/right/interaction_control_command', InteractionControlCommand, queue_size = 1) interaction_options = self.set_interaction_params() if interaction_options: msg=interaction_options.to_msg() pub.publish(msg) # # def get_in_contact_opts(self): interaction_options = self.set_interaction_params() if interaction_options: trajectory_options = TrajectoryOptions() trajectory_options.interaction_control = True trajectory_options.interpolation_type = self._trajType trajectory_options.interaction_params = interaction_options.to_msg() return trajectory_options else: return None ############################################################## # # Open the gripper def gripper_open(self): if self._gripper and self._gripper.is_ready(): if self._is_clicksmart: if 'grip' in self._gripper.get_ee_signals() : self._gripper.set_ee_signal_value('grip', self._gripper_reverse) else: return None else: self._gripper.open() return True # # Close the gripper def gripper_close(self): if self._gripper and self._gripper.is_ready(): if self._is_clicksmart: if 'grip' in self._gripper.get_ee_signals() : print(self._gripper_reverse) self._gripper.set_ee_signal_value('grip', not self._gripper_reverse) else: return None else: self._gripper.close() return True # Grippper vacuum: True:off, False:on def gripper_vacuum(self, stat=True): if self._gripper and self._gripper.is_ready(): if self._is_clicksmart: if 'vacuumOn' in self._gripper.get_ee_signals() : self._gripper.set_ee_signal_value('vacuumOn', stat) return self._gripper.get_ee_signal_value('vacuumOn') return None def is_gripping(self): if self._gripper and self._gripper.is_ready(): if self._is_clicksmart: if self._gripper.get_ee_signal_value('grip') is None: return not self._gripper.get_ee_signal_value('vacuumOn') else: return self._gripper.get_ee_signal_value('grip') else: return self._gripper.is_gripping() return None ############################################################### # # stop the thread of velocity control loop def stop_vctrl(self): self._running=False self._vctrl_th.join() self._vctrl_th=None # # start vctrl_loop with Thread def start_vctrl(self, hz=100): self._vctrl_th=threading.Thread(target=self.vctrl_loop, args=(hz,self.report,)) self._vctrl_th.start() # # velocity control mode, one cycle def _vctrl_one_cycle(self, func=None): cmd={} cur=self._limb.joint_ordered_angles() dv=np.array(self._target) - np.array(cur) if np.linalg.norm(dv) < self._accuracy: if func: func(self) self._is_moving=False else: self._is_moving=True vels = map(lambda x: x*self._vrate, dv) for i,name in enumerate(self._joint_names): cmd[name]=maxmin(vels[i] , self._velocity_limits[name]*self._vmax, -self._velocity_limits[name]*self._vmax) self._limb.set_joint_velocities(cmd) # # velocity control loop def vctrl_loop(self, hz, func=None): rate=rospy.Rate(hz) self._running=True while self._running and (not rospy.is_shutdown()) : cuff_state=self._cuff.cuff_button() if cuff_state : self.set_target() elif self._limb.has_collided() : self.set_target() else: self._vctrl_one_cycle(func) rate.sleep() self._limb.exit_control_mode() print("Terminated") # # callback function for Subscriber def set_target_joint_pos(self, data): self._target=eval(data.data) # # def set_next_target(self, data): try: self.unset_subscriber('current_joint_pos') next_target=self._target_motion.pop(0) self.set_target(next_target) self.set_motion_sequencer() except: self.unset_subscriber('current_joint_pos') pass # # Publish current position def report(self,val): cur=self._limb.joint_ordered_angles() self._pub['current_joint_pos'].publish(str(cur)) # # Set target joint positions (Publish target joint positions) def set_target(self, val=None, relative=False): if val is None: val=self._limb.joint_ordered_angles() if type(val) is str: val=self._joint_positions[val] elif relative: if len(self._target) != len(val) : print("Dimension mismatch") return for i,v in enumerate(self._target): val[i]=v + val[i] self._pub['target_joint_pos'].publish(str(val)) def set_target_seq(self, targets): self._target_motion=targets self.set_next_target('star') def show_positions(self): print(self._joint_positions.keys()) # def set_cart_target(self, x,y,z,roll=9,pitch=0,yew=0, in_tip_frame=True): #pose = self.convert_Cart2Joint(x,y,z, relativa, end_point) pos=self.calc_cart_move2joints(x,y,z,roll, pitch,yew, in_tip_frame=in_tip_frame) val=self.joint_pos_d2l(pos) self._pub['target_joint_pos'].publish(str(val)) # # Set movement of target joint posistions def move_joint(self, idxs, vals): for i,v in enumerate(idxs) : self._target[v] += vals[i] self._pub['target_joint_pos'].publish(str(self._target)) # # end_point should be 'right_hand' or 'right_arm_base_link'. def convert_Cart2Joint(self, x,y,z, relative=False, end_point='right_hand'): _pose=self.endpoint_pose() if relative: _pose.position.x += x _pose.position.y += y _pose.position.z += z else: _pose.position.x = x _pose.position.y = y _pose.position.z = z return self._limb.ik_request(_pose, end_point=end_point) # # def convert_Joint2Cart(self, pos): _pos=self._limb.joint_angles() for i,name in enumerate(self._joint_names): _pos[name]=pos[i] return self._limb.joint_angles_to_cartesian_pose(_pos) def endpoint_pose(self): return self._limb.tip_state('right_hand').pose #return self._limb.joint_angles_to_cartesian_pose(self._limb.joint_angles()) def calc_relative_pose(self, x,y,z,roll=0,pitch=0,yew=0, in_tip_frame=True): _pose=self.endpoint_pose() ######################################## # set target position trans = PyKDL.Vector(x,y,z) rot = PyKDL.Rotation.RPY(roll,pitch,yew) f2 = PyKDL.Frame(rot, trans) if in_tip_frame: # endpoint's cartesian systems _pose=posemath.toMsg(posemath.fromMsg(_pose) * f2) else: # base's cartesian systems _pose=posemath.toMsg(f2 * posemath.fromMsg(_pose)) return _pose def calc_cart_move2joints(self, x,y,z,roll=0,pitch=0,yew=0, in_tip_frame=True): _pose=self.calc_relative_pose(x,y,z,roll,pitch,yew, in_tip_frame) return self._limb.ik_request(_pose) ######################################################################## # # onExecuted # def onExecute(self): cuff_state=self._cuff.cuff_button() if self._is_pause : self._limb.set_joint_velocities(self._stop_cmd) elif cuff_state : self.set_target() elif self._limb.has_collided() : self.set_target() else: self._vctrl_one_cycle(self.report) # # for RTC(Common) # def clearAlarms(self): print('No alerm..') return True def getActiveAlarm(self): res=[] return res def getFeedbackPosJoint(self): res=map(lambda x: np.rad2deg(x), self._limb.joint_ordered_angles()) res.append(self.gripper_state()) return res # manifactur: string # type: string # axisNum: ULONG # cmdCycle: ULONG # isGripper: boolea def getManipInfo(self): if self._gripper : res=['RethinkRobotics', 'Sawyer', 7, 100, True] else: res=['RethinkRobotics', 'Sawyer', 7, 100, False] return res def getSoftLimitJoint(self): return None def getState(self): stat=self._rs.state() if stat.enabled : res=0x01 if self._is_moveing : res=0x01 | 0x02 else: res=0x00 return res def servoOFF(self): return None def servoON(self): return None def setSoftLimitJoint(self, softLimit): return None # # for RTC(Middle) # def openGripper(self): if self._gripper_type == 'vacuum': self.gripper_vacuum(False) else: self.gripper_open() return True def closeGripper(self): if self._gripper_type == 'vacuum': self.gripper_vacuum(True) else: self.gripper_close() return True def moveGripper(self, angleRatio): print('Move gripper is not supported') return None def getBaseOffset(self): return None def getFeedbackPosCartesian(self): return None def getMaxSpeedCartesian(self): return None def getMaxSpeedJoint(self): return None def getMinAccelTimeCartesian(self): return None def getMinAccelTimeJoint(self): return None def getSoftLimitCartesian(self): return None def moveLinearCartesianAbs(self, carPos, elbow, flag): return None def moveLinearCartesianRel(self, carPos, elbow, flag): return None def movePTPCartesianAbs(self, carPos, elbow, flag): mx=np.array(carPos) posm=map(lambda x: x/1000.0, mx[:,3]) mx=np.vstack((mx, [0,0,0,1])) qtn=tf.transformations.quaternion_from_matrix(mx) pose=newPose(posm, qtn) angles=self._limb.ik_request(pose) self._target=self.joint_pos_d2l(angles) return True def movePTPCartesianRel(self, carPos, elbow, flag): ep=self.endpoint_pose() mx=np.array(carPos) p=map(lambda x: x/1000.0, mx[:,3]) mx=np.vstack((mx, [0,0,0,1])) el = tf.transformations.euler_from_matrix(mx) pos=self.calc_cart_move2joints(p[0],p[1],p[2],el[0],el[1],el[2], (flag==1)) self._target=self.joint_pos_d2l(pos) return True def movePTPJointAbs(self, jointPoints): if len(jointPoints) >= 7: pos=map(lambda x: np.deg2rad(x), jointPoints[:7]) self._target=pos return True else: return False def movePTPJointRel(self, jointPoints): if len(jointPoints) >= 7: pos=map(lambda x: np.deg2rad(x), jointPoints[:7]) self._target=np.array(self._target) + np.array(pos) return True else: return False def pause_motion(self): self._is_pause=True self._limb.set_joint_velocities(self._stop_cmd) return True def resume_motion(self): self._is_pause=False return True def stop_motion(self): self.set_target() return True def setAccelTimeCartesian(self, aclTime): return None def setAccelTimeJoint(self, aclTime): return None def setBaseOffset(self, offset): return None def setControlPointOffset(self, offset): return None def setMaxSpeedCartesian(self, sp_trans, sp_rot): return None def setMaxSpeedJoint(self, speed): return None def setMinAccelTimeCartesian(self, aclTime): return None def setMinAccelTimeJoint(self, aclTime): return None def setSoftLimitCartesian(self, xLimit, yLimit, zLimit): return None def setSpeedCartesian(self, spdRatio): return None def setSpeedJoint(self, spdRatio): self.set_speed(spdRatio) return True def moveCircularCartesianAbs(self, carPointR, carPointT): return None def moveCircularCartesianRel(self, carPointR, carPointT): return None def setHome(self, jointPoint): self._home_pos=jointPoint return True def getHome(self): return self._home_pos def goHome(self): self._target=self._home_pos return True # # def moveCartesianRel(self, pos, rot, flag): p=map(lambda x: x/1000.0, pos) r=map(lambda x: np.deg2rad(x), rot) self.set_cart_target(p[0], p[1], p[2], r[0], r[1], r[2], flag) return True
def main(): arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) ##### parser.add_argument( "-p", "--position", type=float, nargs='+', default=[0, 0, 0], help="Desired end position: X, Y, Z") parser.add_argument( "-o", "--orientation", type=float, nargs='+', default=[0.704020578925, 0.710172716916, 0.00244101361829, 0.00194372088834], help="Orientation as a quaternion (x, y, z, w)") ##### parser.add_argument( "-q", "--joint_angles", type=float, nargs='+', default=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0], help="A list of joint angles, one for each of the 7 joints, J0...J6") parser.add_argument( "-s", "--speed_ratio", type=float, default=0.2, help="A value between 0.001 (slow) and 1.0 (maximum joint velocity)") parser.add_argument( "-a", "--accel_ratio", type=float, default=0.05, help="A value between 0.001 (slow) and 1.0 (maximum joint accel)") parser.add_argument( "-t", "--trajType", type=str, default='JOINT', choices=['JOINT', 'CARTESIAN'], help="trajectory interpolation type") parser.add_argument( "-st", "--interaction_active", type=int, default=1, choices = [0, 1], help="Activate (1) or Deactivate (0) interaction controller") parser.add_argument( "-k", "--K_impedance", type=float, nargs='+', default=[1300.0, 1300.0, 1300.0, 30.0, 30.0, 30.0], help="A list of desired stiffnesses, one for each of the 6 directions -- stiffness units are (N/m) for first 3 and (Nm/rad) for second 3 values") parser.add_argument( "-m", "--max_impedance", type=int, nargs='+', default=[1, 1, 1, 1, 1, 1], choices = [0, 1], help="A list of impedance modulation state, one for each of the 6 directions (a single value can be provided to apply the same value to all the directions) -- 0 for False, 1 for True") parser.add_argument( "-md", "--interaction_control_mode", type=int, nargs='+', default=[1, 1, 1, 1, 1, 1], choices = [1,2,3,4], help="A list of desired interaction control mode (1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit), one for each of the 6 directions") parser.add_argument( "-fr", "--interaction_frame", type=float, nargs='+', default=[0, 0, 0, 1, 0, 0, 0], help="Specify the reference frame for the interaction controller -- first 3 values are positions [m] and last 4 values are orientation in quaternion (w, x, y, z)") parser.add_argument( "-ef", "--in_endpoint_frame", action='store_true', default=False, help="Set the desired reference frame to endpoint frame; otherwise, it is base frame by default") parser.add_argument( "-en", "--endpoint_name", type=str, default='right_hand', help="Set the desired endpoint frame by its name; otherwise, it is right_hand frame by default") parser.add_argument( "-f", "--force_command", type=float, nargs='+', default=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], help="A list of desired force commands, one for each of the 6 directions -- in force control mode this is the vector of desired forces/torques to be regulated in (N) and (Nm), in impedance with force limit mode this vector specifies the magnitude of forces/torques (N and Nm) that the command will not exceed") parser.add_argument( "-kn", "--K_nullspace", type=float, nargs='+', default=[5.0, 10.0, 5.0, 10.0, 5.0, 10.0, 5.0], help="A list of desired nullspace stiffnesses, one for each of the 7 joints (a single value can be provided to apply the same value to all the directions) -- units are in (Nm/rad)") parser.add_argument( "-dd", "--disable_damping_in_force_control", action='store_true', default=False, help="Disable damping in force control") parser.add_argument( "-dr", "--disable_reference_resetting", action='store_true', default=False, help="The reference signal is reset to actual position to avoid jerks/jumps when interaction parameters are changed. This option allows the user to disable this feature.") parser.add_argument( "-rc", "--rotations_for_constrained_zeroG", action='store_true', default=False, help="Allow arbitrary rotational displacements from the current orientation for constrained zero-G (use only for a stationary reference orientation)") parser.add_argument( "--timeout", type=float, default=None, help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.") args = parser.parse_args(rospy.myargv()[1:]) try: rospy.init_node('path_planner_py') limb = Limb() traj = MotionTrajectory(limb = limb) wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=args.speed_ratio, max_joint_accel=args.accel_ratio) waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb) # joint_angles = limb.joint_ordered_angles() # waypoint.set_joint_angles(joint_angles = joint_angles) # traj.append_waypoint(waypoint.to_msg()) # joint = ik_service_client(poses).values()[::-1] # joint angles from J0 to J6 # if len(joint_angles) != len(joint_angles): # rospy.logerr('The number of joint_angles must be %d', len(joint_angles)) # return None # # waypoint.set_joint_angles(joint_angles = args.joint_angles) # waypoint.set_joint_angles(joint_angles = joint) #####divide the whole path into three parts: soft begin, uniform motion, soft stop#### final_pos = args.position # get endpoint state endpoint_state = limb.tip_state('right_hand') current_pos = endpoint_state.pose.position dis = [final_pos[0]-current_pos.x, final_pos[1]-current_pos.y, final_pos[2]-current_pos.z] uniform_motion = [current_pos.x + dis[0]/5, current_pos.y + dis[1]/5, current_pos.z + dis[2]/5] soft_stop = [current_pos.x + 4*dis[0]/5, current_pos.y + 4*dis[1]/5, current_pos.z + 4*dis[2]/5] ####################################################################################### # waypoint = path_planning(uniform_motion, args.orientation, 0.25, 0.01) # traj.append_waypoint(waypoint.to_msg()) # waypoint = path_planning(soft_stop, args.orientation, 0.25, 0) # traj.append_waypoint(waypoint.to_msg()) # waypoint = path_planning(final_pos, args.orientation, 0.25, 0.01) # # joint = path_planning(uniform_motion, args.orientation, 0.2, 0.1) # joint angles from J0 to J6 # # waypoint.set_joint_angles(joint_angles = joint) # # traj.append_waypoint(waypoint.to_msg()) # # joint = path_planning(soft_stop, args.orientation, 0.2, 0.1) # joint angles from J0 to J6 # # waypoint.set_joint_angles(joint_angles = joint) # # traj.append_waypoint(waypoint.to_msg()) # # joint = path_planning(final_pos, args.orientation, 0.2, 0.1) # joint angles from J0 to J6 # # waypoint.set_joint_angles(joint_angles = joint) # traj.append_waypoint(waypoint.to_msg()) ###########open traj file filename = 'traj' with open(filename, 'r') as f: lines = f.readlines() l = len(lines) - 1 wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.5, max_joint_accel=0.01) waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb) for line in lines[1:int(floor(2*l/5))]: print(line) jnt_angles = [float(x) for x in line.rstrip().split(',')[1:8]] waypoint.set_joint_angles(joint_angles = jnt_angles) traj.append_waypoint(waypoint.to_msg()) wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.5, max_joint_accel=0) waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb) for line in lines[int(floor(2*l/5)):int(floor(3*l/5))]: print(line) jnt_angles = [float(x) for x in line.rstrip().split(',')[1:8]] waypoint.set_joint_angles(joint_angles = jnt_angles) traj.append_waypoint(waypoint.to_msg()) wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.5, max_joint_accel=0.01) waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb) for line in lines[int(floor(3*l/5)):]: print(line) jnt_angles = [float(x) for x in line.rstrip().split(',')[1:8]] waypoint.set_joint_angles(joint_angles = jnt_angles) traj.append_waypoint(waypoint.to_msg()) # set the interaction control options in the current configuration interaction_options = InteractionOptions() trajectory_options = TrajectoryOptions() trajectory_options.interaction_control = True trajectory_options.interpolation_type = args.trajType interaction_options.set_interaction_control_active(int2bool(args.interaction_active)) interaction_options.set_K_impedance(args.K_impedance) interaction_options.set_max_impedance(int2bool(args.max_impedance)) interaction_options.set_interaction_control_mode(args.interaction_control_mode) interaction_options.set_in_endpoint_frame(int2bool(args.in_endpoint_frame)) interaction_options.set_force_command(args.force_command) interaction_options.set_K_nullspace(args.K_nullspace) interaction_options.set_endpoint_name(args.endpoint_name) if len(args.interaction_frame) < 7: rospy.logerr('The number of elements must be 7!') elif len(args.interaction_frame) == 7: quat_sum_square = args.interaction_frame[3]*args.interaction_frame[3] + args.interaction_frame[4]*args.interaction_frame[4] + args.interaction_frame[5]*args.interaction_frame[5] + args.interaction_frame[6]*args.interaction_frame[6] if quat_sum_square < 1.0 + 1e-7 and quat_sum_square > 1.0 - 1e-7: interaction_frame = Pose() interaction_frame.position.x = args.interaction_frame[0] interaction_frame.position.y = args.interaction_frame[1] interaction_frame.position.z = args.interaction_frame[2] interaction_frame.orientation.w = args.interaction_frame[3] interaction_frame.orientation.x = args.interaction_frame[4] interaction_frame.orientation.y = args.interaction_frame[5] interaction_frame.orientation.z = args.interaction_frame[6] interaction_options.set_interaction_frame(interaction_frame) else: rospy.logerr('Invalid input to quaternion! The quaternion must be a unit quaternion!') else: rospy.logerr('Invalid input to interaction_frame!') interaction_options.set_disable_damping_in_force_control(args.disable_damping_in_force_control) interaction_options.set_disable_reference_resetting(args.disable_reference_resetting) interaction_options.set_rotations_for_constrained_zeroG(args.rotations_for_constrained_zeroG) trajectory_options.interaction_params = interaction_options.to_msg() traj.set_trajectory_options(trajectory_options) result = traj.send_trajectory(timeout=args.timeout) if result is None: rospy.logerr('Trajectory FAILED to send!') return if result.result: rospy.loginfo('Motion controller successfully finished the trajectory with interaction options set!') else: rospy.logerr('Motion controller failed to complete the trajectory with error %s', result.errorId) # print the resultant interaction options rospy.loginfo('Interaction Options:\n%s', interaction_options.to_msg()) except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user. %s', 'Exiting before trajectory completion.')
def interaction_joint_trajectory( limb, joint_angles, trajType, interaction_active, interaction_control_mode, interaction_frame, speed_ratio, accel_ratio, K_impedance, max_impedance, in_endpoint_frame, force_command, K_nullspace, endpoint_name, timeout, disable_damping_in_force_control, disable_reference_resetting, rotations_for_constrained_zeroG): try: traj = MotionTrajectory(limb=limb) wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=speed_ratio, max_joint_accel=accel_ratio) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) # one single waypoint current_joint_angles = limb.joint_ordered_angles() waypoint.set_joint_angles(joint_angles=current_joint_angles) traj.append_waypoint(waypoint.to_msg()) if len(current_joint_angles) != len(joint_angles): rospy.logerr('The number of joint_angles must be %d', len(current_joint_angles)) return None # ----- testing intermediate points with real robot middle_joint_angles = [ (current_joint_angles[i] + joint_angles[i]) / 2.0 for i in xrange(len(current_joint_angles)) ] waypoint.set_joint_angles(joint_angles=middle_joint_angles) traj.append_waypoint(waypoint.to_msg()) waypoint.set_joint_angles(joint_angles=joint_angles) traj.append_waypoint(waypoint.to_msg()) # ----- end testing intermediate points with real robot # set the interaction control options in the current configuration interaction_options = InteractionOptions() trajectory_options = TrajectoryOptions() trajectory_options.interaction_control = True trajectory_options.interpolation_type = trajType interaction_options.set_interaction_control_active( int2bool(interaction_active)) interaction_options.set_K_impedance(K_impedance) interaction_options.set_max_impedance(int2bool(max_impedance)) interaction_options.set_interaction_control_mode( interaction_control_mode) interaction_options.set_in_endpoint_frame(int2bool(in_endpoint_frame)) interaction_options.set_force_command(force_command) interaction_options.set_K_nullspace(K_nullspace) interaction_options.set_endpoint_name(endpoint_name) if len(interaction_frame) < 7: rospy.logerr('The number of elements must be 7!') elif len(interaction_frame) == 7: quat_sum_square = interaction_frame[3] * interaction_frame[ 3] + interaction_frame[4] * interaction_frame[ 4] + interaction_frame[5] * interaction_frame[ 5] + interaction_frame[6] * interaction_frame[6] if quat_sum_square < 1.0 + 1e-7 and quat_sum_square > 1.0 - 1e-7: target_interaction_frame = Pose() target_interaction_frame.position.x = interaction_frame[0] target_interaction_frame.position.y = interaction_frame[1] target_interaction_frame.position.z = interaction_frame[2] target_interaction_frame.orientation.w = interaction_frame[3] target_interaction_frame.orientation.x = interaction_frame[4] target_interaction_frame.orientation.y = interaction_frame[5] target_interaction_frame.orientation.z = interaction_frame[6] interaction_options.set_interaction_frame( target_interaction_frame) else: rospy.logerr( 'Invalid input to quaternion! The quaternion must be a unit quaternion!' ) else: rospy.logerr('Invalid input to interaction_frame!') interaction_options.set_disable_damping_in_force_control( disable_damping_in_force_control) interaction_options.set_disable_reference_resetting( disable_reference_resetting) interaction_options.set_rotations_for_constrained_zeroG( rotations_for_constrained_zeroG) trajectory_options.interaction_params = interaction_options.to_msg() traj.set_trajectory_options(trajectory_options) result = traj.send_trajectory(timeout=timeout) if result is None: rospy.logerr('Trajectory FAILED to send!') return if result.result: rospy.loginfo( 'Motion controller successfully finished the trajectory with interaction options set!' ) else: rospy.logerr( 'Motion controller failed to complete the trajectory with error %s', result.errorId) # print the resultant interaction options rospy.loginfo('Interaction Options:\n%s', interaction_options.to_msg()) except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user. %s', 'Exiting before trajectory completion.')