# Forward and inverse kinematics of 6-DOF robot

## brief introduction

This paper mainly solves the forward and inverse kinematics of the traditional six degree of freedom robot, selects the Han robot Elfin05 as the analysis object, and the development language is C + +. (under improvement)

## Robot forward kinematics

Robot forward kinematics derivation process General method for determining the coordinate system of each joint:

1. The Z axis of the coordinate system coincides with the rotation center axis of each joint
2. The X axis of the coordinate system coincides with the common vertical line along the two adjacent Z axes
3. The Y-axis of the coordinate system can be determined by the right-hand rule

When two adjacent Z axes intersect, the method to determine the coordinate system is as follows:

1. The Y axis of the coordinate system. The extension line along the first Z axis intersecting the next X axis is the Y axis
2. The X-axis of the coordinate system can be determined by the right-hand rule

When two adjacent Z axes are parallel, the method to determine the coordinate system is as follows:

1. X axis of coordinate system: the extension line along the first Z axis intersecting the next X axis is the Y axis
2. The X-axis of the coordinate system can be determined by the right-hand rule

Then:
Rod 1-rod 2: rod length 220mm, joint rod length 0mm, rotate 0 degrees around z and 90 degrees around x;
Rod 2-rod 3: rod length: 0mm, joint rod length: 455mm, rotate 90 degrees around z and 0 degrees around x;
Rod 3-Rod 4: rod length: 0mm, joint rod length: 0mm, rotate 0 degrees around z and 90 degrees around x;
Rod 4-rod 5: rod length 495mm, joint rod length 0mm, rotate 0 degrees around z and 90 degrees around x;
Rod 5-rod 6: rod length: 0mm, joint rod length: 0mm, rotate 90 degrees around z and 90 degrees around x;
Rod 6-TCP: rod length - 155mm, joint rod length 0mm, rotate 0 degrees around z and 0 degrees around x.

DH parameter table

190°220mm0±360°
20450mm90°±360°
390°00±360°
490°4950±360°
590°0090°±360°
690°-1550±360°

matlab code block

```% Link Function call format: L(i)=Link( [theta，D，A，alpha，sigma]，'convention')
% Its parameters and D-H Corresponding parameters
% The first four parameters represent parameters in turn:'theta'Represents joint angle, parameter'D'Represents the transverse distance,
% parameter'A'Represents member length, parameter'alpha'Represents torsion angle, parameter'sigma'Represents joint type: 0 represents rotating joint, non-0 represents moving joint, and the default value is 0.
% parameter 'convention' Indicates use D-H Type of robot model created by parameter method:
% 'standard'Indicates the standard used D-H Parameter method to create robot model;'modified'Indicates that the improvement is adopted D-H The robot model is created by parameter method. The default value is'standard'，Use default values in code.
clear;
clc;
%Build robot model
%       theta    d        a        alpha     offset
L1=Link([0       0.220     0        pi/2       0     ],'standard'); %Defines the of the connecting rod D-H parameter
L2=Link([0       0         0.455     0         0     ],'standard');
L3=Link([0       0         0        pi/2       0     ],'standard');
L4=Link([0       0.495     0        pi/2       0     ],'standard');
L5=Link([0       0         0        pi/2       0     ],'standard');
L6=Link([0       -0.155    0         0         0     ],'standard');
L(1).qlim =[-2*pi, 2*pi];
L(2).qlim =[-2*pi, 2*pi];
L(3).qlim =[-2*pi, 2*pi];
L(4).qlim =[-2*pi, 2*pi];
L(5).qlim =[-2*pi, 2*pi];
L(6).qlim =[-2*pi, 2*pi];
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %Connect the connecting rod and name the robot manman
figure(1);
robot.display();
teach(robot);
```

Solving steps of inverse kinematics

Input: P=[x,y,z] E=[alpha,beta,gamma] // P represents the position of TCP at the end of the robot, E represents the attitude of TCP at the end of the robot Output:[q1,q2,q3,q4,q5,q6] / / represents the values of 6 joint angles of the robot
1.[n,s,a]←forward_euler(E) / / solve the rotation transformation matrix of TCP at the end of the robot
2.Pw ← E - d7a * / / solve the position W at wcp at the intersection of robot wrist joint
3.[q1,q2,q3]←inverse_kinematic(Pw) / / four sets of solutions can be obtained by solving three joint values with analytical method
4.R0_3←inverse_kinematic_0_3([q1,q2,q3]) / / derive the positive solution and calculate the transformation matrix R0 at the base relative to the spherical wrist wcp_ three
5.R3_6←R0_3_T[n,s,a] / / calculate the transformation matrix R3 of wcp relative to the end TCP_ six
6.[q4,q5,q6]←inverse_euler(R3_6) / / calculate the values of the three joints after wcp, because 4 r0s can be determined in steps 3-5_ 3. In step 6, two sets of solutions can be obtained, so a total of 8 sets of solutions are determined.
7.generate_ik([q1,q2,q3,q4,q5,q6]) / / find all solutions within the range of joint angle ± 2 π according to 8 groups of solutions, and select the nearest solution.

c + + code

```/**
* @description: Calculating inverse kinematics, i.e. calculating an incorrect angle, will have 8 solutions (but not all solutions are valid)
* 				 tcp.position tcp Location; tcp.orientation tcp direction
* @param[in] tcp: Robot position + attitude [x,y,z,ox,oy,oz]
* @param[in] current: Current joint angle of robot
* @param[in & out] solutions:Document effective solutions
*/
void Kinematics::computeInverseKinematicsCandidates(const Pose& tcp, const JointAngles& current, std::vector<KinematicsSolutionType> &solutions)
{
// 1. Calculate Angle 0 step by step, calculate Transformations Matrix T06 outside TCP, and convert TCP along the TCP direction through Wrist Len gt h - > wrist center point
// The angle 0 is calculated by the arctan of the projection from the wrist center point to the ground, resulting in two possible solutions, called forward solution and backward solution
double sinx = sin(tcp.orientation[X]);
double cosx = cos(tcp.orientation[X]);
double siny = sin(tcp.orientation[Y]);
double cosy = cos(tcp.orientation[Y]);
double sinz = sin(tcp.orientation[Z]);
double cosz = cos(tcp.orientation[Z]);
// Set the transformation matrix T06. The upper left 3X3 part is the rotation matrix in the Euler angle in the zyx model
// (in fact, only the third and fourth columns are required, except calculating everything to debug) x-y-z fixed angle( http://www-home.htwg-konstanz.de/~bittel/ain_robo/Vorlesung/02_PositionUndOrientierung.pdf)
HomMatrix T06 ;
// Rotation around the moving axis X-Y-Z (fixed angle)
T06  <<
cosz*cosy,	cosz*siny*sinx-sinz*cosx,	cosz*siny*cosx+sinz*sinx,	tcp.position,
sinz*cosy,	sinz*siny*sinx+cosz*cosx,	sinz*siny*cosx-cosz*sinx,	tcp.position,
-siny,		cosy*sinx,					cosy*cosx,					tcp.position,
0,			0,							0,							1 ;

T06 *= view2Hand.inverse();

HomVector wcp_from_tcp_perspective = { 0,0, HandLength,1 }; // The third is the distance from the end to the wrist
HomVector wcp = T06 * wcp_from_tcp_perspective; // Wrist position
// By calculating the projection base angle under the XOY plane through the wrist position, we have two possible solutions, looking forward and looking back;
// According to the sign of tcp x coordinate, we assign two solutions to forward and backward angles
double angle0_solution1 = atan2( wcp[Y],  wcp[X]);
double angle0_solution2 = atan2(-wcp[Y], -wcp[X]);
// singularity check: if we are right above the origin, keep angle0
// Singularity detection: if we are above the origin, keep the angle 0
if ((fabs(wcp[Y]) < floatPrecision) &&  (fabs(wcp[X]) < floatPrecision)) {
angle0_solution1 = current;
angle0_solution2 = M_PI_set_2 - current;
}
// Initializes the forward and backward angle values
double angle0_forward = 0;
double angle0_backward = 0;

//Determine whether X is a non negative value
bool tcpXPositive = tcp.position[X] >= 0;

if (tcpXPositive) {
angle0_forward =  angle0_solution1;
angle0_backward = angle0_solution2;
} else {
angle0_forward =  angle0_solution2;
angle0_backward = angle0_solution1;
}

// 2. Calculate anlge1 and anlge2, and use the triangles of joint1, joint2 and joint3
// The sides of the triangle a = forearm length, b = upper arm length, c = distance (wcp and base), and the angle is calculated by cosine theorem

// Calculate the height difference between joint 1 and wcp
double z_distance_joint1_wcp = wcp[Z] - HipHeight; // TCP - 220
// Calculate the oblique side length of the top view of the base and wcp
double distance_base_wcp_from_top = hypothenuseLength(wcp[X],wcp[Y]); // Solve the sum of squares

// Calculate the c-side (door shape) Z of the triangle_ distance_ joint1_ WCP and distance_base_wcp_from_top
double c = hypothenuseLength( z_distance_joint1_wcp , distance_base_wcp_from_top );		// Distance from intersection point of robot J0-J1 axis to robot wrist
double b = UpperArmLength;																// Axis length of robot J1-J2
double a = TotalForearmLength;															// Axis length of robot J2-J3
double alpha = triangleAlpha(a,b,c); 			// Calculate the included angle corresponding to TotalForearmLength
double gamma = triangleGamma(a,b,c);			// Calculate the angle corresponding to the c side of the triangle

// Flip flag: is the triangle in the flip position or non flip position
double flipFlag_forward = tcpXPositive ? 1.0 : -1.0;
double flipFlag_backward = tcpXPositive ? -1.0 : 1.0;

// Calculate the angle of J2 elbow at the bottom and elbow at the top
double delta_forward = atan2(z_distance_joint1_wcp, flipFlag_forward * distance_base_wcp_from_top);
double delta_backward = atan2(z_distance_joint1_wcp, flipFlag_backward * distance_base_wcp_from_top);

// Calculate the four possible known trilateral lengths c/b/a of angle 1 and calculate the angle
double angle1_forward_sol1  = - (M_PI_set_2 - ( delta_forward + alpha));
double angle1_forward_sol2  = - (M_PI_set_2 - ( delta_forward - alpha));
double angle1_backward_sol1 = - (M_PI_set_2 - ( delta_backward + alpha));
double angle1_backward_sol2 = - (M_PI_set_2 - ( delta_backward - alpha));

// Record angle2 two possibilities
double angle2_sol1 = gamma - M_PI_set;
double angle2_sol2 = M_PI_set  - gamma;

// 3. Calculate angle3 /angle4 /angle5
// First calculate the rotation matrix R03, and then calculate the Inverse R03 (equal to the transpose matrix)
// Finally, take R06 of T06 and calculate the inverse R03 ×  R06 obtains R36, and calculates angle3-4-5 by solving R36
computeIKUpperAngles(tcp, current, PoseConfigurationType::PoseDirectionType::FRONT, PoseConfigurationType::PoseFlipType::NO_FLIP,
angle0_forward, angle1_forward_sol1, angle2_sol1, T06, solutions, solutions);

computeIKUpperAngles(tcp, current, PoseConfigurationType::PoseDirectionType::FRONT, PoseConfigurationType::PoseFlipType::FLIP,
angle0_forward, angle1_forward_sol2, angle2_sol2, T06, solutions, solutions);

computeIKUpperAngles(tcp, current, PoseConfigurationType::PoseDirectionType::BACK, PoseConfigurationType::PoseFlipType::NO_FLIP,
angle0_backward, angle1_backward_sol1, angle2_sol1, T06, solutions, solutions);

computeIKUpperAngles(tcp, current, PoseConfigurationType::PoseDirectionType::BACK, PoseConfigurationType::PoseFlipType::FLIP,
angle0_backward, angle1_backward_sol2, angle2_sol2, T06, solutions, solutions);

}

/**
* @description: Calculate the last three angles and the first three angles of TCP (2 solutions)
* @param[in] tcp:Robot position + attitude
* @param[in] current:Current joint angle of robot
* @param[in] poseDirection:Is the elbow up or down
* @param[in] poseFlip:Flip or non flip
* @param[in] angle0:Calculated joint angle J0
* @param[in] angle1:Calculated joint angle J1
* @param[in] angle2:Calculated joint angle J2
* @param[in] T06:Transformation matrix T06
* @param[in & out] sol_up:Solution (because there are two solutions, they are stored separately)
* @param[in & out] sol_down:Solution (because there are two solutions, they are stored separately)
*/
void Kinematics::computeIKUpperAngles(
const Pose& tcp, const JointAngles& current,
PoseConfigurationType::PoseDirectionType poseDirection,
PoseConfigurationType::PoseFlipType poseFlip,
double angle0, double angle1, double angle2, const HomMatrix &T06,
KinematicsSolutionType &sol_up, KinematicsSolutionType &sol_down)
{

// UP
sol_up.config.poseFlip = poseFlip;						// Flip or non flip, depending on the input
sol_up.config.poseDirection = poseDirection;			// The elbow is up or down, depending on the input
sol_up.config.poseTurn = PoseConfigurationType::UP;		// Up
sol_up.angles = angle0;
sol_up.angles = angle1;
sol_up.angles = angle2;

// DOWN
sol_down.config.poseFlip = poseFlip;					// Flip or non flip, depending on the input
sol_down.config.poseDirection = poseDirection;			// The elbow is up or down, depending on the input
sol_down.config.poseTurn = PoseConfigurationType::DOWN;	// down
sol_down.angles = angle0;
sol_down.angles = angle1;
sol_down.angles = angle2;

// First calculate the rotation matrix R03, and then calculate the Inverse R03 (equal to the transpose matrix)
// Finally, take out R06 of T06 and calculate Inverse R03 ×  R06 obtains R36, and calculates angle3-4-5 by solving R36

HomMatrix T01, T12, T23;
// Calculation matrix T01-T12-T23
computeDHMatrix(HIP, 		angle0, 					T01);

for (int  i = 0; i < 3; ++i)
{
for (int j = 0; j < 3 ;++j)
{
if( fabs(T01(i,j)) > 0.0   && fabs(T01(i,j)) < (0.0 + floatPrecision) )
{	T01(i, j) = 0.0;}
if( fabs(T12(i,j)) > 0.0   && fabs(T12(i,j)) < (0.0 + floatPrecision) )
{	T12(i, j) = 0.0;}
if( fabs(T23(i,j)) > 0.0   && fabs(T23(i,j)) < (0.0 + floatPrecision) )
{	T23(i, j) = 0.0;}
}
}

// According to the matrix T01-T12-T23, the attitude matrix R01-R12-R23 is extracted
RotationMatrix R01 = T01.topLeftCorner(3,3);
RotationMatrix R12 = T12.topLeftCorner(3,3);
RotationMatrix R23 = T23.topLeftCorner(3,3);

// Find attitude matrix R02-R03
RotationMatrix R02 = R01*R12;
RotationMatrix R03 = R02*R23;
RotationMatrix R03_inv = R03;
// The inverse R03 of R03 is calculated by manual transposition_ inv = R03.inverse();
swap(R03_inv(0,1), R03_inv(1,0));
swap(R03_inv(0,2), R03_inv(2,0));
swap(R03_inv(1,2), R03_inv(2,1));

// Extract the attitude matrix R06 of T06
RotationMatrix R06 = T06.topLeftCorner(3,3);
RotationMatrix R36 = R03_inv * R06;

// //Joint 5
// sol_up.angles = acos(R36(2,2));
// sol_down.angles = -sol_up.angles;
// So a very dangerous possibility is that any of c4, s4, c5, s5, c6 or s6 can appear in the denominator.
// If c5 is zero, the equation will degenerate.
if(  R36(2,2) == 1 )
{
sol_up.angles = atan2( sqrt(pow(R36(1,2),2) + pow(R36(0,2),2)) , R36(2,2));
sol_down.angles = -sol_up.angles;
}
else if(R36(2,2) == 0 )
{
sol_up.angles = atan2( sqrt(pow(R36(1,2),2) + pow(R36(0,2),2)) , -R36(2,2));
sol_down.angles = -sol_up.angles;
}
else
{
sol_up.angles = atan2( sqrt(pow(R36(1,2),2) + pow(R36(0,2),2)) , -R36(2,2));
sol_down.angles = -sol_up.angles;
}

// 4. Calculate the angle5 atan2 value range [- 180180]

double sin_angle4_1 = sin(sol_up.angles);
double sin_angle4_2 = - sin_angle4_1;

double cos_angle4_1 = cos(sol_up.angles);
// 5-6. Calculate angle4 and angle6
// If the angle5 wrist is 0 degrees, there are countless solutions (singular), which requires special treatment to keep the angle near the current position
if ( pow(sin_angle4_1 , 2) < floatPrecision)
{
// Keep angle4 stable, moving only other angles or directly to zero
sol_up.angles = current == 0 ? 0 : current;
sol_down.angles = current == 0 ? 0 : current;

//  [- pi/2,+pi/2] when angle5 is 0
double asinR36_10 ;
if( cos_angle4_1 > 0 ) 	// angle5 = 0
{
asinR36_10 = asin(R36(1, 0)); // [-pi/2,+pi/2]
sol_up.angles = sol_up.angles - asinR36_10;
sol_down.angles= sol_down.angles - asinR36_10;
}
else		// angle5 = 180
{
asinR36_10 = asin( - R36(1, 0)); // [-pi/2,+pi/2]
sol_up.angles  = asinR36_10  - sol_up.angles;
sol_down.angles= asinR36_10  - sol_down.angles;
}
// Normalize the angle by placing it in the - PI and PI intervals
while ((abs( sol_up.angles - current) > abs( sol_up.angles + M_PI_set - current))
&& (sol_up.angles + M_PI_set <= actuatorConfigType.maxAngle))
{
sol_up.angles   += M_PI_set;
sol_down.angles += M_PI_set;
}
while ((abs( sol_up.angles - current) > abs( sol_up.angles - M_PI_set - current))
&& (sol_up.angles - M_PI_set >= actuatorConfigType.minAngle))
{
sol_up.angles   -= M_PI_set;
sol_down.angles -= M_PI_set;
}
}
else
{
// Start here, sin_ angle_ 4_ If x is not close to 0
sol_up.angles   = atan2( - R36(2,1)/sin_angle4_1, R36(2,0)/sin_angle4_1);
sol_down.angles = atan2( - R36(2,1)/sin_angle4_2, R36(2,0)/sin_angle4_2);

sol_up.angles   =  atan2( R36(1,2)/sin_angle4_1, R36(0,2)/sin_angle4_1);
sol_down.angles =  atan2( R36(1,2)/sin_angle4_2, R36(0,2)/sin_angle4_2);
}
// The 6-axis tandem joint robot has three kinds of singularity: wrist singularity, shoulder singularity and elbow singularity.
//   1. The wrist singularity occurs when the 4-axis and 6-axis coincide (parallel).
//   2. The shoulder singularity occurs when the center of the wrist is located at the rotation centerline of axis 1.
//   3. The elbow singularity occurs in the center of the wrist and a line of 2-axis and 3-axis.
// terms of settlement:
//  1. Avoid the robot passing through singular points as much as possible in the planned path.
//  2. Combined with robot kinematics, optimize the robot inverse solution algorithm to ensure the stability of pseudo inverse solution near singular points.
//  3.
}
}
```

Multiple solutions

```/**
* @description: According to the calculated inverse solution of the manipulator, more solutions are generated
* @param joints[in]:Angle of each joint
* @param ik[in]:Intermediate processing results
* @param new_ik[out]:All solutions
*/
void Kinematics::generate_all_ik(const std::vector<std::vector<double>> &joints, std::vector<double> &ik, std::vector<std::vector<double>> &new_ik)
{
size_t i = ik.size();

if (i == joints.size())
{
new_ik.push_back(ik);
return;
}
//
for (auto &j : joints[i])
{
ik.push_back(j);
generate_all_ik(joints, ik, new_ik);
ik.pop_back();
}
}

/**
* @description: return all possible joint value in [min_position, max_position)
* @param[in] max_position: Maximum joint angle
* @param[in] min_position: Minimum joint angle
* @param[in] joint: Joint angle
* @return[out] results :All possible joint angle values in the joint angle range
*/
std::vector<double> Kinematics::lim(const double &max_position,const double &min_position, double joint)
{
std::vector<double> results;

// Judge the problem of joint angle out of bounds, and shrink the positive and negative 2 PI of the out of bounds solution back to the range of joint angle
if (joint > max_position)
{
while (joint > max_position)
{
joint -= 2 * M_PI_set;
}

while (joint > min_position)
{
results.push_back(joint);
joint -= 2 * M_PI_set;
}
}
else if (joint < min_position)
{
while (joint < min_position)
{
joint += 2 * M_PI_set;
}

while (joint < max_position)
{
results.push_back(joint);
joint += 2 * M_PI_set;
}
}
else    // Within the range of joint angle
{
// Put the original results into results
results.push_back(joint);

double joint_tmp = joint - 2 * M_PI_set;
while (joint_tmp > min_position)
{
results.push_back(joint_tmp);
joint_tmp -= 2 * M_PI_set;
}
joint_tmp = joint + 2 * M_PI_set;
while (joint_tmp < max_position)
{
results.push_back(joint_tmp);
joint_tmp += 2 * M_PI_set;
}
}
return results;
}

/**
* @description: return all possible joint value in [min_position, max_position) Find more solutions according to the range of joint angles STD:: vector < kinematicssolutiontype >
* @param[in] Solution: 8 sets of solutions obtained by inverse connection of mechanical ratio
* @param[out] MoreSolutions: All possible joint angle values in the joint angle range
*/
void Kinematics::ToLimits(const std::vector<KinematicsSolutionType> &Solution, std::vector<KinematicsSolutionType> &MoreSolutions)
{
// New solution
std::vector<std::vector<double>> new_ik;

for (size_t i = 0; i < Solution.size(); ++i)
{
auto &ik = Solution[i];
// Allocate memory
std::vector<std::vector<double>> ik_joints( (NumberOfActuators-1) , std::vector<double>());

for (size_t j = 0; j < NumberOfActuators-1; ++j)
{
// Solve the joint angles available for each axis
auto joints = lim(actuatorConfigType[j].maxAngle,actuatorConfigType[j].minAngle, ik.angles[j]);
ik_joints[j] = joints;
}
// Intermediate results
std::vector<double> ik_path;
// output
generate_all_ik(ik_joints, ik_path, new_ik);
}

for (size_t j = 0; j < new_ik.size(); ++j)
{

KinematicsSolutionType ik_new;
for (size_t k = 0; k < NumberOfActuators-1; ++k)
{
ik_new.angles[k] = new_ik[j][k];
}
MoreSolutions.push_back(ik_new);
}
}

```

Tags: Algorithm Robot

Posted on Tue, 07 Dec 2021 01:14:03 -0500 by sONOCOOLO