/// 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!");
}
}
}
