def readInPoses(self): poses = rospy.get_param('~poses') rospy.loginfo("Found %d poses on the param server", len(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;
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 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 handle_camera_direction(self, subject): command = JointTrajectory() command.joint_names = ["head_2_joint", "head_1_joint"] point1 = JointTrajectoryPoint() #point2 = JointTrajectoryPoint() point1.velocities = [0.0, 0.0] #point2.velocities = [0] point1.time_from_start = Duration(2.0, 0.0) rospy.loginfo(subject) if subject[0] == 'up': self.head_ud = self.head_ud + self.head_move_step_size if subject[0] == 'down': self.head_ud = self.head_ud - self.head_move_step_size if subject[0] == 'left': self.head_lr = self.head_lr + self.head_move_step_size if subject[0] == 'right': self.head_lr = self.head_lr - self.head_move_step_size rospy.loginfo(self.head_ud) point1.positions = [self.head_ud, self.head_lr] command.points = [point1] self.pub_head.publish(command) return
def CreateTrajectory(self, value): """ creates custom trajectory for the head @ returns ROS followjointTrajectory """ max_vel = self.GetMaxVelocity() curr_val = self.GetJointState() traj_msg = JointTrajectory() traj_msg.header.stamp = rospy.Time.now() traj_msg.header.seq = 1 traj_msg.joint_names = self.GetJointNames() traj_msg.points = [] orig_point = JointTrajectoryPoint() orig_point.positions = self.GetJointState() orig_point.velocities = [0.0, 0.0] orig_point.accelerations = [0.0, 0.0] orig_point.time_from_start = rospy.Duration.from_sec(0.0) traj_msg.points.append(orig_point) end_point = JointTrajectoryPoint() end_point.positions = value end_point.velocities = [0.0, 0.0] end_point.accelerations = [0.0, 0.0] pan_transit = numpy.absolute( (orig_point.positions[0] - value[0]) / max_vel[0]) tilt_transit = numpy.absolute( (orig_point.positions[1] - value[1]) / max_vel[1]) end_point.time_from_start = rospy.Duration.from_sec( numpy.amax([pan_transit, tilt_transit]) / 15.) traj_msg.points.append(end_point) return traj_msg
def go_home(): pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=10) rate = rospy.Rate(25) Tmsg = JointTrajectory() # Tmsg.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint','elbow_joint','wrist_1_joint','wrist_2_joint', 'wrist_3_joint'] Tmsg.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] Tmsg.header.stamp = rospy.Time.now() Tpointmsg = JointTrajectoryPoint() Tpointmsg.velocities = [0, 0, 0, 0, 0, 0] Tpointmsg.time_from_start = rospy.Duration(0.5) Tpointmsg.positions = [-np.pi / 2, -np.pi / 2, np.pi / 2, 0, 0, 0] Tmsg.points = [Tpointmsg] # print "publishing" joint_states = rospy.wait_for_message("joint_states", JointState) joint_states.position = [round(num, 3) for num in joint_states.position] while (joint_states.position[0:3] != [round(np.pi / 2, 3), round(-np.pi / 2, 3), round(-np.pi / 2, 3)]): # print joint_states.position[0:3] pub.publish(Tmsg) joint_states = rospy.wait_for_message("joint_states", JointState) joint_states.position = [ round(num, 3) for num in joint_states.position ] rospy.sleep(1)
def velocitiesPublisher(output): #### newly added #### global arm_current #### newly added #### # 6x1 matrix for end_effector work-space velocities # Obtained by multiplying NPF with output end_effector_velocities = NPS.dot(output) correction_velocities = Vector3() formatOutput(end_effector_velocities, correction_velocities) #### newly added #### # Frequency which TIAGo runs at freq = 50 r= rospy.Rate(freq) duration = Duration(nsecs-1e9/float(freq)*3.0) # Publisher arm_pub = rospy.Publisher('/arm_controller/safe_command', JointTrajectory, queue_size=1) arm_msg = JointTrajectory() # Test to send velocity commands to TIAGo joint 6 and joint 7 joint_angles_6 = correction_velocities.z joint_angles_7 = correction_velocities.x arm_pub.publish(correction_velocities) arm_msg.joint_names = ["arm_6_joint", "arm_7_joint"] arm_point = JointTrajectoryPoint() arm_point.time_from_start = duration arm_msg.points = [arm_point]
def to_robot_trajectory(self, num_waypoints=300, jointspace=True): """ Parameters ---------- num_waypoints : float how many points in the :obj:`moveit_msgs.msg.RobotTrajectory` jointspace : bool What kind of trajectory. Joint space points are 7x' and describe the angle of each arm. Workspace points are 3x', and describe the x,y,z position of the end effector. """ traj = JointTrajectory() traj.joint_names = self.limb.joint_names() points = [] for t in np.linspace(0, self.total_time, num=num_waypoints): point = self.trajectory_point(t, jointspace) # make sure this point is the right type of message points.append(point) #print(point) # We want to make a final point at the end of the trajectory so that the # controller has time to converge to the final point. # extra_point = self.trajectory_point(self.total_time, jointspace) # extra_point.time_from_start = rospy.Duration.from_sec(self.total_time + 1) # points.append(extra_point) traj.points = points traj.header.frame_id = 'base' robot_traj = RobotTrajectory() robot_traj.joint_trajectory = traj return robot_traj
def send_goal(self, position): # Message type in JointTrajectory goal_msg = JointTrajectory() # Joint trajectory points jtp = JointTrajectoryPoint() jtp.velocities = [0.0] jtp.time_from_start.sec = 0 jtp.time_from_start.nanosec = 0 jtp.positions = [position.data[0]] if position.data[1] == 1.0: joint = "eyes_shift_horizontal_joint" else: joint = "eyes_shift_vertical_joint" # Build message jointNames = [] #print(joint) jointNames.append(joint) goal_msg.points = [jtp] goal_msg.joint_names = jointNames # Assign goal goal = FollowJointTrajectory.Goal() goal.trajectory = goal_msg self.eye_action_client_horizontal.wait_for_server() self.get_logger().info('Goal: %f ' % jtp.positions[0]) self.eye_action_client_horizontal.send_goal_async(goal)
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 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 activate(self, config, ff_wrench=Wrench()): # パラメータが存在するかチェック configs = rospy.get_param("/hsrb/impedance_control/config_names") if config not in configs: rospy.logerr(config + "is invalid name.") return False # 現在の姿勢から非常に長い(1日分)現在姿勢の軌道を作成 arm_joint, arm_value = utils.extract_joint_positions( self._joint_states_cache.data, _ARM_JOINT_NAMES) odom_joint, odom_value = utils.extract_odom_positions( self._odom_states_cache.data, _ODOM_JOINT_NAMES) trajectory = JointTrajectory() trajectory.header.stamp = rospy.get_rostime() trajectory.joint_names = arm_joint + odom_joint trajectory_point = JointTrajectoryPoint() trajectory_point.time_from_start = rospy.Duration(86400.0) trajectory_point.positions = arm_value + odom_value trajectory.points = [trajectory_point] # 軌道を投げる goal = FollowJointTrajectoryGoal() goal.trajectory = trajectory goal.preset_parameters_name = config goal.feed_forward_wrench.wrench = ff_wrench self._client.send_goal(goal) return True
def on_enter(self, userdata): self.return_code = None try: if (len(userdata.joint_names) != len(userdata.joint_values)): Logger.logerr( 'Mismatch in joint names and values (%d vs. %d) -' % (len(userdata.joint_values), len(self.joint_names))) self.return_code = 'param_error' return # Action Initialization joint_trajectory = JointTrajectory() joint_trajectory.header.stamp = rospy.Time.now() joint_trajectory.joint_names = [ name for name in userdata.joint_names ] joint_trajectory.points = [JointTrajectoryPoint()] joint_trajectory.points[0].time_from_start = rospy.Duration( self.duration) joint_trajectory.points[0].positions = [ value for value in userdata.joint_values ] # Assign to the user data userdata.joint_trajectory = joint_trajectory self.return_code = 'done' except Exception as e: Logger.logerr( 'Unable to convert joint values to trajectory - \n%s' % str(e)) self.return_code = 'param_error'
def init_pos(): # Simple script server does not work for raw3-1 therefore using trajectory controller directly switch_controller = rospy.ServiceProxy( '/arm/controller_manager/switch_controller', SwitchController) print(switch_controller([ 'joint_trajectory_controller', ], None, 1)) # switch on # Limits arm_elbow_joint: lower=\"-3.14159265\" upper=\"3.14159265\ # Limits arm_shoulder_lift_joint: lower=\"-3.14159265\" upper=\"3.14159265\" # Limits arm_shoulder_pan_joint: lower=\"-3.14159265\" upper=\"3.14159265\" # Limits arm_wrist_1_joint: lower=\"-3.14159265\" upper=\"3.14159265\"\ # Limits arm_wrist_2_joint: lower=\"-3.14159265\" upper=\"3.14159265\" # Limits arm_wrist_3_joint: lower=\"-3.14159265\" upper=\"3.14159265\" jnt_traj = JointTrajectory() jnt_traj.joint_names = [ 'arm_elbow_joint', 'arm_shoulder_lift_joint', 'arm_shoulder_pan_joint', 'arm_wrist_1_joint', 'arm_wrist_2_joint', 'arm_wrist_3_joint' ] jtp = JointTrajectoryPoint() jtp.positions = [1.69, -1.04, -1.38, -0.67, -0.69, 0.04] jtp.time_from_start.secs = 1.0 jnt_traj.points = [jtp] time.sleep(1) pub.publish(jnt_traj) time.sleep(1)
def move(self, p, v_scale=0.1, duration_low_bound=1, start_duration=0.5): target_q = self.get_inverse_kin(p) if target_q is None: return False traj = JointTrajectory() traj.header = rospy.Header(frame_id="1", stamp=rospy.Time()) traj.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] dist = np.abs(np.subtract(target_q, self.current_state.positions)).max() duration = rospy.Duration( nsecs=int(max(dist / v_scale, duration_low_bound) * 1e9)) # print(dist,duration.secs,duration.nsecs) end_point = JointTrajectoryPoint( positions=target_q, velocities=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], time_from_start=duration) start_point = JointTrajectoryPoint( positions=(np.array(self.current_state.positions) + np.array(self.current_state.velocities) * (start_duration + 0.01)).tolist(), velocities=self.current_state.velocities, time_from_start=rospy.Duration(0, int(start_duration * 1e9))) traj.points = [start_point, end_point] self.pub.publish(traj) return True
def visualize_movement(start, path): display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', DisplayTrajectory, queue_size=100) # for visualizing the robot movement; sleep(0.5) display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = convertStateToRobotState(start) trajectory = RobotTrajectory() points = [] joint_trajectory = JointTrajectory() joint_trajectory.joint_names = get_joint_names('right') for state, _ in path: point = JointTrajectoryPoint() point.positions = convertStateToRobotState(state).joint_state.position points.append(point) joint_trajectory.points = points trajectory.joint_trajectory = joint_trajectory # print("The joint trajectory is: %s" % trajectory) display_trajectory.trajectory.append(trajectory) display_trajectory_publisher.publish(display_trajectory)
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 main(): rospy.init_node('send_joints') rospy.loginfo("###########################################################") pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=10) # Create the topic message traj = JointTrajectory() traj.header = Header() # Joint names for UR5 traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] rate = rospy.Rate(1) pts = JointTrajectoryPoint() traj.header.stamp = rospy.Time.now() pts.positions = [0.0, -1.57, 1.57, -1.57, -1.57, 0.0] pts.time_from_start = rospy.Duration(1.0) # Set the points to the trajectory traj.points = [] traj.points.append(pts) # Publish the message pub.publish(traj) rospy.loginfo("-------------------------------------------------------------") rospy.spin()
def get_trajectory_message(self, action, robot_id=0): """ Helper function. Wraps an action vector of joint angles into a JointTrajectory message. The velocities, accelerations, and effort do not control the arm motion """ # Set up a trajectory message to publish. action_msg = JointTrajectory() action_msg.joint_names = self.environment['joint_order'] # Create a point to tell the robot to move to. target = JointTrajectoryPoint() action_float = [float(i) for i in action] target.positions = action_float # These times determine the speed at which the robot moves: # it tries to reach the specified target position in 'slowness' time. if (self.slowness_unit == 'sec') or (self.slowness_unit is None): target.time_from_start.secs = self.slowness elif (self.slowness_unit == 'nsec'): target.time_from_start.nsecs = self.slowness else: print("Unrecognized unit. Please use sec or nsec.") # Package the single point into a trajectory of points with length 1. action_msg.points = [target] return action_msg
def _publish_joint_trajectory(self, *_): if not self._enabled: return if self._target_values is None: return msg = JointTrajectory() msg.header.stamp = rospy.Time.now() msg.joint_names = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6' ] point = JointTrajectoryPoint() point.positions = self._target_values point.time_from_start = rospy.Duration(self._timer_rate) msg.points = [point] self._joint_trajectory_publisher.publish(msg) # Reset Target self._target_values = None # Save state for next time self._last_target_values = point.positions self._last_robot_state_published = self._new_robot_state
def pan_and_tilt(self, pan, tilt, duration=1.0, feedback_cb=None, done_cb=None): """ Moves the robot's head to the point specified in the duration specified :param pan: The pan - expected to be between HeadClient.PAN_LEFT and HeadClient.PAN_RIGHT :param tilt: The tilt - expected to be between HeadClient.TILT_UP and HeadClient.TILT_DOWN :param duration: The amount of time to take to get the head to the specified location. :param feedback_cb: Same as send_trajectory's feedback_cb :param done_cb: Same as send_trajectory's done_cb """ if(pan < self.PAN_RIGHT or pan > self.PAN_LEFT): return if(tilt > self.TILT_DOWN or tilt < self.TILT_UP): return point = JointTrajectoryPoint() point.positions = [float(pan), float(tilt)] point.time_from_start = rospy.Duration(duration) trajectory = JointTrajectory() trajectory.points = [point] trajectory.joint_names = [self.JOINT_PAN, self.JOINT_TILT] return self.send_trajectory(traj=trajectory, feedback_cb=feedback_cb, done_cb=done_cb)
def velocitiesPublisher(output): #Newly added global arm_current freq = 20 r = rospy.Rate(freq) # ROS publishers duration = Duration(nsecs=1e9 / float(freq) * 3.0) arm_pub = rospy.Publisher('/arm_controller/safe_command', JointTrajectory, queue_size=1) arm_msg = JointTrajectory() correction_velocities = Vector3() formatOutput(output, correction_velocities) print('correction_velocities[] ', correction_velocities) joint_angles_6 = correction_velocities.z joint_angles_7 = correction_velocities.x arm_pub.publish(correction_velocities) #arm_msg.joint_names = ["arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint", "arm_6_joint", # "arm_7_joint"] arm_msg.joint_names = ["arm_6_joint", "arm_7_joint"] arm_point = JointTrajectoryPoint() arm_point.time_from_start = duration arm_msg.points = [arm_point] # Subscribers arm_sub = rospy.Subscriber('/arm_controller/state', JointTrajectoryControllerState, cb_arm_state)
def go_to_q(self, q, dt=1., wait=True, exact=False): """ dt deprecated; NEW: scale sucht that maximum joint velocity is 1/4 rad/s """ q = np.asarray(q) if not exact and np.linalg.norm(q - np.asarray( self.move_group.get_current_joint_values())) < 0.1: print("Close enough already") return client = actionlib.SimpleActionClient( '/panda_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) #print("Waiting for action server ...") client.wait_for_server() goal = FollowJointTrajectoryGoal() joint_traj = JointTrajectory() joint_traj.joint_names = [ "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7" ] point1 = JointTrajectoryPoint() point1.positions = q.tolist() point1.velocities = [0., 0., 0., 0., 0., 0., 0.] delta_joints = np.abs( np.asarray(point1.positions) - np.asarray(self.move_group.get_current_joint_values())) point1.time_from_start = rospy.Duration( max(0.5, np.max(delta_joints) / 0.25)) joint_traj.points = [point1] goal.trajectory = joint_traj joint_traj.header.stamp = rospy.Time.now() + rospy.Duration(1.0) client.send_goal_and_wait(goal) if wait: self.wait_for_joint_arrival(q)
def talker(): pub = rospy.Publisher('/ur_driver/joint_speed', JointTrajectory, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(125) # 10hz trajectory = JointTrajectory() i_seq = 0 qvel = [0, 0, 0, 0, 0, 0] while not rospy.is_shutdown(): # hello_str = "hello world %s" % rospy.get_time() for i in range(6): qvel[i] += (np.random.rand() - 0.5) * 2 * 0.01 # qvel[-2] += (np.random.rand()-0.5)*2* 0.01 current_time = rospy.Time.now() trajectory.header.seq = i_seq i_seq += 1 trajectory.header.stamp = current_time trajectory.joint_names = [ 'elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] trajectory.points = [JointTrajectoryPoint(velocities=qvel)] rospy.loginfo(trajectory) pub.publish(trajectory) rate.sleep()
def main(data): pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=10) # Create the topic message arm = JointTrajectory() arm.header = Header() # Joint names for UR5 arm.joint_names = [ 'arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_gripper_right_joint', 'arm_gripper_left_joint' ] rate = rospy.Rate(50) # 50hz rospy.loginfo(arm) arm.header.stamp = rospy.Time.now() pts = JointTrajectoryPoint() pts.positions = [ data.position[3], data.position[0], data.position[4], data.position[1], data.position[2] ] pts.time_from_start = rospy.Duration(1.0) # Set the points to the trajectory arm.points = [] arm.points.append(pts) # Publish the message pub.publish(arm) rate.sleep()
def talker(): rospy.init_node('joint_trajectory_publisher', anonymous=True) pub = rospy.Publisher('arm_controller/command', JointTrajectory, queue_size=10) #pub = rospy.Publisher('/joint_group_position_controller/command', Float64MultiArray, queue_size=10) rospy.sleep(0.5) #tes = Float64MultiArray() msg = JointTrajectory() msg.header.stamp = rospy.Time.now() msg.joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] msg.points = [JointTrajectoryPoint() for i in range(1)] msg.points[0].positions = [ -math.pi / 2, -2.8 * math.pi / 6, -math.pi / 3, -1 * math.pi / 1.5, math.pi / 2, -math.pi / 2 ] msg.points[0].time_from_start = rospy.Time(1.0) #tes.data = [math.pi/2, -math.pi/6.0, -math.pi/2, -math.pi/3, -math.pi/2, 0.0] #tes.data = [-math.pi/2, -2.8 * math.pi/6, -math.pi/3, -1*math.pi/1.5, math.pi/2, -math.pi/2]; pub.publish(msg) rospy.sleep(0.5)
def talker_4(): try: pub = rospy.Publisher('/abb_irb6640_controller/command', JointTrajectory, queue_size=10) rate = rospy.Rate(125) qvel = [0, 0, 0, 0, 0, 0] i_seq = 0 global qvel_global, mutex trajectory = JointTrajectory() trajectory.joint_names = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6' ] while not rospy.is_shutdown(): if mutex.acquire(): acc_max = 0.01 for i in range(6): if (qvel_global[i] - qvel[i]) > acc_max: qvel[i] += acc_max elif (qvel_global[i] - qvel[i]) < -acc_max: qvel[i] -= acc_max else: qvel[i] = qvel_global[i] mutex.release() trajectory.header.seq = i_seq i_seq += 1 current_time = rospy.Time.now() trajectory.header.stamp = current_time trajectory.points = [JointTrajectoryPoint(velocities=qvel)] pub.publish(trajectory) rate.sleep() except rospy.ROSInterruptException: pass
def main(): rospy.init_node('send_joints') pub = rospy.Publisher('/position_trajectory_controller/command', JointTrajectory, queue_size=10) sub = rospy.Subscriber('/position_trajectory_controller/state', JointTrajectoryControllerState, _observation_callback, queue_size=10) # Create the topic message traj = JointTrajectory() traj.header = Header() # Joint names for UR5 traj.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] rate = rospy.Rate(10) while not rospy.is_shutdown(): traj.header.stamp = rospy.Time.now() pts = JointTrajectoryPoint() pts.positions = [0.0, 0.0, 1.0, 0.0, 0.0, 0.0] pts.time_from_start = rospy.Duration(0.1) # Set the points to the trajectory traj.points = [] traj.points.append(pts) # Publish the message pub.publish(traj)
def tuck(self): return rospy.loginfo('tuck_arm tuck') # 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 move_arm(self, points_list): # # Unpause the physics # rospy.wait_for_service('/gazebo/unpause_physics') # unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) # unpause_gazebo() self.client.wait_for_server() goal = FollowJointTrajectoryGoal() # We need to fill the goal message with its components # # check msg structure with: rosmsg info FollowJointTrajectoryGoal # It is composed of 4 sub-messages: # "trajectory" of type trajectory_msgs/JointTrajectory # "path_tolerance" of type control_msgs/JointTolerance # "goal_tolerance" of type control_msgs/JointTolerance # "goal_time_tolerance" of type duration trajectory_msg = JointTrajectory() # check msg structure with: rosmsg info JointTrajectory # It is composed of 3 sub-messages: # "header" of type std_msgs/Header # "joint_names" of type string # "points" of type trajectory_msgs/JointTrajectoryPoint trajectory_msg.joint_names = [ "j2n6s300_joint_1", "j2n6s300_joint_2", "j2n6s300_joint_3", "j2n6s300_joint_4", "j2n6s300_joint_5", "j2n6s300_joint_6" ] points_msg = JointTrajectoryPoint() # check msg structure with: rosmsg info JointTrajectoryPoint # It is composed of 5 sub-messages: # "positions" of type float64 # "velocities" of type float64 # "accelerations" of type float64 # "efforts" of type float64 # "time_from_start" of type duration points_msg.positions = [0, 0, 0, 0, 0, 0] points_msg.velocities = [0, 0, 0, 0, 0, 0] points_msg.accelerations = [0, 0, 0, 0, 0, 0] points_msg.effort = [0, 1, 0, 0, 0, 0] points_msg.time_from_start = rospy.Duration(0.01) # fill in points message of the trajectory message trajectory_msg.points = [points_msg] # fill in trajectory message of the goal goal.trajectory = trajectory_msg print(trajectory_msg) # self.client.send_goal_and_wait(goal) self.client.send_goal(goal) self.client.wait_for_result() rospy.sleep(2) # wait for 2s
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 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 arm2home_sim(): """ Moves arm to home position in the simulation """ joint_command_pub = rospy.Publisher("/arm_controller/command", JointTrajectory, queue_size=10) finger_pub = rospy.Publisher('/gripper_controller/command', JointTrajectory, queue_size=10) joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] joint_traj = JointTrajectory() joint_traj.joint_names = joint_names joint_traj.points = [ JointTrajectoryPoint( positions=[3 * pi / 4, -pi / 2, pi / 2, -pi / 2, -pi / 2, 0], velocities=[0] * 6, time_from_start=rospy.Duration(0.1)) ] finger_names = [ 'finger_joint', 'left_inner_finger_joint', 'left_inner_knuckle_joint', 'right_inner_finger_joint', 'right_inner_knuckle_joint', 'right_outer_knuckle_joint' ] close_level = 0 finger_angle = close_level * (0.77 / 5) finger_traj = JointTrajectory() # finger_traj.header = msg.header finger_traj.joint_names = finger_names finger_joint_config = np.array([1.0, 1.0, -1.0, 1.0, -1.0, -1.0 ]) * finger_angle finger_traj.points = [ JointTrajectoryPoint(positions=finger_joint_config, velocities=[0] * 6, time_from_start=rospy.Duration(3.0)) ] rospy.sleep(0.01) for i in range(100): joint_command_pub.publish(joint_traj) finger_pub.publish(finger_traj) rospy.sleep(0.01)
def test_controller(self): from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint topic = "/pr2/torso_controller/command" rospy.init_node('test_pr2_torso_controller', log_level=rospy.DEBUG, disable_signals=True) rospy.loginfo("Preparing to publish on %s" % topic) ctl = rospy.Publisher(topic, JointTrajectory) self.assertEquals(getjoint("torso_lift_joint"), 0.0) duration = 0.1 traj = JointTrajectory() traj.joint_name = "torso_lift_joint" initialpoint = JointTrajectoryPoint() initialpoint.positions = 0.0 initialpoint.velocities = 0.0 initialpoint.time_from_start = rospy.Duration(0.0) finalpoint = JointTrajectoryPoint() finalpoint.positions = 0.195 finalpoint.velocities = 0.0 finalpoint.time_from_start = rospy.Duration(duration) # First, move up traj.points = [initialpoint, finalpoint] ctl.publish(traj) time.sleep(duration + 0.1) self.assertEquals(getjoint("torso_lift_joint"), 0.195) # Go back to initial position finalpoint.time_from_start = rospy.Duration(0.0) initialpoint.time_from_start = rospy.Duration(duration) traj.points = [finalpoint, initialpoint] ctl.publish(traj) time.sleep(duration + 0.1) self.assertEquals(getjoint("torso_lift_joint"), 0.0)
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 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 moveToJointPosition(self, req): """ Callback for the MoveToJointPosition service Constructs a joint space trajectory plan that interpolates from the current position to the desired position in joint space :param req: robot_msgs.srv.MoveToJointPositionRequest :type req: :return: :rtype: """ jointStateFinal = req.joint_state jointStateStart = self.getRobotState() rospy.loginfo("moving robot to joint position %s", str(jointStateFinal.position)) # figure out the duration based on max joint degrees per second print "jointStateStart \n", jointStateStart print "jointStateFinal \n", jointStateFinal print "jointStateStart.position = ", jointStateStart.position q_start = np.array(jointStateStart.position) q_end = np.array(jointStateFinal.position) minPlanTime = 1.0 maxDeltaRadians = np.max(np.abs(q_end - q_start)) duration = maxDeltaRadians / np.deg2rad( req.max_joint_degrees_per_second) duration = max(duration, minPlanTime) rospy.loginfo("rescaled plan duration is %.2f seconds", duration) trajectory = JointTrajectory() trajectory.header.stamp = rospy.Time.now() startTime = rospy.Duration.from_sec(0) endTime = rospy.Duration.from_sec(duration) startPoint = RobotMovementService.jointTrajectoryPointFromJointState( jointStateStart, startTime) endPoint = RobotMovementService.jointTrajectoryPointFromJointState( jointStateFinal, endTime) trajectory.points = [startPoint, endPoint] trajectory.joint_names = self.jointNames joint_traj_action_goal = robot_msgs.msg.JointTrajectoryGoal() joint_traj_action_goal.trajectory = trajectory self.joint_space_trajectory_action.send_goal(joint_traj_action_goal) self.joint_space_trajectory_action.wait_for_result() result = self.joint_space_trajectory_action.get_result() finished_normally = ( result.status.status == result.status.FINISHED_NORMALLY) return finished_normally
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 test_controller(self): from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint topic = "/pr2/torso_controller/command" rospy.init_node("test_pr2_torso_controller", log_level=rospy.DEBUG, disable_signals=True) rospy.loginfo("Preparing to publish on %s" % topic) ctl = rospy.Publisher(topic, JointTrajectory) self.assertEquals(getjoint("torso_lift_joint"), 0.0) duration = 0.1 traj = JointTrajectory() traj.joint_name = "torso_lift_joint" initialpoint = JointTrajectoryPoint() initialpoint.positions = 0.0 initialpoint.velocities = 0.0 initialpoint.time_from_start = rospy.Duration(0.0) finalpoint = JointTrajectoryPoint() finalpoint.positions = 0.195 finalpoint.velocities = 0.0 finalpoint.time_from_start = rospy.Duration(duration) # First, move up traj.points = [initialpoint, finalpoint] ctl.publish(traj) time.sleep(duration + 0.1) self.assertEquals(getjoint("torso_lift_joint"), 0.195) # Go back to initial position finalpoint.time_from_start = rospy.Duration(0.0) initialpoint.time_from_start = rospy.Duration(duration) traj.points = [finalpoint, initialpoint] ctl.publish(traj) time.sleep(duration + 0.1) self.assertEquals(getjoint("torso_lift_joint"), 0.0)
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 send_msg(self): msg = JointTrajectory() msg.joint_names = self.arm_joint_names point = JointTrajectoryPoint() self.lock.acquire() point.positions = self.position self.lock.release() point.time_from_start = rospy.Duration(1.0) msg.points = [point] if not rospy.is_shutdown(): self.pub.publish(msg)
def jointTrajHelper(self, joint_names, positions): # Setup the joint trajectory jtm = JointTrajectory() jtp = JointTrajectoryPoint() jtm.joint_names = joint_names jtp.time_from_start = rospy.Duration(1.0) jtp.positions = [positions] jtm.points = [jtp] return jtm
def zliftCallback(self, msg): # Setup the joint trajectory jtm = JointTrajectory() jtp = JointTrajectoryPoint() jtm.joint_names = self.zlift_joint_names jtp.time_from_start = rospy.Duration(1.0) jtp.positions = [msg.position[0]*0.001] #Convert to meters jtm.points = [jtp] self.zlift_pub.publish(jtm)
def publish_trajectory(self, pub, joint_traj_positions): trajectory = JointTrajectory() trajectory.header.stamp = rospy.Time.now() trajectory.points = [JointTrajectoryPoint() for p in joint_traj_positions] for i,position in enumerate(joint_traj_positions): trajectory.points[i].positions = joint_traj_positions[i] trajectory.points[i].velocities = 0.0 trajectory.points[i].time_from_start = rospy.Duration(i*self.time_step) print "Publishing trajectory at ",trajectory.header.stamp pub.publish(trajectory)
def send_joint_angles(self,angles): pub = rospy.Publisher("/fullbody_controller/command", JointTrajectory) point = JointTrajectoryPoint() point.positions = [ x * math.pi / 180.0 for x in angles ] point.time_from_start = rospy.Duration(5.0) msg = JointTrajectory() msg.header.stamp = rospy.Time().now() msg.joint_names = ["J1","J2","J3","J4","J5","J6","J7"] msg.points = [point] pub.publish(msg) rospy.sleep(rospy.Duration(6.0))
def handle_head_pitch(self, pitch_degree): self.head_pitch_cmd_deg = pitch_degree self.head_pitch_pub.publish(data=math.radians(pitch_degree)) #Publish neck trajectory trajectory = JointTrajectory() trajectory.header.stamp = rospy.Time.now() trajectory.joint_names = ['neck_ry'] trajectory.points = [JointTrajectoryPoint()] trajectory.points[0].positions = [math.radians(pitch_degree)] trajectory.points[0].velocities = [0.0] trajectory.points[0].time_from_start = rospy.Duration(0.75) self.head_trajectory_pub.publish(trajectory)
def publish_robot_joint_state(self): print "publishing new joint states for " + self.topic + " to Atlas" joint_trajectory = JointTrajectory() joint_trajectory.joint_names = [joint.name for joint in self.joints] joint_trajectory.points = [JointTrajectoryPoint()] # joint_trajectory.points[0].positions = [joint.current_position for joint in self.joints] joint_trajectory.points[0].positions = [joint.position for joint in self.joints] # joint_trajectory.points[0].time_from_start = rospy.Duration(0.000) joint_trajectory.points[0].time_from_start = rospy.Duration(self.duration) # joint_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.25) # print joint_trajectory self.pub_robot.publish(joint_trajectory)
def sendRightArmTrajectory(): msg = JointTrajectory() msg.joint_names = RIGHT_NAMES trajectoryPoints = [createTrajectoryPoint(2.0, FLYING_RIGHT), createTrajectoryPoint(4.0, DOWN_RIGHT), createTrajectoryPoint(6.0, FLYING_RIGHT), createTrajectoryPoint(8.0, DOWN_RIGHT), createTrajectoryPoint(10.0, HOME_RIGHT)] msg.points = trajectoryPoints rospy.loginfo( 'publishing right trajectory') armTrajectoryPublisher.publish(msg)
def send_cmd_msg(self): jtm = JointTrajectory() jtm.joint_names = self.joint_names jtp = JointTrajectoryPoint() # Check if we're dealing with one single float value if len(self.joint_names) > 1: jtp.positions = self.position else: jtp.positions = [self.position]*len(self.joint_names) jtp.time_from_start = rospy.Duration(1.0) jtm.points = [jtp] return jtm
def publishThumbPosition(self, hand, position): ''' Publish for single value (thumb) ''' jtm = JointTrajectory() jtp = JointTrajectoryPoint() jtp.time_from_start = rospy.Duration(1.0) jtp.positions = [position] jtm.points = [jtp] if hand == MekaControllerConverter.RIGHT_HAND: jtm.joint_names = ['right_hand_j0'] else: jtm.joint_names = ['left_hand_j0'] self.hand_thumb_publishers[hand].publish(jtm)
def publishRobotPelvisPose(self): print "publishing new pelvis pose ..." bdi_pelvis_pose = PoseStamped() bdi_pelvis_pose.header.stamp = rospy.Time.now() bdi_pelvis_pose.pose.position.x = self.forward_position bdi_pelvis_pose.pose.position.y = self.lateral_position bdi_pelvis_pose.pose.position.z = self.height_position # Use BDI yaw*roll*pitch concatenation xaxis, yaxis, zaxis = (1, 0, 0), (0, 1, 0), (0, 0, 1) Rx = tf.transformations.rotation_matrix(self.roll_position, xaxis) Ry = tf.transformations.rotation_matrix(self.pitch_position, yaxis) Rz = tf.transformations.rotation_matrix(self.yaw_position, zaxis) R = tf.transformations.concatenate_matrices(Rz, Rx, Ry) q = tf.transformations.quaternion_from_matrix(R) bdi_pelvis_pose.pose.orientation.w = q[3] bdi_pelvis_pose.pose.orientation.x = q[0] bdi_pelvis_pose.pose.orientation.y = q[1] bdi_pelvis_pose.pose.orientation.z = q[2] # w = math.cos(self.yaw_position*0.5) # bdi_pelvis_pose.pose.orientation.x = 0.0 # bdi_pelvis_pose.pose.orientation.y = 0.0 # bdi_pelvis_pose.pose.orientation.z = math.sin(self.yaw_position*0.5) print bdi_pelvis_pose print q euler = tf.transformations.euler_from_quaternion(q) print euler self.pub_robot.publish(bdi_pelvis_pose) # Now publish the trajectory form for the new controllers trajectory = JointTrajectory() trajectory.header.stamp = rospy.Time.now() trajectory.joint_names = ["com_v1", "com_v0", "pelvis_height", "pelvis_roll", "pelvis_pitch", "pelvis_yaw"] trajectory.points = [JointTrajectoryPoint()] trajectory.points[0].positions = [ self.lateral_position, self.forward_position, self.height_position, self.roll_position, self.pitch_position, self.yaw_position, ] trajectory.points[0].velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] trajectory.points[0].time_from_start = rospy.Duration(0.75) self.pelvis_trajectory_pub.publish(trajectory)
def look_at_board(self): msg = JointTrajectory() msg.joint_names = self.joints msg.points = list() point = JointTrajectoryPoint() point.positions = [0.0, self.home_tilt] point.time_from_start = rospy.Duration(3.0) msg.points.append(point) msg.header.stamp = rospy.Time.now() + rospy.Duration(0.1) goal = FollowJointTrajectoryGoal() goal.trajectory = msg self._client.send_goal(goal)
def publish_jvrc1_jointtrajectory(jname='NECK_Y', jdegree=0): rad = float('{0:.4f}'.format(math.radians(float(jdegree)))) pub = rospy.Publisher('/JVRC_1/torque_control/set_joint_trajectory', JointTrajectory, latch=True, queue_size=10) rospy.init_node('jvrc1_choreonoid_ros_test', anonymous=True) msg = JointTrajectory() msg.joint_names = [ jname ] msg.points = [] p = JointTrajectoryPoint() p.time_from_start = rospy.rostime.Duration(0) p.positions = [ rad ] msg.points.append(p) rospy.loginfo(msg) pub.publish(msg) rospy.Rate(1).sleep()
def get2PointTrajectory( self, positions, time_from_start ): if self.positions is None or positions is None: return None jt = JointTrajectory() jt.joint_names = self.joint_names jt.points = [] jp = JointTrajectoryPoint() jp.time_from_start = rospy.Time.from_seconds( 0.0 ) jp.positions = self.positions jt.points.append( jp ) jp = JointTrajectoryPoint() jp.time_from_start = rospy.Time.from_seconds( time_from_start ) jp.positions = positions jt.points.append( jp ) jt.header.stamp = rospy.Time.now() return jt
def actionCb(self, req): goal = FollowJointTrajectoryGoal() msg = JointTrajectory() msg.joint_names = self.joints msg.points = list() point = JointTrajectoryPoint() point.positions = [ 0.0 for servo in msg.joint_names ] point.velocities = [ 0.0 for servo in msg.joint_names ] point.time_from_start = rospy.Duration(2.0) msg.points.append(point) msg.header.stamp = rospy.Time.now() + rospy.Duration(0.01) goal.trajectory = msg self.client.send_goal(goal) self.client.wait_for_result() self.server.set_succeeded( ResetArmResult() )
def humanoidCallback(self, msg): ''' When callback receives a humanoid control command convert it into the proper controllers (arms, head, etc.) for gazebo ''' trajectory_store = dict() # Initialize the dictionary for all of the parts for part in self.joint_controllers: jtm = JointTrajectory() jtp = JointTrajectoryPoint() jtp.time_from_start = rospy.Duration(1.0) jtp.positions = [] jtm.points = [jtp] trajectory_store[part] = jtm chain_values = [] # Go through the message and fill in each command separately for i in range(len(msg.chain)): chain_num = ord(msg.chain[i]) chain_values.append(chain_num) jtm = trajectory_store[chain_num] jtm.points[0].positions.append(msg.position[i]) trajectory_store[chain_num] = jtm # Figure out the unique joints that were called sent_controllers = np.unique(chain_values) # Go through the actual unique commands and populate the fields for chain_cmd in sent_controllers: jtm = trajectory_store[chain_cmd] jtm.joint_names = get_param(self.joint_controllers[chain_cmd]+'joints','') # Special case for fingers - add zeros to command if chain_cmd == MekaControllerConverter.RIGHT_HAND or chain_cmd == MekaControllerConverter.LEFT_HAND: jtm.points[0].positions.extend([0.0]*(len(jtm.joint_names)-len(jtm.points[0].positions))) trajectory_store[chain_cmd] = jtm # Only go through the controllers that were actually sent for part in sent_controllers: # Get the actual controller name controller = self.joint_controllers[part] pub = self.publishers[controller] pub.publish(trajectory_store[part])
def talker(): pub_traj = rospy.Publisher('joint_path_command', JointTrajectory) rospy.init_node('path_publisher') traj = JointTrajectory() traj.header = Header(frame_id='base_link', stamp=rospy.Time.now() + rospy.Duration(1.0)) traj.joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] traj.points = [JointTrajectoryPoint(positions=[0, 0, 0, 0, 0, 0], velocities=[0, 0, 0, 0, 0, 0], time_from_start=rospy.Duration(1)), JointTrajectoryPoint(positions=[1, 0, 0, 0, 0, 0], velocities=[0, 0, 0, 0, 0, 0])] rospy.loginfo(traj) pub_traj.publish(traj) rospy.sleep(1.0)
def jointtrajectory_publisher(): pub = rospy.Publisher('/HRP1/set_joint_trajectory', JointTrajectory, queue_size=10) rospy.init_node('pa10test', anonymous=True) r = rospy.Rate(0.1) while not rospy.is_shutdown(): now = rospy.get_time() msg = JointTrajectory() msg.joint_names = ["J1", "J2"] msg.points = [] for i in [0, 1]: p = JointTrajectoryPoint() p.time_from_start = rospy.rostime.Duration(2) p.positions = [i, i] msg.points.append(p) rospy.loginfo("hello world %s" % now) pub.publish(msg) r.sleep()
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()