Arm Tutorial 2 project build order
A) If you have built all the other projects from the previous solutions step by step, you just need to build the projects in Column A.
B) If you haven’t built any of the previous tutorial solutions, you need to build all included projects in the order shown in Column B.
Column A
1. Transformation
2. KUKATutorial2MotionPlanning
3. KUKATutorial2Dashboard

Column B
1. ArticulatedArm (in abstract services)
2. Util
3. SimulatedLBR3Arm
4. KUKAArmTutorialSimulation
5. Transformation
6. KUKATutorial2MotionPlanning
7. KUKATutorial2Dashboard

Moving the end of arm to specific cartesian coordinates with a specific orientation
In arm tutorial 1, the robot was moved by setting target joint angles. Neither did we know the position and orientation of the endofarm nor could we move the robot to a desired cartesian coordinate.
In practice it's probably more useful to know about the Cartesian position and Euler angle orientation of the tool center point (TCP) than the knowledge about the joint angles of the robot. Furthermore it makes sense to command the robot via Cartesian coordinate with euler angle parameters instead joint angles. E.g. in practice you want to command 'move the TCP to Cartesian position _{}with orientation _{}'.
As you may noticed all robot arms in the arm tutorials have no manipulators or tools, that's why we use the notion 'end of arm' (EOA). As the last element of the robot kinematic chain, a mounted manipulator is called endeffector. An endeffector has usually a specific defined point at his end, called tool center point (TCP).
So for an application point of view it makes sense to transform from the joint angles robot arm state to cartesian coordinates with angular orientation and vice versa. Arm tutorial 2 implements a service that provides such transformations.
Fig.1: Direct vs. inverse kinematic transformation
The transformation from the robot joint angles to a cartesian coordinate with angular orientation is called 'forward kinematic transformation' (Fig. 1). Mathematically this transformation is easy. One reason for it being easy is that we have no multiple solutions. We just take the axis values of each axis and calculate from one frame to the next.
The backward transformation from a Cartesian coordinate with angular orientation to the robot joint angle configuration is called 'inverse kinematic transformation' and is more complex. Like indicated in the following picture, multiple arm configurations are possible for a single TCP position and orientation. This impacts the complexity on the application design.
E.g.: A programmer wants to move the TCP to a target position with a specific orientation and the inverse kinematic transformation service returns up to 8 possible solutions. Which solution is the best in terms of collision, minimized joint state changes which implies e.g. minimized travel time?
Coding for Arm Tutorial 2:
In arm tutorial 2, we allow to define a destination for the endofarm in terms of a cartesian position. In this case, the motion planning service needs to find out the target axis values. This is done with a call to the newly introduced transformation service.
Arm Tutorial 2 Services Overview:
New services for arm tutorial 2:
· Armtutorial2Dashboard: 
User Interface with added fields to enter cartesian positions 
· Armtutorial2MotionPlanning: 
Calculates the arm motion and sends axis values to the simulated LBR3 
· Transformation: 
Calculates direct and inverse kinematics 
Handling a PTP move with a cartesian destination:
As soon as a ‘PTPMove’ object with a cartesian destination is processed in the PTPMove Handler of the motion planning service, it will cause a request for an inverse kinematic calculation.
This request is sent to the ‘_transformationPort’, which represents the transformation service. This request arrives at the ‘GetInverseKinematikHandler’ of the service.
The calculation of the inverse kinematic inside this handler as well as the calculation for the direct kinematic is shown in the following sections.
It starts with the calculation of the general transformation matrix for the kinematic chain.
Calculation of the transformation matrix for the kinematic chain (class Matrix, method GetTransformationMatrix)
This method computes the translation matrix frame _{ }to frame _{}. To each frame _{ }a DenavitHartenberg parameter set is assigned to holding values for _{} and_{ }. The DenavitHartenberg parameter sets for all joints in Fig. 1 are listed in Table 1, for their definition refer to (1).

Joint _{} 
Rotation_{} 
Link length_{} 
Joint angle_{} 
Link offset_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 
_{} 

Fig. 2: Robot arm configuration 
Table1: DenavitHartenberg parameters 
Using [14] and [15] we get the complete transformation from frame _{ }to frame _{}:
_{}
Direct kinematics calculation (file class dirKinematik, method dirKinematik)
This method computes the position in cartesian coordinates and the orientation in RPY  angles of the TCP.
We start with the calculation of the transformation matrix_{ } for the whole kinematic chain that ends with the tool frame _{}. Through a call of the method GetTransformationMatrix we evaluate [14]:
_{}
The TCP position can be read out directly from matrix_{ }:
_{}
The orientation in roll _{}, pitch _{} and yaw_{ }angles are calculated from the rotational sub matrix of_{ } using [9]:
Inverse kinematics calculation though a geometric approach (class InvKinematik, method InvKinematik)
From a given cartesian position and RPY  orientation of the TCP this method calculates the joint angles for a seven axis robot. For a single TCP position the method computes all eight possible arm configurations, see Fig. 2. For more information about the inverse kinematic calculation refer to (2).
Fig.3: Possible solutions of the inverse kinematics calculation
Joint 1 angle
Fig.4: Joint angle 1
Angle _{} can be calculated from the position projection of frame _{} on the xz plane of frame _{}, see Fig. 3. Hence at first we have to compute the position of _{}. Twist joints don't change the position but the orientation of its subsequent frame. We use this property to simplify our calculations. In our robot arm we have three twist joints: Joint 1, 4 and 6. As shown in Fig. 3, the angular state of joint 6 influences just the orientation of the TCP, not its position. Through_{} we get the position vector _{}. The unit vector _{} points to the _{} axis of _{} is retrieved from the third column of the rotation sub matrix of _{}:
_{}
Two solutions _{} and _{}yield to the same _{} position see Fig. 3:
_{}
_{
}
For the sake of clarity we use a simple geometric approach for our inverse kinematic algorithm. Hence we didn't consider all special cases that might result in ambiguous results, e.g. for the _{} function. An improvement to the algorithm is left for the user as an exercise.
Joint angle 3
Fig.5: Joint angle 3
Via the knowledge of _{} we get angle _{ }which in turns leads to joint angle_{ }. Again we take advantage of a twist joint property. As shown in Fig. 4, the angular state of joint 4 influences the orientation of frame _{}, not its position. Hence vector _{} can be calculated. We get the position_{ }from the translation part of_{}:
_{}
We compute angle_{ } through the law of cosine:
_{}
Finally knowledge of_{ }offers two possible solutions for _{}:
_{}
_{}
_{}
_{}
Joint angle 2:
As shown in Fig. 5, angle _{} can be computed via _{ }and _{} which in turn we get through the vector _{}. For an easy calculation of _{ }we want to look at _{}. As seen from _{}, vector _{} only has an x and z component. Therefore we transform _{} from _{} to _{ }using the inverse of the sub matrix_{} of _{}:
_{}
_{}
Because_{} is orthogonal we get its inverse through its transpose:
_{}
Fig.6: Joint angle 2
We get _{ }and _{ }through (see Fig. 5):
_{}
_{}
Finally all solutions for _{ }come out through:
_{}
_{}
_{}
_{}
Joint angle 5:
Joint angle 5 can be calculated through the inner product between the unit vectors _{ }and _{} (Fig. 6):
_{}
_{}
Fig.7: Joint angle 5
The orientation of vector_{ }can be retrieved from the rotation sub matrix of the transformation_{}. Matrix_{} can be calculated via the angle results we already have. Angle _{} doesn't influence the position of _{} therefore we assume _{}. Using the prior results_{ }, _{} and _{ }we evaluate [14] and [15]:
_{}
Two arm configurations yield to the same position for frame _{}, see Fig. 2. Hence we calculate the transformation _{ }for both situations resulting in _{} and _{}:
Now we extract the rotation sub matrix from _{} which column vectors are the base unit vectors of frame 4:
_{}
_{}
With the column vector _{ }of _{} we finally compute all solutions for angle _{}:
_{}
_{}
_{}
_{
}
Joint angles 4 and 6:
The total rotation can be composed as follows:
_{}
Because matrix _{ }is orthogonal its inverse can also be calculated through its transpose:
_{}
Matrix _{ }is composed by three sequential rotations around ZYZ:
_{}
Hence we state:
_{}
_{}
Again we have to calculate two matrices to retrieve all possible solutions:
_{}
_{
}
Using the relation _{} angle _{} can be calculated through the elements _{}and_{ }of _{}:_{ }
_{}
Now all possible solutions for _{} can be calculated:
_{}
_{}
_{}
_{
}
The same way we get all solution for _{} through the elements _{ }and _{ }of the matrix _{}:
_{}
_{}
_{}
_{
}
As you can see in Fig. 6, for _{} and _{ }unlimited solutions are possible if _{} becomes zero. In that case the mean value of _{ }and _{} is assigned to _{} and _{}:
Finally all angles are wrapped to the closed interval _{}:
At last the GetInverseKinematikHandler selects one of the eight possible arm configurations. For each possible arm configuration _{} a quality function value is computed. The calculation takes the prior joint angle states _{} into account:
_{}
The arm configuration with the smallest quality function value becomes the new target arm configuration:
Linear coordinate transformation
_{} : = Basis vectors _{}of the new coordinate system
_{} : = Transformation matrix _{}
Coordinate transformation matrix:
_{}
[1]
Coordinate transformation:
_{}
[2]
Orthogonality:
_{}
[3]
Coordinate system orientation:
_{}
[4]
Rotational transformation around axis x:
_{}
[5]
Rotational transformation around axis y:
_{}
[6]
Rotational transformation around axis z:
_{}
[7]
Roll, pitch and yaw (RPY) angles
RPY  rotation matrix (X, Y', Z'' rotation):
_{}
_{}
[8]
RPY angles from rotation a matrix _{}:
_{}
_{}
_{}
[9]
Frames
_{} : = Translation vector _{}
_{} : = Rotation matrix _{}
_{} : = Transformation matrix _{}
_{} : = Vector in frame_{ }:_{}
_{} : = Vector in frame _{}:_{}
Frame rotation ("Orientate _{} via _{}"):
_{}
[10]
Frame translation ("Translate _{} by _{}"):
_{}
[11]
Transformation matrix from frame _{} to frame _{} ("Orientate _{} via _{} and translate it by _{}"):
_{}
[12]
Transformation from frame _{} to frame _{}:
_{}
[13]
Chained transformation from frame _{} to frame _{} (note that the multiplication is not commutative and must happen from left to right):
_{}
[14]
DenavitHartenberg transformation convention
As described in (1), four operations, two rotations and two translations are sufficient to compute a transformation matrix that transforms from frame _{} to frame _{} within a robot kinematic link chain based on the DenavitHartenberg convention:
Step 1: Rotation by link twist _{}  Rotate frame _{ }around its axis _{ }until _{}becomes parallel to axis _{ }of the next frame _{}.
Step 2: Translation by link length _{}  From its current position after step 1, move frame _{} into the direction of axis _{} until _{ }comes to overlap with axis _{} of the next frame_{ }.
Step 3: Rotation by joint angle _{}  From its current position after step 2, rotate frame_{} around axis _{ }of the next frame _{} until _{} becomes parallel to axis _{}.
Step 4: Translation by link offset _{}  From its current position after step 3, move frame _{} into the direction of axis _{ }until its origin matches the origin of the next frame _{}.
The total transformation matrix for frame _{} to frame _{} is calculated through multiplication of the four single transformations. Note that the multiplication is not commutative and must happen from left to right:
_{}
_{}
_{}
[15]
Symbols:
_{} : = _{}
_{} : = _{}
_{} := _{}
_{} := _{}
_{} : = Element row _{}column _{}of a matrix _{}
_{} : = Vector
References
(1) J. Craig, "Introduction to Robotics: Mechanics and Control", Addison Wesley Longman Publishing Co, 1986
(2) W. Weber, "Industrieroboter: Methoden der Steuerung und Regelung", Fachbuchverlag Leipzig, 2002
