Python msg.JointTrajectory方法代码示例

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectory [as 别名]
def __init__(self, pr2, controller_name):
        self.pr2 = pr2
        self.controller_name = controller_name

        self.joint_names = rospy.get_param("/%s/joints" % controller_name)

        self.n_joints = len(self.joint_names)

        msg = self.pr2.get_last_joint_message()
        self.ros_joint_inds = [msg.name.index(name) for name in self.joint_names]
        self.rave_joint_inds = [pr2.robot.GetJointIndex(name) for name in self.joint_names]

        self.controller_pub = rospy.Publisher("%s/command" % controller_name, tm.JointTrajectory)

        all_vel_limits = self.pr2.robot.GetDOFVelocityLimits()
        self.vel_limits = np.array([all_vel_limits[i_rave] * VEL_RATIO for i_rave in self.rave_joint_inds])
        all_acc_limits = self.pr2.robot.GetDOFVelocityLimits()
        self.acc_limits = np.array([all_acc_limits[i_rave] * ACC_RATIO for i_rave in self.rave_joint_inds]) 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectory [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 JointTrajectory [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 JointTrajectory [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 JointTrajectory [as 别名]
def __init__(self, move_group_arm):
        self.move_group_arm = move_group_arm
        self.traj_finished_event = threading.Event()
        self.current_goal_id = None
        self.current_goal_result = GoalStatus.LOST
        rospy.Subscriber('/niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal',
                         FollowJointTrajectoryActionGoal, self.callback_current_goal)

        rospy.Subscriber('/niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result',
                         FollowJointTrajectoryActionResult, self.callback_goal_result)

        # Direct topic to joint_trajectory_controller
        # Used ONLY when goal is aborted, to enter position hold mode
        self.joint_trajectory_publisher = rospy.Publisher(
            '/niryo_one_follow_joint_trajectory_controller/command',
            JointTrajectory, queue_size=10) 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectory [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 JointTrajectory [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 JointTrajectory [as 别名]
def __init__(self):
        rospy.init_node('gripper_controller')
        self.msg = None
       
       
        self.r_pub = rospy.Publisher('gripper_controller/command',
                Float64,
                queue_size=10)
        """
        self.t_pub = rospy.Publisher('arm_controller/command',
                JointTrajectory,
                queue_size=10)
        self.js_sub = rospy.Subscriber('joint_states', JointState,
                self._jointStateCb)
        """

        # subscribe to command and then spin
        self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
        self.server.start()
        rospy.spin() 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectory [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 JointTrajectory [as 别名]
def add_demonstration(self, demonstration):
        """
        Add a new  demonstration and update the model
        :param demonstration: RobotTrajectory or JointTrajectory object
        :return:
        """
        if isinstance(demonstration, RobotTrajectory):
            demonstration = demonstration.joint_trajectory
        elif not isinstance(demonstration, JointTrajectory):
            raise TypeError("ros.ProMP.add_demonstration only accepts RT or JT, got {}".format(type(demonstration)))

        if len(self.joint_names) > 0 and self.joint_names != demonstration.joint_names:
            raise ValueError("Joints must be the same and in same order for all demonstrations, this demonstration has joints {} while we had {}".format(demonstration.joint_names, self.joint_names))

        self._durations.append(demonstration.points[-1].time_from_start.to_sec() - demonstration.points[0].time_from_start.to_sec())
        self.joint_names = demonstration.joint_names
        demo_array = [jtp.positions for jtp in demonstration.points]
        self.promp.add_demonstration(demo_array) 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectory [as 别名]
def stop(self):
        jt = tm.JointTrajectory()
        jt.joint_names = self.joint_names
        jt.header.stamp = rospy.Time.now()
        self.controller_pub.publish(jt) 

# 需要导入模块: from trajectory_msgs import msg [as 别名]
# 或者: from trajectory_msgs.msg import JointTrajectory [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 JointTrajectory [as 别名]
def set_position_hold_mode(self):
        msg = JointTrajectory()
        msg.header.stamp = rospy.Time.now()
        msg.points = []
        self.joint_trajectory_publisher.publish(msg) 

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