Python JointTrajectoryPoint.time_from_start方法代码示例

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

  if sorted(start.name) <> sorted(end.name):
    rospy.logerr('Start and End position joint_names mismatch')
    raise

  # assume the start-position joint-ordering
  joint_names = start.name

  # 始点定義
  start_pt = JointTrajectoryPoint()
  start_pt.positions = start.position
  start_pt.velocities = [0]*len(start.position)
  start_pt.time_from_start = rospy.Duration(0.0)  # 始点なので、待ち時間 = 0 [sec]

  # 中間地点定義
  middle_pt = JointTrajectoryPoint()
  for j in joint_names:
    idx = end.name.index(j)
    middle_pt.positions.append((start.position[idx] + end.position[idx])/2.0)
    middle_pt.velocities.append(0)
  middle_pt.time_from_start = rospy.Duration(duration) # 中間地点までの到達時間 = duration [sec]

  # 執着地点
  end_pt = JointTrajectoryPoint()
  for j in joint_names:
    idx = end.name.index(j)
    end_pt.positions.append(end.position[idx])
    end_pt.velocities.append(0.0)
  end_pt.time_from_start = rospy.Duration(duration*2) # 終点までの到達時間 = duration*2 [sec]
  
  return JointTrajectory(joint_names=joint_names, points=[start_pt, middle_pt, end_pt])

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

  # print start.name
  # print end.name
  if sorted(start.name) <> sorted(end.name):
    rospy.logerr('Start and End position joint_names mismatch')
    raise

  # assume the start-position joint-ordering
  joint_names = start.name
 
  start_pt = JointTrajectoryPoint()
  start_pt.positions = start.position
  start_pt.velocities = [0.05]*len(start.position)
  start_pt.time_from_start = rospy.Duration(0.0)

  end_pt = JointTrajectoryPoint()
  # end_pt = start_pt;
  # end_pt.positions[3] = end_pt.positions[3]+.1
  for j in joint_names:
    idx = end.name.index(j)
    end_pt.positions.append(end.position[idx])  # reorder to match start-pos joint ordering
    end_pt.velocities.append(0)
  end_pt.time_from_start = rospy.Duration(duration)

  return JointTrajectory(joint_names=joint_names, points=[start_pt, end_pt])

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import time_from_start [as 别名]
def scale_trajectory_speed(traj, scale):
    # Create a new trajectory object
    new_traj = RobotTrajectory()
    # Initialize the new trajectory to be the same as the planned trajectory
    new_traj.joint_trajectory = traj.joint_trajectory
    # Get the number of joints involved
    n_joints = len(traj.joint_trajectory.joint_names)
    # Get the number of points on the trajectory
    n_points = len(traj.joint_trajectory.points)
    # Store the trajectory points
    points = list(traj.joint_trajectory.points)
    # Cycle through all points and scale the time from start, speed and acceleration
    for i in range(n_points):
        point = JointTrajectoryPoint()
        point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
        point.velocities = list(traj.joint_trajectory.points[i].velocities)
        point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
        point.positions = traj.joint_trajectory.points[i].positions
        for j in range(n_joints):
            point.velocities[j] = point.velocities[j] * scale
            point.accelerations[j] = point.accelerations[j] * scale * scale
        points[i] = point
    # Assign the modified points to the new trajectory
    new_traj.joint_trajectory.points = points
    # Return the new trajectory
    return new_traj

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import time_from_start [as 别名]
    def joint_trajectory(self, joint_trajectory):
        '''
        Returns just the part of the trajectory corresponding to the arm.

        **Args:**
        
            **joint_trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to convert

        **Returns:**
            A trajectory_msgs.msg.JointTrajectory corresponding to only this arm in joint_trajectory

        **Raises:**
            
            **ValueError:** If some arm joint is not found in joint_trajectory
        '''
        arm_trajectory = JointTrajectory()
        arm_trajectory.header = copy.deepcopy(joint_trajectory.header)
        arm_trajectory.joint_names = copy.copy(self.joint_names)
        indexes = [-1]*len(self.joint_names)
        for (i, an) in enumerate(self.joint_names):
            for (j, n) in enumerate(joint_trajectory.joint_names):
                if an == n:
                    indexes[i] = j
                    break
            if indexes[i] < 0:
                raise ValueError('Joint '+n+' is missing from joint trajectory')
        
        for joint_point in joint_trajectory.points:
            arm_point = JointTrajectoryPoint()
            arm_point.time_from_start = joint_point.time_from_start
            for i in indexes:
                arm_point.positions.append(joint_point.positions[i])
            arm_trajectory.points.append(arm_point)
        return arm_trajectory

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import time_from_start [as 别名]
    def convert_trajectory(self, traj):
        """ Converts a trajectory into a joint trajectory
        Args:
            traj: Trajectory to convert
        Returns:
            joint trajectory
        """
        new_traj = JointTrajectory()

        new_traj.header = traj.header
        # Take each joint in the trajectory and add it to the joint trajectory
        for joint_name in traj.joint_names:
            new_traj.joint_names.append(self.joint_map[joint_name].joint_name)
        # For each poiint in the trajectory
        for point in traj.points:
            new_point = JointTrajectoryPoint()
            for i, pos in enumerate(point.positions):
                new_point.positions.append(self.joint_map[new_traj.joint_names[i]].convert_angle(pos))
            for i, vel in enumerate(point.velocities):
                new_point.velocities.append(self.joint_map[new_traj.joint_names[i]].convert_velocity(vel))

            new_point.time_from_start = point.time_from_start
            new_traj.points.append(new_point)

        return new_traj

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import time_from_start [as 别名]
    def createHeadGoal(self):
        (roll, pitch, yaw) = euler_from_quaternion([self.last_oculus_msg.pose.orientation.x,
                                                    self.last_oculus_msg.pose.orientation.y,
                                                    self.last_oculus_msg.pose.orientation.z,
                                                    self.last_oculus_msg.pose.orientation.w])
        rospy.loginfo("roll, pitch, yaw: ")
        rospy.loginfo(str(roll) + " " + str(pitch) + " " + str(yaw))
        head_goal = FollowJointTrajectoryGoal()
        head_goal.trajectory.joint_names.append('head_1_joint') # 1 positivo izquierda, -1 derecha
        head_goal.trajectory.joint_names.append('head_2_joint') # -1 arriba, 1 abajo
        # Current position trajectory point
        jtp_current = JointTrajectoryPoint()
        jtp_current.positions.append(self.last_head_state.actual.positions[0])
        jtp_current.positions.append(self.last_head_state.actual.positions[1])
        jtp_current.velocities.append(self.last_head_state.actual.velocities[0])
        jtp_current.velocities.append(self.last_head_state.actual.velocities[1])
        jtp_current.time_from_start = rospy.Duration(0.0)
        
        # Next goal position
        jtp = JointTrajectoryPoint()
        jtp.positions.append(yaw *-1)
        jtp.positions.append(pitch *-1)
#         jtp.positions.append((yaw *-1) + 1.5707963267948966) # + 180 deg
#         jtp.positions.append((roll + 1.5707963267948966) *-1) # + 180 deg because of torso2
        jtp.velocities.append(0.0)
        jtp.velocities.append(0.0)
        rospy.loginfo("Sending: " + str(jtp.positions))
        #jtp.time_from_start.secs = 1
        jtp.time_from_start.nsecs = 100
        head_goal.trajectory.points.append(jtp_current)
        head_goal.trajectory.points.append(jtp)
        head_goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
        return head_goal

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import time_from_start [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 JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import time_from_start [as 别名]
    def move(self, angles):
        goal = JointTrajectoryGoal()
        char = self.name[0] #either 'r' or 'l'
        goal.trajectory.joint_names = [char+'_shoulder_pan_joint',
                                       char+'_shoulder_lift_joint',
                                       char+'_upper_arm_roll_joint',
                                       char+'_elbow_flex_joint',
                                       char+'_forearm_roll_joint',
                                       char+'_wrist_flex_joint',
                                       char+'_wrist_roll_joint']
        point = JointTrajectoryPoint()
        
        new_angles = list(angles)
        
        #replace angles that have None with last commanded joint position from this Arm class.
        for i,ang in enumerate(angles):
            if ang is None:
                assert self.last_angles is not None
                new_angles[i] = self.last_angles[i]

        self.last_angles = new_angles
        point.positions = new_angles

        point.time_from_start = rospy.Duration(3)
        goal.trajectory.points.append(point)
        self.jta.send_goal_and_wait(goal)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import time_from_start [as 别名]
    def set_jep(self, arm, q, duration=0.15):
        if q is None or len(q) != 7:
            raise RuntimeError("set_jep value is " + str(q))
        self.arm_state_lock[arm].acquire()

        jtg = JointTrajectoryGoal()
        jtg.trajectory.joint_names = self.joint_names_list[arm]
        jtp = JointTrajectoryPoint()
        jtp.positions = q
        #jtp.velocities = [0 for i in range(len(q))]
        #jtp.accelerations = [0 for i in range(len(q))]
        jtp.time_from_start = rospy.Duration(duration)
        jtg.trajectory.points.append(jtp)
        self.joint_action_client[arm].send_goal(jtg)

        self.jep[arm] = q
        cep, r = self.arms.FK_all(arm, q)
        self.arm_state_lock[arm].release()

        o = np.matrix([0.,0.,0.,1.]).T
        cep_marker = hv.single_marker(cep, o, 'sphere',
                        '/torso_lift_link', color=(0., 0., 1., 1.),
                            scale = (0.02, 0.02, 0.02),
                            m_id = self.cep_marker_id)

        cep_marker.header.stamp = rospy.Time.now()
        self.marker_pub.publish(cep_marker)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import time_from_start [as 别名]
 def execute(self, userdata):
     rospy.loginfo('In create_move_group_joints_goal')
     
     _move_joint_goal = FollowJointTrajectoryGoal()
     _move_joint_goal.trajectory.joint_names = userdata.move_joint_list
     
     jointTrajectoryPoint = JointTrajectoryPoint()
     jointTrajectoryPoint.positions = userdata.move_joint_poses
     
     for x in range(0, len(userdata.move_joint_poses)):
         jointTrajectoryPoint.velocities.append(0.0)
      
     jointTrajectoryPoint.time_from_start = rospy.Duration(2.0)
     
     _move_joint_goal.trajectory.points.append(jointTrajectoryPoint)
     
     for joint in _move_joint_goal.trajectory.joint_names:
         goal_tol = JointTolerance()
         goal_tol.name = joint
         goal_tol.position = 5.0
         goal_tol.velocity = 5.0
         goal_tol.acceleration = 5.0
         _move_joint_goal.goal_tolerance.append(goal_tol)
     _move_joint_goal.goal_time_tolerance = rospy.Duration(2.0)
     _move_joint_goal.trajectory.header.stamp = rospy.Time.now()
     
     rospy.loginfo('Move_Joint_GOAL: ' + str(_move_joint_goal))
     userdata.move_joint_goal = _move_joint_goal
     return 'succeeded'