In the previous tutorials we commanded the position of the robot joints. However, the ultimate goal for a robotic arm is to control the tool center point (tcp) where we might have a gripper attached or any other tool capable of manipulating our environment. We want to command the tpc to a specific position in space, more specific, in cartesian space. But how do we get from controlling single robot joints to directly commanding the tcp position? That is where the concepts of forward and inverse kinematics come in.
To get a real grasp of it, we will implement a C++ program that allows us to move the tcp in cartesian space. We will create a ROS node that uses KDL for our kinematic calculations and that interfaces with our simulated robot in Gazebo. We will first run the program and then go through the code step by step. At the end of the post, we will discuss inverse kinematics in detail and have a look at how it’s implemented in the KDL library. Let’s get right into it!
Clone, build and execute the TCP Position Controller
From our previous tutorial we already have tuned position controllers from the ros_control package for our UR5 joints and our robot simulation in Gazebo. If you are interested and have not done so you can go through that tutorial first. Otherwise, all the files you need – including (roughly) tuned joint controller parameters – are in the repository so you don’t have to worry about it.
First, make sure you have Gazebo and ros_control, both of which we already worked with in the previous tutorials, installed.
$ curl -sSL http://get.gazebosim.org | sh
$ sudo apt-get install ros-$ROS_DISTRO-ros-control ros-$ROS_DISTRO-ros-controllersIf you didn’t set up a catkin workspace yet, please do so following the instruction in this link.
Second, we need the universal robots package. Again, if you did one of the previous tutorials, this should already be there. Otherwise we need to install it from source. Clone the repository into the source folder of your catkin workspace and check the dependencies:
~/catkin_ws/src$ git clone -b <distro>-devel https://github.com/ros-industrial/universal_robot.git
$ rosdep update
$ rosdep install --rosdistro $ROS_DISTRO --ignore-src --from-paths srcNow go ahead and also clone the following two repositories into the src folder of your catkin workspace (You might have the first one cloned already if you did the last tutorial on joint controllers):
~/catkin_ws/src$ git clone https://github.com/dairal/ur5-joint-position-control.git
~/catkin_ws/src$ git clone https://github.com/dairal/ur5-tcp-position-control.gitIf you are not comfortable working with git, you can also download the zip-files under the following links: joint position control and tcp position control. Afterwards, just unpack them into the src folder of your catkin workspace.
Then build your workspace:
~/catkin_ws/src$ cd ..
~/catkin_ws$ catkin_make
~/catkin_ws$ source devel/setup.bashThis will build our tcp position controller node. The launch file that we are gonna use for bringing up the robot makes sure we start Gazebo in paused mode and spawn the robot with a defined initial pose. In order to do that and to launch the joint position controllers, run:
$ roslaunch ur5-joint-position-control ur5_gazebo_joint_position_control.launchAfter everything is loaded, you will see the UR5 robot lying on the ground and the simulation being paused. When you unpause the simulation, you will see the robot moving to the initial pose defined in the ur5_gazebo_pose.launch file. Now go ahead and start the tcp position controller node that we built before by running:
$ rosrun ur5-tcp-position-control tcp_position_controllerGreat, now the program will print the current tcp position of your robot and ask you to define how far you want to move in direction of each of the coordinate axes and in which time frame. Start with small numbers like 0.2 for the movement because the unit of the command is meters and the workspace of our robot is limited. In addition, our kinematic solvers might get into trouble for certain goal positions. The time property expects you to define the time frame of the movement in seconds.
As you can see, the controllers are still performing far from optimal but it works! If you play around with the program you might find that some goal positions work better than others and some don’t work at all. Sometimes the robot even goes crazy and collapses into itself (in which case you should close everything and start again by running the launch file and starting the node). This can be due to the kinematic computations that are very fragile or because the desired goal position is outside the robot workspace. Let’s have a look now on how all this is done in code.
The Code: Functionality and Interface
Note: The method described here is only one – very naive – way to command the tcp position of a robot arm. This simplistic implementation aims to gain an understanding of the underlying concepts of forward and inverse kinematics.
In the following, I will highlight the important parts of the code. Showing the complete file would be too much for one post and it contains some of unexciting boiler plate code but you can you can find the source file ur5_cartesian_position_controller.cpp in the cloned ur5-tcp-position-control.git repository or you can view its content following this link.
Publishing and Subscribing
At the beginning we want to get the current joint positions of our robot to compute the tcp position we start from. As we saw in the last tutorial, our joint position controllers publish this value under the topic <name_of_the_joint>_position_controller/state/process_value. So what we are gonna do is to subscribe to the process_value topic of every single position controller. The following code snippet shows what that looks like for the shoulder pan joint:
ros::Subscriber shoulder_pan_joint_sub = n.subscribe("/shoulder_pan_joint_position_controller/state", 1000, get_shoulder_pan_joint_position);
In the callback function which is called whenever our subscriber receives new data we store all the position values in the jnt_pos_start array.
const int Joints = 6;
KDL::JntArray jnt_pos_start(Joints);
void get_shoulder_pan_joint_position(const control_msgs::JointControllerState::ConstPtr& ctr_msg) {
jnt_pos_start(0) = ctr_msg->process_value;
}
Later, when we are done with the computations we want to publish the position command to all joints on the topic name <joint_name>_position_controller/command, so we create six publishers and store them in the joint_com_pub array such that we can easily iterate over our joint commands later. The following code shows the array initialization and the assignment of the shoulder pan joint command topic.
ros::Publisher joint_com_pub[6];
joint_com_pub[0] = n.advertise<std_msgs::Float64>("/shoulder_pan_joint_position_controller/command", 1000);
Kinematic Solvers from the KDL library
The KDL library provides a framework for modeling and computing kinematic chains which is exactly what we need to find out our current tcp position, given our joint angles. In order to be able to do these kinematic calculations, KDL needs to extract the kinematic information of the robot. In Line 109 we ask KDL to create a tree representation from the URDF. The URDF is an xml document that describes the structure of our robot and the one we use in this tutorial is described in detail in the two previous tutorials about visualizing and simulating the UR5 robot. From the tree representation, KDL extracts the kinematic chain in Line 116. For that we have to tell the function where it should start and where it should end. In our case we want the chain from our robot base up to the last wrist_3_link.
//Parse urdf model and generate KDL tree
KDL::Tree ur5_tree;
if (!kdl_parser::treeFromFile(urdf_path, ur5_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
//Generate a kinematic chain from the robot base to its tcp
KDL::Chain ur5_chain;
ur5_tree.getChain("base_link", "wrist_3_link", ur5_chain);
Now we are ready to create the solvers. We have one solver for the forward kinematics that computes our tcp position based on the joint positions and one for inverse kinematics that computes our joint positions based on the tcp position. The inverse kinematics position solver also requires an IK solver for the velocity as an argument.
KDL::ChainIkSolverVel_pinv vel_ik_solver(ur5_chain, 0.0001, 1000);
KDL::ChainIkSolverPos_NR ik_solver(ur5_chain, fk_solver, vel_ik_solver, 1000);
We will dive deeper into how the forward and the inverse kinematics is computed by the end of this article and I will also explain the magic numbers that we hand over as arguments for our solvers. With these things set, we can start with the actual work.
Using the forward kinematics solver we compute the tcp-position. In line 134 the solver takes the joint position jnt_pos_start and an empty KDL::Frame tcp_pos_start to store the resulting tpc pose. A KDL::Frame is much like a coordinate frame and is defined by its orientation and the position of its origin.
KDL::Frame tcp_pos_start;
fk_solver.JntToCart(jnt_pos_start, tcp_pos_start);
Now we know the tcp position we start from, which is the origin of the computed KDL:Frame. We print this information to the user and ask how he/she wants the tcp to move. This happens inside the get_goal_tcp_and_time() function in line 142. As an additional input, we get the duration of the movement in seconds. From the x, y, z input we can then compute the origin of the frame of our goal position vec_tcp_pos_goal by simply adding the values defined by the user to the coordinates. With this new tcp position and with keeping the orientation tcp_pos_start.M as it is, we can construct our goal frame tcp_pos_goal in line 144.
get_goal_tcp_and_time(tcp_pos_start, &vec_tcp_pos_goal, &t_max);
KDL::Frame tcp_pos_goal(tcp_pos_start.M, vec_tcp_pos_goal);
Now we can invoke our inverse kinematics solver to compute the joint positions for our new tcp pose in line 148.
//Compute inverse kinematics
KDL::JntArray jnt_pos_goal(Joints);
ik_solver.CartToJnt(jnt_pos_start, tcp_pos_goal, jnt_pos_goal);
As you can see, the solver also takes the starting joint positions jnt_pos_start as an input. Later we will discuss the reason for that. The result is stored in the jnt_pos_goal array and that is all for the kinematic calculations!
Commanding the joints
Now all we have to do is command our joints to their new position in a mannered way. We use the simplest way possible which is a function that interpolates linearly between our starting and our goal positions within the time frame specified by the user.
float compute_linear(double q_start, double q_goal, float t, float t_max) {
return((q_goal - q_start) * (t/t_max) + q_start);
}
Every cycle of our while loop, we compute the next position command for our joint controllers. The time step in between the computations is defined by the rate of the ROS node. In our case, the node runs at 100 Hz so we compute our new position command (line 155) every 10 milliseconds. We publish the new values to every joint position controller in line 156, sleep for 10 milliseconds and compute the next time step until we have reached the end of the time frame t_max.
while(t<t_max) {
std_msgs::Float64 position[6];
for(int i=0; i<Joints; i++) {
position[i].data = compute_linear(jnt_pos_start(i), jnt_pos_goal(i), t, t_max);
joint_com_pub[i].publish(position[i]);
}
ros::spinOnce();
loop_rate.sleep();
++count;
t += t_step;
}
And that is everything we need in order to control the tcp position of our robot. As you might have noticed there are a few shortcomings of this method. While we command the joint we don’t check the joint position feedback and at the end we just stop the robot without making sure that the joints reached their goal position. In essence we trust the position controllers to be able to closely follow our commands. This seems to be legit for small movements that are not too fast. However, for bigger and quicker motions our joint level controllers might fall behind or overshoot, especially if the controller parameters are not very well tuned.
On another note, computing the intermediate position commands in a linear way is not the best solution for commanding real robot joints. Usually one would compute polynomials to be able to e.g. define the velocity at the beginning and the end of the motion and avoid large accelerations. Still, we have a very simple program that allows us to control the robot tcp position. And it allows us to concentrate on the concepts of forward and inverse kinematics. Now we will have a quick deep-dive into these topics which are fundamental in order to understand how a robot operates. We will also have a look at how the KDL library implements the necessary computations. No worries, it is not as complex as it might seem at first sight and we won’t make it overly complicated.
Forward/Inverse Kinematics in KDL
So how can we compute joint positions from a given tcp position in cartesian space? In general, there are two approaches to do that: The analytical and the numeric way. We will only discuss the numeric way which is the approach used in KDL and thus in this tutorial.
In the numeric method, we try to move the joint configuration (angles) step by step to approach the desired goal position for our tcp. Therefore, we try to reduce an error function. In our case, we want to minimize the error between our current tcp pose and the desired tcp pose. The tcp pose (lets call it \(X\)) consists of the position \(x_{x/y/z}\) and the orientation \(x_{Rx/Ry/Rz}\) which sums up to six degrees of freedom. When we have a robot with \(n\) joints, every degree of freedom is dependent on the joint angles \(q_{1/…/n}\) i.e. is a function of the joint angles (\(X = F(Q)\)).
If we assume that \(X\) (a \(6×1\) matrix) is the desired tcp pose, \(Q\) (a \(nx1\) matrix) is our iterative guess for the joint positions and \(\Delta X\) is the distance between the tcp pose that results from the guess and our desired pose, we can write the error function as: \[\Delta X = X – F(Q)\] where (F(Q)) is the tcp position for our current guess of joint angles. We want to minimize the error so if we set \(\Delta X\) to zero, i.e. \(0 = X – F(Q)\) we end up with the following equation: \[X = F(Q)\] The function \(F()\) represents our forward kinematics which can be calculated very easily as we briefly discussed in the Rviz tutorial. Unfortunately, we want to know \(Q\) based on \(X\) (the inverse) which is not quite as easy so we need to do some tricks.
When we differentiate both sides, we get an expression that is very fundamental in robotics:
This formula expresses the relation between the tcp velocity \(v\) in cartesian space and the joint velocities \(\dot{q}\). The Jacobian matrix \(J\) is dependent on the robot kinematics and on the joint angles \(q\) in the current configuration and maps the joint velocities to the tcp velocity. We are interested in the joint angles \(q\) that lead to a certain tcp pose so we divide by the Jacobian and bring it to the other side of the equation: \[v * J(q)^{-1} = \dot{q} \]
The inverse \(J(q)^{-1}\) of a Jacobian matrix can only be directly computed if it is a square matrix i.e. if the number of degrees of freedom matches the number of joints \(n\). Unfortunately, the UR5 has 7 joints so we need to approximate the inverse with a so called pseudo-inverse Jacobian (\(J(q)^+\)): \[v * J(q)^+ = \dot{q}\]
And that already is the main equation we need for the inverse computation, so let’s have a look at how it’s done in KDL. As we already saw before, when constructing the IK position solver ik_solver, KDL takes the computed chain ur5_chain, a forward kinematics solver fk_solver, an IK velocity solver vel_ik_solver and an integer value for the maximum amount of iterations as arguments.
The IK velocity solver vel_ik_solver that is constructed before takes three arguments: The kinematic chain, a threshold value for the error \(\Delta X\) and the maximum number of iterations.
KDL::ChainIkSolverVel_pinv vel_ik_solver(ur5_chain, 0.0001, 1000);
KDL::ChainIkSolverPos_NR ik_solver(ur5_chain, fk_solver, vel_ik_solver, 1000);
Now lets look at the ik_solver function inside the KDL library that converts the cartesian tcp position to the joint angles. You can also view the source file here.
int ChainIkSolverPos_NR::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
{
if (nj != chain.getNrOfJoints())
return (error = E_NOT_UP_TO_DATE);
if(q_init.rows() != nj || q_out.rows() != nj)
return (error = E_SIZE_MISMATCH);
q_out = q_init;
unsigned int i;
for(i=0;i<maxiter;i++){
if (E_NOERROR > fksolver.JntToCart(q_out,f) )
return (error = E_FKSOLVERPOS_FAILED);
delta_twist = diff(f,p_in);
const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q);
if (E_NOERROR > rc)
return (error = E_IKSOLVER_FAILED);
// we chose to continue if the child solver returned a positive
// "error", which may simply indicate a degraded solution
Add(q_out,delta_q,q_out);
if(Equal(delta_twist,Twist::Zero(),eps))
// converged, but possibly with a degraded solution
return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR);
}
return (error = E_MAX_ITERATIONS_EXCEEDED); // failed to converge
}
After initializing the array of joint positions q_out with the initial joint positions q_init, the starting tcp position is computed with the forward kinematics solver in line 54, which gives us our initial \(F(Q)\). In line 56, the distance beween the initial tcp pose f and the goal tcp pose p_in is differentiated over a time frame of 1 second so we get our tcp velocity \(v\) (delta_twist) which at the same time is our \(\Delta X\). Then we call the CartToJnt (line 57) function of our IK velocity solver. This function takes the current joint position estimate q_out (the starting joint positions q_init in case of the first iteration) and the velocity \(v\) as an argument and computes the joint velocity \(\dot{q}\) as shown before with the equation: \[ \dot{q} = J(q)^+ * v\] The function stores its result for \(\dot{q}\) in qdot_out.
int ChainIkSolverVel_pinv::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
{
if (nj != chain.getNrOfJoints())
return (error = E_NOT_UP_TO_DATE);
if (nj != q_in.rows() || nj != qdot_out.rows())
return (error = E_SIZE_MISMATCH);
//Let the ChainJntToJacSolver calculate the jacobian "jac" for
//the current joint positions "q_in"
error = jnt2jac.JntToJac(q_in,jac);
if (error < E_NOERROR) return error;
double sum;
unsigned int i,j;
// Initialize near zero singular value counter
nrZeroSigmas = 0 ;
//Do a singular value decomposition of "jac" with maximum
//iterations "maxiter", put the results in "U", "S" and "V"
//jac = U*S*Vt
svdResult = svd.calculate(jac,U,S,V,maxiter);
if (0 != svdResult)
{
qdot_out.data.setZero();
return (error = E_SVD_FAILED);
}
// We have to calculate qdot_out = jac_pinv*v_in
// Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
// qdot_out = V*S_pinv*Ut*v_in
//first we calculate Ut*v_in
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.rows();j++) {
sum+= U[j](i)*v_in(j);
}
//If the singular value is too small (<eps), don't invert it but
//set the inverted singular value to zero (truncated svd)
if ( fabs(S(i))<eps ) {
tmp(i) = 0.0 ;
// Count number of singular values near zero
++nrZeroSigmas ;
}
else {
tmp(i) = sum/S(i) ;
}
}
//tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
//it with V to get qdot_out
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.columns();j++) {
sum+=V[i](j)*tmp(j);
}
//Put the result in qdot_out
qdot_out(i)=sum;
}
// Note if the solution is singular, i.e. if number of near zero
// singular values is greater than the full rank of jac
if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
return (error = E_CONVERGE_PINV_SINGULAR); // converged but pinv singular
} else {
return (error = E_NOERROR); // have converged
}
}
After the Jacobian \(J(q)\) dependent on the joint positions is computed in line 72, the pseudo-inverse \(J(q)^+\) is approximated with a method called singular value decomposition in line 84 which is called with an argument for the maximum amount of iterations. The decomposition looks as follows: \[J(q)^+ = V * S_{pinv} * U_t \] When the computation is done, the resulting \(\dot{q}\) is stored in the qdot_out array in line 120. After returning to the IK position function, the joint velocity is integrated because in the end we want to know the joint position. The integration is done in the CartToJnt function of the position ik_solver in line 62 by simply adding \(\dot{q}\) to the current estimate of the joint positions q_out. Afterwards, the next iteration starts with this new value. This continues until the tcp velocity \(v\) (i.e. \(\Delta X\)) resulting from the differentiation of the distance between current tcp pose for the estimated joint configuration and desired tcp pose lies under a certain threshold. Then, the current pose with the estimated joint positions is close enough to the desired pose for our purposes.
That was it! This is how inverse kinematics computation is done in KDL. I hope you found it helpful and could follow along with the description. Of course we didn’t look into every detail, but if you want to know more, e.g. about singular value decomposition, a google search will give you a ton of results. There are other ways to do inverse kinematics like the very popular ikfast algorithm but that is for another time. Thanks for sticking with me through the complete article. If you want to learn an easier way how to command the tcp position of a robot with the motion planning framework Moveit check out the next tutorial. Here you can find an overview of all tutorials.
