std::vector<double> darwinop_kinematics_inverse_larm(const double *dArm)
{
std::vector<double> qArm(3,-999); // Init the 3 angles with value 0
//printf("\n");
double dx = dArm[0];
double dy = dArm[1] - shoulderOffsetY;
double dz = dArm[2] - shoulderOffsetZ;
double dc_sq = pow(dx,2)+pow(dy,2)+pow(dz,2);
//printf("dx :%.3f, dy:%.3f, dz: %.3f\n",dx,dy,dz);
// Get the elbow angle
double tmp = (pow(upperArmLength,2)+pow(lowerArmLength,2)-dc_sq) / (2*upperArmLength*lowerArmLength);
//double dc = sqrt( dc_sq );
//printf("dc: %.3f, tmp: %.3f\n",dc,tmp);
tmp = tmp>1 ? 1 : tmp;
tmp = tmp<-1 ? -1 : tmp;
double qbow = acos( tmp );
//printf("tmp: %.3f, qbow: %.2f\n",tmp,qbow*180/PI);
qArm[2] = -1*(PI - qbow);
// Get to the correct y coordinate
double hyp = upperArmLength+lowerArmLength*cos(PI-qbow);
tmp = dy / hyp;
tmp = tmp>1?1:tmp;
tmp = tmp<-1?-1:tmp;
double qroll = asin( tmp );
//printf("dy: %.2f, hyp: %.2f\n",dy,hyp);
//printf("qroll: %.2f\n",qroll*180/PI);
qArm[1] = qroll;
/*
tmp = (pow(hyp,2)-pow(dy,2));
double x0 = sqrt( tmp<0?0:tmp );
double z0 = lowerArmLength*sin(PI-qbow);
double th0 = atan2(z0,x0);
//printf("x0: %.2f, z0: %.2f\n",x0,z0);
//printf("th0: %.2f, dth: %.2f\n",th0*180/PI,dth*180/PI);
*/
qArm[0] = 0;
Transform tbow = darwinop_kinematics_forward_larm(&qArm[0]);
double dz0 = tbow(2,3) - shoulderOffsetZ;
double th0 = atan2( dz0, tbow(0,3) );
//double th0 = atan2( tbow(2,3), tbow(0,3) );
// Want to be at this theta
double dth = atan2(dz,dx);
//printf("qArm: %.2f, %.2f, %.2f\n",qArm[0],qArm[1],qArm[2]);
//printf("\n");
//printTransform(tbow);
//printf("th0: %.2f, dth: %.2f\n",th0*180/PI,dth*180/PI);
qArm[0] = -1*(dth-th0);
return qArm;
}
static int forward_larm(lua_State *L) {
std::vector<double> q = lua_checkvector(L, 1);
Transform t = darwinop_kinematics_forward_larm(&q[0]);
lua_pushtransform(L, t);
return 1;
}
