Python msg.JointTrajectoryPoint方法代码示例

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
def goto_joint_positions(self, positions_goal):

        positions_cur = self.get_joint_positions()
        assert len(positions_goal) == len(positions_cur)

        duration = norm((r_[positions_goal] - r_[positions_cur]) / self.vel_limits, ord=inf)

        jt = tm.JointTrajectory()
        jt.joint_names = self.joint_names
        jt.header.stamp = rospy.Time.now()

        jtp = tm.JointTrajectoryPoint()
        jtp.positions = positions_goal
        jtp.velocities = zeros(len(positions_goal))
        jtp.time_from_start = rospy.Duration(duration)

        jt.points = [jtp]
        self.controller_pub.publish(jt)

        rospy.loginfo("%s: starting %.2f sec traj", self.controller_name, duration)
        self.pr2.start_thread(JustWaitThread(duration)) 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
def follow_timed_joint_trajectory(self, positions, velocities, times):

        jt = tm.JointTrajectory()
        jt.joint_names = self.joint_names
        jt.header.stamp = rospy.Time.now()

        for (position, velocity, time) in zip(positions, velocities, times):
            jtp = tm.JointTrajectoryPoint()
            jtp.positions = position
            jtp.velocities = velocity
            jtp.time_from_start = rospy.Duration(time)
            jt.points.append(jtp)

        self.controller_pub.publish(jt)
        rospy.loginfo("%s: starting %.2f sec traj", self.controller_name, times[-1])
        self.pr2.start_thread(JustWaitThread(times[-1])) 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
def follow_timed_trajectory(self, times, angs):
        times_up = np.arange(0, times[-1] + 1e-4, .1)
        angs_up = np.interp(times_up, times, angs)

        jt = tm.JointTrajectory()
        jt.header.stamp = rospy.Time.now()
        jt.joint_names = ["%s_gripper_joint" % self.lr]
        for (t, a) in zip(times, angs):
            jtp = tm.JointTrajectoryPoint()
            jtp.time_from_start = rospy.Duration(t)
            jtp.positions = [a]
            jt.points.append(jtp)
        self.diag_pub.publish(jt)

        self.pr2.start_thread(GripperTrajectoryThread(self, times_up, angs_up))
        # self.pr2.start_thread(GripperTrajectoryThread(self, times, angs)) 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
def move_ur5_joint_trajectory(xopt, duration=0.1):
    '''
    generally for use with gazebo
    :param xopt:
    :return:
    '''
    global jt_pub_ur5
    jt_ur5 = JointTrajectory()
    jt_ur5.joint_names = ['shoulder_pan_joint','shoulder_lift_joint','elbow_joint',
                      'wrist_1_joint','wrist_2_joint','wrist_3_joint']

    jtpt = JointTrajectoryPoint()
    jtpt.positions = [xopt[0], xopt[1], xopt[2], xopt[3], xopt[4], xopt[5]]
    jtpt.time_from_start = rospy.Duration.from_sec(duration)

    jt_ur5.points.append(jtpt)
    jt_pub_ur5.publish(jt_ur5) 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
def move_sawyer_joint_trajectory(xopt, duration=0.1):
    '''
    generally for use with gazebo
    :param xopt:
    :return:
    '''
    global jt_pub_sawyer
    jt_sawyer = JointTrajectory()
    jt_sawyer.joint_names = ['right_j0','right_j1','right_j2',
                      'right_j3','right_j4','right_j5', 'right_j6']

    jtpt = JointTrajectoryPoint()
    jtpt.positions = [xopt[0], xopt[1], xopt[2], xopt[3], xopt[4], xopt[5], xopt[6]]
    jtpt.time_from_start = rospy.Duration.from_sec(duration)

    jt_sawyer.points.append(jtpt)
    jt_pub_sawyer.publish(jt_sawyer) 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
def parseXapPoses(self, xaplibrary):
        try:
            poses = xapparser.getpostures(xaplibrary)
        except RuntimeError as re:
            rospy.logwarn("Error while parsing the XAP file: %s" % str(re))
            return

        for name, pose in poses.items():

            trajectory = JointTrajectory()

            trajectory.joint_names = pose.keys()
            joint_values = pose.values()

            point = JointTrajectoryPoint()
            point.time_from_start = Duration(2.0) # hardcoded duration!
            point.positions = pose.values()
            trajectory.points = [point]

            self.poseLibrary[name] = trajectory 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
def GetBezierPoint(self, b_matrix, idx, t, cmd_time, dimensions_dict):
        pnt = JointTrajectoryPoint()
        pnt.time_from_start = rospy.Duration(cmd_time)
        num_joints = b_matrix.shape[0]
        pnt.positions = [0.0] * num_joints
        if dimensions_dict['velocities']:
            pnt.velocities = [0.0] * num_joints
        if dimensions_dict['accelerations']:
            pnt.accelerations = [0.0] * num_joints
        for jnt in range(num_joints):
            b_point = bezier.bezier_point(b_matrix[jnt, :, :, :], idx, t)
            # Positions at specified time
            pnt.positions[jnt] = b_point[0]
            # Velocities at specified time
            if dimensions_dict['velocities']:
                pnt.velocities[jnt] = b_point[1]
            # Accelerations at specified time
            if dimensions_dict['accelerations']:
                pnt.accelerations[jnt] = b_point[-1]
        return pnt 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
def command_pan_tilt_vel(self, pan_vel, tilt_vel, dt=0.2):
        pan, tilt = self.get_joint_positions()
        jt = tm.JointTrajectory()
        jt.header.stamp = rospy.Time.now() + rospy.Duration(0.01)
        jt.joint_names = ["head_pan_joint", "head_tilt_joint"]
        jtp = tm.JointTrajectoryPoint(positions=[pan + pan_vel * dt,
                                                 tilt + tilt_vel * dt],
                                      velocities=[pan_vel,
                                                  tilt_vel],
                                      time_from_start=rospy.Duration(dt))
        jt.points = [jtp]
        self.controller_pub.publish(jt)
        self.pr2.start_thread(JustWaitThread(dt)) 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
def publish_joint_trajectory(self, positions, duration):
        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 = positions
        point.time_from_start = rospy.Duration(duration)
        msg.points = [point]

        self.joint_trajectory_publisher.publish(msg) 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
def _jointStateCb(self, msg):
        self.msg = msg
        print msg
        traj = JointTrajectory()
        traj.joint_names = msg.name
        pt = JointTrajectoryPoint()
        pt.positions = list(msg.position)
        pt.positions[1] = 1
        traj.points = [pt]
        self.t_pub.publish(traj) 

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

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
def move_joint(angles):
    goal = FollowJointTrajectoryGoal()
    goal.trajectory.joint_names = ['arm_base_joint', 'shoulder_joint','bottom_wrist_joint' ,'elbow_joint', 'top_wrist_joint']
    point = JointTrajectoryPoint()
    point.positions = angles
    point.time_from_start = rospy.Duration(3)
    goal.trajectory.points.append(point)
    client.send_goal_and_wait(goal) 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
def numpy_to_trajectory(trajectory, joint_names, duration):
        rt = RobotTrajectory()
        rt.joint_trajectory.joint_names = joint_names
        for point_idx, point in enumerate(trajectory):
            time = point_idx*duration/float(len(trajectory))
            jtp = JointTrajectoryPoint(positions=map(float, point), time_from_start=Duration(time))
            rt.joint_trajectory.points.append(jtp)
        return rt 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
def generate_trajectory(self, randomness=1e-10, duration=-1):
        """
        Generate a new trajectory from the given demonstrations and parameters
        :param duration: Desired duration, auto if duration < 0
        :return: the generated RobotTrajectory message
        """
        trajectory_array = self.promp.generate_trajectory(randomness)
        rt = RobotTrajectory()
        rt.joint_trajectory.joint_names = self.joint_names
        duration = float(self.mean_duration) if duration < 0 else duration
        for point_idx, point in enumerate(trajectory_array):
            time = point_idx*duration/float(self.num_points)
            jtp = JointTrajectoryPoint(positions=map(float, point), time_from_start=Duration(time))
            rt.joint_trajectory.points.append(jtp)
        return rt 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
def _add_motion_command_to_queue(self,command):
        tmp = JointTrajectory()
        tmp.header = command.header
        tmp.joint_names = ['pan_joint','pan_joint']
        tmp.points = [JointTrajectoryPoint()]
        tmp.points[0].positions = [command.pan_cmd.pos_rad,command.tilt_cmd.pos_rad]
        tmp.points[0].velocities = [command.pan_cmd.vel_rps,command.tilt_cmd.vel_rps]
        tmp.points[0].accelerations = [command.pan_cmd.acc_rps2,command.tilt_cmd.acc_rps2]
        
        self._cmd_buffer.put(tmp.points[0])