This post briefly describes how to define coordinate systems for robots and then explains how to use Orocos KDL to perform forward kinematics.

#### A Simple Robot

To make things a little easier, we’ll be using the simple robot model shown below. It has four links (L1 — L4) and three degrees of freedom. Link 1 is fixed to the ground while Link 2 and Link 3 can rotate in the screen plane. Link 4 can rotate around like a periscope, about the axis shown in the schematic.

Our goal is to find the pose of the last link, the end effector, given the measurement of three joint angles (J1, J2, J3).

### Defining Coordinate Frames

Let’s start with defining the coordinate frames for each link. ** F0** is our reference coordinate frame, the frame relative to which we would like to find our end effector’s pose,

**.**

*F4*I am using the widely used Denavit-Hartenberg convention to define the intermediate links. Specifically, it says that the Z axis should be in the direction of the joint axis.

is attached to Link 1 with the Z axis aligned to Joint 1, coming out of the plane.*F1*is attached to Link 2 with the Z axis aligned to Joint 2, coming out of the plane.*F2*is attached to Link 3 with the Z axis aligned to Joint 3, along the axis of rotation for Link 4.*F3*

Here’s another image to illustrate how the coordinate frames are attached to the links and joints.

- Joint 1 is rotated -45°
- Joint 2 is rotated 90°
- Joint 3 is rotated 180°

Assuming that the origin is at F0, the end effector’s position, F4, is roughly at (-1.5, 3.8, 0). We’ll calculate this as well the the full pose using forward kinematics in the next section.

### Forward Kinematics

#### What is Orocos KDL?

Orocos (Open Robot Control Software) is a suite of libraries for robot arm control. Orocos KDL (Kinematics and Dynamics Library) provides the ability to create *kinematic chains* to perform forward and inverse kinematics.

#### Step 1: Creating Segments

In KDL, kinematic chains consist of *segments*. Segments are essentially the links of the robot. A segment is a combination of a *Joint* and a *Frame*. The joint tells the segment how a frame moves as the joint angle changes. For example, it specifies if the frame rotates about the Z axis.

Frames are specified relative to the preceding frame:

is defined relative to*F1**F0*is defined relative to*F2**F1*is defined relative to*F3**F2*is defined relative to*F4**F3*

Let’s create an empty chain to which we’ll add the segments.

ChainkdlChain =Chain();

For the first segment, note how ** F1** is oriented the same way as

**, just located at (0, 1, 0). Hence, we can initialize our frame using a simple vector. Also note that**

*F0***doesn’t move relative to**

*F1***. In this case we’ll use a**

*F0**Joint::None*while creating the segment.

Jointjoint1(Joint::None);Frameframe1 =Frame(Vector(0.0, 1.0, 0.0)); kdlChain.addSegment(Segment(joint1, frame1));

Similarly, for ** F2** there is again no change in orientation of the frame, just a translation to (0, 2, 0). Link 2 however rotates about the Z axis of the previous frame, so we’ll use

*Joint::RotZ*.

Jointjoint2(Joint::RotZ);Frameframe2 =Frame(Vector(0.0, 2.0, 0.0)); kdlChain.addSegment(Segment(joint2, frame2));

** F3** is rotated about the X axis by -90° and then translated 2 units about the new Z axis. We can multiply two intermediate frames to construct the final one.

Jointjoint3(Joint::RotZ);Frameframe3 =Frame(Rotation::EulerZYX(0.0, 0.0, -M_PI / 2)) *Frame(Vector(0.0, 0.0, 2.0)); kdlChain.addSegment(Segment(joint3, frame3));

** F4** first reverses the previous rotation and is then translated 1 unit in the X and Y directions.

Jointjoint4(Joint::RotZ);Frameframe4 = Frame(Rotation::EulerZYX(0.0, 0.0, M_PI / 2)) * Frame(Vector(1.0, 1.0, 0.0)); kdlChain.addSegment(Segment(joint4, frame4));

#### Step 2: Joint Angles

Next we’ll construct a joint angles variable that will contain the three angles for which we wish to perform forward kinematics.

JntArrayjointAngles =JntArray(3); jointAngles(0) = -M_PI / 4.;// Joint 1jointAngles(1) = M_PI / 2.;// Joint 2jointAngles(2) = M_PI;// Joint 3

#### Step 3: Forward Kinematics

Finally, we can run forward kinematics for the joint angles in step 2, using the chain we constructed in step 1. This will give is the end effector pose.

ChainFkSolverPos_recursiveFKSolver =ChainFkSolverPos_recursive(kdlChain);FrameeeFrame;FKSolver.JntToCart(jointAngles, eeFrame);

Forward kinematics solution (contents of *eeFrame*):

-0.7071 -0.7071 0. -1.414 -0.7071 0.7071 0. 3.828 0. 0. -1. 0. 0. 0. 0. 1.

Let’s verify a couple of things against the diagram.

- the end effector’s position is roughly at (-1.5, 3.8, 0); the calculated position is (-1.414, 3.828, 0.)
- the end effector frame’s Z axis unit vector is pointing along
’s -Z Axis*F0* - the X axis unit vector is (-0.7071, -0.7071, 0.) which checks out too since in the image it’s in the screen plane and pointing down and to the left — negative x, negative y relative to
*F0* - similarly, the Y axis unit vector is (-0.7071, 0.7071, 0.) which means it should be in the screen plane and pointing up and to the left — negative x, positive y relative to
*F0*

### The code

Code is available on Github.

Here’s the relevant code all at once:

Continue at: https://medium.com/@sarvagya.vaish/forward-kinematics-using-orocos-kdl-da7035f9c8e

The text above is owned by the site above referred.

Here is only a small part of the article, for more please follow the link

Also see:

https://dadoswebaxx.weebly.com/