# create an instance of baxter_interface's Limb and Gripper classes
right_limb = baxter_interface.Limb("right")
left_limb = baxter_interface.Limb("left")
right_gripper = baxter_interface.Gripper('right')
left_gripper = baxter_interface.Gripper('left')
# set limb and gripper parameters
right_limb.set_joint_position_speed(0.6) #0-1.0
right_gripper.set_velocity(100.0) #0-100.0
right_gripper.calibrate()
left_limb.set_joint_position_speed(0.6) #0-1.0
left_gripper.set_velocity(100.0) #0-100.0
left_gripper.calibrate()
# initialize current and goal pose of the robot
goalPose = RobotPose()
currentPose = RobotPose()
updateCurrentPose()
goalPose = currentPose
# move to starting pose
#move_to_human_neutral()
#rockstar()
# initialise subscribers
create_subs()
send_image(FACE_IMAGE_PATH)
#rospy.sleep(5) # wait for user to get ready
# while not rospy.is_shutdown():
left_angles['left_s0']=-0.5
left_angles['left_s1']=-1
left_angles['left_e0']=0.0
left_angles['left_e1']=2.3
left_angles['left_w0']=0.0
left_angles['left_w1']=-1
left_angles['left_w2']=0.0
right_limb.move_to_joint_positions(right_angles)
left_limb.move_to_joint_positions(left_angles)
if __name__ == '__main__':
# initialize the ROS node, registering it with the Master
rospy.init_node("Baxter_Move_Dynamic")
# initialise robot pose
goalPose = RobotPose()
# create an instance of baxter_interface's Limb class
right_limb = baxter_interface.Limb("right")
left_limb = baxter_interface.Limb("left")
while not rospy.is_shutdown():
update()
print(goalPose.RH_pos_y)
right_limb.set_joint_positions(ik_solve('right', goalPose))
left_limb.set_joint_positions(ik_solve('left', goalPose))
rospy.sleep(0.1)
# quit
quit()