How to make fetch follow a specified trajectory

[wangjiwang]

I used the rostopic echo /joint_states to get the trajectory. And I want to make fetch move following the specified trajectory. But when I run the following python code, fetch i gazebo did not move at all. (but the value of joint_states changed as the specified trajectory )

step 1：roslaunch fetch_gazebo playground.launch
step 2 ：run the following python code

import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import String

def talker():
rospy.init_node('talker', anonymous=True)
pub = rospy.Publisher('/joint_states', JointState, queue_size=1)
rate = rospy.Rate(10)
States=JointState();

while not rospy.is_shutdown():
    States.header.stamp = rospy.Time.now()
    States.name=['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']
    States.position=[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]
    States.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]
    States.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]
    pub.publish(States)
    print (States)

if name == 'main':
try:
talker()
except rospy.ROSInterruptException:
pass

[mikeferguson]

The "joint_states" topic is an output from the drivers - it tells you where the robot is - you can't command it through that interface. To send commands to the robot, you need to use one of the action-based interfaces - see http://docs.fetchrobotics.com/api_overview.html#arm-and-torso