/// Setup the transform from one joint to another void FIXEDJOINT::setup_joint() { const unsigned X = 0, Y = 1, Z = 2; shared_ptr<const POSE3> GLOBAL; // get the two poses shared_ptr<const POSE3> inboard = get_inboard_pose(); shared_ptr<const POSE3> outboard = get_outboard_pose(); if (!inboard || !outboard) return; // get the rotation matrices MATRIX3 Ri = inboard->q; MATRIX3 Ro = outboard->q; // TODO: fix this // compute the relative transform /* MATRIX3 Rrel = MATRIX3::transpose(Ro) * Ri; _T->q = Rrel; _T->x.set_zero(); _T->source = ; // compute the vector from the inner link to the outer link in inner link // frame VECTOR3 ox(outboard->x, To); // _ui = inboard->inverse_transform(ox); */ // compute the constant orientation term _rconst[X] = VECTOR3(Ri.get_row(X), GLOBAL).dot(VECTOR3(Ro.get_row(X), GLOBAL)); _rconst[Y] = VECTOR3(Ri.get_row(Y), GLOBAL).dot(VECTOR3(Ro.get_row(Y), GLOBAL)); _rconst[Z] = VECTOR3(Ri.get_row(Z), GLOBAL).dot(VECTOR3(Ro.get_row(Z), GLOBAL)); }