tuto prep part2

Tutorial 1

Pre-prequest

We suppose you have done Tutorial 1 part 1

Part 2 : Writing an IK plugin for MoveIt

Why not use KDL generic IK solvers ?

design reason : Starting with hydro, the kinematics plugin based on KDL and provided in MoveIt! can solve coupled joints if described with the mimic tag. However, the Shadow hand cannot use mimic joints because both joint angles are measured and the slave joint should not be overriden by the joint_state_publisher due to mimic of the master joint.
historical reason : Even in the case of non-coupled joints, 3D IK was not accessible when the first arm_navigation config was generated for the Shadow Hand. An option to set KDL to solve position-only IK (3D IK) is available now.

Concept

The idea is to write a specific IK solver that handles 3D IK and coupled-joints without mimic tag.
The solver will be wrapped in a Kinematics Plugin.
The original tutorial to create and integrate custom kinematics as plugins in MoveIt! can be found ~~here~~ (deadlink) similar tuto here

Preparation

The currently used solver (sr_hand_kinematics) is complex as it handles both services and a plugin. A simpler version just acting as a plugin is presented in this repository. It works for fingers not for the thumb.

Simplified IK with coupling
- The code for this tutorial is in hand_ik_coupling_plugin
- Utilities functions are in hand_ik_utils.cpp to provide
  - KDL chain access
  - Coupling functions to maintain or update the coupling
- KDL library extension is available in kdl_coupling_extension
Base Kinematic Template
- MoveIt! provides a kinematic plugin base class with virtual and pure virtual methods.
- Our plugin must implement at least the pure virtual methods
  - initialize()
  - getJointNames()
  - getLinkNames()
  - getPositionFK()
  - getPositionIK()
  - searchPositionIK()

Development / Fill in the Kinematics Base template

The code in hand_ik_coupling_plugin.cpp is detailed below

Plugin declaration.

The plugin is declared along with the base template it derives from

PLUGINLIB_EXPORT_CLASS( hand_kinematics::HandIKCouplingPlugin, kinematics::KinematicsBase)

Initialize()

Initialize the plugin by using the robot description name to be loaded, the joint group to consider and the frames. Search discretization is not used in our plugin

  bool HandIKCouplingPlugin::initialize(const std::string& robot_description,
                                          const std::string& group_name,
                                          const std::string& base_frame,
                                          const std::string& tip_frame,
                                          double search_discretization)
  {
    // Store the given values in the plugin
    setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
    urdf::Model robot_model;
    std::string xml_string;
    
    // Handle to access the parameters for this group
    ros::NodeHandle private_handle("~/"+group_name);
    
    // Load the robot model
    while(!loadRobotModel(private_handle,robot_model,base_frame_,tip_frame_,xml_string) && private_handle.ok())
    {
      ROS_ERROR("Could not load robot model. Are you sure the robot model is on the parameter server?");
      ros::Duration(0.5).sleep();
    }

    // Loading KDL Tree
    if(!getKDLChain(xml_string,base_frame_,tip_frame_,kdl_chain_))
    {
      ROS_ERROR("Could not load kdl tree");
      return false;
    }

The coupling parameters are also setup in the initialize method. One must set the callback functions. These functions are available in hand_ik_utils.h. The coupling callback functions are static but could handle complex coupling over the range of motion.

    // LF has 5 joints and specific update coupling function
    if(tip_frame_.find("lftip")!=string::npos)
    {
      // Assign update function for dynamic coupling
      kdl_chain_.setUpdateCouplingFunction(updateCouplingLF); 
      dimension_=5;
    }
    else
    {
      if(tip_frame_.find("fftip")!=string::npos||
      tip_frame_.find("mftip")!=string::npos ||
      tip_frame_.find("rftip")!=string::npos
      )
      {
        // Assign update function for dynamic coupling
        kdl_chain_.setUpdateCouplingFunction(updateCoupling); 
        dimension_=4;
      }
      else
      {
        ROS_ERROR("Cannot solve for %s",tip_frame_.c_str());
        return false;
      }
    }

The fingers have only 3DOF and hence the IK can solve the problem for 3 task-space variables. In the Weighted Damped Least Square (WLSQ) method, a task-space weighting matrix is used to set the orientation to zero to not consider them.

    Eigen::MatrixXd Mx(6,6); 
    for(unsigned int i=0; i < 6; i++)
    {
      for(unsigned int j=0; j < 6; j++)
      {
        Mx(i,j)= 0.0;
      }
    }
    
    Mx(0,0)= 1.0; // coordinate X
    Mx(1,1)= 1.0; // coordinate Y
    Mx(2,2)= 1.0; // coordinate Z
    Mx(3,3)= 0.0; // rotation X
    Mx(4,4)= 0.0; // rotation Y
    Mx(5,5)= 0.0; // rotation Z

Other parameters are set to init the solvers

    // Set solver parameters
    int maxIterations=1000; // solver max loops
    double epsilon=0.001; // loop finish condition
    double lambda=0.01; // used in WDLS algorithm;

    // Init the solver infos
    init_ik(robot_model,base_frame_,tip_frame_, joint_min_,joint_max_,ik_solver_info_);

    fk_solver = new KDL::ChainFkSolverPos_recursive(kdl_chain_); 

    ik_solver_vel= new KDL::ChainIkSolverVel_wdls_coupling(kdl_chain_,epsilon,maxIterations);
    ik_solver_vel->setLambda(lambda);
    ik_solver_vel->setWeightTS(Mx);
    // inverse kinematic for position, passing the ik vel and other parameters
    ik_solver_pos= new KDL::ChainIkSolverPos_NR_JL_coupling(kdl_chain_,
       joint_min_,joint_max_,*fk_solver, *ik_solver_vel, maxIterations, epsilon);
        
    return true;
  }

getPositionIK()

The main function called by the planner for Inverse kinematics

  bool HandIKCouplingPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
                                             const std::vector<double> &ik_seed_state,
                                             std::vector<double> &solution,
                                             moveit_msgs::MoveItErrorCodes &error_code,
                                             const kinematics::KinematicsQueryOptions &options) const
  {
    // Get the pose in KDL format
    KDL::Frame pose_desired;
    tf::poseMsgToKDL(ik_pose, pose_desired);

    KDL::JntArray jnt_pos_in;
    KDL::JntArray jnt_pos_out;
    jnt_pos_in.resize(dimension_);

One must take special care on the seed state which should match the coupling. A function is available in hand_ik_utils.h to enforce the coupling.

    // Get the seed state in KDL format
    for(int i=0; i < dimension_; i++)
    {
      jnt_pos_in(i) = ik_seed_state[i];
    }
    // Enforce the coupling in case the seed state given value do not
    hand_kinematics::maintainCoupling(jnt_pos_in, tip_frame_);

The iterative solver sometimes need a new random seed state to get rid of local minima. A loop is added around the IK request. a each random seed the coupling must be maintained.

    int ik_valid=-1;
    // restart 10 times with different rand if no success
    for(int i=0; i < 10 && ik_valid < 0; i++)
    {
      // DO the IK
      ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
      if(!ik_valid)
      {
        // generate a new random seed
        generateRandomJntSeed(jnt_pos_in);
        // Enforce the coupling
        hand_kinematics::maintainCoupling(jnt_pos_in, tip_frame_);
      }
    }

Finally the result is extracted.

    // Extract the result
    if(ik_valid >= 0)
    {
      solution.resize(dimension_);
      for(int i=0; i < dimension_; i++)
      {
        solution[i] = jnt_pos_out(i);
      }
      error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
      return true;
    }
    else
    {
      error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
      return false;
    }
  }

searchPositionIK()

Four overloaded methods permit to retrieve the IK with more parameters and settings. In the tutorial, all of them will basically call the standard getPositionIK(), ignoring the extra parameters.

  bool HandIKCouplingPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
                                              const std::vector<double> &ik_seed_state,
                                              double timeout,
                                              const std::vector<double> &consistency_limits,
                                              std::vector<double> &solution,
                                              const IKCallbackFn &solution_callback,
                                              moveit_msgs::MoveItErrorCodes &error_code,
                                              const kinematics::KinematicsQueryOptions &options) const
  {
    return getPositionIK(ik_pose,ik_seed_state,solution,error_code,options);
  }

getPositionFK()

Implements the forward kinematics. Coupling is not a problem in this computation, the standard code is used.

  bool HandIKCouplingPlugin::getPositionFK(const std::vector<std::string> &link_names,
                                           const std::vector<double> &joint_angles,
                                           std::vector<geometry_msgs::Pose> &poses) const
  {
    KDL::Frame p_out;
    KDL::JntArray jnt_pos_in;
    geometry_msgs::PoseStamped pose;
    tf::Stamped<tf::Pose> tf_pose;

    jnt_pos_in.resize(dimension_);
    for(int i=0; i < dimension_; i++)
    {
      jnt_pos_in(i) = joint_angles[i];
    }

    poses.resize(link_names.size());

    bool valid = true;

    for(unsigned int i=0; i < poses.size(); i++)
    {
      if(fk_solver->JntToCart(jnt_pos_in,p_out,hand_kinematics::getKDLSegmentIndex(kdl_chain_,link_names[i])) >=0)
      {
        tf::poseKDLToMsg(p_out,poses[i]);
      }
      else
      {
        ROS_ERROR("Could not compute FK for %s",link_names[i].c_str());
        valid = false;
      }
    }
    return valid;
  }

getJointNames() & getLinkNames()

Both methods return the info stored in the ik_solver_info.

  const std::vector<std::string> &HandIKCouplingPlugin::getJointNames() const
  {
    return ik_solver_info_.joint_names;
  }

  const std::vector<std::string> &HandIKCouplingPlugin::getLinkNames() const
  {
    return fk_solver_info_.link_names;
  }

generateRandomJntSeed()

This was added originally and stayed there although new way to randomize the joint state exist now.

  void HandIKCouplingPlugin::generateRandomJntSeed(KDL::JntArray &jnt_pos_in) const
  {
    for(int i=0; i < dimension_; i++)
    {
      double min= ik_solver_info_.limits[i].min_position;
      double max= ik_solver_info_.limits[i].max_position;
      double r= min + ((double)rand()) / RAND_MAX *(max-min);
      jnt_pos_in(i)= r;
    }
  }

Testing

A good practice it to test the implemented code using gtest. The original test was taken from the pr2_moveit package and modified to handle coupling in the random generator.

The code in test_kinematics_as_plugin.cpp is detailed below

Testing class

First a testing class is declared to load and initialize the plugin. A plugin loader is created to handle a kinematics plugin

    // plugin loader refering to the base class.
    kinematics_loader_.reset(new pluginlib::ClassLoader<kinematics::KinematicsBase>("moveit_core", "kinematics::KinematicsBase"));

Then our plugin is instantiated

    std::string plugin_name="hand_ik_coupling_plugin/HandIKCouplingPlugin";
    kinematics_solver_ = kinematics_loader_->createUniqueInstance(plugin_name);

and initialized. A first test is done on the result on the initialization

if(kinematics_solver_->initialize("robot_description",finger_group_name,root_name,tip_name,search_discretization))
      return true;
    else
    {
      EXPECT_TRUE(0);
      return false;
    }
  };

Tests

The tests are embedded in Test functions called by the main. We implemented two tests

one for initialization and settings checks
one for forward and inverse kinematics at the same time

TEST(HandIKPlugin, initialize)
{
  // Test initialization
  ASSERT_TRUE(my_test.initialize());
  // Test getting chain information
  std::string root_name = my_test.kinematics_solver_->getBaseFrame();
  EXPECT_TRUE(root_name == std::string("palm"));
  std::string tool_name = my_test.kinematics_solver_->getTipFrame();
  EXPECT_TRUE(tool_name.find("tip")!=std::string::npos);
  std::vector<std::string> joint_names = my_test.kinematics_solver_->getJointNames();

All the joints are tested for chain validity testing

  if(tool_name.find("fftip")!=std::string::npos)
  {
		EXPECT_EQ((int)joint_names.size(),4);
		EXPECT_TRUE(joint_names[0].find("FFJ4")!=std::string::npos);
		EXPECT_TRUE(joint_names[1].find("FFJ3")!=std::string::npos);
		EXPECT_TRUE(joint_names[2].find("FFJ2")!=std::string::npos);
		EXPECT_TRUE(joint_names[3].find("FFJ1")!=std::string::npos);
 }

FK and IK tests are done repeatedly in a loop

  for(unsigned int i=0; i < (unsigned int) number_ik_tests; ++i)
  {
    seed.resize(my_test.kinematics_solver_->getJointNames().size(), 0.0);
    fk_values.resize(my_test.kinematics_solver_->getJointNames().size(), 0.0);
    kinematic_state.setToRandomPositions(joint_model_group);
    kinematic_state.copyJointGroupPositions(joint_model_group, fk_values);

We must enforce the coupling to get good initialization of our IK solver

    // Make the coupling 1:1 in random values
    if(joint_names[0].find("TH")==std::string::npos && joint_names[0].find("LF")==std::string::npos)
    {
      fk_values[3]=fk_values[2];
    }
    else if(joint_names[0].find("LF")!=std::string::npos)
    {
      fk_values[4]=fk_values[3];
    }

And compute a pose to test using the FK.

    bool result_fk = my_test.kinematics_solver_->getPositionFK(fk_names, fk_values, poses);
    // By the same way we check that the FK worked
    ASSERT_TRUE(result_fk);

Then the IK generates a joint space solution if found, including a test for success

    bool result = my_test.kinematics_solver_->searchPositionIK(poses[0], seed, timeout, solution, error_code);
    EXPECT_TRUE(result);

Finally the joint values are used to compute a new pose and checked against the original pose.

    result_fk = my_test.kinematics_solver_->getPositionFK(fk_names, solution, new_poses);
    // Check if the original pose matches the new pose
    EXPECT_NEAR(poses[0].position.x, new_poses[0].position.x, IK_NEAR);
    EXPECT_NEAR(poses[0].position.y, new_poses[0].position.y, IK_NEAR);
    EXPECT_NEAR(poses[0].position.z, new_poses[0].position.z, IK_NEAR);

Run the test

To run the test in your workspace

catkin build --catkin-make-args run_tests

you should see a result such as

[ROSUNIT] Outputting test results to /home/guihome/work/dev/catkin_ws/build/test_results/hand_ik_coupling_plugin/rostest-test_hand_kinematics_plugin_test.xml
testhand_kinematics_plugin_test_ff ... ok
testhand_kinematics_plugin_test_lf ... ok
testhand_kinematics_plugin_test_mf ... ok
testhand_kinematics_plugin_test_rf ... ok

[ROSTEST]-----------------------------------------------------------------------

[hand_ik_coupling_plugin.rosunit-hand_kinematics_plugin_test_ff/initialize][passed]
[hand_ik_coupling_plugin.rosunit-hand_kinematics_plugin_test_ff/searchIKandFK][passed]
[hand_ik_coupling_plugin.rosunit-hand_kinematics_plugin_test_lf/initialize][passed]
[hand_ik_coupling_plugin.rosunit-hand_kinematics_plugin_test_lf/searchIKandFK][passed]
[hand_ik_coupling_plugin.rosunit-hand_kinematics_plugin_test_mf/initialize][passed]
[hand_ik_coupling_plugin.rosunit-hand_kinematics_plugin_test_mf/searchIKandFK][passed]
[hand_ik_coupling_plugin.rosunit-hand_kinematics_plugin_test_rf/initialize][passed]
[hand_ik_coupling_plugin.rosunit-hand_kinematics_plugin_test_rf/searchIKandFK][passed]

SUMMARY
 * RESULT: SUCCESS
 * TESTS: 8
 * ERRORS: 0
 * FAILURES: 0

Note: Sometimes the lf fails to solve some positions.

Advertize the plugin

Plugin description file

The file hand_ik_coupling_plugin.xml describes the plugin

<library path="lib/libhand_ik_coupling_plugin">
  <class name="hand_ik_coupling_plugin/HandIKCouplingPlugin" type="hand_kinematics::HandIKCouplingPlugin" base_class_type="kinematics::KinematicsBase">
    <description>
      Computation of forward and inverse kinematics for the fingers of the Shadow Hand, including coupling.
    </description>
  </class>
</library>

Export the plugin

In package.xml the description file is exported

<export>
      <moveit_core plugin="${prefix}/hand_ik_coupling_plugin.xml" />
</export>

Check that the plugin is seen by ROS as a plugin
- source the new environment
```
source devel/setup.bash
```
- launch
```
rospack plugins --attrib=plugin moveit_core
```
You should see your new plugin in the list

Integration

To integrate the newly created plugin into MoveIt! two techniques are possible

Edit the files manually

Edit the kinematics.yaml file in sr_moveit_hand_config and change

first_finger:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin

by

first_finger:
  kinematics_solver: hand_ik_coupling_plugin/HandIKCouplingPlugin

Relaunch the wizard to update the kinematics settings
- Relaunch the wizard
```
roslaunch sr_moveit_hand_config setup_assistant.launch
```
- In planning groups, double click on first_finger and select the newly added plugin

Test with demo mode

Starting MoveIt! demo
- Launch the MoveIt! demo for the Shadow Hand
```
roslaunch sr_moveit_hand_config demo.launch
```
Moving the first finger with the new IK
- In the Planning Request panel, activate the Query Start State
- Using the mouse, try move the marker at the tip using the arrows first (we are in a singular position)
- The plugin we created now solves the IK.
Prepare a Goal state
- In the Planning Request panel, activate the Query Goal State
- Using the mouse, try move the marker at the tip using the arrows first (we are in a singular position)
- Re-activate both Start and Goal states
Planning a movement
- In the Motion Planning panel, planning tab click on Plan
- You get a solution in a few seconds.

Next see Tutorial 2 dealing with using the MoveIt! configuration of the hand for dexterous motion planning.

Provide feedback

Saved searches

Use saved searches to filter your results more quickly