/// Gets the spatial constraints for this joint SMatrix6N& Joint::get_spatial_constraints(ReferenceFrameType rftype, SMatrix6N& s) { const unsigned X = 0, Y = 1, Z = 2; Real Cq[7]; // get the outboard link and its orientation quaternion RigidBodyPtr outboard = get_outboard_link(); assert(outboard); const Quat& q = outboard->get_orientation(); // resize the spatial constraint matrix s.resize(6, num_constraint_eqns()); // calculate the constraint Jacobian in relation to the outboard link for (unsigned i=0; i< num_constraint_eqns(); i++) { // calculate the constraint Jacobian calc_constraint_jacobian_rodrigues(outboard, i, Cq); // convert the differential quaternion constraints to an angular velocity // representation Vector3 omega = q.G_mult(Cq[3], Cq[4], Cq[5], Cq[6]) * (Real) 0.5; // setup the column of the constraint Jacobian s(0,i) = omega[X]; s(1,i) = omega[Y]; s(2,i) = omega[Z]; s(3,i) = Cq[X]; s(4,i) = Cq[Y]; s(5,i) = Cq[Z]; } // TODO: this should be in link-global frame -- fix this! assert(false); // convert to link frame if necessary (constraints computed in global frame) if (rftype == eLink) { SpatialTransform Xi0 = outboard->get_spatial_transform_global_to_link(); for (unsigned i=0; i< num_constraint_eqns(); i++) { SVector6 scol = s.get_column(i); scol = Xi0.transform(scol); s.set_column(i, scol); } } return s; }
/// Computes the time derivative of the constraint jacobian with respect to a body void SphericalJoint::calc_constraint_jacobian_dot_rodrigues(RigidBodyPtr body, unsigned index, Real Cq[7]) { const unsigned X = 0, Y = 1, Z = 2, SPATIAL_DIM = 7; // get the two links RigidBodyPtr inner = get_inboard_link(); RigidBodyPtr outer = get_outboard_link(); // mke sure that body is one of the links if (inner != body && outer != body) { for (unsigned i=0; i< SPATIAL_DIM; i++) Cq[i] = (Real) 0.0; return; } // setup the constraint equations (from Shabana, p. 432) if (body == inner) { // get the information necessary to compute the constraint equations const Quat& q = inner->get_orientation(); const Vector3& p = inner->get_outer_joint_data(outer).com_to_joint_vec; const Real px = p[X]; const Real py = p[Y]; const Real pz = p[Z]; Quat qd = Quat::deriv(q, inner->get_avel()); const Real dqw = qd.w; const Real dqx = qd.x; const Real dqy = qd.y; const Real dqz = qd.z; switch (index) { case 0: Cq[0] = 0.0; Cq[1] = 0.0; Cq[2] = 0.0; Cq[3] = 4*px*dqw + 2*pz*dqy - 2*py*dqz; Cq[4] = 4*dqx*px + 2*dqy*py + 2*dqz*pz; Cq[5] = 2*pz*dqw + 2*py*dqx; Cq[6] = 2*pz*dqx - 2*py*dqw; break; case 1: Cq[0] = 0.0; Cq[1] = 0.0; Cq[2] = 0.0; Cq[3] = 4*py*dqw - 2*pz*dqx + 2*px*dqz; Cq[4] = 2*dqy*px - 2*dqw*pz; Cq[5] = 2*px*dqx + 4*py*dqy + 2*pz*dqz; Cq[6] = 2*px*dqw + 2*pz*dqy; break; case 2: Cq[0] = 0.0; Cq[1] = 0.0; Cq[2] = 0.0; Cq[3] = 4*pz*dqw + 2*py*dqx - 2*px*dqy; Cq[4] = 2*dqz*px + 2*dqw*py; Cq[5] = 2*py*dqz - 2*px*dqw; Cq[6] = 4*pz*dqz + 2*py*dqy + 2*px*dqx; break; default: throw std::runtime_error("Invalid joint constraint index!"); } } else { // get the information necessary to compute the constraint equations const Quat& q = outer->get_orientation(); const Vector3& p = body->get_inner_joint_data(inner).joint_to_com_vec_of; const Real px = -p[X]; const Real py = -p[Y]; const Real pz = -p[Z]; Quat qd = Quat::deriv(q, outer->get_avel()); const Real dqw = qd.w; const Real dqx = qd.x; const Real dqy = qd.y; const Real dqz = qd.z; switch (index) { case 0: Cq[0] = 0.0; Cq[1] = 0.0; Cq[2] = 0.0; Cq[3] = -(4*px*dqw + 2*pz*dqy - 2*py*dqz); Cq[4] = -(4*dqx*px + 2*dqy*py + 2*dqz*pz); Cq[5] = -(2*pz*dqw + 2*py*dqx); Cq[6] = -(2*pz*dqx - 2*py*dqw); break; case 1: Cq[0] = 0.0; Cq[1] = 0.0; Cq[2] = 0.0; Cq[3] = -(4*py*dqw - 2*pz*dqx + 2*px*dqz); Cq[4] = -(2*dqy*px - 2*dqw*pz); Cq[5] = -(2*px*dqx + 4*py*dqy + 2*pz*dqz); Cq[6] = -(2*px*dqw + 2*pz*dqy); break; case 2: Cq[0] = 0.0; Cq[1] = 0.0; Cq[2] = 0.0; Cq[3] = -(4*pz*dqw + 2*py*dqx - 2*px*dqy); Cq[4] = -(2*dqz*px + 2*dqw*py); Cq[5] = -(2*py*dqz - 2*px*dqw); Cq[6] = -(4*pz*dqz + 2*py*dqy + 2*px*dqx); break; default: throw std::runtime_error("Invalid joint constraint index!"); } } }