IK works in planning visualizer but often fails on real robot

edit

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 ...