ORBITER-FORUM will be temporarily closed at 2026-07-23 18:00 UTC while we complete some OF maintenance tasks. The amount of downtime is expected to take up to one hour, but probably less.
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;
}
//IK setup
VECTOR3 shoulder_pos = _V(-9.784, -2.10, 2.05);
VECTOR3 elbow_pos = _V(-3.3, -2.26, 1.7);
VECTOR3 wrist_pos = _V(3.55, -2.26, 1.7);
elbow_pos -= shoulder_pos;
wrist_pos -= shoulder_pos;
shoulder_pos -= shoulder_pos;
lu = length(elbow_pos);
ll = length(elbow_pos - wrist_pos);
shoulder_neutral = 0.0136; //In anim coordinate
shoulder_range = 147; //in deg
shoulder_min = shoulder_range*-shoulder_neutral; //Min angle, deg
shoulder_max = shoulder_range*(1 - shoulder_neutral); //Max angle, deg
elbow_neutral = 0.0123;
elbow_range = 162;
elbow_min = elbow_range*-elbow_neutral; //Min angle, deg
elbow_max = elbow_range*(1 - elbow_neutral); //Max angle, deg
wrist_neutral = 0.5; //In anim coordinate
wrist_range = 240; //in deg
wrist_min = wrist_range*-wrist_neutral; //Min angle, deg
wrist_max = wrist_range*(1 - wrist_neutral); //Max angle, deg
arm_wrist_pos = wrist_pos;
so a good thing
I was running the SSU and was able to switch the modes but still not able to move the arm? I did the CTRL A and saw controlling rms.
I think you are trying to move both attachment points, when you only need to move one.
OBSS_ATTACHMENT_POINT+MPM_MESH_OFFSET, _V(0,1,0), _V(0,0,1), "OBSS");
armobss_tip[0] = OBSS_POS;
armobss_tip[1] = OBSS_POS + _V(0.0, 1.0, 0.0);
armobss_tip[2] = OBSS_POS + _V(1.0, 0.0, 0.0);
SetAttachmentParams(OBSS_attach, armobss_tip[0], armobss_tip[1] - armobss_tip[0], armobss_tip[2] - armobss_tip[0]);
//mpmobss
static UINT OBSSmpm[1] = { 0 };
static MGROUP_ROTATE OBSS_A(7, OBSSmpm, 1,_V(2.544, 1.187, 9.613), _V(0, 0, 1), (float)(-30 * RAD)); // -180 .. +180
anim_MPMOBSS = CreateAnimation(0.0);
obssparent = AddAnimationComponent(anim_MPMOBSS, 0, 1, &OBSS_A);
obss_anim[0] = new MGROUP_ROTATE(LOCALVERTEXLIST, MAKEGROUPARRAY(armobss_tip), 3, OBSS_POS, _V(0, 0, 1), (float)(0.0));
AddAnimationComponent(anim_MPMOBSS, 0, 1, obss_anim[0], obssparent);
[CHILD_ATTACH_0] ; To MPM's
POS = (-0.13,0.32,2.210)
DIR = (0,-1,0)
ROT = (1,0,0)
ID = "OS"
P -0.05 -0.275 1.47 0 -1 0 0 0 1 OS ; To MPM's
Someone rotated it, of course.:yes: