how to use arm_with_torso_controller/follow_joint_trajectory controller?

[wangjiwang]

Thanks for your reply. And I try to use arm_with_torso_controller/follow_joint_trajectory controller to execute a pre-defined trajectory. It did not work. I can not find the reason, can you help me?

#!/usr/bin/env python


import sys

import rospy
import actionlib
from control_msgs.msg import (FollowJointTrajectoryAction,
                              FollowJointTrajectoryGoal)
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

arm_joint_names = ['l_wheel_joint', 'r_wheel_joint', 'torso_lift_joint', 'bellows_joint', 'head_pan_joint', 'head_tilt_joint','shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint',  'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint', 'l_gripper_finger_joint', 'r_gripper_finger_joint']
arm_joint_positions  =  [2.3345511882210843, -0.634820929568157, 1.3874957609389449e-11, 0.006625115661578738, -0.006317351703414076, 0.6313382345293581, 1.318234031118565, 1.3956276493932007, -0.2089051625781302, 1.7008202010543583, -0.016423456036472217, 1.6468142324488904, -0.008023511866540822, 0.02927438379458224, 0.02988700000824869]
velocity = [12.07760822395682, 12.531155056815715, 0.00551848394843501, -0.006297094941102944, -0.027727924188788666, -0.8389266373685278, -0.01006305278808928, 0.06190390257346417, 0.014009208799648233, 0.046633352775736804, -0.016161009170007727, 0.08276079812692123, -0.01060212434698555, -0.003579291965645062, 0.003610553717902847]
effort   = [8.85, 8.85, -450.0, -1.4943480513086147, 0.09047042943010608, -0.10530705118107508, -1.8543929008080047, 8.507774908689282, 1.273713991824661, 14.859113858920301, -7.935003515304931, -6.557514430844646, -1.7112672692034643, -9.663865221953854, -10.336134778046146]
if __name__ == "__main__":
    rospy.init_node("prepare_simulated_robot")


    if rospy.get_param("robot/serial") != "ABCDEFGHIJKLMNOPQRSTUVWX":
        rospy.logerr("This script should not be run on a real robot")
        sys.exit(-1)



    rospy.loginfo("Waiting for arm_controller...")
    arm_client = actionlib.SimpleActionClient("arm_with_torso_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
    arm_client.wait_for_server()
    rospy.loginfo("...connected.")

    trajectory = JointTrajectory()
    trajectory.joint_names = arm_joint_names

    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[0].positions = arm_joint_positions
    trajectory.points[0].velocities =  velocity
    trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions)
    trajectory.points[0].effort = effort
    trajectory.points[0].time_from_start = rospy.Duration(1.0)



    arm_goal = FollowJointTrajectoryGoal()
    arm_goal.trajectory = trajectory
    arm_goal.goal_time_tolerance = rospy.Duration(0.0)


    rospy.loginfo("Setting positions...")

    arm_client.send_goal(arm_goal)

    arm_client.wait_for_result(rospy.Duration(6.0))

    rospy.loginfo("...done")