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
- Transformation
- KUKATutorial2MotionPlanning
- KUKATutorial2Dashboard
|
Column B
- ArticulatedArm (in abstract services)
- Util
- SimulatedLBR3Arm
- KUKAArmTutorialSimulation
- Transformation
- KUKATutorial2MotionPlanning
- 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 end-of-arm 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 end-effector. An end-effector 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 end-of-arm 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.
/// <summary>
/// Handler for PTP movements
/// </summary>
/// <param name="move">the PTPMove paramter</param>
/// <returns>tasks to perform</returns>
[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public virtual IEnumerator<ITask> PTPMove(PTPMove move)
{
. . .
transformation.KinematikPose req = new transformation.KinematikPose();
req.Position = new Vector3(position.X, position.Y, position.Z);
req.Orientation = new Vector3(orientation.X, orientation.Y, orientation.Z);
req.Joints = _startJoints;
yield return Arbiter.Choice(
_transformationPort.GetInverseKinematik(req),
delegate(transformation.KinematikJoints resp) { _destinationJoints = resp.Joints; },
FaultHandler
);
This request is sent to the ‘_transformationPort’, which represents the transformation service. This request arrives at the ‘GetInverseKinematikHandler’ of the service.
/// <summary>
/// Processes the inverse kinematics
/// (gets the position and orientation of the tool center point (TCP) and calculates the
/// corresponding joint angles)
/// </summary>
/// <param name="req">inverse kinematic parameter</param>
/// <returns></returns>
[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public virtual IEnumerator<ITask> GetInverseKinematikHandler(GetInverseKinematik req)
{
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 Denavit-Hartenberg parameter set is assigned to holding values for  and  . The Denavit-Hartenberg 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: Denavit-Hartenberg parameters |
Using [14] and [15] we get the complete transformation from frame to frame :
public static Matrix GetTransformationMatrix(int lowIndex, int highIndex, float[] theta, float[] alpha, float[] a, float[] d)
{
Matrix[] matrices = new Matrix[highIndex];
float[,] values;
for (int i = lowIndex + 1; i < highIndex + 1; i++)
{
values = new float[4, 4];
values[0, 0] = (float)(Math.Cos(theta[i]));
values[0, 1] = -(float)(Math.Sin(theta[i]));
values[0, 2] = 0;
values[0, 3] = a[i - 1];
values[1, 0] = (float)(Math.Cos(alpha[i - 1]) * Math.Sin(theta[i]));
values[1, 1] = (float)(Math.Cos(alpha[i - 1]) * Math.Cos(theta[i]));
values[1, 2] = -(float)(Math.Sin(alpha[i - 1]));
values[1, 3] = -(float)(d[i] * Math.Sin(alpha[i - 1]));
values[2, 0] = (float)(Math.Sin(alpha[i - 1]) * Math.Sin(theta[i]));
values[2, 1] = (float)(Math.Sin(alpha[i - 1]) * Math.Cos(theta[i]));
values[2, 2] = (float)(Math.Cos(alpha[i - 1]));
values[2, 3] = (float)(d[i] * Math.Cos(alpha[i - 1]));
values[3, 0] = 0;
values[3, 1] = 0;
values[3, 2] = 0;
values[3, 3] = 1;
matrices[i - (lowIndex + 1)] = new Matrix(values);
}
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]:
Matrix T6 = Matrix.GetTransformationMatrix(0, 7, theta, state.Alpha, state.A, state.D);
The TCP position can be read out directly from matrix :
_x = T6.Values[0, 3];
_y = T6.Values[1, 3];
_z = T6.Values[2, 3];
The orientation in roll  , pitch  and yaw  angles are calculated from the rotational sub matrix of using [9]:
Pitch = (float)(Math.Atan2(-T6.Values[2, 0],
Math.Sqrt(T6.Values[0, 0] * T6.Values[0, 0] + T6.Values[1, 0] * T6.Values[1, 0])));
if (Pitch == (float)(Math.PI / 2))
{
Roll = 0;
Yaw = (float)(Math.Atan2(T6.Values[0,1], T6.Values[1,1]));
}
else if (Pitch == -(float)(Math.PI / 2))
{
Roll = 0;
Yaw = (float)(-Math.Atan2(T6.Values[0,1], T6.Values[1,1]));
}
else
{
Roll = (float)(Math.Atan2(T6.Values[2, 1], T6.Values[2, 2]));
Yaw = (float)(Math.Atan2(T6.Values[1, 0], T6.Values[0, 0]));
}
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 x-z 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  :
p04K0[0] = (float)(Math.Round(x - state.D[7] * n[0], 5));
p04K0[1] = (float)(Math.Round(y - state.D[7] * n[1], 5));
p04K0[2] = (float)(Math.Round(z - state.D[7] * n[2], 5));
p04K0[3] = 1;
Two solutions and  yield to the same position see Fig. 3:
_theta[0][0] = _theta[1][0] = _theta[2][0] = _theta[3][0]
= (float)(Math.Atan2(-p04K0[2], p04K0[0]));
_theta[4][0] = _theta[5][0] = _theta[6][0] = _theta[7][0]
= (float)(Math.Atan2(-p04K0[2], p04K0[0]) + Math.PI);
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 :
Matrix T01 = Matrix.GetTransformationMatrix(0, 1,
new float[] {0, _theta[0][0] },
state.Alpha, state.A, state.D);
float[] p14K0 = new float[4];
p14K0[0] = (float)(p04K0[0] - T01.Values[0, 3]);
p14K0[1] = (float)(p04K0[1] - T01.Values[1, 3]);
p14K0[2] = (float)(p04K0[2] - T01.Values[2, 3]);
p14K0[3] = 1;
We compute angle through the law of cosine:
float p14BetQuad = (float)(p14K0[0] * p14K0[0] + p14K0[1] * p14K0[1] + p14K0[2] * p14K0[2]);
float help3 = (float)(((state.A[2] * state.A[2]) + (state.D[4] * state.D[4]) - p14BetQuad)
/ (2 * Math.Abs(state.A[2]) * Math.Abs(state.D[4])));
float phi = (float)(Math.Acos(Math.Round(help3, 2)));
Finally knowledge of offers two possible solutions for :
_theta[0][2] = _theta[1][2] = (float)(Math.PI - phi);
_theta[4][2] = _theta[5][2] = -_theta[0][2];
_theta[2][2] = _theta[3][2] = (float)(Math.PI + phi);
_theta[6][2] = _theta[7][2] = -_theta[2][2];
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:
T01.Values[0, 3] = 0;
T01.Values[1, 3] = 0;
T01.Values[2, 3] = 0;
float[] p14K1 = T01.transpose().times(p14K0);
Fig.6: Joint angle 2
We get and through (see Fig. 5):
float help2 = (float)((state.A[2] * state.A[2] + p14BetQuad - state.D[4] * state.D[4]) /
(2 * Math.Abs(state.A[2]) * Math.Sqrt(p14BetQuad)));
float beta1 = (float)(Atan2(p14K1[0], p14K1[2]));
float beta2;
...
beta2 = (float)(Math.Acos(help2));
Finally all solutions for come out through:
Theta[0][1] = _theta[1][1] = -(beta1 + beta2);
Theta[2][1] = _theta[3][1] = -(beta1 - beta2);
Theta[4][1] = Theta[5][1] = -Theta[0][1];
Theta[6][1] = Theta[7][1] = -Theta[2][1];
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  :
Matrix A0_4S1 = Matrix.GetTransformationMatrix(0, 4, new float[] {state.Theta[0],
_theta[0][0] + state.Theta[1],
_theta[0][1] + state.Theta[2],
_theta[0][2] + state.Theta[3],
state.Theta[4]},
state.Alpha, state.A, state.D);
Matrix A0_4S2 = Matrix.GetTransformationMatrix(0, 4, new float[] {state.Theta[0],
_theta[2][0] + state.Theta[1],
_theta[2][1] + state.Theta[2],
_theta[2][2] + state.Theta[3],
state.Theta[4]},
state.Alpha, state.A, state.D);
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 :
float[] z4K0S1 = new float[] { -A0_4S1.Values[0, 2], -A0_4S1.Values[1, 2], -A0_4S1.Values[2, 2], 0 };
float[] z4K0S2 = new float[] { -A0_4S2.Values[0, 2], -A0_4S2.Values[1, 2], -A0_4S2.Values[2, 2], 0 };
float help5S1 = (float)((z4K0S1[0] * n[0]) + (z4K0S1[1] * n[1]) + (z4K0S1[2] * n[2]));
float help5S2 = (float)((z4K0S2[0] * n[0]) + (z4K0S2[1] * n[1]) + (z4K0S2[2] * n[2]));
Theta[0][4] = (float)Math.Acos(help5S1);
Theta[1][4] = -Theta[0][4];
Theta[2][4] = (float)Math.Acos(help5S2);
Theta[3][4] = -Theta[2][4];
Theta[4][4] = Theta[0][4];
Theta[5][4] = -Theta[4][4];
Theta[6][4] = Theta[2][4];
Theta[7][4] = -Theta[6][4];>
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 Z-Y-Z:
Hence we state:
Again we have to calculate two matrices to retrieve all possible solutions:
Matrix A4_0S1 = A0_4S1.transpose();
Matrix A4_0S2 = A0_4S2.transpose();
Matrix A7_4S1 = A4_0S1.times(rot0_7);
Matrix A7_4S2 = A4_0S2.times(rot0_7);
Using the relation  angle can be calculated through the elements  and  of :
Now all possible solutions for can be calculated:
_theta[0][3] = (float)(Math.Atan2(A7_4S1.Values[1, 2], A7_4S1.Values[0, 2]));
_theta[2][3] = (float)(Math.Atan2(A7_4S2.Values[1, 2], A7_4S2.Values[0, 2]));
_theta[4][3] = (float)(_theta[0][3] + Math.PI);
_theta[6][3] = (float)(_theta[2][3] + Math.PI);
_theta[1][3] = _theta[4][3];
_theta[3][3] = _theta[6][3];
_theta[5][3] = _theta[0][3];
_theta[7][3] = _theta[2][3];
The same way we get all solution for through the elements and of the matrix :
_theta[0][5] = (float)(-Math.Atan2(-A7_4S1.Values[2, 1], A7_4S1.Values[2, 0]));
_theta[2][5] = (float)(-Math.Atan2(-A7_4S2.Values[2, 1], A7_4S2.Values[2, 0]));
_theta[4][5] = _theta[0][5];
_theta[6][5] = _theta[2][5];
_theta[1][5] = _theta[0][5] + (float)Math.PI;
_theta[3][5] = _theta[2][5] + (float)Math.PI;
_theta[5][5] = _theta[4][5] + (float)Math.PI;
_theta[7][5] = _theta[6][5] + (float)Math.PI;
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 :
for (int i = 0; i < _theta.GetLength(0); i++)
{
if (Math.Round(Theta[i][4], 2) == 0)
{
help46 = (_theta[i][3] + _theta[i][5]) % (float)(2f * Math.PI);
_theta[i][3] = _theta[i][5] = help46 / 2f;
if (i % 2 == 1)
{
_theta[i][3] = _theta[i][5] += (float)Math.PI;
}
}
}
Finally all angles are wrapped to the closed interval  :
for (int i = 0; i < _theta.GetLength(0); i++)
{
for (int j = 0; j < _theta[i].GetLength(0); j++)
{
Theta[i][j] = (float)(Theta[i][j] % (Math.PI * 2));
if (Theta[i][j] < -(float)Math.PI)
Theta[i][j] += (float)(2f * Math.PI);
else if (Theta[i][j] > (float)Math.PI)
Theta[i][j] -= (float)(2f * Math.PI);
}
}
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:
deviation = 0;
k = 0;
for (int j = 0; j < invKin.Theta[i].Length; j++)
{
if (!_state.IsUseable[k]) k++;
jointangle = req.Body.Joints[k] % (float)(Math.PI * 2f);
if (jointangle > Math.PI)
jointangle -= (float)(Math.PI * 2f);
if (jointangle < -Math.PI)
jointangle += (float)(Math.PI * 2f);
deviation += Math.Abs(invKin.Theta[i][j] - jointangle);
k++;
}
if (deviation < min_deviation)
{
min_deviation = deviation;
min_deviationID = i;
}
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]
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
|