def _guarded_move_to_joint_position_two(self, set_joint_angles, speed_ratio = 0.4, accel_ratio = 0.2, timeout = 10.0): try: traj = MotionTrajectory(limb = self._limb) wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=speed_ratio, max_joint_accel=accel_ratio) waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = self._limb) joint_angles = self._limb.joint_ordered_angles() waypoint.set_joint_angles(joint_angles = joint_angles) traj.append_waypoint(waypoint.to_msg()) if len(set_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 = set_joint_angles) traj.append_waypoint(waypoint.to_msg()) 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!') else: rospy.logerr('Motion controller failed to complete the trajectory with error %s', result.errorId) except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.')
def goToPose(self): limb = Limb() traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options=traj_options, limb=limb) wpt_opts = MotionWaypointOptions(max_linear_speed=0.1, max_linear_accel=0.1, max_rotational_speed=0.1, max_rotational_accel=0.1, max_joint_speed_ratio=1.0) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) joint_names = limb.joint_names() waypoint.set_joint_angles(self.ikResults.values(), "left_hand", joint_names) traj.append_waypoint(waypoint.to_msg()) result = traj.send_trajectory(timeout=30.0) if result is None: rospy.logerr('Trajectory Failed') return if result.result: rospy.loginfo('Trajectory Success') else: rospy.logerr('Trajectory failed with error %s', result.errorId)
def linear_movement(self,position, linear_speed = 0.2, linear_accel = 0.2, rotational_speed = 0.1, rotational_accel = 0.1): try: traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options = traj_options, limb = self._limb) wpt_opts = MotionWaypointOptions(max_linear_speed=linear_speed, max_linear_accel=linear_accel, max_rotational_speed=rotational_speed, max_rotational_accel=rotational_accel, max_joint_speed_ratio=0.2) waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = self._limb) poseStamped = PoseStamped() poseStamped.pose = position waypoint.set_cartesian_pose(poseStamped, self._tip_name) rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string()) traj.append_waypoint(waypoint.to_msg()) result = traj.send_trajectory(timeout=5.0) if result is None: rospy.logerr('Trajectory FAILED to send') return if result.result: rospy.loginfo('Motion controller successfully finished the trajectory!') else: rospy.logerr('Motion controller failed to complete the trajectory with error %s', result.errorId) except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.')
def move_to_joint_positions(self, positions, speed=None, acceleration=None): speed = speed if speed is not None else self.speed acceleration = acceleration if acceleration is not None else self.acceleration traj = MotionTrajectory(limb=self.limb) wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=speed, max_joint_accel=acceleration) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=self.limb) if isinstance(positions, dict): joint_angles = positions.values() waypoint.set_joint_angles(joint_angles=joint_angles) traj.set_joint_names(positions.keys()) traj.append_waypoint(waypoint.to_msg()) else: rospy.logerr("Incorrect inputs to move_to_joint_positions") return result = traj.send_trajectory(timeout=30) 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)
def joint_angles(args): # arg_fmt = argparse.RawDescriptionHelpFormatter # parser = argparse.ArgumentParser(formatter_class=arg_fmt, # description=main.__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.5, # 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( # "--timeout", type=float, default=None, # help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.") try: #rospy.init_node('go_to_joint_angles_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()) 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!') return True else: rospy.logerr( 'Motion controller failed to complete the trajectory with error %s', result.errorId) except rospy.ROSInterruptException: rospy.logerr( 'Keyboard interrupt detected from the user. Exiting before trajectory completion.' )
def path_planning(position, orientation ,max_speed = 0.1, max_accel = 0.1): """ Plan the path from current position to the desired position Parameters ---------------- position: list, the desired destination max_speed: float, the maximum value of the speed we want max_accel: float, the maximum value of the accel we want Return ---------------- waypoint: MotionWaypoint the planned path. May only be a part of the final trajectory """ poses = { 'right': PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id='base'), pose=Pose( position=Point( x=position[0], y=position[1], z=position[2], ), orientation=Quaternion( x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3], ), ), ), } limb = Limb() wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=max_speed, max_joint_accel=max_accel) waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb) joint = ik_service_client(poses).values()[::-1] # joint angles from J0 to J6 if len(joint) != 7: rospy.logerr('The number of joint_angles must be 7') return None waypoint.set_joint_angles(joint_angles = joint) return waypoint
def move(self, point_list, wait = True, MAX_LIN_SPD=7.0, MAX_LIN_ACCL=1.5): # one point = [x_coord, y_coord, z_coord, x_deg, y_deg, z_deg] try: limb = Limb() # point_list = [pointA, pointB, pointC, ...] traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options=traj_options, limb=limb) except: print("There may have been an error while exiting") if self.STOP: traj.stop_trajectory() return True wpt_opts = MotionWaypointOptions(max_linear_speed=MAX_LIN_SPD, max_linear_accel=MAX_LIN_ACCL, corner_distance=0.002) for point in point_list: q_base = quaternion_from_euler(0, 0, math.pi/2) #q_rot = quaternion_from_euler(math.radians(point[3]), math.radians(point[4]), math.radians(point[5])) q_rot = quaternion_from_euler(point[3], point[4], point[5]) q = quaternion_multiply(q_rot, q_base) newPose = PoseStamped() newPose.header = Header(stamp=rospy.Time.now(), frame_id='base') newPose.pose.position.x = point[0] + 0.65 newPose.pose.position.y = point[1] + 0.0 newPose.pose.position.z = point[2] + 0.4 newPose.pose.orientation.x = q[0] newPose.pose.orientation.y = q[1] newPose.pose.orientation.z = q[2] newPose.pose.orientation.w = q[3] waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) waypoint.set_cartesian_pose(newPose, "right_hand", limb.joint_ordered_angles()) traj.append_waypoint(waypoint.to_msg()) if(wait): print(" \n --- Sending trajectory and waiting for finish --- \n") result = traj.send_trajectory(wait_for_result=wait) if result is None: rospy.logerr('Trajectory FAILED to send') success = False elif result.result: rospy.loginfo('Motion controller successfully finished the trajcetory') success = True else: rospy.logerr('Motion controller failed to complete the trajectory. Error: %s', result.errorId) success = False else: print("\n --- Sending trajector w/out waiting --- \n") traj.send_trajectory(wait_for_result=wait) success = True return success
def go_to_pose(self, position, orientation): try: traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options=traj_options, limb=self._right_arm) wpt_opts = MotionWaypointOptions(max_linear_speed=0.6, max_linear_accel=0.6, max_rotational_speed=1.57, max_rotational_accel=1.57, max_joint_speed_ratio=1.0) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=self._right_arm) pose = Pose() pose.position.x = position[0] pose.position.y = position[1] pose.position.z = position[2] pose.orientation.x = orientation[0] pose.orientation.y = orientation[1] pose.orientation.z = orientation[2] pose.orientation.w = orientation[0] poseStamped = PoseStamped() poseStamped.pose = pose joint_angles = self._right_arm.joint_ordered_angles() waypoint.set_cartesian_pose(poseStamped, "right_hand", joint_angles) rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string()) traj.append_waypoint(waypoint.to_msg()) result = traj.send_trajectory(timeout=10) if result is None: rospy.logerr('Trajectory FAILED to send') return if result.result: rospy.loginfo( 'Motion controller successfully finished the trajectory!') else: rospy.logerr( 'Motion controller failed to complete the trajectory with error %s', result.errorId) except rospy.ROSInterruptException: rospy.logerr( 'Keyboard interrupt detected from the user. Exiting before trajectory completion.' )
def moveRoboticArm(position, orientation, linear_speed, linear_accel): """ Move the robot arm to the specified configuration given a positionX, positionY, positionZ, quaternion array and max linear speed. """ try: limb = Limb() traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options=traj_options, limb=limb) wpt_opts = MotionWaypointOptions(max_linear_speed=linear_speed, max_linear_accel=linear_accel) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) joint_names = limb.joint_names() endpoint_state = limb.tip_state('right_hand') pose = endpoint_state.pose if position is not None and len(position) == 3: pose.position.x = position[0] pose.position.y = position[1] pose.position.z = position[2] if orientation is not None and len(orientation) == 4: pose.orientation.x = orientation[0] pose.orientation.y = orientation[1] pose.orientation.z = orientation[2] pose.orientation.w = orientation[3] poseStamped = PoseStamped() poseStamped.pose = pose joint_angles = limb.joint_ordered_angles() waypoint.set_cartesian_pose(poseStamped, 'right_hand', joint_angles) rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string()) traj.append_waypoint(waypoint.to_msg()) result = traj.send_trajectory() if result is None: rospy.logerr('Trajectory FAILED to send') return if result.result: rospy.loginfo( 'Motion controller successfully finished the trajectory!') else: rospy.logerr( 'Motion controller failed to complete the trajectory with error %s', result.errorId) except rospy.ROSInterruptException: print("Something went wrong") rospy.logerr( 'Keyboard interrupt detected from the user. Exiting before trajectory completion.' )
def go_to_joint_angles( self, joint_angles=[math.pi / 2, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0], speed_ratio=0.5, accel_ratio=0.5, timeout=None, ways=False): try: if isinstance(joint_angles, dict): joint_angles_arr = [0] * len(joint_angles) for j in range(len(joint_angles_arr)): joint_angles_arr[j] = joint_angles['right_j' + str(j)] joint_angles = joint_angles_arr traj = MotionTrajectory(limb=self) wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=speed_ratio, max_joint_accel=accel_ratio) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=self) joint_angles_start = self.joint_ordered_angles() waypoint.set_joint_angles(joint_angles=joint_angles_start) traj.append_waypoint(waypoint.to_msg()) if len(joint_angles) != len(joint_angles_start): rospy.logerr('The number of joint_angles must be %d', len(joint_angles_start)) return None waypoint.set_joint_angles(joint_angles=joint_angles) traj.append_waypoint(waypoint.to_msg()) 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!') return True else: #rospy.logerr('Motion controller failed to complete the trajectory with error %s',result.errorId) if ways == True: rospy.loginfo('Collision anticipated') self.go_to_joint_angles() return True else: rospy.loginfo( 'Could not complete trajectory due to collision') return False except rospy.ROSInterruptException: rospy.logerr( 'Keyboard interrupt detected from the user. Exiting before trajectory completion.' )
def __init__(self): self.robot_state = 0 # normal self.breath_state =0 # false rospy.Subscriber("cs_sawyer/head_light", UInt8, self.callback_update_breath1) rospy.Subscriber("cs_sawyer/breath", Bool, self.callback_update_breath2) self.rospack = rospkg.RosPack() # Set the trajectory options self.limb = Limb() traj_opts = TrajectoryOptions() traj_opts.interpolation_type = 'JOINT' self.traj = MotionTrajectory(trajectory_options = traj_opts, limb = self.limb) # Set the waypoint options wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.05, joint_tolerances=0.7) #max_joint_accel=0.1) waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = self.limb) # Append a waypoint at the current pose waypoint.set_joint_angles(self.limb.joint_ordered_angles()) #self.traj.append_waypoint(waypoint.to_msg()) #self.limb.set_joint_position_speed(0.3) with open(join(self.rospack.get_path("cs_sawyer"), "config/poses.json")) as f: self.poses = json.load(f) joint_angles= [self.poses["pause"][j] for j in [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6']] j1= joint_angles[1] j2= joint_angles[2] x=0 while x < 16*pi: new_j1 = 0.07*sin(x)+j1 new_j2=0.09*sin(0.5*x)+j2 joint_angles[1]=new_j1 joint_angles[2]=new_j2 x=x+pi/40 waypoint.set_joint_angles(joint_angles = joint_angles) self.traj.append_waypoint(waypoint.to_msg())
def goto_cartesian(self, x, y, z): print("goto_cartesian called with x={}, y={}, z={}".format(x, y, z)) limb = Limb() traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options=traj_options, limb=limb) wpt_opts = MotionWaypointOptions() waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) joint_names = limb.joint_names() endpoint_state = limb.tip_state('right_hand') if endpoint_state is None: print('Endpoint state not found') return self.MOVE_ERROR pose = endpoint_state.pose pose.position.x = x pose.position.y = y pose.position.z = z pose.orientation.x = self.orientation_x pose.orientation.y = self.orientation_y pose.orientation.z = self.orientation_z pose.orientation.w = self.orientation_w poseStamped = PoseStamped() poseStamped.pose = pose waypoint.set_cartesian_pose(poseStamped, 'right_hand', []) traj.append_waypoint(waypoint.to_msg()) result = traj.send_trajectory() if result is None: print("Trajectory FAILED to send") return self.MOVE_ERROR if result.result: print('Motion controller successfully finished the trajectory!') self.cur_x = x self.cur_y = y self.cur_z = z return self.MOVE_SUCCESS else: print('Motion controller failed to complete the trajectory %s', result.errorId) return self.MOVE_ERROR
def gripper_pose(x, y, z): limb = Limb() traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options=traj_options, limb=limb) wpt_opts = MotionWaypointOptions() waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) joint_names = limb.joint_names() endpoint_state = limb.tip_state('right_hand') if endpoint_state is None: print('Endpoint state not found') return False pose = endpoint_state.pose pose.position.x = x pose.position.y = y pose.position.z = z pose.orientation.x = 0 pose.orientation.y = 0 pose.orientation.z = 0 pose.orientation.w = 1 poseStamped = PoseStamped() poseStamped.pose = pose waypoint.set_cartesian_pose(poseStamped, 'right_hand', []) traj.append_waypoint(waypoint.to_msg()) result = traj.send_trajectory() if result is None: print("Trajectory FAILED to send") return False if result.result: return True else: print('Motion controller failed to complete the trajectory %s', result.errorId) return False
def move_move(limb, group, target, speed_ratio=None, accel_ratio=None, timeout=None): if speed_ratio is None: speed_ratio = 0.3 if accel_ratio is None: accel_ratio = 0.1 plan = group.plan(target) # rospy.sleep(1) step = [] for point in plan.joint_trajectory.points: step.append(point.positions) 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) for point in step: waypoint.set_joint_angles(joint_angles=point) traj.append_waypoint(waypoint.to_msg()) traj.send_trajectory(timeout=timeout) group.stop() group.clear_pose_targets()
def go_to_angles(joint_angles_goal, speed_ratio_goal, accel_ratio_goal, timeout_goal): try: #rospy.init_node('go_to_joint_angles_py') # initialze once limb = Limb() traj = MotionTrajectory(limb=limb) wpt_opts = MotionWaypointOptions( max_joint_speed_ratio=speed_ratio_goal, max_joint_accel=accel_ratio_goal) 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(joint_angles_goal) != len(joint_angles): rospy.logerr('The number of joint_angles must be %d', len(joint_angles)) return None waypoint.set_joint_angles(joint_angles=joint_angles_goal) traj.append_waypoint(waypoint.to_msg()) result = traj.send_trajectory(timeout=timeout_goal) if result is None: rospy.logerr('Trajectory FAILED to send') return if result.result: rospy.loginfo( 'Motion controller successfully finished the trajectory!') else: rospy.logerr( 'Motion controller failed to complete the trajectory with error %s', result.errorId) except rospy.ROSInterruptException: rospy.logerr( 'Keyboard interrupt detected from the user. Exiting before trajectory completion.' )
def move_move_haha(limb, group, positions, speed_ratio=None, accel_ratio=None, timeout=None): # this move method is obtained from Intera motion control interface # limb -- the limb object of the Sawyer robot # position -- a list with a length of 7, specifying the goal state joint position # speed_ratio -- the speed of robot arm, [0, 1] # accel_ratio -- the acceleration of the robot arm, [0, 1] # timeout -- the timeout for this specific motion; infinite when None rospy.sleep(1) if accel_ratio is None: accel_ratio = 0.1 if speed_ratio is None: speed_ratio = 0.3 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) waypoint.set_joint_angles(joint_angles=positions) traj.append_waypoint(waypoint.to_msg()) traj.send_trajectory(timeout=timeout) rospy.sleep(1)
def gen_rand_waypoint(self): newX = (random.random() / 5) - 0.1 newY = (random.random() / 5) - 0.7 newZ = (random.random() / 5) + 0.4 newPose = PoseStamped() newPose.header = Header(stamp=rospy.Time.now(), frame_id='base') newPose.pose.position.x = newX newPose.pose.position.y = newY newPose.pose.position.z = newZ newPose.pose.orientation.x = 0.5 newPose.pose.orientation.y = -0.5 newPose.pose.orientation.z = 0.5 newPose.pose.orientation.w = 0.5 limb = Limb() wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=1.0, max_joint_accel=1.0, corner_distance=0.05) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) waypoint.set_cartesian_pose(newPose, "right_hand", limb.joint_ordered_angles()) self.waypoints.append(waypoint)
self.prevX = nextX self.prevY = nextY newPose = PoseStamped() newPose.header = Header(stamp=rospy.Time.now(), frame_id='base') newPose.pose.position.x = nextX newPose.pose.position.y = nextY newPose.pose.position.z = nextZ newPose.pose.orientation.x = q[0] newPose.pose.orientation.y = q[1] newPose.pose.orientation.z = q[2] newPose.pose.orientation.w = q[3] wpt_opts = MotionWaypointOptions(max_linear_speed=7.0, max_linear_accel=5.0, corner_distance=0.085) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) waypoint.set_cartesian_pose(newPose, "right_hand", limb.joint_ordered_angles()) self.waypoints.append(copy(waypoint)) if len(self.waypoints) > 240: self.waypoints.pop(0) def run(self): rate = rospy.Rate(100) limb = Limb() traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options=traj_options, limb=limb) wpt_opts = MotionWaypointOptions(max_linear_speed=7.0, max_linear_accel=1.5, corner_distance=0.05) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
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.')
def main(): """ Send a random-walk trajectory, starting from the current pose of the robot. Can be used to send both joint and cartesian trajectories. WARNING: Make sure the surrounding area around the robot is collision free prior to sending random trajectories. Call using: $ rosrun intera_examples send_random_trajectory.py [arguments: see below] -n 5 -t JOINT -s 0.5 --> Send a trandom joint trajectory with 5 waypoints, using a speed ratio of 0.5 for all waypoints. Use default random walk settings. -d 0.1 -b 0.2 --> Send a random trajectory with default trajectory settings. Use a maximum step distance of 0.1*(upper joint limit - lower joint limit) and avoid the upper and lower joint limits by a boundary of 0.2*(upper joint limit - lower joint limit). -o ~/Desktop/fileName.bag --> Save the trajectory message to a bag file -p --> Prints the trajectory to terminal before sending """ arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) parser.add_argument( "-n", "--waypoint_count", type=int, default=5, help="number of waypoints to include in the trajectory") parser.add_argument( "-d", "--stepDistance", type=float, default=0.05, help="normalized random walk step distance") parser.add_argument( "-b", "--boundaryPadding", type=float, default=0.1, help="normalized padding to apply to joint limits") parser.add_argument( "-t", "--trajType", type=str, default='JOINT', choices=['JOINT', 'CARTESIAN'], help="trajectory interpolation type") parser.add_argument( "-s", "--speed_ratio", type=float, default=0.5, help="A value between 0.0 (slow) and 1.0 (fast)") parser.add_argument( "-a", "--accel_ratio", type=float, default=0.5, help="A value between 0.0 (slow) and 1.0 (fast)") parser.add_argument( "-o", "--output_file", help="Save the trajectory task to a bag file") parser.add_argument( "--output_file_type", default="yaml", choices=["csv", "yaml"], help="Select the save file type") parser.add_argument( "-p", "--print_trajectory", action='store_true', default=False, help="print the trajectory after loading") parser.add_argument( "--do_not_send", action='store_true', default=False, help="generate the trajectory, but do not send to motion controller.") parser.add_argument( "--log_file_output", help="Save motion controller log messages to this file name") 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() if args.waypoint_count < 1: args.waypoint_count = 1 rospy.logwarn('Input out of bounds! Setting waypoint_count = 1') try: rospy.init_node('send_random_joint_trajectory_py') if not args.do_not_send: rospy.logwarn('Make sure the surrounding area around the robot is ' 'collision free prior to sending random trajectories.') k = input("Press 'Enter' when the robot is clear to continue...") if k: rospy.logerr("Please press only the 'Enter' key to begin execution. Exiting...") sys.exit(1) # Set the trajectory options limb = Limb() traj_opts = TrajectoryOptions() traj_opts.interpolation_type = args.trajType traj = MotionTrajectory(trajectory_options = traj_opts, limb = limb) # Set the waypoint options 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) # Append a waypoint at the current pose waypoint.set_joint_angles(limb.joint_ordered_angles()) traj.append_waypoint(waypoint.to_msg()) # Set up the random walk generator walk = RandomWalk() limits = JointLimits() walk.set_lower_limits(limits.get_joint_lower_limits(limb.joint_names())) walk.set_upper_limits(limits.get_joint_upper_limits(limb.joint_names())) walk.set_last_point(waypoint.get_joint_angles()) walk.set_boundary_padding(args.boundaryPadding) walk.set_maximum_distance(args.stepDistance) for i in range(0, args.waypoint_count): joint_angles = walk.get_next_point() waypoint.set_joint_angles(joint_angles = joint_angles) traj.append_waypoint(waypoint.to_msg()) if args.output_file is not None: if args.output_file_type == "csv": traj.to_csv_file(args.output_file) elif args.output_file_type == "yaml": traj.to_yaml_file(args.output_file) else: rospy.logwarn("Did not recognize output file type") if args.print_trajectory: rospy.loginfo('\n' + traj.to_string()) if args.log_file_output is not None: traj.set_log_file_name(args.log_file_output) if not args.do_not_send: result = traj.send_trajectory(timeout=args.timeout) if result is None: rospy.logerr('Trajectory FAILED to send') elif result.result: rospy.loginfo('Motion controller successfully finished the trajectory!') else: rospy.logerr('Motion controller failed to complete the trajectory with error %s', result.errorId) except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user. ' 'Exiting before trajectory completion.')
def cartesian_pose(args): """ Move the robot arm to the specified configuration. Call using: $ rosrun intera_examples go_to_cartesian_pose.py [arguments: see below] -p 0.4 -0.3 0.18 -o 0.0 1.0 0.0 0.0 -t right_hand --> Go to position: x=0.4, y=-0.3, z=0.18 meters --> with quaternion orientation (0, 1, 0, 0) and tip name right_hand --> The current position or orientation will be used if only one is provided. -q 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 --> Go to joint angles: 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 using default settings --> If a Cartesian pose is not provided, Forward kinematics will be used --> If a Cartesian pose is provided, the joint angles will be used to bias the nullspace -R 0.01 0.02 0.03 0.1 0.2 0.3 -T -> Jog arm with Relative Pose (in tip frame) -> x=0.01, y=0.02, z=0.03 meters, roll=0.1, pitch=0.2, yaw=0.3 radians -> The fixed position and orientation paramters will be ignored if provided arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description="cartesian_pose.__doc__") parser.add_argument( "-p", "--position", type=float, nargs='+', help="Desired end position: X, Y, Z") parser.add_argument( "-o", "--orientation", type=float, nargs='+', help="Orientation as a quaternion (x, y, z, w)") parser.add_argument( "-R", "--relative_pose", type=float, nargs='+', help="Jog pose by a relative amount in the base frame: X, Y, Z, roll, pitch, yaw") parser.add_argument( "-T", "--in_tip_frame", action='store_true', help="For relative jogs, job in tip frame (default is base frame)") parser.add_argument( "-q", "--joint_angles", type=float, nargs='+', default=[], help="A list of joint angles, one for each of the 7 joints, J0...J6") parser.add_argument( "-t", "--tip_name", default='right_hand', help="The tip name used by the Cartesian pose") parser.add_argument( "--linear_speed", type=float, default=0.6, help="The max linear speed of the endpoint (m/s)") parser.add_argument( "--linear_accel", type=float, default=0.6, help="The max linear acceleration of the endpoint (m/s/s)") parser.add_argument( "--rotational_speed", type=float, default=1.57, help="The max rotational speed of the endpoint (rad/s)") parser.add_argument( "--rotational_accel", type=float, default=1.57, help="The max rotational acceleration of the endpoint (rad/s/s)") 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.") """ try: #rospy.init_node('go_to_cartesian_pose_py') limb = Limb() traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options=traj_options, limb=limb) wpt_opts = MotionWaypointOptions( max_linear_speed=args.linear_speed, max_linear_accel=args.linear_accel, max_rotational_speed=args.rotational_speed, max_rotational_accel=args.rotational_accel, max_joint_speed_ratio=1.0) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) joint_names = limb.joint_names() if args.joint_angles and len(args.joint_angles) != len(joint_names): rospy.logerr('len(joint_angles) does not match len(joint_names!)') return None if (args.position is None and args.orientation is None and args.relative_pose is None): if args.joint_angles: # does Forward Kinematics waypoint.set_joint_angles(args.joint_angles, args.tip_name, joint_names) else: rospy.loginfo( "No Cartesian pose or joint angles given. Using default") waypoint.set_joint_angles(joint_angles=None, active_endpoint=args.tip_name) else: endpoint_state = limb.tip_state(args.tip_name) if endpoint_state is None: rospy.logerr('Endpoint state not found with tip name %s', args.tip_name) return None pose = endpoint_state.pose if args.relative_pose is not None: if len(args.relative_pose) != 6: rospy.logerr( 'Relative pose needs to have 6 elements (x,y,z,roll,pitch,yaw)' ) return None # create kdl frame from relative pose rot = PyKDL.Rotation.RPY(args.relative_pose[3], args.relative_pose[4], args.relative_pose[5]) trans = PyKDL.Vector(args.relative_pose[0], args.relative_pose[1], args.relative_pose[2]) f2 = PyKDL.Frame(rot, trans) # and convert the result back to a pose message if args.in_tip_frame: # end effector frame pose = posemath.toMsg(posemath.fromMsg(pose) * f2) else: # base frame pose = posemath.toMsg(f2 * posemath.fromMsg(pose)) else: if args.position is not None and len(args.position) == 3: pose.position.x = args.position[0] pose.position.y = args.position[1] pose.position.z = args.position[2] if args.orientation is not None and len(args.orientation) == 4: pose.orientation.x = args.orientation[0] pose.orientation.y = args.orientation[1] pose.orientation.z = args.orientation[2] pose.orientation.w = args.orientation[3] poseStamped = PoseStamped() poseStamped.pose = pose waypoint.set_cartesian_pose(poseStamped, args.tip_name, args.joint_angles) rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string()) traj.append_waypoint(waypoint.to_msg()) 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!') else: rospy.logerr( 'Motion controller failed to complete the trajectory with error %s', result.errorId) except rospy.ROSInterruptException: rospy.logerr( 'Keyboard interrupt detected from the user. Exiting before trajectory completion.' )
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.')
def go_to_cartesian(position=None, orientation=None, joint_angles=None, linear_speed=0.1, rotational_speed=0.57, linear_accel=0.3, tip_name='right_hand', relative_pose=None, rotational_accel=0.57, timeout=None): if not rospy.is_shutdown(): try: limb = Limb() traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options=traj_options, limb=limb) wpt_opts = MotionWaypointOptions( max_linear_speed=linear_speed, max_linear_accel=linear_accel, max_rotational_speed=rotational_speed, max_rotational_accel=rotational_accel, max_joint_speed_ratio=1.0) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) joint_names = limb.joint_names() if joint_angles and len(joint_angles) != len(joint_names): rospy.logerr( 'len(joint_angles) does not match len(joint_names!)') return None if (position is None and orientation is None and relative_pose is None): if joint_angles: # does Forward Kinematics waypoint.set_joint_angles(joint_angles, tip_name, joint_names) else: rospy.loginfo( "No Cartesian pose or joint angles given. Using default" ) waypoint.set_joint_angles(joint_angles=None, active_endpoint=tip_name) else: endpoint_state = limb.tip_state(tip_name) if endpoint_state is None: rospy.logerr('Endpoint state not found with tip name %s', tip_name) return None pose = endpoint_state.pose if relative_pose is not None: if len(relative_pose) != 6: rospy.logerr( 'Relative pose needs to have 6 elements (x,y,z,roll,pitch,yaw)' ) return None # create kdl frame from relative pose rot = PyKDL.Rotation.RPY(relative_pose[3], relative_pose[4], relative_pose[5]) trans = PyKDL.Vector(relative_pose[0], relative_pose[1], relative_pose[2]) f2 = PyKDL.Frame(rot, trans) # and convert the result back to a pose message if in_tip_frame: # end effector frame pose = posemath.toMsg(posemath.fromMsg(pose) * f2) else: # base frame pose = posemath.toMsg(f2 * posemath.fromMsg(pose)) else: if position is not None: pose.position.x = position.x pose.position.y = position.y pose.position.z = position.z if orientation is not None: pose.orientation.x = orientation.x pose.orientation.y = orientation.y pose.orientation.z = orientation.z pose.orientation.w = orientation.w poseStamped = PoseStamped() poseStamped.pose = pose waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles) #rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string()) traj.append_waypoint(waypoint.to_msg()) result = traj.send_trajectory(timeout=timeout) if result is None: return if result.result: rospy.loginfo( 'Motion controller successfully finished the trajectory!') else: rospy.logerr( 'Motion controller failed to complete the trajectory with error %s', result.errorId) except rospy.ROSInterruptException: rospy.logerr( 'Keyboard interrupt detected from the user. Exiting before trajectory completion.' )
def main(): # if len(sys.argv) < 4: # print('{0} <BindIP><Server IP><Message>'.format(sys.argv[0])) # sys.exit() bindIP = '192.168.101.5' #sys.argv[1] serverIP = '192.168.101.12' #sys.argv[2] sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # SOCK_STREAM은 TCP socket을 뜻함 sock.bind((bindIP, 0)) sock.connect((serverIP, 5425)) # 서버에 연결 요청 rospy.init_node('sawyer_client') arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__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.5, 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( "--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:]) goal_joint_angles = [0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0] while True: # 서버로 부터 수신 rbuff = sock.recv(1024) # 메시지 수신 received = str(rbuff) # print('수신:{0}'.format(received)) if received.count >= 5: center_str = received.split('SM')[1].split('SE')[0] print(center_str) if center_str == 'Q': break elif center_str == 'A': goal_joint_angles = [0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0] elif center_str == 'B': goal_joint_angles = [0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 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()) waypoint.set_joint_angles(joint_angles = goal_joint_angles) traj.append_waypoint(waypoint.to_msg()) result = traj.send_trajectory(timeout=args.timeout) sock.close()
def main(): """ Move the robot arm to the specified configuration. Call using: $ rosrun intera_examples go_to_joint_angles.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 [...] with a speed ratio of 0.9 and an accel ratio of 0.1 """ arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__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.5, 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( "--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('go_to_joint_angles_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()) 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!') else: rospy.logerr( 'Motion controller failed to complete the trajectory with error %s', result.errorId) except rospy.ROSInterruptException: rospy.logerr( 'Keyboard interrupt detected from the user. Exiting before trajectory completion.' )
def moveTo(myArgs): arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main_server.__doc__) parser.add_argument( "-p", "--position", type=float, nargs='+', help="Desired end position: X, Y, Z") parser.add_argument( "-o", "--orientation", type=float, nargs='+', help="Orientation as a quaternion (x, y, z, w)") parser.add_argument( "-R", "--relative_pose", type=float, nargs='+', help="Jog pose by a relative amount in the base frame: X, Y, Z, roll, pitch, yaw") parser.add_argument( "-T", "--in_tip_frame", action='store_true', help="For relative jogs, job in tip frame (default is base frame)") parser.add_argument( "-q", "--joint_angles", type=float, nargs='+', default=[], help="A list of joint angles, one for each of the 7 joints, J0...J6") 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(myArgs.call.split(" ")) print(args.position) #test_string = ['-p','0.5', '0.3', '0.5'] #args = parser.parse_args(test_string) try: limb = Limb() traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options = traj_options, limb = limb) wpt_opts = MotionWaypointOptions(max_linear_speed=0.4, max_linear_accel=0.4, max_rotational_speed=1.57, max_rotational_accel=1.5, max_joint_speed_ratio=1.0) waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb) joint_names = limb.joint_names() if args.joint_angles and len(args.joint_angles) != len(joint_names): rospy.logerr('len(joint_angles) does not match len(joint_names!)') return "failed" if (args.position is None and args.orientation is None and args.relative_pose is None): if args.joint_angles: # does Forward Kinematics waypoint.set_joint_angles(args.joint_angles,'right_hand', joint_names) else: rospy.loginfo("No Cartesian pose or joint angles given. Using default") waypoint.set_joint_angles(joint_angles=None, active_endpoint='right_hand') else: endpoint_state = limb.tip_state('right_hand') if endpoint_state is None: rospy.logerr('Endpoint state not found with tip name %s', 'right_hand') return "failed" pose = endpoint_state.pose if args.relative_pose is not None: if len(args.relative_pose) != 6: rospy.logerr('Relative pose needs to have 6 elements (x,y,z,roll,pitch,yaw)') return "failed" # create kdl frame from relative pose rot = PyKDL.Rotation.RPY(args.relative_pose[3], args.relative_pose[4], args.relative_pose[5]) trans = PyKDL.Vector(args.relative_pose[0], args.relative_pose[1], args.relative_pose[2]) f2 = PyKDL.Frame(rot, trans) # and convert the result back to a pose message if args.in_tip_frame: # end effector frame pose = posemath.toMsg(posemath.fromMsg(pose) * f2) else: # base frame pose = posemath.toMsg(f2 * posemath.fromMsg(pose)) else: if args.position is not None and len(args.position) == 3: pose.position.x = args.position[0] pose.position.y = args.position[1] pose.position.z = args.position[2] if args.orientation is not None and len(args.orientation) == 4: pose.orientation.x = args.orientation[0] pose.orientation.y = args.orientation[1] pose.orientation.z = args.orientation[2] pose.orientation.w = args.orientation[3] poseStamped = PoseStamped() poseStamped.pose = pose waypoint.set_cartesian_pose(poseStamped, 'right_hand', args.joint_angles) rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string()) traj.append_waypoint(waypoint.to_msg()) result = traj.send_trajectory(timeout=args.timeout) if result is None: rospy.logerr('Trajectory FAILED to send') return 'Trajectory FAILED to send' if result.result: rospy.loginfo('Motion controller successfully finished the trajectory!') return 'Motion Success' else: rospy.logerr('Motion controller failed to complete the trajectory with error %s', result.errorId) return result.errorId except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.') return "failed"
def main(): """ Move the robot arm to the specified configuration. Call using: $ rosrun intera_examples go_to_cartesian_pose.py [arguments: see below] -p 0.4 -0.3 0.18 -o 0.0 1.0 0.0 0.0 -t right_hand --> Go to position: x=0.4, y=-0.3, z=0.18 meters --> with quaternion orientation (0, 1, 0, 0) and tip name right_hand --> The current position or orientation will be used if only one is provided. -q 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 --> Go to joint angles: 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 using default settings --> If a Cartesian pose is not provided, Forward kinematics will be used --> If a Cartesian pose is provided, the joint angles will be used to bias the nullspace -R 0.01 0.02 0.03 0.1 0.2 0.3 -T -> Jog arm with Relative Pose (in tip frame) -> x=0.01, y=0.02, z=0.03 meters, roll=0.1, pitch=0.2, yaw=0.3 radians -> The fixed position and orientation paramters will be ignored if provided """ epilog = """ See help inside the example with the '?' key for key bindings. """ rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() if not valid_limbs: rp.log_message(("Cannot detect any limb parameters on this robot. " "Exiting."), "ERROR") return arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__, epilog=epilog) parser.add_argument("-r", "--record_point_pose", type=bool, default=0, nargs='+', help="record pose or not") parser.add_argument("-p", "--position", type=float, nargs='+', help="Desired end position: X, Y, Z") parser.add_argument("-o", "--orientation", type=float, nargs='+', help="Orientation as a quaternion (x, y, z, w)") parser.add_argument( "-R", "--relative_pose", type=float, nargs='+', help= "Jog pose by a relative amount in the base frame: X, Y, Z, roll, pitch, yaw" ) parser.add_argument( "-T", "--in_tip_frame", action='store_true', help="For relative jogs, job in tip frame (default is base frame)") parser.add_argument( "-q", "--joint_angles", type=float, nargs='+', default=[], help="A list of joint angles, one for each of the 7 joints, J0...J6") parser.add_argument("-t", "--tip_name", default='right_hand', help="The tip name used by the Cartesian pose") parser.add_argument("--linear_speed", type=float, default=0.6, help="The max linear speed of the endpoint (m/s)") parser.add_argument( "--linear_accel", type=float, default=0.6, help="The max linear acceleration of the endpoint (m/s/s)") parser.add_argument( "--rotational_speed", type=float, default=1.57, help="The max rotational speed of the endpoint (rad/s)") parser.add_argument( "--rotational_accel", type=float, default=1.57, help="The max rotational acceleration of the endpoint (rad/s/s)") 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." ) parser.add_argument( "-l", "--limb", dest="limb", default=valid_limbs[0], choices=valid_limbs, help="Limb on which to run the gripper keyboard example") args = parser.parse_args(rospy.myargv()[1:]) try: rospy.init_node('go_to_cartesian_pose_py') limb = Limb() traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options=traj_options, limb=limb) wpt_opts = MotionWaypointOptions( max_linear_speed=args.linear_speed, max_linear_accel=args.linear_accel, max_rotational_speed=args.rotational_speed, max_rotational_accel=args.rotational_accel, max_joint_speed_ratio=1.0) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) joint_names = limb.joint_names() rate = rospy.Rate(10) # 10hz if args.record_point_pose: origin_trans = get_position_now(limb) return None base_quaternions = np.array([0.0, 0.0, 1.0, 0.0]) bin_point = np.array([0.35216134766, 0.621893054464, 0.371810527511]) base_angle = 0.0 while not rospy.is_shutdown(): ch = raw_input("waiting for next step:") if ch == 'r': origin_trans = get_position_now(limb) if ch == 'g': map_keyboard(args.limb) if ch == 'p': coordinates_text = raw_input("type in coordinates:") base_angle = float(coordinates_text) # if float(coordinates_text)>np.pi: # base_rotated_angle=2*np.pi-float(coordinates_text) # else: # base_rotated_angle=np.pi-float(coordinates_text) # base_firest=np.array([0.0,0.0,1.0,0.0]) # base_quaternions=quaternions_after_rotation(base_firest,base_rotated_angle) # print("After reset the rotated, base quaternions are:") # print(base_quaternions) if ch == 'drop': coordinates_text = raw_input("type in coordinates:") bin_point = np.array([ float(coordinates_text.split()[0]), float(coordinates_text.split()[1]), float(coordinates_text.split()[2]) ]) if ch == 'q': return None if ch == 'd': only_grip(args.limb, 'q') coordinates_text = raw_input("type in coordinates:") target_point = np.array([ float(coordinates_text.split()[0]), float(coordinates_text.split()[1]), float(coordinates_text.split()[2]) ]) pick_angle = float(coordinates_text.split()[3]) pick_angle = float(coordinates_text.split()[3]) - base_angle pick_angle = -pick_angle temp = base_quaternions target_after = quaternions_after_rotation(temp, pick_angle) grip_rotated_this_time = target_after pick_quaternions = [ float(grip_rotated_this_time[1]), float(grip_rotated_this_time[2]), float(grip_rotated_this_time[3]), float(grip_rotated_this_time[0]) ] traj_1 = MotionTrajectory(trajectory_options=traj_options, limb=limb) waypoint_1 = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) result_up = go_to_the_point( target_point, args=args, limb=limb, waypoint=waypoint_1, traj=traj_1, target_quaterniond=pick_quaternions) only_grip(args.limb, 'o') matrix = quaternions.quat2mat([ pick_quaternions[3], pick_quaternions[0], pick_quaternions[1], pick_quaternions[2] ]) down_point = target_point + np.dot(matrix, down_distance) traj_2 = MotionTrajectory(trajectory_options=traj_options, limb=limb) waypoint_2 = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) result_down = go_to_the_point( down_point, args=args, limb=limb, waypoint=waypoint_2, traj=traj_2, target_quaterniond=pick_quaternions) only_grip(args.limb, 'q') traj_3 = MotionTrajectory(trajectory_options=traj_options, limb=limb) waypoint_3 = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) result_up = go_to_the_point( target_point, args=args, limb=limb, waypoint=waypoint_3, traj=traj_3, target_quaterniond=pick_quaternions) traj_4 = MotionTrajectory(trajectory_options=traj_options, limb=limb) waypoint_4 = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) result_up = go_to_the_point( bin_point, args, limb, waypoint_4, traj_4, target_quaterniond=pick_quaternions) only_grip(args.limb, 'o') if ch == 's': coordinates_text = raw_input( "type in coordinates and rotation angle:") target_point = [ float(coordinates_text.split()[0]), float(coordinates_text.split()[1]), float(coordinates_text.split()[2]) ] rotatain_angle_from_base = float( coordinates_text.split()[3]) - base_angle rotatain_angle_from_base = -rotatain_angle_from_base temp = base_quaternions target_after = quaternions_after_rotation( temp, rotatain_angle_from_base) grip_rotated_this_time = target_after target_quaterniond = [ float(grip_rotated_this_time[1]), float(grip_rotated_this_time[2]), float(grip_rotated_this_time[3]), float(grip_rotated_this_time[0]) ] traj_new = MotionTrajectory(trajectory_options=traj_options, limb=limb) waypoint_new = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) result = go_to_the_point(target_point, args=args, limb=limb, waypoint=waypoint_new, traj=traj_new, target_quaterniond=target_quaterniond) if result is None: rospy.logerr('Trajectory FAILED to send') return if result.result: rospy.loginfo( 'Motion controller successfully finished the trajectory!' ) else: rospy.logerr( 'Motion controller failed to complete the trajectory with error %s', result.errorId) except rospy.ROSInterruptException: rospy.logerr( 'Keyboard interrupt detected from the user. Exiting before trajectory completion.' )
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 move2cartesian(position=None, orientation=None, relative_pose=None, in_tip_frame=False, joint_angles=[], tip_name='right_hand', linear_speed=0.6, linear_accel=0.6, rotational_speed=1.57, rotational_accel=1.57, timeout=None, neutral=False): """ Move the robot arm to the specified configuration. Call using: $ rosrun intera_examples go_to_cartesian_pose.py [arguments: see below] -p 0.4 -0.3 0.18 -o 0.0 1.0 0.0 0.0 -t right_hand --> Go to position: x=0.4, y=-0.3, z=0.18 meters --> with quaternion orientation (0, 1, 0, 0) and tip name right_hand --> The current position or orientation will be used if only one is provided. -q 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 --> Go to joint angles: 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 using default settings --> If a Cartesian pose is not provided, Forward kinematics will be used --> If a Cartesian pose is provided, the joint angles will be used to bias the nullspace -R 0.01 0.02 0.03 0.1 0.2 0.3 -T -> Jog arm with Relative Pose (in tip frame) -> x=0.01, y=0.02, z=0.03 meters, roll=0.1, pitch=0.2, yaw=0.3 radians -> The fixed position and orientation paramters will be ignored if provided """ try: #rospy.init_node('go_to_cartesian_pose_py') limb = Limb() traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options = traj_options, limb = limb) wpt_opts = MotionWaypointOptions(max_linear_speed=linear_speed, max_linear_accel=linear_accel, max_rotational_speed=rotational_speed, max_rotational_accel=rotational_accel, max_joint_speed_ratio=1.0) waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb) joint_names = limb.joint_names() if joint_angles and len(joint_angles) != len(joint_names): rospy.logerr('len(joint_angles) does not match len(joint_names!)') return None if neutral == True: limb.move_to_neutral() else: if (position is None and orientation is None and relative_pose is None): if joint_angles: # does Forward Kinematics waypoint.set_joint_angles(joint_angles, tip_name, joint_names) else: rospy.loginfo("No Cartesian pose or joint angles given. Using default") waypoint.set_joint_angles(joint_angles=None, active_endpoint=tip_name) else: endpoint_state = limb.tip_state(tip_name) if endpoint_state is None: rospy.logerr('Endpoint state not found with tip name %s', tip_name) return None pose = endpoint_state.pose if relative_pose is not None: if len(relative_pose) != 6: rospy.logerr('Relative pose needs to have 6 elements (x,y,z,roll,pitch,yaw)') return None # create kdl frame from relative pose rot = PyKDL.Rotation.RPY(relative_pose[3], relative_pose[4], relative_pose[5]) trans = PyKDL.Vector(relative_pose[0], relative_pose[1], relative_pose[2]) f2 = PyKDL.Frame(rot, trans) # and convert the result back to a pose message if in_tip_frame: # end effector frame pose = posemath.toMsg(posemath.fromMsg(pose) * f2) else: # base frame pose = posemath.toMsg(f2 * posemath.fromMsg(pose)) else: if position is not None and len(position) == 3: pose.position.x = position[0] pose.position.y = position[1] pose.position.z = position[2] if orientation is not None and len(orientation) == 4: pose.orientation.x = orientation[0] pose.orientation.y = orientation[1] pose.orientation.z = orientation[2] pose.orientation.w = orientation[3] poseStamped = PoseStamped() poseStamped.pose = pose if not joint_angles: # using current joint angles for nullspace bais if not provided joint_angles = limb.joint_ordered_angles() waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles) else: waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles) rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string()) traj.append_waypoint(waypoint.to_msg()) 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!') else: rospy.logerr('Motion controller failed to complete the trajectory with error %s', result.errorId) except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.')
def run(self): rate = rospy.Rate(100) limb = Limb() traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options=traj_options, limb=limb) wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.5, max_joint_accel=0.5, corner_distance=0.05) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) self.pose.header = Header(stamp=rospy.Time.now(), frame_id='base') self.pose.pose.position.x = 0.0 self.pose.pose.position.y = -0.6 self.pose.pose.position.z = 0.5 self.pose.pose.orientation.x = 0.5 self.pose.pose.orientation.y = -0.5 self.pose.pose.orientation.z = 0.5 self.pose.pose.orientation.w = 0.5 joint_angles = limb.joint_ordered_angles() waypoint.set_cartesian_pose(self.pose, "right_hand", joint_angles) self.waypoints.append(waypoint) rospy.loginfo("Sending inital waypoint: %s", self.waypoints[0].to_string()) traj.append_waypoint(self.waypoints[0].to_msg()) result = traj.send_trajectory() if result is None: rospy.logerr('Trajectory FAILED to send') elif result.result: rospy.loginfo( 'Motion controller successfully finished the trajectory!') else: rospy.logerr( 'Motion controller failed to complete the trajectory with error %s', result.errorId) traj.clear_waypoints() l = Lights() l_name = 'head_green_light' initial_state = l.get_light_state(l_name) for i in range(0, 2): state = not initial_state l.set_light_state(l_name, state) rospy.sleep(0.5) state = not state l.set_light_state(l_name, state) rospy.sleep(0.5) l.set_light_state(l_name, True) for i in range(0, 19): self.gen_rand_waypoint() sendCommand = True while not rospy.is_shutdown(): traj.clear_waypoints() for i in range(0, 19): self.waypoints.pop(i) self.gen_rand_waypoint() for point in self.waypoints: traj.append_waypoint(point.to_msg()) print(len(self.waypoints)) result = traj.send_trajectory(wait_for_result=True) self.firstShutdown = True def clean_shutdown(): if self.firstShutdown: print("STOPPING TRAJECTORY") traj.stop_trajectory() traj.clear_waypoints() l = Lights() l.set_light_state('head_green_light', False) self.firstShutdown = False rospy.on_shutdown(clean_shutdown) rate.sleep() return