Creating Custom IK Solver with constrained_ik

Description: Creating custom instance of IK solver using constrained IK package

Tutorial Level: INTERMEDIATE

Basic Implementation of IK Solver

The most basic IK implementation is called BasicIK, and is located in ../include/constrained_ik/ik/basic_ik.h. This solver uses a 6 DOF pose and joint-limit avoidance in its solution. Here the declaration of the solver is laid out to prepare a more in-depth discussion of more complicated solvers.

1. The includes for the solver are minimal. At least required is constrained_ik and the headers for the particular constraints you will use. Here, the constraint headers are avoid_joint_limits.h and goal_pose.h.

   #include "constrained_ik/constrained_ik.h"
   #include "constrained_ik/constraints/avoid_joint_limits.h"
   #include "constrained_ik/constraints/goal_pose.h" 

2. Define a class that inherits from Constrained_IK and has a public constructor and destructor.
   

Because the constraints are defined as pointers (later on in the tutorial) they must be initialized in the constructor, preferably, but not necessarily chained in the constructor definition.

• class Basic_IK : public Constrained_IK
  {
  public:
    Basic_IK(): goal_pose_(new constraints::GoalPose), avoid_joint_limits_(new constraints::AvoidJointLimits)

• We will define the constructor here because it is short and simple. There is no source file for this solver. The construction of the solver includes adding constraints via the addConstraint() function, then optionally changing the weights for each constraint. The order of the constraints does not matter. Note: goal_pose is internally made up of two constraints, hence the particular call to setWeightOrientation() rather than the standard call of setWeight()

  {
      addConstraint(goal_pose_);
      addConstraint(avoid_joint_limits_);
      Eigen::Vector3d w_ori;
      w_ori << 1,1,1;
      goal_pose_->setWeightOrientation(w_ori);
    }

• Nothing special with the desctuctor

  ~Basic_IK() {}; 

• Each constraint is defined as a class member, and that is the whole definition of the solver!

  protected:
  
    constraints::GoalPose* goal_pose_;
    constraints::AvoidJointLimits* avoid_joint_limits_;
  
  }; // class Basic_IK

The definition that comes from the repository is https://github.com/ros-industrial/industrial_moveit/blob/groovy-devel/constrained_ik/include/constrained_ik/ik/basic_ik.h

Implementing a New Solver

New solvers are not any more complicated to design and implement. The basic format is

1. Include headers for your constraints.
2. Define your class with a constructor and destructor.
3. Include your constraints as class members.
4. Complete the constructor by adding the constraints and setting their weights.

Including Your Solver in the MoveIt! Kinematics Plugin

The interface of your IK solver to MoveIt! takes place through a plugin. The particular plugin for this solver is ../src/constrained_ik_plugin.cpp.

Replace Your Solver in the Plugin

1. Open constrained_ik_plugin.cpp
2. Add the include to your solver alongside the others.

3. The easiest way to add your solver is to typedef it to Solver. Before the plugin constructor, comment out the current typedef and add your own.

For example:

#include <constrained_ik/constrained_ik_plugin.h>
#include <constrained_ik/ik/basic_ik.h>
#include <constrained_ik/ik/test_ik.h>
#include <ros/ros.h>

#include <constrained_ik/ik/new_solver.h> //Include my new solver

...


namespace constrained_ik
{

//typedef basic_ik::Basic_IK Solver;
//typedef test_ik::Test_IK Solver;     //I commented this one out.
typedef new_solver::NewSolver Solver;  //I replaced it with this one.

ConstrainedIKPlugin::ConstrainedIKPlugin():active_(false), dimension_(0)
{
}