About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Running arm navigation on non-PR2 arm

Description: This tutorial describes the steps necessary to make arm_navigation stack running on an arm other than PR2

Tutorial Level: ADVANCED

There are several components that we need to implement, including:

• Inverse/Forward Kinematics solver
• Constraint Aware Inverse Kinematics solver
• Joint Trajectory Execution

Many off-the-shelf arms already provide all of the above, and if that's the case all we would need to do is write thin wrappers that expose all this functionality to ROS in the form topics and/or services.

I will describe a case of a custom-built arm, that has none of the above. The arm in question has 7 Degrees of Freedom, and uses Robotis Dynamixel servo motors for all its joints (models EX-106, RX-64, and RX-28). I was able to use the arm_navigation stack to plan collision-free trajectories to a point in Cartesian space. After going through all the steps in this tutorial you should be able to do the same with your arm.

Inverse/Forward Kinematics solver

The first step is implementing an Inverse and Forward kinematics for your arm. There are at least two packages that can help you get going:

• arm_kinematics - a generic kinematics solver that might work for any arm, uses an iterative approach to solve the IK problem.

  • NOTE: This package was deprecated in ROS Electric, so you need to install various packages manually to get it working. This tutorial shows how to call a similar kinematics node from C++ code: http://ros.org/wiki/pr2_kinematics/Tutorials/Tutorial%204/

• openrave and its ikfast module - generates a C++ file with an analytic solution for any 6+ DOF arm.

The first option is probably the easiest to setup, but openrave's IK has a much faster execution speed. Whatever you choose, your arm_kinematics node should provide the following four services:

• get_fk (kinematics_msgs/GetPositionFK) - This is the basic service call to get position forward kinematics information from a position kinematics solver.

• get_ik (kinematics_msgs/GetPositionIK) - This is the basic service call to get inverse kinematics solutions from a position kinematics solver.

• get_fk_solver_info (kinematics_msgs/GetKinematicSolverInfo) - This service will let you get information from the forward kinematics solver as to the set of joints it deals with and the set of links that it can solve kinematics for.

• get_ik_solver_info (kinematics_msgs/GetKinematicSolverInfo) - Same as above but for the inverse kinematics solver.

Packages:

• PR2 Arm: pr2_arm_kinematics

• Dynamixel Arm: wubble_arm_kinematics with source available here: http://code.google.com/p/ua-ros-pkg/source/browse/#svn/trunk/arrg/ua_controllers/wubble_arm_kinematics

I started with pr2_arm_kinematics package as the base. The main thing that we have to replace here is computeIKShoulderPan function in pr2_arm_ik.cpp file that performs IK for the PR2 arm:

void PR2ArmIK::computeIKShoulderPan(const Eigen::Matrix4f &g_in, const double &t1_in)

Since I have a separate library that performs IK for my arm, I didn't need to use Eigen matrices, so I changed the signature and modified all calls to this function in pr2_arm_ik_solver.cpp.

The rest of the supporting code should work pretty much unchanged.

Constraint Aware Inverse Kinematics solver

Basically a wrapper around arm_kinematics. Hooks up to planning environment and checks the initial and desired poses for collisions, also publishes some visualization markers. Adds get_constraint_aware_ik service to all those provided by arm_kinematics:

• get_constraint_aware_ik (kinematics_msgs/GetConstraintAwarePositionIK) - This is the service call to get collision free inverse kinematics solutions from a kinematics solver.

Packages:

• PR2 Arm: pr2_arm_kinematics_constraint_aware

• Dynamixel Arm: wubble_arm_kinematics_constraint_aware with source code available here: http://code.google.com/p/ua-ros-pkg/source/browse/#svn/trunk/arrg/ua_controllers/wubble_arm_kinematics_constraint_aware

This step is even simpler than the previous one. Again, start with pr2_arm_kinematics_constraint_aware as the base and replace the PR2 specific includes in pr2_arm_kinematics_constraint_aware.h, pr2_arm_kinematics_constraint_aware.cpp and main.cpp files. That's it!

Joint Trajectory Execution

joint_trajectory_action_controller - given a joint trajectory executes it on the arm. If your arm already does that, I think all you'd need to do is write a wrapper that creates a ROS action interface to it.

Packages:

• PR2 Arm: joint_trajectory_action

• Dynamixel Arm: ax12_controller_core with source code available here: http://code.google.com/p/ua-ros-pkg/source/browse/#svn/trunk/arrg/ua_controllers/ax12_controller_core/nodes

Configuration

After that you would need to write a bunch of yaml configuration files:

• parameters for motion planning
• parameters for collision checks and self filtering
• for collision map - collision sources, basically something that provides a point cloud of stuff around you, like a tilting laser.
• for collision map - self filter, stuff that should not be present in the cloud above.
• some padding parameters for planning_environment
• parameters for ompl_planning and ompl_search

Please see their respective stack wiki pages for tutorials and parameters description.

PR2 Arm: pr2_arm_navigation stack has all those scattered across a bunch of packages.

Dynamixel Arm: the following launch file starts everything that is needed for motion_planning stack (it is rather long but it has everything in one place): http://code.google.com/p/ua-ros-pkg/source/browse/trunk/arrg/ua_robots/wubble_description/launch/wubble2_empty_world.launch

All the parameters that it loads can be found here (w2_*.yaml): http://code.google.com/p/ua-ros-pkg/source/browse/#svn/trunk/arrg/ua_robots/wubble_description/params

Using The Wizard

To get started, you can generate a basic arm_navigation stack using the Planning Description Configuration Wizard. Tutorials on this wizard are available in the wiki for arm_navigation.