IK works in planning visualizer but often fails on real robot
Hello arm navigation gurus,
I am running the latest and greatest Electric arm_navigation Debian packages on an Ubuntu 10.04 machine. Using the planning wizard is a breeze and I have defined one kinematic group for my robot's 6-dof right arm (see here for URDF). Next I tried things out in the planning visualizer and the IK engine has no trouble finding solutions for random movements of the right hand (no gripper).
Turning to the real robot, I am using the dynamixel_motor stack to drive the arm and I have successfully tested the basic topics and services that control the servos. The joint states are published on the /joint_states topic at 20 Hz and moving the arm passively is reflected smoothly in RViz.
I then wrote a simple move_arm test script in Python that mimics the C++ tutorial found here. I have included the script below. The target pose in the script was taken from the FK when the arm was passively moved in front of the robot and slight bent. I then moved the arm around into various joint positions and ran my test script manually to see if an IK solution could be found.
In some cases, a solution is found and the arm moves over a smooth trajectory to place the hand correctly. However, there seems to be only a small bubble around the desired pose where IK solutions can be found. This is in contrast to the planning visualizer where solutions were not a problem. I'm guessing there is something missing from my test script related to initializing the environment scene but I'm not sure how to do that with Python.
Here now is my test script:
#! /usr/bin/python import roslib roslib.load_manifest('pi_dynamixel_controller') import rospy import actionlib from actionlib_msgs.msg import * from pr2_controllers_msgs.msg import * from trajectory_msgs.msg import * from dynamixel_controllers.srv import TorqueEnable from arm_navigation_msgs.msg import * from control_msgs.msg import FollowJointTrajectoryAction def poseConstraintToPositionOrientationConstraints(pose_constraint): position_constraint = PositionConstraint() orientation_constraint = OrientationConstraint() position_constraint.header = pose_constraint.header position_constraint.link_name = pose_constraint.link_name position_constraint.position = pose_constraint.pose.position position_constraint.constraint_region_shape.type = 0 position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.x) position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.y) position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.z) position_constraint.constraint_region_orientation.x = 0.0 position_constraint.constraint_region_orientation.y = 0.0 position_constraint.constraint_region_orientation.z = 0.0 position_constraint.constraint_region_orientation.w = 1.0 position_constraint.weight = 1.0 orientation_constraint.header = pose_constraint.header orientation_constraint.link_name = pose_constraint.link_name orientation_constraint.orientation = pose_constraint.pose.orientation orientation_constraint.type = pose_constraint.orientation_constraint_type orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance orientation_constraint.weight = 1.0 return (position_constraint, orientation_constraint) def addGoalConstraintToMoveArmGoal(pose_constraint, move_arm_goal): position_constraint, orientation_constraint = poseConstraintToPositionOrientationConstraints(pose_constraint); move_arm_goal.motion_plan_request.goal_constraints.position_constraints.append(position_constraint) move_arm_goal.motion_plan_request.goal_constraints.orientation_constraints.append(orientation_constraint) rospy.init_node('place_hand', anonymous=True) side = rospy.get_param("~side", "right") move_arm = actionlib.SimpleActionClient("move_" + side + "_arm", MoveArmAction) dynamixel_namespace = '/dynamixel_controller' dynamixels = rospy.get_param(dynamixel_namespace + '/dynamixels', dict()) servo_torque_enable = list() for name in sorted(dynamixels): torque_enable_service = dynamixel_namespace + '/' + name + '_controller/torque_enable' rospy.wait_for_service(torque_enable_service) servo_torque_enable.append(rospy.ServiceProxy(torque_enable_service, TorqueEnable)) move_arm ...