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.
Any thoughts?
--patrick
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 ...