KinematicChainsArduino/IKSolver.cpp at master · KinematicsArduino/KinematicChainsArduino · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
/*
 * IKSolver.cpp
 *
 *  Created on: Apr 27, 2021
 *      Author: aksha
 */

#include <IKSolver.h>

IKSolver::IKSolver() {
	// TODO Auto-generated constructor stub

}

IKSolver::~IKSolver() {
	// TODO Auto-generated destructor stub
}

// I suggest following the example laid out here for 6dof IK:
// https://github.com/madhephaestus/6dofServoArm/blob/master/DefaultDhSolver.groovy
// and from this Wrist Normalizer code:
// https://github.com/NeuronRobotics/java-bowler/blob/c2aedca8e378d76af4c7212b5759041a6a8c3753/src/main/java/com/neuronrobotics/sdk/addons/kinematics/WristNormalizer.java
IKResult IKSolver::IK(Matrix<4,4> &Target, float* Result, Link** links, int numberOfLinks){
	//PrintMatrix(Target, "_ Limb 0 IK");
	if(numberOfLinks!=3)return NumberOfLinksError;
	float x = Target(0,3);
	float y = Target(1,3);
	//Serial.println(String(x)+":Xval  "+String(y)+":Yval  ");
	float Link0Angle;

	//Projection on Limb XY
	if((abs(x)<0.1) && (abs(y)<0.1)){return ElbowTriangleSingularity;}
	if(x<0){return Quadrants2and3Unreachable;}
	Link0Angle = atan2(y,x)+links[0]->DH_Theta;

	//Serial.println("Theta0: "+String(Link0Angle));

	//Find link one angle in radians
	Result[0] = Link0Angle*180/3.14159;
	Matrix<4,4> T1 = Identity<4,4>();

	//Find the transfomation from link0 zFrame to link1 zFrame
	T1 = links[0]->computeStep(T1, Result[0]);
	//PrintMatrix(T1, "T1");
	Matrix<4,4> T2 = T1.Inverse()*Target;
	//PrintMatrix(T2, "T2");
	x = T2(0,3);
	y = T2(1,3);
	float z = T2(2,3);
	//Serial.println(String(x)+":Xval  "+String(y)+":Yval  ");
	if(abs(z)>0.1) return DHConfigError;
	if(((abs(x)<0.1) && (abs(y)<0.1))){return ElbowTriangleSingularity;}
	//Cannot access quadrants 2&3, edit to do so
	float TriangleAngle = atan2(y,x);
	//Serial.println("TriangleAngle: "+String(TriangleAngle));
	Matrix<4,4> T3 = Identity<4,4>();
	T3=RotateZ(T3,-1*TriangleAngle);

	//PrintMatrix(T3, "T3");
	Matrix<4,4> T4 =T3*T2;

	//PrintMatrix(T4, "T4");

	float C = T4(0,3);
	float A = links[1]->DH_R;
	float B = links[2]->DH_R;

	if((A+B)< C){ return OutsideOfWorkspace;}

	float Link1Theta = acos((pow(B,2)-pow(A,2)-pow(C,2))/(-(2*A*C)));

	//Serial.println("Link1Theta "+String(Link1Theta));
	Link1Theta= Link1Theta+links[1]->DH_Theta-TriangleAngle;

	Link1Theta*=-1;
	//Serial.println("Link1Theta "+String(Link1Theta));
	Result[1] = 180/3.14159*Link1Theta;

	float Link2Theta = acos((pow(C,2)-pow(A,2)-pow(B,2))/(-(2*A*B)));

	//Serial.println("Link2Theta "+String(Link2Theta));
	Link2Theta-= links[2]->DH_Theta;

	//Serial.println("Link2Theta "+String(Link2Theta));

	Link2Theta*=-1;
	Result[2] = 180/3.14159*Link2Theta;

	return IKSuccess;
}