def interpolate_joint_space(self, goal, total_time, nb_points, start=None): """ Interpolate a trajectory from a start state (or current state) to a goal in joint space :param goal: A RobotState to be used as the goal of the trajectory param total_time: The time to execute the trajectory :param nb_points: Number of joint-space points in the final trajectory :param start: A RobotState to be used as the start state, joint order must be the same as the goal :return: The corresponding RobotTrajectory """ dt = total_time*(1.0/nb_points) # create the joint trajectory message traj_msg = JointTrajectory() rt = RobotTrajectory() if start == None: start = self.get_current_state() joints = [] start_state = start.joint_state.position goal_state = goal.joint_state.position for j in range(len(goal_state)): pose_lin = np.linspace(start_state[j],goal_state[j],nb_points+1) joints.append(pose_lin[1:].tolist()) for i in range(nb_points): point = JointTrajectoryPoint() for j in range(len(goal_state)): point.positions.append(joints[j][i]) # append the time from start of the position point.time_from_start = rospy.Duration.from_sec((i+1)*dt) # append the position to the message traj_msg.points.append(point) # put name of joints to be moved traj_msg.joint_names = self.joint_names() # send the message rt.joint_trajectory = traj_msg return rt
def send_command(self, cmd): rospy.logdebug(" Sending cmd to controller [%s]"%self._controller_id) cmd_msg = JointTrajectory() cmd_msg.header.stamp = rospy.Time().now() cmd_msg.joint_names = self._config["joint_names"] cmd_msg.points = [self._build_segment(x) for x in cmd["segments"]] self._pub.publish(cmd_msg)
def main(): pub = rospy.Publisher('sevenbot/joint_traj_controller/trajectory_command',JointTrajectory) rospy.init_node('traj_test') p1 = JointTrajectoryPoint() p1.positions = [0,0,0,0,0,0,0] p1.velocities = [0,0,0,0,0,0,0] p1.accelerations = [0,0,0,0,0,0,0] p2 = JointTrajectoryPoint() p2.positions = [0,0,1.0,0,-1.0,0,0] p2.velocities = [0,0,0,0,0,0,0] p2.accelerations = [0,0,0,0,0,0,0] p2.time_from_start = rospy.Time(4.0) p3 = JointTrajectoryPoint() p3.positions = [0,0,-1.0,0,1.0,0,0] p3.velocities = [0,0,0,0,0,0,0] p3.accelerations = [0,0,0,0,0,0,0] p3.time_from_start = rospy.Time(20.0) traj = JointTrajectory() traj.joint_names = ['j1','j2','j3','j4','j5','j6','j7'] traj.points = [p1,p2,p3] rospy.loginfo(traj) r = rospy.Rate(1) # 10hz pub.publish(traj) r.sleep() pub.publish(traj)
def convert_trajectory(self, traj): """ Converts a trajectory into a joint trajectory Args: traj: Trajectory to convert Returns: joint trajectory """ new_traj = JointTrajectory() new_traj.header = traj.header # Take each joint in the trajectory and add it to the joint trajectory for joint_name in traj.joint_names: new_traj.joint_names.append(self.joint_map[joint_name].joint_name) # For each poiint in the trajectory for point in traj.points: new_point = JointTrajectoryPoint() for i, pos in enumerate(point.positions): new_point.positions.append(self.joint_map[new_traj.joint_names[i]].convert_angle(pos)) for i, vel in enumerate(point.velocities): new_point.velocities.append(self.joint_map[new_traj.joint_names[i]].convert_velocity(vel)) new_point.time_from_start = point.time_from_start new_traj.points.append(new_point) return new_traj
def create_placings_from_object_pose(self, posestamped): """ Create a list of PlaceLocation of the object rotated every 15deg""" place_locs = [] pre_grasp_posture = JointTrajectory() # Actually ignored.... pre_grasp_posture.header.frame_id = self._grasp_pose_frame_id pre_grasp_posture.joint_names = [ name for name in self._gripper_joint_names.split()] jtpoint = JointTrajectoryPoint() jtpoint.positions = [ float(pos) for pos in self._gripper_pre_grasp_positions.split()] jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture) pre_grasp_posture.points.append(jtpoint) # Generate all the orientations every step_degrees_yaw deg for yaw_angle in np.arange(0.0, 2.0 * pi, radians(self._step_degrees_yaw)): pl = PlaceLocation() pl.place_pose = posestamped newquat = quaternion_from_euler(0.0, 0.0, yaw_angle) pl.place_pose.pose.orientation = Quaternion( newquat[0], newquat[1], newquat[2], newquat[3]) # TODO: the frame is ignored, this will always be the frame of the gripper # so arm_tool_link pl.pre_place_approach = self.createGripperTranslation( Vector3(1.0, 0.0, 0.0)) pl.post_place_retreat = self.createGripperTranslation( Vector3(-1.0, 0.0, 0.0)) pl.post_place_posture = pre_grasp_posture place_locs.append(pl) return place_locs
def __create_spin_command(self, arm, theta=math.pi): if arm == 'l': jnts = self.robot_state.left_arm_positions if arm == 'r': jnts = self.robot_state.right_arm_positions command = JointTrajectory() command.joint_names = ['%s_shoulder_pan_joint' % arm[0], '%s_shoulder_lift_joint' % arm[0], '%s_upper_arm_roll_joint' % arm[0], '%s_elbow_flex_joint' % arm[0], '%s_forearm_roll_joint' % arm[0], '%s_wrist_flex_joint' % arm[0], '%s_wrist_roll_joint' % arm[0]] jnts[-1] += theta command.points.append(JointTrajectoryPoint( positions=jnts, velocities = [0.0] * (len(command.joint_names)), accelerations = [], time_from_start = rospy.Duration(0.1))) goal = JointTrajectoryGoal() goal.trajectory = command return goal
def check_collision(self): if not self.collsion_srv: return False # Create a new trajectory by combining both the trajectories, assumption is that the timing is same # The order of joining should be [left + right] self.adjust_traj_length() traj = JointTrajectory() traj.joint_names = self.left._goal.trajectory.joint_names + self.right._goal.trajectory.joint_names no_points = len(self.left._goal.trajectory.points) for index in xrange(no_points): pt = JointTrajectoryPoint() left_pt = self.left._goal.trajectory.points[index] right_pt = self.right._goal.trajectory.points[index] pt.positions = left_pt.positions + right_pt.positions pt.velocities = left_pt.velocities + right_pt.velocities pt.time_from_start = left_pt.time_from_start traj.points.append(pt) msg = CollisionCheckRequest() msg.trajectory = traj try: collision_service = rospy.ServiceProxy("collision_check", CollisionCheck) resp = collision_service(msg) if resp.collision_found: rospy.logwarn("Collision Found") else: rospy.loginfo("Collision not found, good to go :)") return resp.collision_found except rospy.ServiceException as e: rospy.logwarn("Exception on collision check {}".format(e)) return True
def send_arm_goal(self, arm_goal): arm_joint_names = ['joint_lift', 'joint_waist', 'joint_big_arm', 'joint_forearm', 'joint_wrist_pitching', 'joint_wrist_rotation'] rospy.loginfo("Waiting for follow_joint_trajectory server") self.arm_client.wait_for_server() rospy.loginfo("Connected to follow_joint_trajectory server") rospy.sleep(1) arm_trajectory = JointTrajectory() arm_trajectory.joint_names = arm_joint_names arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal arm_trajectory.points[0].time_from_start = rospy.Duration(10) rospy.loginfo("Preparing for moving the arm to goal position!") rospy.sleep(1) arm_goal_pos = FollowJointTrajectoryGoal() arm_goal_pos.trajectory = arm_trajectory arm_goal_pos.goal_time_tolerance = rospy.Duration(0) self.arm_client.send_goal(arm_goal_pos) rospy.loginfo("Send goal to the trajectory server successfully!") self.arm_client.wait_for_result()
def make_trajectory_msg(plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id="/base_link"): """ This function will convert the plan to a joint trajectory compatible with the /arm_N/arm_controller/command topic """ theplan = plan # create joint trajectory message jt = JointTrajectory() # fill the header jt.header.seq = seq jt.header.stamp.secs = 0 # secs jt.header.stamp.nsecs = 0 # nsecs jt.header.frame_id = frame_id # specify the joint names jt.joint_names = theplan.joint_trajectory.joint_names # fill the trajectory points and computer constraint times njtp = len(theplan.joint_trajectory.points) for ii in range(0, njtp): jtp = JointTrajectoryPoint() jtp = copy.deepcopy(theplan.joint_trajectory.points[ii]) jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0] jtp.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0] jtp.time_from_start = rospy.Duration.from_sec(secs + dt * (ii + 1)) jt.points.append(jtp) return jt
def __init__(self): rospy.init_node('arm_simple_trajectory') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Which joints define the arm? arm_joints = ['joint_lift', 'joint_waist', 'joint_big_arm', 'joint_forearm', 'joint_wrist_pitching', 'joint_wrist_rotation'] if reset: # Set the arm back to the resting position arm_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] else: # Set a goal configuration for the arm arm_goal = [1.0, 1.0, 1.0, 1.0, 0.0, 0.0] # Connect to the right arm trajectory action server rospy.loginfo('Waiting for xm arm trajectory controller...') arm_client = actionlib.SimpleActionClient('xm_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) arm_client.wait_for_server() rospy.loginfo('...connected.') rospy.sleep(1) # Create a single-point arm trajectory with the arm_goal as the end-point arm_trajectory = JointTrajectory() arm_trajectory.joint_names = arm_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal arm_trajectory.points[0].velocities = [0.0 for i in arm_joints] arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(3) # Send the trajectory to the arm action server rospy.loginfo('Moving the arm to goal position...') rospy.sleep(1) # Create an empty trajectory goal arm_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above arm_goal.trajectory = arm_trajectory # Specify zero tolerance for the execution time arm_goal.goal_time_tolerance = rospy.Duration(0) # Send the goal to the action server arm_client.send_goal(arm_goal) rospy.loginfo('...done') rospy.sleep(1)
def joint_trajectory(self, joint_trajectory): ''' Returns just the part of the trajectory corresponding to the arm. **Args:** **joint_trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to convert **Returns:** A trajectory_msgs.msg.JointTrajectory corresponding to only this arm in joint_trajectory **Raises:** **ValueError:** If some arm joint is not found in joint_trajectory ''' arm_trajectory = JointTrajectory() arm_trajectory.header = copy.deepcopy(joint_trajectory.header) arm_trajectory.joint_names = copy.copy(self.joint_names) indexes = [-1]*len(self.joint_names) for (i, an) in enumerate(self.joint_names): for (j, n) in enumerate(joint_trajectory.joint_names): if an == n: indexes[i] = j break if indexes[i] < 0: raise ValueError('Joint '+n+' is missing from joint trajectory') for joint_point in joint_trajectory.points: arm_point = JointTrajectoryPoint() arm_point.time_from_start = joint_point.time_from_start for i in indexes: arm_point.positions.append(joint_point.positions[i]) arm_trajectory.points.append(arm_point) return arm_trajectory
def fill_ik_traj(self, poses, dist=None, duration=None, tot_points=None): if dist is None: dist = self.calc_dist(poses[0], poses[-1]) if duration is None: duration = dist*30 if tot_points is None: tot_points = 500*dist ik_fracs = np.linspace(0, 1, len(poses)) #A list of fractional positions along course to evaluate ik req = self.form_ik_request(poses[0]) ik_goal = self.ik_pose_proxy(req) seed = ik_goal.solution.joint_state.position ik_points = [[0]*7 for i in range(len(poses))] for i, p in enumerate(poses): request = self.form_ik_request(p) request.ik_request.ik_seed_state.joint_state.position = seed ik_goal = self.ik_pose_proxy(request) ik_points[i] = ik_goal.solution.joint_state.position seed = ik_goal.solution.joint_state.position # seed the next ik w/previous points results try: ik_points = np.array(ik_points) except: print "Value error" print 'ik_points: ', ik_points print 'ik_points_array: ', ik_points ang_fracs = np.linspace(0,1, tot_points) diff = np.max(np.abs(np.diff(ik_points,1,0)),0) print 'diff: ', diff for i in xrange(len(ik_points)): if ik_points[i,4]<0.: ik_points[i,4] += 2*math.pi if any(ik_points[:,4]>3.8): print "OVER JOINT LIMITS!" ik_points[:,4] -= 2*math.pi # Linearly interpolate angles between ik-defined points (non-linear in cartesian space, but this is reduced from dense ik sampling along linear path. Used to maintain large number of trajectory points without running IK on every one. angle_points = np.zeros((7, tot_points)) for i in xrange(7): angle_points[i,:] = np.interp(ang_fracs, ik_fracs, ik_points[:,i]) #Build Joint Trajectory from angle positions traj = JointTrajectory() traj.header.frame_id = poses[0].header.frame_id traj.joint_names = self.ik_info.kinematic_solver_info.joint_names #print 'angle_points', len(angle_points[0]), angle_points for i in range(len(angle_points[0])): traj.points.append(JointTrajectoryPoint()) traj.points[i].positions = angle_points[:,i] traj.points[i].velocities = [0]*7 traj.points[i].time_from_start = rospy.Duration(ang_fracs[i]*duration) angles_safe = self.check_trajectory(traj) if angles_safe: print "Check Passed: Angles Safe" return traj else: print "Check Failed: Unsafe Angles" return None
def goal_cb(self, pt_msg): rospy.loginfo("[{0}] New LookatIK goal received.".format(rospy.get_name())) if (pt_msg.header.frame_id != self.camera_ik.base_frame): try: now = pt_msg.header.stamp self.tfl.waitForTransform(pt_msg.header.frame_id, self.camera_ik.base_frame, now, rospy.Duration(1.0)) pt_msg = self.tfl.transformPoint(self.camera_ik.base_frame, pt_msg) except (LookupException, ConnectivityException, ExtrapolationException): rospy.logwarn("[{0}] TF Error tranforming point from {1} to {2}".format(rospy.get_name(), pt_msg.header.frame_id, self.camera_ik.base_frame)) target = np.array([pt_msg.point.x, pt_msg.point.y, pt_msg.point.z]) # Get IK Solution current_angles = list(copy.copy(self.joint_angles_act)) iksol = self.camera_ik.lookat_ik(target, current_angles[:len(self.camera_ik.arm_indices)]) # Start with current angles, then replace angles in camera IK with IK solution # Use desired joint angles to avoid sagging over time jtp = JointTrajectoryPoint() jtp.positions = list(copy.copy(self.joint_angles_des)) for i, joint_name in enumerate(self.camera_ik.arm_joint_names): jtp.positions[self.joint_names.index(joint_name)] = iksol[i] deltas = np.abs(np.subtract(jtp.positions, current_angles)) duration = np.max(np.divide(deltas, self.max_vel_rot)) jtp.time_from_start = rospy.Duration(duration) jt = JointTrajectory() jt.joint_names = self.joint_names jt.points.append(jtp) rospy.loginfo("[{0}] Sending Joint Angles.".format(rospy.get_name())) self.joint_traj_pub.publish(jt)
def move_head(self, pan , tilt): # Which joints define the head? head_joints = ['head_pan_joint', 'head_tilt_mod_joint'] # Create a single-point head trajectory with the head_goal as the end-point head_trajectory = JointTrajectory() head_trajectory.joint_names = head_joints head_trajectory.points.append(JointTrajectoryPoint()) head_trajectory.points[0].positions = pan , tilt head_trajectory.points[0].velocities = [0.0 for i in head_joints] head_trajectory.points[0].accelerations = [0.0 for i in head_joints] head_trajectory.points[0].time_from_start = rospy.Duration(3.0) # Send the trajectory to the head action server rospy.loginfo('Moving the head to goal position...') head_goal = FollowJointTrajectoryGoal() head_goal.trajectory = head_trajectory head_goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal self.head_client.send_goal(head_goal) # Wait for up to 5 seconds for the motion to complete self.head_client.wait_for_result(rospy.Duration(2.0)) rospy.loginfo('...done')
def tuck(self): # prepare a joint trajectory msg = JointTrajectory() msg.joint_names = servos msg.points = list() point = JointTrajectoryPoint() point.positions = forward point.velocities = [ 0.0 for servo in msg.joint_names ] point.time_from_start = rospy.Duration(5.0) msg.points.append(point) # point = JointTrajectoryPoint() #point.positions = to_side #point.velocities = [ 0.0 for servo in msg.joint_names ] #point.time_from_start = rospy.Duration(8.0) #msg.points.append(point) #point = JointTrajectoryPoint() #point.positions = tucked #point.velocities = [ 0.0 for servo in msg.joint_names ] #point.time_from_start = rospy.Duration(11.0) #msg.points.append(point) # call action msg.header.stamp = rospy.Time.now() + rospy.Duration(0.1) goal = FollowJointTrajectoryGoal() goal.trajectory = msg self._client.send_goal(goal)
def dual_arm_test(): pub = rospy.Publisher('/lwr/lbr_controller/command', JointTrajectory) cob_pub = rospy.Publisher('/cob3_1/lbr_controller/command', JointTrajectory) rospy.init_node('dual_arm_test') rospy.sleep(1.0) while not rospy.is_shutdown(): traj = JointTrajectory() traj.joint_names = ['lbr_1_joint', 'lbr_2_joint', 'lbr_3_joint', 'lbr_4_joint', 'lbr_5_joint', 'lbr_6_joint', 'lbr_7_joint'] ptn = JointTrajectoryPoint() ptn.positions = [0.5, 0.8, 0.8, -0.8, 0.8, 0.8, -0.3] ptn.velocities = [0.4, 0.4, 0.8, 0.1, 0.8, 0.8, 0.1] ptn.time_from_start = rospy.Duration(2.0) traj.header.stamp = rospy.Time.now() traj.points.append(ptn) pub.publish(traj) cob_pub.publish(traj) rospy.sleep(3.0) ptn.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] ptn.velocities = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] ptn.time_from_start = rospy.Duration(2.0) traj.points = [] traj.header.stamp = rospy.Time.now() traj.points.append(ptn) pub.publish(traj) cob_pub.publish(traj) rospy.sleep(3.0)
def exectute_joint_traj(self, joint_trajectory, timing): '''Moves the arm through the joint sequence''' # First, do filtering on the trajectory to fix the velocities trajectory = JointTrajectory() # Initialize the server # When to start the trajectory: 0.1 seconds from now trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1) trajectory.joint_names = self.joint_names ## Add all frames of the trajectory as way points for i in range(len(timing)): positions = joint_trajectory[i].joint_pose velocities = [0] * len(positions) # Add frames to the trajectory trajectory.points.append(JointTrajectoryPoint(positions=positions, velocities=velocities, time_from_start=timing[i])) output = self.filter_service(trajectory=trajectory, allowed_time=rospy.Duration.from_sec(20)) rospy.loginfo('Trajectory for arm ' + str(self.arm_index) + ' has been filtered.') traj_goal = JointTrajectoryGoal() # TO-DO: check output.error_code traj_goal.trajectory = output.trajectory traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.joint_names = self.joint_names # Sends the goal to the trajectory server self.traj_action_client.send_goal(traj_goal)
def make_gripper_posture(self, pose): t = JointTrajectory() t.joint_names = GRIPPER_JOINT_NAMES tp = JointTrajectoryPoint() tp.positions = [pose for j in t.joint_names] tp.effort = GRIPPER_EFFORT t.points.append(tp) return t
def head_up_msg(self): jtm = JointTrajectory() jtm.joint_names = self.joint_names jtp = JointTrajectoryPoint() jtp.positions = [0]*len(self.joint_names) jtp.time_from_start = rospy.Duration(1.0) jtm.points = [jtp] return jtm
def talker(): if 1 == 0: pub = rospy.Publisher("/ihmc_ros/atlas/control/arm_joint_trajectory2", JointTrajectory, queue_size=10) jn = ["l_arm_shz", "l_arm_shx", "l_arm_ely", "l_arm_elx", "l_arm_wry", "l_arm_wrx", "l_arm_wry2"] jn_r = ["r_arm_shz", "r_arm_shx", "r_arm_ely", "r_arm_elx", "r_arm_wry", "r_arm_wrx", "r_arm_wry2"] else: pub = rospy.Publisher("/ihmc_ros/valkyrie/control/arm_joint_trajectory2", JointTrajectory, queue_size=10) jn = [ "LeftShoulderPitch", "LeftShoulderRoll", "LeftShoulderYaw", "LeftElbowPitch", "LeftForearmYaw", "LeftWristRoll", "LeftWristPitch", ] jn_r = [ "RightShoulderPitch", "RightShoulderRoll", "RightShoulderYaw", "RightElbowPitch", "RightForearmYaw", "RightWristRoll", "RightWristPitch", ] # this doesnt work: # jn = ["l_arm_shz","l_arm_shx","l_arm_ely","l_arm_elx","l_arm_wry","l_arm_wrx","l_arm_wry2", "r_arm_shz","l_arm_shx","r_arm_ely","r_arm_elx","r_arm_wry","r_arm_wrx","r_arm_wry2"] # value = 0 rospy.init_node("send_arm_test", anonymous=True) rate = rospy.Rate(1) # 10hz while not rospy.is_shutdown(): msg = JointTrajectory() value = 0.1 value_r = 0.1 msg.joint_names = jn pt = JointTrajectoryPoint() pt.positions = [value] * len(jn) pt.velocities = [0] * len(jn) pt.accelerations = [0] * len(jn) pt.effort = [0] * len(jn) pt.time_from_start = rospy.Duration.from_sec(3) msg.points.append(pt) print msg.joint_names, rospy.get_time() pub.publish(msg) # TODO: add a sleep here between left and right msg.joint_names = jn_r msg.points[0].positions = [value_r] * len(jn) print msg.joint_names, rospy.get_time() pub.publish(msg) rate.sleep()
def up_msg(self): jtm = JointTrajectory() jtm.joint_names = self.joint_names jtp = JointTrajectoryPoint() jtp.positions = [1.]*len(self.joint_names) jtp.velocities = [0.]*len(self.joint_names) jtp.time_from_start = rospy.Duration(5.0) jtm.points = [jtp] return jtm
def up_msg(self): jtm = JointTrajectory() jtm.joint_names = self.joint_names jtp = JointTrajectoryPoint() jtp.positions = [self.filtered_angle]*len(self.joint_names) print self.controller, self.filtered_angle jtp.time_from_start = rospy.Duration(1.0) jtm.points = [jtp] return jtm
def random_msg(self): jtm = JointTrajectory() jtm.joint_names = self.joint_names jtp = JointTrajectoryPoint() jtp.positions = 2*np.random.random(len(self.joint_names)) - 1 jtp.velocities = [0.]*len(self.joint_names) jtp.time_from_start = rospy.Duration(5.) jtm.points = [jtp] return jtm
def get_post_place_posture(): pre_grasp_posture = JointTrajectory() pre_grasp_posture.header.frame_id = END_EFFECTOR pre_grasp_posture.header.stamp = rospy.Time.now() pre_grasp_posture.joint_names = GRIPPER_JOINTS pos = JointTrajectoryPoint() # open gripper pos.positions = [0.0] pre_grasp_posture.points.append(pos) return pre_grasp_posture
def get_post_place_posture(): post_place_posture = JointTrajectory() post_place_posture.header.frame_id = EEF_LINK post_place_posture.header.stamp = rospy.Time.now() post_place_posture.joint_names = GRIPPER_JOINTS pos = JointTrajectoryPoint() # gripper open after place pos.positions = [0.0] post_place_posture.points.append(pos) return post_place_posture
def __init__(self): rospy.init_node('trajectory_demo') # Set to True to move back to the starting configurations reset = rospy.get_param('~reset', False) # Which joints define the arm? arm_joints = ['link_1_joint', 'servo_2_joint', 'link_3_joint', 'link_4_joint', 'link_5_joint'] if reset: # Set the arm back to the resting position arm_goal = [0, 0, 0, 0, 0] else: # Set a goal configuration for the arm arm_goal = [-0.525487, 0.904972, 0.480017, -1.331166, 0.731413] # Connect to the right arm trajectory action server rospy.loginfo('Waiting for right arm trajectory controller...') arm_client = actionlib.SimpleActionClient('right_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) arm_client.wait_for_server() rospy.loginfo('...connected.') # Create a single-point arm trajectory with the arm_goal as the end-point arm_trajectory = JointTrajectory() arm_trajectory.joint_names = arm_joints arm_trajectory.points.append(JointTrajectoryPoint()) arm_trajectory.points[0].positions = arm_goal arm_trajectory.points[0].velocities = [0.0 for i in arm_joints] arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints] arm_trajectory.points[0].time_from_start = rospy.Duration(3.0) # Send the trajectory to the arm action server rospy.loginfo('Moving the arm to goal position...') # Create an empty trajectory goal arm_goal = FollowJointTrajectoryGoal() # Set the trajectory component to the goal trajectory created above arm_goal.trajectory = arm_trajectory # Specify zero tolerance for the execution time arm_goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal to the action server arm_client.send_goal(arm_goal) rospy.loginfo('...done')
def set_position(self,goal): traj = JointTrajectory() traj_p = JointTrajectoryPoint() traj.joint_names = ["torso_lift_joint"] traj_p.positions = [goal] traj_p.velocities = [0.] traj_p.accelerations = [0.] traj_p.time_from_start = rospy.Duration(2.) traj.points.append(traj_p) self.pub.publish(traj)
def move_torso_topic(self, joint_1, joint_2, time=1.0): jt = JointTrajectory() jt.joint_names = ['torso_1_joint', 'torso_2_joint'] p = JointTrajectoryPoint() p.positions = [joint_1, joint_2] rospy.loginfo("Moving torso to: " + str(p.positions)) p.time_from_start = rospy.Duration(time) jt.points.append(p) self.torso_pub.publish(jt)
def make_joint_trajectory(names, points): jt = JointTrajectory() jt.joint_names = names pt = JointTrajectoryPoint() pt.positions = points pt.effort = [0]*len(points) pt.velocities = [0]*len(points) pt.accelerations = [0]*len(points) jt.points = [pt] return jt
def __init__(self): rospy.init_node('joint_trajectory_command_handler') self.real_robot = rospy.get_param("~real_robot") ac_rate = rospy.get_param("~action_cycle_rate") self.rate = rospy.Rate(ac_rate) # Publisher to JointTrajectory robot controller if self.real_robot: # self.jt_pub = rospy.Publisher('/scaled_pos_traj_controller/command', JointTrajectory, queue_size=10) self.jt_pub = rospy.Publisher('/pos_traj_controller/command', JointTrajectory, queue_size=10) else: self.jt_pub = rospy.Publisher('/eff_joint_traj_controller/command', JointTrajectory, queue_size=10) # Subscriber to JointTrajectory Command coming from Environment rospy.Subscriber('env_arm_command', JointTrajectory, self.callback_env_joint_trajectory, queue_size=1) self.msg = JointTrajectory() # Queue with maximum size 1 self.queue = Queue(maxsize=1) # Flag used to publish empty JointTrajectory message only once when interrupting execution self.stop_flag = False
def move1(): global joints_pos g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() g.trajectory.joint_names = JOINT_NAMES try: joint_states = rospy.wait_for_message("joint_states", JointState) joints_pos = joint_states.position g.trajectory.points = [ JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)), JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)), JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)), JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))] client.send_goal(g) client.wait_for_result() except KeyboardInterrupt: client.cancel_goal() raise except: raise
def moveJoint (jointcmds,prefix,nbJoints): # topic_name = '/' + prefix + '/effort_joint_trajectory_controller/command' topic_name = '/' + prefix + '/position_joint_trajectory_controller/command' pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1) jointCmd = JointTrajectory() point = JointTrajectoryPoint() jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0); point.time_from_start = rospy.Duration.from_sec(2.0) for i in range(0, nbJoints): # jointCmd.joint_names.append(prefix +'_joint_'+str(i+1)) jointCmd.joint_names.append('j2s7s300_joint_'+str(i+1)) point.positions.append(jointcmds[i]) # point.velocities.append(0.01) # point.accelerations.append(0) # point.effort.append(0) jointCmd.points.append(point) rate = rospy.Rate(100) while (not pub.get_num_connections()): rate.sleep() pub.publish(jointCmd)
def moveJoint(self, jointcmds, nbJoints): topic_name = '/j2n6s300/effort_joint_trajectory_controller/command' pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1) jointCmd = JointTrajectory() point = JointTrajectoryPoint() jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0) point.time_from_start = rospy.Duration.from_sec(5.0) for i in range(0, nbJoints): jointCmd.joint_names.append('j2n6s300_joint_' + str(i + 1)) point.positions.append(jointcmds[i]) point.velocities.append(0) point.accelerations.append(0) point.effort.append(0) jointCmd.points.append(point) rate = rospy.Rate(100) count = 0 while count < 500: pub.publish(jointCmd) count = count + 1 rate.sleep()
def get_trajectory_goal(joint_names, q1, q2, q3): joint_num = len(joint_names) g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() g.trajectory.joint_names = joint_names g.trajectory.points = [ JointTrajectoryPoint(positions=q1, velocities=[0] * joint_num, time_from_start=rospy.Duration(0.0)), JointTrajectoryPoint(positions=q2, velocities=[0] * joint_num, time_from_start=rospy.Duration(2.0)), JointTrajectoryPoint(positions=q3, velocities=[0] * joint_num, time_from_start=rospy.Duration(4.0)), JointTrajectoryPoint(positions=q1, velocities=[0] * joint_num, time_from_start=rospy.Duration(6.0)) ] return g
def set_head_state(self, jvals): """ Sets goal for head using provided joint values""" # Build trajectory message head_goal = JointTrajectory() head_goal.joint_names.append('head_pan_joint') head_goal.joint_names.append('head_tilt_joint') head_goal.points.append(JointTrajectoryPoint()) head_goal.points[0].time_from_start = rospy.Duration(0.25) #self.head_goal.header.frame_id = 'base_link' for i in range(len(head_goal.joint_names)): head_goal.points[0].positions.append(jvals[i]) head_goal.points[0].velocities.append(1) head_goal.header.stamp = rospy.get_rostime() + rospy.Duration(0.01) try: #print head_goal self.publishers["head"].publish(head_goal) if self.verbose: print "published [%s] to head_traj_controller/command topic" % jvals except: print "failed to publish head position!"
def gripper_send_position_goal(self, position=0.3, velocity=0.4, action='move'): """Send position goal to the gripper Keyword Arguments: position {float} -- Gripper angle (default: {0.3}) velocity {float} -- Gripper velocity profile (default: {0.4}) action {str} -- Gripper movement (default: {'move'}) """ self.turn_gripper_position_controller_on() duration = 0.2 if action == 'pre_grasp_angle': gripper_angle_grasp = deepcopy(self.d[-2]) max_distance = 0.085 angular_coeff = 0.11 K = 1 # lower numbers = higher angles angle = (max_distance - gripper_angle_grasp) / angular_coeff * K position = angle velocity = 0.4 elif action == 'pick': position = 0.75 velocity = 0.05 duration = 8.0 goal = FollowJointTrajectoryGoal() goal.trajectory = JointTrajectory() goal.trajectory.joint_names = self.robotiq_joint_name goal.trajectory.points.append( JointTrajectoryPoint(positions=[position], velocities=[velocity], accelerations=[0.0], time_from_start=rospy.Duration(duration))) self.client_gripper.send_goal(goal) if action == 'pick': while not rospy.is_shutdown( ) and not self.left_collision and not self.right_collision: pass self.client_gripper.cancel_goal()
def send_joint_position_actionlib(settings): # rosnode の初期化 rospy.init_node('send_joint_position') # トピック名,メッセージ型を使って ActionLib client を定義 client = actionlib.SimpleActionClient( '/fullbody_controller/follow_joint_trajectory', FollowJointTrajectoryAction) client.wait_for_server() # ActionLib のサーバと通信が接続されることを確認 # ActionLib client の goal を指定 # http://wiki.ros.org/actionlib_tutorials/Tutorials の # Writing a Simple Action Client (Python) を参照 # __TOPIC_PREFIX__Action で actionlib.SimpleActionClient を初期化 # ゴールオブジェクトは __TOPIC_PREFIX__Goal を使って生成 goal = FollowJointTrajectoryGoal() goal.trajectory = JointTrajectory() goal.trajectory.header.stamp = rospy.Time.now() goal.trajectory.joint_names = [ 'arm_joint1', 'arm_joint2', 'arm_joint3', 'arm_joint4', 'arm_joint5', 'arm_joint6' ] for i in range(5): point = JointTrajectoryPoint() point.positions = [ math.pi / 2, 0, math.pi / 4 * (i % 2), 0, math.pi / 2, math.pi / 2 ] point.time_from_start = rospy.Duration(1.0 + i) goal.trajectory.points.append(point) client.send_goal(goal) rospy.loginfo("wait for goal ...") while (1): key = getKey() if (client.get_state() == 3): rospy.loginfo("done") break else: if (key == 'c'): client.cancel_goal() rospy.loginfo("canceling") break client.wait_for_result() # ロボットの動作が終わるまで待つ.
def OARbot_nav(): global velocity, bot, goal,x,y x=0 y=0 rospy.init_node('OARbot_control', anonymous=True) rate=rospy.Rate(100) goal=location(x,y) bot=rospy.Publisher('/cmd_vel', Twist, queue_size=10) arm=rospy.Publisher('/j2n6s300/effort_joint_trajectory_controller/command',JointTrajectory, queue_size=1) rospy.Subscriber('/odom',Odometry, pose_callback) velocity=Twist() jointcmds=[0.0,2.9,1.3,4.2,1.4,0.0] jointCmd=JointTrajectory() point=JointTrajectoryPoint() jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0); point.time_from_start = rospy.Duration.from_sec(5.0) for i in range(0, 6): jointCmd.joint_names.append('j2n6s300_joint_'+str(i+1)) point.positions.append(jointcmds[i]) point.velocities.append(5) point.accelerations.append(0) point.effort.append(0) jointCmd.points.append(point) rospy.sleep(2) key=raw_input("ready?") x=float(raw_input("Where to?")) y=float(raw_input()) goal=location(x,y) walk_thread=threading.Thread(target=walk, args=()) walk_thread.start() talk_thread=threading.Thread(target=talk, args=()) talk_thread.start() rospy.sleep(1) while not rospy.is_shutdown(): x=float(raw_input("Where to?")) y=float(raw_input()) goal=location(x,y) rospy.sleep(1)
def arms_down(): pubL = rospy.Publisher('/left_arm_controller/command', JointTrajectory, queue_size=1) rospy.init_node('arms_down', anonymous=True) rate = rospy.Rate(2) pubR = rospy.Publisher('/right_arm_controller/command', JointTrajectory, queue_size=1) left_traj = JointTrajectory() left_traj.joint_names = [ "left_arm_elbow_pitch_joint", 'left_arm_elbow_yaw_joint', 'left_arm_shoulder_pitch_joint', 'left_arm_shoulder_roll_joint', 'left_arm_shoulder_yaw_joint', 'left_arm_wrist_pitch_joint', 'left_arm_wrist_roll_joint' ] left_traj_points = JointTrajectoryPoint() left_traj_points.positions = [0.0, 0.0, -1.57, -1.5, 0.0, 0.0, 0.0] left_traj_points.time_from_start = rospy.Duration.from_sec(0.5) left_traj.points.append(left_traj_points) right_traj = JointTrajectory() right_traj.joint_names = [ "right_arm_elbow_pitch_joint", 'right_arm_elbow_yaw_joint', 'right_arm_shoulder_pitch_joint', 'right_arm_shoulder_roll_joint', 'right_arm_shoulder_yaw_joint', 'right_arm_wrist_pitch_joint', 'right_arm_wrist_roll_joint' ] right_traj_points = JointTrajectoryPoint() right_traj_points.positions = [0.0, 0.0, -1.57, 1.5, 0.0, 0.0, 0.0] right_traj_points.time_from_start = rospy.Duration.from_sec(1) right_traj.points.append(right_traj_points) rospy.sleep(8) i = 0 while i < 10: left_traj_points.time_from_start = rospy.Duration.from_sec(1 + i * 0.1) pubL.publish(left_traj) rate.sleep() right_traj_points.time_from_start = rospy.Duration.from_sec(1 + i * 0.15) pubR.publish(right_traj) i += 1 rospy.loginfo("sent goal: %d", i)
def init_trajectory(self): """Initialize a new target trajectory.""" state = self.jointStatePublisher.last_joint_states self.trajectory_t0 = self.robot.getTime() self.trajectory = self.gripper_trajectory = JointTrajectory() self.trajectory.joint_names = self.prefixedJointNames self.gripper_trajectory_names = self.gripper self.trajectory.points = [ JointTrajectoryPoint(positions=state.position if state else [0] * 7, velocities=[0] * 7, accelerations=[0] * 7, time_from_start=rospy.Duration(0.0)) ] self.gripper_trajectory.points = [ JointTrajectoryPoint(positions=state.position if state else [0] * 2, velocities=[0] * 2, accelerations=[0] * 2, time_from_start=rospy.Duration(0.0)) ]
def MoveToStartPosition(): global joints_pos g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() g.trajectory.joint_names = JOINT_NAMES try: joint_states = rospy.wait_for_message("joint_states", JointState) joints_pos = joint_states.position g.trajectory.points = [ JointTrajectoryPoint(positions=joints_pos, velocities=[0] * 6, time_from_start=rospy.Duration(3.0)), JointTrajectoryPoint(positions=STARTING_POSITION, velocities=[0] * 6, time_from_start=rospy.Duration(6.0)) ] client.send_goal(g) client.wait_for_result() except: raise
def moveJoint(jointcmds, prefix, nbJoints): topic_name = '/' + prefix + '/arm_controller/command' print("topic_name: ", topic_name) pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1) jointCmd = JointTrajectory() point = JointTrajectoryPoint() jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0) point.time_from_start = rospy.Duration.from_sec(0.) for i in range(0, nbJoints): jointCmd.joint_names.append('drive' + str(i + 1) + '_joint') point.positions.append(jointcmds[i]) point.velocities.append(0) point.accelerations.append(0) point.effort.append(0) jointCmd.points.append(point) rate = rospy.Rate(1) count = 0 while (count < 2): pub.publish(jointCmd) count += 1 rate.sleep()
def __init__(self, arm_prefix): rospy.init_node('dmp_fitter', anonymous=True) rospy.Subscriber('joint_states', JointState, self.callback) self.joint_names = [ arm_prefix + joint for joint in [ "_shoulder_pan_joint", "_shoulder_lift_joint", "_upper_arm_roll_joint", "_elbow_flex_joint", "_forearm_roll_joint", "_wrist_flex_joint", "_wrist_roll_joint" ] ] self.js_0 = None self.js_1 = None self.jt = JointTrajectory() self.traj_pub = rospy.Publisher('motion_segment', JointTrajectory, queue_size=1) self.moving = False self.traj = [] self.dmp_count = 0
def readInPoses(self): xaplibrary = rospy.get_param('~xap', None) if xaplibrary: self.parseXapPoses(xaplibrary) poses = rospy.get_param('~poses', None) if poses: for key,value in poses.iteritems(): try: # TODO: handle multiple points in trajectory trajectory = JointTrajectory() trajectory.joint_names = value["joint_names"] point = JointTrajectoryPoint() point.time_from_start = Duration(value["time_from_start"]) point.positions = value["positions"] trajectory.points = [point] self.poseLibrary[key] = trajectory except: rospy.logwarn("Could not parse pose \"%s\" from the param server:", key); rospy.logwarn(sys.exc_info()) # add a default crouching pose: if not "crouch" in self.poseLibrary: trajectory = JointTrajectory() trajectory.joint_names = ["Body"] point = JointTrajectoryPoint() point.time_from_start = Duration(1.5) point.positions = [0.0,0.0, # head 1.545, 0.33, -1.57, -0.486, 0.0, 0.0, # left arm -0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # left leg -0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # right leg 1.545, -0.33, 1.57, 0.486, 0.0, 0.0] # right arm trajectory.points = [point] self.poseLibrary["crouch"] = trajectory; rospy.loginfo("Loaded %d poses: %s", len(self.poseLibrary), self.poseLibrary.keys())
def moveFingers (jointcmds,prefix,nbJoints): # topic_name = '/' + prefix + '/effort_finger_trajectory_controller/command' topic_name = '/' + prefix + '/position_finger_trajectory_controller/command' pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1) jointCmd = JointTrajectory() point = JointTrajectoryPoint() jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0); point.time_from_start = rospy.Duration.from_sec(5.0) for i in range(0, nbJoints): jointCmd.joint_names.append(prefix +'_joint_finger_'+str(i+1)) point.positions.append(jointcmds[i]) point.velocities.append(0) point.accelerations.append(0) point.effort.append(0) jointCmd.points.append(point) rate = rospy.Rate(100) count = 0 while (count < 50): pub.publish(jointCmd) count = count + 1 rate.sleep()
def TOPPRA2JointTrajectory(self, jnt_traj, f): # Sampling frequency is required to get the time samples correctly. # The number of points in ts_sample is duration*frequency. ts_sample = np.linspace(0, jnt_traj.get_duration(), int(jnt_traj.get_duration() * f)) # Sampling. This returns a matrix for all DOFs. Accessing specific one is # simple: qs_sample[:, 0] qs_sample = jnt_traj.eval(ts_sample) qds_sample = jnt_traj.evald(ts_sample) qdds_sample = jnt_traj.evaldd(ts_sample) n = qs_sample.shape[0] dof = qs_sample.shape[1] # Transform into JointTrajectory joint_trajectory = JointTrajectory() for i in range(0, n): temp_point = JointTrajectoryPoint() for j in range(0, dof): temp_point.positions.append(qs_sample[i, j]) temp_point.velocities.append(qds_sample[i, j]) temp_point.accelerations.append(qdds_sample[i, j]) temp_point.time_from_start = rospy.Duration.from_sec(i / f) joint_trajectory.points.append(temp_point) # Add last point with zero velocity and acceleration last_point = JointTrajectoryPoint() for i in range(0, dof): last_point.positions.append(qs_sample[n - 1, i]) last_point.velocities.append(0.0) last_point.accelerations.append(0.0) last_point.time_from_start = rospy.Duration.from_sec((n) / f) joint_trajectory.points.append(last_point) for i in range(0, dof): joint_trajectory.joint_names.append("joint" + str(i + 1)) return joint_trajectory
def action_server_cb(self, goal): """ :param goal: :type goal: MoveGoal """ rospy.loginfo(u'goal received') self.execute = goal.type == MoveGoal.PLAN_AND_EXECUTE if goal.type == MoveGoal.UNDEFINED: result = MoveResult() # TODO new error code result.error_code = MoveResult.INSOLVABLE self._as.set_aborted(result) else: result = MoveResult() for i, move_cmd in enumerate(goal.cmd_seq): # type: (int, MoveCmd) # TODO handle empty controller case intermediate_result = self.send_to_process_manager_and_wait( move_cmd) result.error_code = intermediate_result.error_code if result.error_code != MoveResult.SUCCESS: # clear traj from prev cmds result.trajectory = JointTrajectory() break result.trajectory = self.append_trajectory( result.trajectory, intermediate_result.trajectory) if i < len(goal.cmd_seq) - 1: self.let_process_manager_continue() else: # if not break rospy.loginfo(u'found solution') if result.error_code == MoveResult.SUCCESS and self.execute: result.error_code = self.send_to_robot(result) self.start_js = None if result.error_code != MoveResult.SUCCESS: self._as.set_aborted(result) else: self._as.set_succeeded(result) self.let_process_manager_continue() rospy.loginfo(u'goal result: {}'.format( ERROR_CODE_TO_NAME[result.error_code]))
def pan_tilt(self, pan, tilt): """Moves the head by setting pan/tilt angles. Args: pan: The pan angle, in radians. A positive value is clockwise. tilt: The tilt angle, in radians. A positive value is downwards. """ # TODO: Check that the pan/tilt angles are within joint limits if not ((pan >= Head.MIN_PAN and pan <= Head.MAX_PAN) and (tilt >= Head.MIN_TILT and tilt <= Head.MAX_TILT)): # Not within join limits return # TODO: Create a trajectory point point = JointTrajectoryPoint() # TODO: Set positions of the two joints in the trajectory point point.positions.append(pan) point.positions.append(tilt) # TODO: Set time of the trajectory point point.time_from_start = rospy.Duration(secs=2.5, nsecs=0) # TODO: Create goal goal = FollowJointTrajectoryGoal() traj = JointTrajectory() # TODO: Add joint names to the list traj.joint_names.append(PAN_JOINT) traj.joint_names.append(TILT_JOINT) # TODO: Add trajectory point created above to trajectory traj.points.append(point) goal.trajectory = traj # TODO: Send the goal self._pan_tilt_client.send_goal(goal) # TODO: Wait for result self._pan_tilt_client.wait_for_result()
def youbot_traj(): rospy.init_node('lab7youbot_traj') rospack = rospkg.RosPack() path = rospack.get_path('lab07_example01') bag = rosbag.Bag(path + '/../lab7data.bag') traj_pub = rospy.Publisher('/EffortJointInterface_trajectory_controller/command', JointTrajectory, queue_size=5) my_traj = JointTrajectory() tfs = 4 my_traj.header.stamp = rospy.Time.now() my_traj.joint_names.append('arm_joint_1') my_traj.joint_names.append('arm_joint_2') my_traj.joint_names.append('arm_joint_3') my_traj.joint_names.append('arm_joint_4') my_traj.joint_names.append('arm_joint_5') for topic, msg, t in bag.read_messages(topics=['my_joint_states']): my_pt = JointTrajectoryPoint() if len(msg.position) != 0: print len(msg.position) for i in range(0, 5): my_pt.positions.append(msg.position[i]) my_pt.velocities.append(msg.velocity[i]) my_pt.time_from_start.secs = tfs tfs = tfs + 4 my_traj.points.append(my_pt) bag.close rospy.sleep(5) traj_pub.publish(my_traj)
def defaultPos(self): dur = rospy.Duration(1) #CalcPos = invkin([0,0,0.563]) #CalcPos.append(self.GripperPos) #print(CalcPos) jtp = JointTrajectoryPoint(positions=[0, 0, 0, 0, 1], velocities=[1.0] * self.N_JOINTS, time_from_start=dur) dur += rospy.Duration(8) newJTP = [] newJTP.append(jtp) self.joint_positions = newJTP self.jt = JointTrajectory(joint_names=self.names, points=self.joint_positions) self.goal = FollowJointTrajectoryGoal(trajectory=self.jt, goal_time_tolerance=dur + rospy.Duration(8)) self.send_command() #self.xyzPosition = [0,0,0.563] return
def build_traj(start, end, duration): if sorted(start.name) <> sorted(end.name): rospy.logerr('Start and End position joint_names mismatch') raise # assume the start-position joint-ordering joint_names = start.name start_pt = JointTrajectoryPoint() start_pt.positions = start.position start_pt.velocities = [0]*len(start.position) start_pt.time_from_start = rospy.Duration(0.0) end_pt = JointTrajectoryPoint() for j in joint_names: idx = end.name.index(j) end_pt.positions.append(end.position[idx]) # reorder to match start-pos joint ordering end_pt.velocities.append(0) end_pt.time_from_start = rospy.Duration(duration) return JointTrajectory(joint_names=joint_names, points=[start_pt, end_pt])
def __init__(self, pub_cmd): super(Rosbag, self).__init__() self.pub_cmd = pub_cmd self.rosbag_dir = "/root/ros/install/share/servo_robot/data" self.rosbag_done = True self.rosbag_run = False self.rosbag_bag = None self.rosbag_sub = None self.msg_bag = JointTrajectory() self.msg_bag.joint_names = [ 'a1_joint', 'a2_joint', 'a3_joint', 'a4_joint', 'a5_joint', 'a6_joint' ] self.msg_bag.points.append(JointTrajectoryPoint()) self.msg_bag.points[0].time_from_start.secs = 0.0 self.msg_bag.points[0].time_from_start.nsecs = 100000000.0 self.pub_status = rospy.Publisher(SMART_STATUS, DataList, queue_size=10)
def __init__(self): # load metadata config_metadata = self.load_metadata() self.t_scale = float(config_metadata['t_scale']) # APC Net Key Parameter self.lyap_switch_threshold = 0.45 self.lyap_old = 10 self.switch_control = False # Config Local Net self.apcNet_local = DirectSiamese() self.apcNet_local.model_fld = 'direct_siamese_e2e_zoom_01' self.apcNet_local.load_ckpt(load_best=False) # Config Global Net self.apcNet_global = DirectSiamese() self.apcNet_global.model_fld = 'direct_siamese_e2e_01' self.apcNet_global.load_ckpt(load_best=False) # Init msg for publishing self.joint_traj = JointTrajectory() self.joint_traj.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] self.joint_traj.points = [ JointTrajectoryPoint(positions=[0] * 6, velocities=[0] * 6, time_from_start=rospy.Duration(0.1)) ] self.vel_ctrl_old = np.zeros([6, 1]) # Approximated Synchronised Topics self.image_sub = message_filters.Subscriber("/camera/image_color", Image) self.joint_sub = message_filters.Subscriber("/joint_states", JointState) self.synced_msgs = message_filters.ApproximateTimeSynchronizer( [self.image_sub, self.joint_sub], 10, 0.1) self.synced_msgs.registerCallback(self.image_callback) self.joint_command_pub = rospy.Publisher("/ur_driver/joint_speed", JointTrajectory, queue_size=10)
def move_arm(): rospy.init_node('arm_controller_custom', anonymous=False) topic_name = 'custom/arm_controller/command' pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1) rate = rospy.Rate(60) while not rospy.is_shutdown(): trajectory = JointTrajectory() point = JointTrajectoryPoint() trajectory.header.frame_id = "/base_link" trajectory.joint_names.append("waist") trajectory.joint_names.append("shoulder") trajectory.joint_names.append("elbow") trajectory.joint_names.append("wrist_angle") trajectory.joint_names.append("wrist_rotate") print('Enter Value for angles:') a = float(input(' Waist:\t')) b = float(input(' Shoulder:\t')) c = float(input(' Elbow:\t')) d = float(input(' Wrist Angle:\t')) point.positions.append(a) point.positions.append(b) point.positions.append(c) point.positions.append(d) point.positions.append(0.0) point.time_from_start = rospy.Duration(0, 1e8) trajectory.points.append(point) trajectory.header.stamp = rospy.Time.now() pub.publish(trajectory) rate.sleep()
def close_gripper(group): rospy.loginfo('-------------------------------------------------') rospy.loginfo('Closing gripper') rospy.loginfo('-------------------------------------------------') pub = rospy.Publisher('/gripper/command', JointTrajectory, queue_size=10) # traj = JointTrajectory() # h = Header() # h.stamp = rospy.Time.now() # h.frame_id = 'gripper_base_link' # traj.header = h # traj.joint_names.append('gripper_finger1_joint') # traj_point = JointTrajectoryPoint() # traj_point.positions.append(0.5) # traj.points.append(traj_point) # traj.points[0].time_from_start = rospy.Duration.from_sec(1.0) # pub.publish(traj) jt = JointTrajectory() jt.joint_names.append("gripper_finger1_joint") point = JointTrajectoryPoint() point.positions.append(0.37) point.time_from_start = rospy.Duration.from_sec(0.8) jt.points.append(point) # pub.publish(jt) cnt = 0 rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown() and cnt < 30: cnt += 1 pub.publish(jt) rate.sleep() rospy.loginfo('-------------------------------------------------') rospy.loginfo('DONE') rospy.loginfo('-------------------------------------------------')
def on_cancel(self, goal_handle): log("on_cancel") if goal_handle == self.goal_handle: with self.following_lock: # Uses the next little bit of trajectory to slow to a stop STOP_DURATION = 0.5 now = time.time() point0 = sample_traj(self.traj, now - self.traj_t0) point0.time_from_start = rospy.Duration(0.0) point1 = sample_traj(self.traj, now - self.traj_t0 + STOP_DURATION) point1.velocities = [0] * 6 point1.accelerations = [0] * 6 point1.time_from_start = rospy.Duration(STOP_DURATION) self.traj_t0 = now self.traj = JointTrajectory() self.traj.joint_names = joint_names self.traj.points = [point0, point1] self.goal_handle.set_canceled() self.goal_handle = None else: goal_handle.set_canceled()
def move(self, shoulder_lift_cmd_in=-pi / 4.0, elbow_cmd_in=pi / 2.0, duration_in=5.0): wrist_1_cmd = (-1.0) * (shoulder_lift_cmd_in + elbow_cmd_in ) # keep it level wrist_2_cmd = 0.5 * pi # straight along arm g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() g.trajectory.joint_names = self.joint_names p1 = JointTrajectoryPoint() p1.positions = [ 0.0, shoulder_lift_cmd_in, elbow_cmd_in, wrist_1_cmd, wrist_2_cmd, 0.0 ] p1.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] p1.time_from_start = rospy.Duration(duration_in) g.trajectory.points.append(p1) self.client.send_goal(g) rospy.loginfo("Sent goal and waiting for arm") self.client.wait_for_result() rospy.loginfo("Arm move complete")
def publish_solution(self): traj_msg = JointTrajectory() # Build message n_pts = self.RRT.solution[0][0].size # For all times for i in range(n_pts): traj_pt = JointTrajectoryPoint() t = self.RRT.solution[2][i] # Time traj_pt.time_from_start.secs = int(t) traj_pt.time_from_start.nsecs = (t - int(t)) * 1000 # For all DOF for j in range(self.n_DOF): traj_pt.positions.append(self.RRT.solution[0][j][i]) traj_pt.velocities.append(self.RRT.solution[0][j + self.n_DOF][i]) traj_pt.accelerations.append( self.RRT.solution[3][j + self.n_DOF][i]) # For all actuators for j in range(self.RRT.DS.m): traj_pt.effort.append(self.RRT.solution[1][j][i]) traj_msg.points.append(traj_pt) traj_msg.joint_names.append('1') traj_msg.joint_names.append('2') self.pub_traj_sol.publish(traj_msg) if self.verbose: rospy.loginfo("Planner: Trajectory Solution Published ") self.RRT.plot_2D_Tree()
def callback(self, anymsg, topic_name): if self.pause_button.isChecked(): return # In case of control_msgs/FollowJointTrajectoryActionGoal # set trajectory_msgs/JointTrajectory to 'msg' # Convert AnyMsg to trajectory_msgs/JointTrajectory msg_class = self.topic_name_class_map[topic_name] if msg_class == JointTrajectory: msg = JointTrajectory().deserialize(anymsg._buff) elif msg_class == FollowJointTrajectoryActionGoal: msg = FollowJointTrajectoryActionGoal().deserialize(anymsg._buff).goal.trajectory else: rospy.logerr('Wrong message type %s'%msg_class) return; self.time = np.array([0.0] * len(msg.points)) (self.dis, self.vel, self.acc, self.eff) = ({}, {}, {}, {}) for joint_name in msg.joint_names: self.dis[joint_name] = np.array([0.0] * len(msg.points)) self.vel[joint_name] = np.array([0.0] * len(msg.points)) self.acc[joint_name] = np.array([0.0] * len(msg.points)) self.eff[joint_name] = np.array([0.0] * len(msg.points)) for i in range(len(msg.points)): point = msg.points[i] self.time[i] = point.time_from_start.to_sec() for j in range(len(msg.joint_names)): joint_name = msg.joint_names[j] if point.positions: self.dis[joint_name][i] = point.positions[j] if point.velocities: self.vel[joint_name][i] = point.velocities[j] if point.accelerations: self.acc[joint_name][i] = point.accelerations[j] if point.effort: self.eff[joint_name][i] = point.effort[j] if self.joint_names != msg.joint_names: self.joint_names = msg.joint_names self.refresh_tree() self.joint_names = msg.joint_names self.plot_graph()
def move_arm( positions, time_from_start = 3.0, arm = 'r', client = None ): if (client == None): client = init_jt_client(arm) else: arm = client.action_client.ns[0:1]; # ignore arm argument rospy.loginfo( 'move arm \'%s\' to position %s'%( arm, positions ) ) joint_names = get_joint_names( ''.join( ( arm, '_arm_controller' ) ) ) jt = JointTrajectory() jt.joint_names = joint_names jt.points = [] jp = JointTrajectoryPoint() jp.time_from_start = rospy.Time.from_seconds( time_from_start ) jp.positions = positions jt.points.append( jp ) # push trajectory goal to ActionServer jt_goal = JointTrajectoryGoal() jt_goal.trajectory = jt jt_goal.trajectory.header.stamp = rospy.Time.now() # + rospy.Duration.from_sec( time_from_start ) client.send_goal( jt_goal ) client.wait_for_result() return client.get_state()