Baxter limb doesn't move with set_joint_positions()

asked 2019-08-05 23:46:41 -0600

jephph gravatar image

Hi! I want to make a simple pick and place task performed by Baxter robot. I wrote this python script, based on the scripts in Baxter_example package.

This code runs in terminal, but the robot arm is not moving. And the gripper works as expected. What am I doing wrong here?
I want the right limb joints moving to positions listed in starting_joints_angles

Thank you very much! I am new to ROS and Baxter.

!/usr/bin/env python

import argparse import rospy import baxter_interface import baxter_external_devices from baxter_interface import CHECK_VERSION

def main():

print("Initializing node... ")

rospy.init_node("limb_simple_move")

# initialize interfaces

print("Getting robot state... ")

rs = baxter_interface.RobotEnable(CHECK_VERSION)

init_state = rs.state().enabled
limb_left = baxter_interface.Limb('left')
limb_right = baxter_interface.Limb('right')
gripper_left  = baxter_interface.Gripper('left',  CHECK_VERSION)
gripper_right = baxter_interface.Gripper('right', CHECK_VERSION)
lj = limb_left.joint_names()
rj = limb_right.joint_names()

def set_j(limb, joint_name, delta):
    current_position = limb.joint_angle(joint_name)
    joint_command = {joint_name: current_position + delta}
    limb.set_joint_positions(joint_command)


print("Enabling robot... ")
rs.enable()

def set_right_limb(joints_angles):
    print(rj)
    print(joints_angles)
    print(limb_right.joint_angles())

    limb_right.set_joint_positions( {rj[0]: joints_angles[0]})
    limb_right.set_joint_positions( {rj[1]: joints_angles[1]})
    limb_right.set_joint_positions( {rj[2]: joints_angles[2]})
    limb_right.set_joint_positions( {rj[3]: joints_angles[3]})
    limb_right.set_joint_positions( {rj[4]: joints_angles[4]})
    limb_right.set_joint_positions( {rj[5]: joints_angles[5]})
    limb_right.set_joint_positions( {rj[6]: joints_angles[6]})


starting_joints_angles = [0.5763932810479442, 1.4392574742334894, 0.228946632591898, -1.2156797743991905, -0.2174417766827574, 1.3426166845967085, 0.029912625363765568]

rospy.loginfo('moving to initial position')
set_right_limb(starting_joints_angles)
rospy.loginfo('reached initial position')
rospy.sleep(2)
gripper_right.open()
rospy.sleep(2)  
gripper_right.close()
rospy.sleep(2)
gripper_right.open()
#while not rospy.is_shutdown():
    #rospy.sleep(3)


rospy.signal_shutdown("Example finished.")

if __name__ == '__main__': main()

edit retag flag offensive close merge delete