Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |

# Creating Custom IK Solver with constrained_ik

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

**Tutorial Level:**INTERMEDIATE

Contents

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

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"

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

- Include headers for your constraints.
- Define your class with a constructor and destructor.
- Include your constraints as class members.
- 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

- Open constrained_ik_plugin.cpp
- Add the include to your solver alongside the others.
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) { }