Python JointTrajectory.points方法代码示例

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

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

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

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

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

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

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

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

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

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

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