VECTOR3 Atlantis::CalcAnimationFKArm() {
//Do forward kinematics to get the current position of the wrist joint
double current_phi_s = linterp(0, shoulder_min, 1, shoulder_max, arm_sp);
double current_phi_e = linterp(0, elbow_min, 1, elbow_max, arm_ep);
double current_beta_s = linterp(0, -180, 1, 180, arm_sy);
double current_phi_l = current_phi_s - current_phi_e;
double rho_e = lu*cos(RAD*current_phi_s);
double z_e = lu*sin(RAD*current_phi_s);
double rho_w = rho_e + ll*cos(RAD*current_phi_l);
double z_w = z_e + ll*sin(RAD*current_phi_l);
double x_w = rho_w*cos(RAD*current_beta_s);
double y_w = rho_w*sin(RAD*current_beta_s);
// sprintf(oapiDebugString(),"ll %f lu %f arm_sy %f beta_s %f arm_sp %f phi_s %f arm_ep %f phi_e %f FK %f,%f,%f",ll,lu,arm_sy,current_beta_s,arm_sp,current_phi_s,arm_ep,current_phi_e,x_w,y_w,z_w);
return _V(x_w, y_w, z_w);
}
void Atlantis::SetAnimationIKArm(VECTOR3 arm_wrist_dpos) {
arm_wrist_pos = CalcAnimationFKArm();
//Candidate position. Calculate the joints on it...
VECTOR3 arm_wrist_cpos = arm_wrist_pos + arm_wrist_dpos;
double r = length(arm_wrist_cpos);
double beta_s = DEG*atan2(arm_wrist_cpos.y, arm_wrist_cpos.x);
double rho = sqrt(arm_wrist_cpos.x*arm_wrist_cpos.x + arm_wrist_cpos.y*arm_wrist_cpos.y);
double cos_phibar_e = (r*r - lu*lu - ll*ll) / (-2 * lu*ll);
if (fabs(cos_phibar_e)>1) return;//Can't reach new point with the elbow
double phi_e = 180 - DEG*acos(cos_phibar_e);
double cos_phi_s2 = (ll*ll - lu*lu - r*r) / (-2 * lu*r);
if (fabs(cos_phi_s2)>1) return; //Can't reach with shoulder
double phi_s = DEG*(atan2(arm_wrist_cpos.z, rho) + acos(cos_phi_s2));
double anim_phi_s = linterp(shoulder_min, 0, shoulder_max, 1, phi_s);
double anim_phi_e = linterp(elbow_min, 0, elbow_max, 1, phi_e);
double anim_beta_s = linterp(-180, 0, 180, 1, beta_s);
if (anim_beta_s<0 || anim_beta_s>1) return;
if (anim_phi_s<0 || anim_phi_s>1) return;
if (anim_phi_e<0 || anim_phi_e>1) return;
//but only keep it and set the joints if no constraints are violated.
//Limited IK on the wrist
double new_phi_l = phi_s - phi_e;
double current_phi_w = linterp(0, wrist_min, 1, wrist_max, arm_wp);
double current_phi_s = linterp(0, shoulder_min, 1, shoulder_max, arm_sp);
double current_phi_e = linterp(0, elbow_min, 1, elbow_max, arm_ep);
double current_beta_s = linterp(0, -180, 1, 180, arm_sy);
double current_phi_l = current_phi_s - current_phi_e;
double new_phi_w = current_phi_w - new_phi_l + current_phi_l;
double anim_phi_w = linterp(wrist_min, 0, wrist_max, 1, new_phi_w);
arm_sy = anim_beta_s;
SetAnimationArm(anim_arm_sy, arm_sy);
arm_sp = anim_phi_s;
SetAnimationArm(anim_arm_sp, arm_sp);
arm_ep = anim_phi_e;
SetAnimationArm(anim_arm_ep, arm_ep);
arm_wp = anim_phi_w;
SetAnimationArm(anim_arm_wp, arm_wp);
arm_wrist_pos = arm_wrist_cpos;
}