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.wait_for_server() rospy.loginfo("Connected to move_"+side+"_arm server") goal = MoveArmGoal() goal.motion_plan_request.group_name = side + "_arm" goal.motion_plan_request.num_planning_attempts = 1 goal.motion_plan_request.allowed_planning_time = rospy.Duration(10.0) goal.motion_plan_request.planner_id = "" goal.planner_service_name = "ompl_planning/plan_kinematic_path" desired_pose = SimplePoseConstraint() desired_pose.header.frame_id = "lower_torso_link" desired_pose.link_name = side + "_hand_link"; desired_pose.pose.position.x = 0.248939291788; desired_pose.pose.position.y = 0.00880423979233; desired_pose.pose.position.z = -0.0346550990121; desired_pose.pose.orientation.x = 0.317800304267; desired_pose.pose.orientation.y = -0.541804275305; desired_pose.pose.orientation.z = 0.299045455395; desired_pose.pose.orientation.w = 0.71834734598; desired_pose.absolute_position_tolerance.x = 0.05; desired_pose.absolute_position_tolerance.y = 0.05; desired_pose.absolute_position_tolerance.z = 0.05; desired_pose.absolute_roll_tolerance = 0.2; desired_pose.absolute_pitch_tolerance = 0.2; desired_pose.absolute_yaw_tolerance = 0.2; addGoalConstraintToMoveArmGoal(desired_pose, goal) finished_within_time = False move_arm.send_goal(goal) finished_within_time = move_arm.wait_for_result(rospy.Duration(200)); if not finished_within_time: move_arm.cancel_goal() rospy.loginfo("Timed out achieving goal") else: state = move_arm.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Action succeeded!") else: rospy.loginfo("Action failed with error code: " + str(state)) # Relax all servos to give them a rest. for torque_enable in servo_torque_enable: torque_enable(False) print "Finished"
answered 2011-08-24 10:57:29 -0500egiljones
Patrick - sorry, didn't see your comment until now.
It's a little confusing to figure out how to set parameters for a the kdl_plugin - you have to set them in the node and then in the namespace for the group - this is so a node can have several different kinematics solvers. Setting the following parameters for your arm makes IK take about 1.4 seconds when there's no solution on my desktop. Not positive whether it'll work better for you in random poses, but it's worth a try:
<node pkg="arm_kinematics_constraint_aware" type="arm_kinematics_constraint_aware" name="pi_robot_right_arm_kinematics"> <param name="group" type="string" value="right_arm" /> <param name="right_arm/root_name" type="string" value="lower_torso_link" /> <param name="right_arm/tip_name" type="string" value="right_wrist_link" /> <param name="right_arm/max_solver_iterations" type="int" value="10000" /> <param name="right_arm/max_search_iterations" type="int" value="10" /> <param name="kinematics_solver" type="string" value="arm_kinematics_constraint_aware/KDLArmKinematicsPlugin" /> </node>
answered 2011-08-17 09:36:33 -0500egiljones
This is a shortcoming of the current plugin being used in arm_kinematics_constraint_aware. As it's a numerical optimization solver, the KDL IK is very sensitive to the seed state at which the search begins. It will generally find a good solution quickly when the seed state is close to the goal state - this is why the mode that you use in the planning_components_visualizer works so well, as we constantly update the seed state such that each new pose is only slightly different than the old pose.
When you are invoking move_arm this will be set by default to the current state of the robot's arm - when you are dragging the arm further and further away from the desired pose the seed state gets worse and worse, and the numerical optimization may get stuck in a local minima.
There are a couple parameters you can tune in the IK plugin that will make it try harder to find solutions. The first is the 'max_solver_iterations' - it's set to 500 by default, but you may get better performance increasing it to 1000 or more. The second is 'max_search_iterations' - currently set to 3 by default. If KDL fails to converge by the requested number of iterations starting with the supplied seed state, it will randomly reseed the joints in between the limits and try again, repeating this process up to the requested number of search iterations.
The side-effect of increasing these parameters is that when KDL doesn't find a solution it can take a long time to return. On my machine the current time to return with no solution given the default parameters is about 10 milliseconds, which doesn't seem like a long time, but the PR2 analytic solver will return a failure in less than 10 microseconds.
The last option is to explore OpenRAVE's ikfast, which should provide analytical solutions quickly but is not currently integrated into the wizard pipeline. I'll be looking into integrating it but it probably won't make it into a release until F-turtle.
Asked: 2011-08-16 16:18:59 -0500
Seen: 411 times
Last updated: Oct 03 '11