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

#include <iostream>
#include <cmath>
#include <algorithm>
#include <functional>
#include <string>
using namespace std;
const double ARM1_LENGTH=.87f;
const double ARM2_LENGTH=.59f;
double CurrentPosition[3] = [theta1, theta2, theta3];
//double Destination[3] = [1,1,1]; // also for compiling
//int CurrentPosition[3]=[0,0,0]; // for input data to compile
double P[3] = CurrentPosition[3], BMNewThetas[3];
int main()
{
BMlastD = Distance(CurrentPosition[3], Destination[3]); // testing
BMNewThetas[3] = BM(P[3]); // testing
BMNewD = Distance (BMNewThetas[3]); // testing
while (BMlastD != BMNewD)
{
BMlastD = BMNewD;
BMNewThetas[3] = BM(BMNewThetas[3]);
BMNewD = Distance(forwardkinematics(BMNewThetas[3]), Destination[3]);
}
return (BMNewThetas[3])
}
{
double forwardkinematics (double T[3])
x = cos(T[0])*(ARM1_LENGTH*(cos(T[1]))+(ARM2_LENGTH)*cos(T[1]+T[2]));
y = sin(T[0])*(ARM1_LENGTH*(cos(T[1]))+(ARM2_LENGTH)*cos(T[1]+T[2]));
z = (ARM1_LENGTH)*sin(T[1])+ARM2_LENGTH(sin(T[1]+T[2]));
return([x,y,z]);
}
double distance (double A[3],double B[3])
{
return sqrt((B[2]A[2])*(B[2]A[2])+(B[1]A[1])*(B[1]A[1])+(B[0]A[0])*(B[0]A[0]));
}
{
double BM(double T[3])
}
{
double theta1=T[0], theta2=T[1], theta3=T[2];
P[3] = forwardkinematics(T[3]);
shortestdistance = distance (P[3]);
int besti, bestj, bestk;
for(int i = 1; i<2; i++)
{
for(int j = 1; j<2; j++)
{
for(int k = 1; k<2; k++)
{
P[3] = forwardkinematics([theta1+i, theta2+j, theta3+k]);
if (distance(P) < shortestdistance)
{
besti = i; bestj = j; bestk = k;
}
}
}
}
return ([theta1+besti, theta2+bestj, theta3+bestk]);
}
[code]
 