Python JointTrajectoryPoint.accelerations方法代码示例

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

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import accelerations [as 别名]
def traj_speed_up(traj, spd=3.0):
    new_traj = RobotTrajectory()
    new_traj = traj

    n_joints = len(traj.joint_trajectory.joint_names)
    n_points = len(traj.joint_trajectory.points)

    points = list(traj.joint_trajectory.points)
    
    for i in range(n_points):
        point = JointTrajectoryPoint()
        point.time_from_start = traj.joint_trajectory.points[i].time_from_start / spd
        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] * spd
            point.accelerations[j] = point.accelerations[j] * spd * spd

        points[i] = point

    new_traj.joint_trajectory.points = points

    return new_traj

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import accelerations [as 别名]
 def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
     if self._mode == "velocity":
         velocities = [0.0] * len(joint_names)
         cmd = dict(zip(joint_names, velocities))
         while not self._server.is_new_goal_available() and self._alive:
             self._limb.set_joint_velocities(cmd)
             if self._cuff_state:
                 self._limb.exit_control_mode()
                 break
             rospy.sleep(1.0 / self._control_rate)
     elif self._mode == "position" or self._mode == "position_w_id":
         raw_pos_mode = self._mode == "position_w_id"
         if raw_pos_mode:
             pnt = JointTrajectoryPoint()
             pnt.positions = self._get_current_position(joint_names)
             if dimensions_dict["velocities"]:
                 pnt.velocities = [0.0] * len(joint_names)
             if dimensions_dict["accelerations"]:
                 pnt.accelerations = [0.0] * len(joint_names)
         while not self._server.is_new_goal_available() and self._alive:
             self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode)
             # zero inverse dynamics feedforward command
             if self._mode == "position_w_id":
                 pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time)
                 ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt)
                 self._pub_ff_cmd.publish(ff_pnt)
             if self._cuff_state:
                 self._limb.exit_control_mode()
                 break
             rospy.sleep(1.0 / self._control_rate)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import accelerations [as 别名]
def runTrajectory(req):

    global Traj_server;
    
    print "---------------------------------"
    print req.task
    print " "
    print req.bin_num
    print " "
    print req.using_torso
    print "---------------------------------"
    
    if req.using_torso == "y":
        file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/Torso/bin"+req.bin_num);
    else:
        file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/bin"+req.bin_num);

    if req.task == "Forward":
        file_name = file_root+"/forward";
    elif req.task == "Drop":
        file_name = file_root+"/drop";
    else :
        return taskResponse(False);

    f = open(file_name,"r");
    plan_file = RobotTrajectory();
    buf = f.read();
    plan_file.deserialize(buf);
    
    plan = copy.deepcopy(plan_file); 
  
        
    arm_right_group = moveit_commander.MoveGroupCommander("arm_right"); 
    arm_left_group = moveit_commander.MoveGroupCommander("arm_left");	
    arm_left_group.set_start_state_to_current_state();    
        
    StartPnt = JointTrajectoryPoint();
    StartPnt.positions = arm_left_group.get_current_joint_values();
    StartPnt.velocities = [0]*len(StartPnt.positions);
    StartPnt. accelerations = [0]*len(StartPnt.positions);
    
    #print StartPnt;
    plan.joint_trajectory.points[0] = StartPnt;
    #print plan;
    
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
    display_trajectory = moveit_msgs.msg.DisplayTrajectory();    
    robot = moveit_commander.RobotCommander();
    display_trajectory.trajectory_start = robot.get_current_state()
    display_trajectory.trajectory.append(plan)
    display_trajectory_publisher.publish(display_trajectory);  
   
   
    print "============ Waiting while", file_name, " is visualized (again)..." 
    arm_left_group.execute(plan);
    print "Trajectory ", file_name, " finished!"   
    
    f.close();
    
    return taskResponse(True);

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import accelerations [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 JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import accelerations [as 别名]
    def set_position(self,goal):
	traj = JointTrajectory()
	traj_p = JointTrajectoryPoint()
	traj.joint_names = ["torso_lift_joint"]
	traj_p.positions = [goal]
	traj_p.velocities = [0.]
	traj_p.accelerations = [0.]
	traj_p.time_from_start = rospy.Duration(2.)
	traj.points.append(traj_p)
	self.pub.publish(traj)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import accelerations [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 JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import accelerations [as 别名]
    def create_JTG(self, arm, pos_arr, dur_arr, stamp=None, vel_arr=None, acc_arr=None):
        # Hack
        vel_arr = [[0.]*7]*len(pos_arr)
        acc_arr = [[0.]*7]*len(pos_arr)

        ##
        # Compute joint velocities and acclereations.
        def get_vel_acc(q_arr, d_arr):
            vel_arr = [[0.]*7]
            acc_arr = [[0.]*7]
            for i in range(1, len(q_arr)):
                vel, acc = [], []
                for j in range(7):
                    vel += [(q_arr[i][j] - q_arr[i-1][j]) / d_arr[i]]
                    acc += [(vel[j] - vel_arr[i-1][j]) / d_arr[i]]
                vel_arr += [vel]
                acc_arr += [acc]
                print vel, acc
            return vel_arr, acc_arr

        if arm != 1:
            arm = 0
        jtg = JointTrajectoryGoal()
        if stamp is None:
            stamp = rospy.Time.now()
        else:
            jtg.trajectory.header.stamp = stamp

        if len(pos_arr) > 1 and (vel_arr is None or acc_arr is None):
            v_arr, a_arr = get_vel_acc(pos_arr, dur_arr)
            if vel_arr is None:
                vel_arr = v_arr
            if acc_arr is None:
                acc_arr = a_arr

        jtg.trajectory.joint_names = self.joint_names_list[arm]
        for i in range(len(pos_arr)):
            if pos_arr[i] is None or type(pos_arr[i]) is types.NoneType:
                continue
            jtp = JointTrajectoryPoint()
            jtp.positions = pos_arr[i]
            if vel_arr is None:
                vel = [0.] * 7
            else:
                vel = vel_arr[i]
            jtp.velocities = vel
            if acc_arr is None:
                acc = [0.] * 7
            else:
                acc = acc_arr[i]
            jtp.accelerations = acc
            jtp.time_from_start = rospy.Duration(dur_arr[i])
            jtg.trajectory.points.append(jtp)
        return jtg

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import accelerations [as 别名]
    def set_jointangles(self, arm, q, duration=0.15):
        rospy.logwarn('Currently ignoring the arm parameter.')
#        for i,p in enumerate(self.r_arm_pub_l):
#            p.publish(q[i])
        jtg = JointTrajectoryGoal()
        jtg.trajectory.joint_names = self.joint_names_list
        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.send_goal(jtg)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import accelerations [as 别名]
 def position_callback(self, msg):
     action_goal = FollowJointTrajectoryActionGoal()
     action_goal.goal_id.stamp = rospy.get_rostime()
     action_goal.goal.trajectory.joint_names = self._joint_names
     target_point = JointTrajectoryPoint()
     if self._dof == len(msg.positions):
         target_point.positions = msg.positions
     else:
         target_point.positions = []
         for position in msg.positions:
             target_point.positions.append(position)
         for i in range(self._dof - len(msg.positions)):
             target_point.positions.append(0.0)
     target_point.velocities = [self._velocity] * self._dof
     target_point.accelerations = [self._acceleration] * self._dof
     action_goal.goal.trajectory.points.append(target_point)
     self._pub.publish(action_goal)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import accelerations [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 input 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 joints and scale the time from start,
       # as well as joint speed and acceleration
       for i in range(n_points):
           point = JointTrajectoryPoint()
           
           # The joint positions are not scaled so pull them out first
           point.positions = traj.joint_trajectory.points[i].positions

           # Next, scale the time_from_start for this point
           point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
           
           # Get the joint velocities for this point
           point.velocities = list(traj.joint_trajectory.points[i].velocities)
           
           # Get the joint accelerations for this point
           point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
           
           # Scale the velocity and acceleration for each joint at this point
           for j in range(n_joints):
               point.velocities[j] = point.velocities[j] * scale
               point.accelerations[j] = point.accelerations[j] * scale * scale
        
           # Store the scaled trajectory point
           points[i] = point

       # Assign the modified points to the new trajectory
       new_traj.joint_trajectory.points = points

       # Return the new trajecotry
       return new_traj

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import accelerations [as 别名]
 def createGoalWithValueAndPublish(self, value):
     jt = JointTrajectory()
     jt.joint_names = self.joint_names
     jtp = JointTrajectoryPoint()
     # Get current values and modify the one we want
     names, values = self.getNamesAndMsgListAndModifyValue(self.joint_names,
                                                           self.joint_name,
                                                           value)
     
     jtp.positions = values
     jtp.velocities = [0.0] * len(self.joint_names)
     jtp.accelerations = [0.0] * len(self.joint_names)
     # TODO: Make duration dependent on how big is the movement
     scaled_time = abs(value) * 1.5 + 0.2
     jtp.time_from_start = rospy.Duration(scaled_time) 
     jt.points.append(jtp)
     self.pub.publish(jt)
     rospy.sleep(1.0)