bool calcLegAngles(double** out_q) {
rpy = rpyFromRot(R);
cy = cos(rpy[2]);
sy = sin(rpy[2]);
cp = cos(rpy[1]);
sp = sin(rpy[1]);
cr = cos(rpy[0]);
sr = sin(rpy[0]);
Vector3 O1(0.0, sign * robot->l0, -robot->l1);
const Vector3& F = p;
Vector3 V1 = F - O1;
Vector3 XF(cy * cp, sy * cp, -sp);
Vector3 V1xXF = V1.cross(XF);
Vector3 Z2 = -sign * V1xXF / V1xXF.norm();
q[0] = atan2(-sign * Z2[0], sign * Z2[1]);
q[1] = asin(-sign * Z2[2]);
double c1 = cos(q[0]);
double s1 = sin(q[0]);
double c2 = cos(q[1]);
double s2 = sin(q[1]);
double s1s2 = s1 * s2;
double c1s2 = c1 * s2;
slo1 = sign * robot->lo1;
TB2 <<
s1s2, -sign * c1, -sign * s1 * c2, slo1 * s1 + robot->l2 * c1 + robot->lo2 * s1s2,
-c1s2, -sign * s1, sign * c1 * c2, sign * robot->l0 - slo1 * c1 + robot->l2 * s1 - robot->lo2 * c1s2,
-c2, 0.0, -sign * s2, -robot->l1 - robot->lo2 * c2,
0.0, 0.0, 0.0, 1.0;
Vector3 V2 = (TB2.inverse() * Vector4d(F[0], F[1], F[2], 1.0)).head(3);
double D = (V2.squaredNorm() - robot->l3 * robot->l3 - robot->l4 * robot->l4) / (2.0 * robot->l3 * robot->l4);
if(fabs(D) > 1.0){
return false;
}
q[3] = atan2(-sign * sqrt(1.0 - D * D), D);
double c4 = cos(q[3]);
double s4 = sin(q[3]);
double beta = atan2(-V2[1], sqrt(V2[0] * V2[0] + V2[2] * V2[2]));
double alpha = atan2(robot->l4 * s4, robot->l3 + robot->l4 * c4);
q[2] = -(beta - alpha);
q[3] = -q[3];
double c3 = cos(q[2]);
double s3 = sin(q[2]);
double q2q3 = q[2] + q[3];
double c34 = cos(q2q3);
double s34 = sin(q2q3);
Matrix4d T24;
T24 <<
c34, s34, 0, robot->l3 * c3 + robot->l4 * c34,
s34, -c34, 0, robot->l3 * s3 + robot->l4 * s34,
0.0, 0.0, -1, 0.0,
0.0, 0.0, 0, 1.0;
TB4.noalias() = TB2 * T24;
double spsr = sp * sr;
double spcr = sp * cr;
TBF <<
cy * cp, -sy * cr + cy * spsr, sy * sr + cy * spcr, p.x(),
sy * cp, cy * cr + sy * spsr, -cy * sr + sy * spcr, p.y(),
-sp, cp * sr, cp * cr, p.z(),
0, 0, 0, 1.0;
Matrix4d T4F;
T4F.noalias() = TB4.inverse() * TBF;
q[4] = atan2(-sign * T4F(0,0), sign * T4F(1,0));
q[5] = atan2( sign * T4F(2,2), -sign * T4F(2,1));
// Numerical refining
TB0 <<
0.0, -1.0, 0.0, 0.0,
1.0, 0.0, 0.0, sign * robot->l0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0;
Af <<
0.0, 1.0, 0.0, 0.0,
-1.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0;
bool solved = false;
int i;
for(i=0; i < 30; ++i){
if(calcEndPositionDifference()){
solved = true;
break;
}
// Jacobian Calculation
Vector3 Z0 = TB0.block<3,1>(0,2);
Vector3 Z1 = TB1.block<3,1>(0,2);
Vector3 Z2 = TB2.block<3,1>(0,2);
Vector3 Z3 = TB3.block<3,1>(0,2);
Vector3 Z4 = TB4.block<3,1>(0,2);
Vector3 Z5 = TB5.block<3,1>(0,2);
Vector3 O0 = TB0.block<3,1>(0,3);
Vector3 O1 = TB1.block<3,1>(0,3);
Vector3 O2 = TB2.block<3,1>(0,3);
Vector3 O3 = TB3.block<3,1>(0,3);
Vector3 O4 = TB4.block<3,1>(0,3);
Vector3 O5 = TB5.block<3,1>(0,3);
Vector3 O6 = TB6.block<3,1>(0,3);
Matrix6 J;
J <<
Z0.cross(O6 - O0), Z1.cross(O6 - O1), Z2.cross(O6 - O2), Z3.cross(O6 - O3), Z4.cross(O6 - O4), Z5.cross(O6 - O5),
Z0, Z1, Z2, Z3, Z4, Z5 ;
// Levenberg-Marquardt Method
const double lambda = 0.001;
Matrix6 C;
C.noalias() = J.transpose() * (J * J.transpose() + Matrix6::Identity() * lambda * lambda).inverse();
dq.noalias() = C * dp;
q += dq;
if(dq.norm() <= 1.0e-5){
break;
}
}
if(!solved){
solved = calcEndPositionDifference();
}
if(solved){
*out_q[0] = q[0];
*out_q[1] = q[1];
*out_q[2] = q[2] + sign * robot->q2q3offset;
*out_q[3] = q[3] - sign * robot->q2q3offset;
*out_q[4] = q[4];
*out_q[5] = q[5];
}
return solved;
}
bool calcEndPositionDifference() {
// Direct Kinematics Error Calculation
double c1 = cos(q[0]);
double s1 = sin(q[0]);
double c2 = cos(q[1]);
double s2 = sin(q[1]);
double c3 = cos(q[2]);
double s3 = sin(q[2]);
double c4 = cos(q[3]);
double s4 = sin(q[3]);
double c5 = cos(q[4]);
double s5 = sin(q[4]);
double c6 = cos(q[5]);
double s6 = sin(q[5]);
Matrix4d A1, A2, A3, A4, A5, A6;
A1 <<
c1, 0.0, -s1, -slo1 * c1,
s1, 0.0, c1, -slo1 * s1,
0.0, -1.0, 0.0, -robot->l1,
0.0, 0.0, 0.0, 1.0;
A2 <<
-s2, 0.0, sign * c2, -robot->lo2 * s2,
c2, 0.0, sign * s2, robot->lo2 * c2,
0.0, sign, 0.0, -robot->l2,
0.0, 0.0, 0.0, 1.0;
A3 <<
c3, -s3, 0.0, robot->l3 * c3,
s3, c3, 0.0, robot->l3 * s3,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0;
A4 <<
c4, s4, 0.0, robot->l4 * c4,
s4, -c4, 0.0, robot->l4 * s4,
0.0, 0.0, -1.0, 0.0,
0.0, 0.0, 0.0, 1.0;
A5 <<
c5, 0.0, -sign * s5, -robot->lo5 * c5,
s5, 0.0, sign * c5, -robot->lo5 * s5,
0.0, -sign, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0;
A6 <<
-s6, 0.0, -c6, 0.0,
c6, 0.0, -s6, 0.0,
0.0, -1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0;
TB1.noalias() = TB0 * A1;
TB2.noalias() = TB1 * A2;
TB3.noalias() = TB2 * A3;
TB4.noalias() = TB3 * A4;
TB5.noalias() = TB4 * A5;
TB6.noalias() = TB5 * A6;
TBF.noalias() = TB6 * Af;
double yaw = atan2(TBF(1,0), TBF(0,0));
double pitch = asin(-TBF(2,0));
double roll = atan2(TBF(2,1), TBF(2,2));
double dYaw = rpy[2] - yaw;
double dPitch = rpy[1] - pitch;
double dRoll = rpy[0] - roll;
Vector3 pi = TBF.block<3,1>(0,3);
dp <<
(p - pi),
(-sy * dPitch + cy * cp * dRoll),
(cy * dPitch + sy * cp * dRoll),
(dYaw - sp * dRoll);
double errsqr = dp.head(3).squaredNorm() + dp.tail(3).squaredNorm();
return (errsqr < 1.0e-6 * 1.0e-6);
}
