Unverified Commit 67341423 authored by Levi Armstrong's avatar Levi Armstrong Committed by GitHub
Browse files

Merge pull request #372 from qqfly/patch-1

simplify FK equations
parents 1a959407 d3ac1ab3
......@@ -46,33 +46,25 @@ namespace ur_kinematics {
void forward(const double* q, double* T) {
double s1 = sin(*q), c1 = cos(*q); q++;
double q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
double s3 = sin(*q), c3 = cos(*q); q234 += *q; q++;
q234 += *q; q++;
double q23 = *q, q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
double s3 = sin(*q), c3 = cos(*q); q23 += *q; q234 += *q; q++;
double s4 = sin(*q), c4 = cos(*q); q234 += *q; q++;
double s5 = sin(*q), c5 = cos(*q); q++;
double s6 = sin(*q), c6 = cos(*q);
double s23 = sin(q23), c23 = cos(q23);
double s234 = sin(q234), c234 = cos(q234);
*T = ((c1*c234-s1*s234)*s5)/2.0 - c5*s1 + ((c1*c234+s1*s234)*s5)/2.0; T++;
*T = (c6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0) -
(s6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0); T++;
*T = (-(c6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0 -
s6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0)); T++;
*T = ((d5*(s1*c234-c1*s234))/2.0 - (d5*(s1*c234+c1*s234))/2.0 -
d4*s1 + (d6*(c1*c234-s1*s234)*s5)/2.0 + (d6*(c1*c234+s1*s234)*s5)/2.0 -
a2*c1*c2 - d6*c5*s1 - a3*c1*c2*c3 + a3*c1*s2*s3); T++;
*T = c1*c5 + ((s1*c234+c1*s234)*s5)/2.0 + ((s1*c234-c1*s234)*s5)/2.0; T++;
*T = (c6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0) +
s6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0)); T++;
*T = (c6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0) -
s6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0)); T++;
*T = ((d5*(c1*c234-s1*s234))/2.0 - (d5*(c1*c234+s1*s234))/2.0 + d4*c1 +
(d6*(s1*c234+c1*s234)*s5)/2.0 + (d6*(s1*c234-c1*s234)*s5)/2.0 + d6*c1*c5 -
a2*c2*s1 - a3*c2*c3*s1 + a3*s1*s2*s3); T++;
*T = ((c234*c5-s234*s5)/2.0 - (c234*c5+s234*s5)/2.0); T++;
*T = ((s234*c6-c234*s6)/2.0 - (s234*c6+c234*s6)/2.0 - s234*c5*c6); T++;
*T = (s234*c5*s6 - (c234*c6+s234*s6)/2.0 - (c234*c6-s234*s6)/2.0); T++;
*T = (d1 + (d6*(c234*c5-s234*s5))/2.0 + a3*(s2*c3+c2*s3) + a2*s2 -
(d6*(c234*c5+s234*s5))/2.0 - d5*c234); T++;
*T = c234*c1*s5 - c5*s1; T++;
*T = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; T++;
*T = -s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; T++;
*T = d6*c234*c1*s5 - a3*c23*c1 - a2*c1*c2 - d6*c5*s1 - d5*s234*c1 - d4*s1; T++;
*T = c1*c5 + c234*s1*s5; T++;
*T = -c6*(c1*s5 - c234*c5*s1) - s234*s1*s6; T++;
*T = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1; T++;
*T = d6*(c1*c5 + c234*s1*s5) + d4*c1 - a3*c23*s1 - a2*c2*s1 - d5*s234*s1; T++;
*T = -s234*s5; T++;
*T = -c234*s6 - s234*c5*c6; T++;
*T = s234*c5*s6 - c234*c6; T++;
*T = d1 + a3*s23 + a2*s2 - d5*(c23*c4 - s23*s4) - d6*s5*(c23*s4 + s23*c4); T++;
*T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment