Python JointTrajectory.joint_names方法代码示例

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
 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

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
    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;

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
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()

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
 def create_hand_goal(self, closing_amount):
     jt = JointTrajectory()
     if self.robot == "reemc":
         jt.joint_names = ['hand_right_thumb_joint', 'hand_right_index_joint', 'hand_right_mrl_joint']
     if self.robot == "tiago":
         jt.joint_names = ['hand_thumb_joint', 'hand_index_joint', 'hand_mrl_joint']
     jtp = JointTrajectoryPoint()
     # Hardcoded joint limits
     jtp.positions = [closing_amount * 6.0, closing_amount * 6.0, closing_amount * 9.0]
     jtp.time_from_start = rospy.Duration(0.5)
     jt.points.append(jtp)
     return jt

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
 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)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
 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')

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
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()

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
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)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
    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

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
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

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
    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)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
 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)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
    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)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
    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

# 需要导入模块: from trajectory_msgs.msg import JointTrajectory [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectory import joint_names [as 别名]
    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()