/// Updates the spatial axis for this joint
void ScrewJoint::update_spatial_axes()
{
const unsigned X = 0, Y = 1, Z = 2;
// call parent method
Joint::update_spatial_axes();
// make sure the outboard link exists
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
if (!inboard || !outboard)
return;
try
{
// get the joint to com vector in outer link coordinates
const Vector3d& di = outboard->get_inner_joint_data(inboard).joint_to_com_vec_of;
// get the joint axis in outer link coordinates
Vector3d u0 = inboard->get_transform().mult_vector(_u);
Vector3d ui = outboard->get_transform().transpose_mult_vector(u0);
// update the spatial axis in link coordinates
Vector3d x = Vector3d::cross(ui, di);
_s[0].set_angular(ui);
_s[0].set_lower(ui*_pitch + x);
// setup s_bar
calc_s_bar_from_s();
}
catch (std::runtime_error e)
{
// do nothing -- joint data has not yet been set in the link
}
}
/**
* \note these spatial axes are not constant, unlike many joints.
*/
const SMatrix6N& SphericalJoint::get_spatial_axes(ReferenceFrameType rftype)
{
const unsigned X = 0, Y = 1, Z = 2;
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
if (!inboard)
throw std::runtime_error("SphericalJoint::get_spatial_axes() called with NULL inboard link");
if (!outboard)
throw std::runtime_error("SphericalJoint::get_spatial_axes() called with NULL outboard link");
// get current values of q
const VectorN& q = this->q;
const VectorN& q_tare = this->_q_tare;
// get the outboard link's joint to com vector in outer link coordinates
const Vector3& p = outboard->get_inner_joint_data(inboard).joint_to_com_vec_of;
// get the set of spatial axes
Real c1 = std::cos(q[DOF_1]+q_tare[DOF_1]);
Real c2 = std::cos(q[DOF_2]+q_tare[DOF_2]);
Real s1 = std::sin(q[DOF_1]+q_tare[DOF_1]);
Real s2 = std::sin(q[DOF_2]+q_tare[DOF_2]);
// form untransformed spatial axes -- this are the vectors describing each axis, after
// rotation by preceding axis/axes; note that first axis always points toward 1,0,0
Vector3 uu2(0, c1, s1);
Vector3 uu3(s2, -c2*s1, c1*c2);
// transform the spatial axes into the outer link frame
Vector3 u1;
_R.get_column(X, u1.begin());
Vector3 u2 = _R * uu2;
Vector3 u3 = _R * uu3;
// update the spatial axis in link coordinates
SVector6 si1, si2, si3;
si1.set_upper(u1);
si2.set_upper(u2);
si3.set_upper(u3);
si1.set_lower(Vector3::cross(u1, p));
si2.set_lower(Vector3::cross(u2, p));
si3.set_lower(Vector3::cross(u3, p));
_si.set_column(eAxis1, si1);
_si.set_column(eAxis2, si2);
_si.set_column(eAxis3, si3);
// setup the complement of the spatial axes in link coordinates
calc_s_bar_from_si();
// use the Joint function to do the rest
return Joint::get_spatial_axes(rftype);
}
/**
* \note these spatial axes are not constant, unlike many joints.
*/
const SMatrix6N& SphericalJoint::get_spatial_axes_dot(ReferenceFrameType rftype)
{
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
if (!inboard)
throw std::runtime_error("SphericalJoint::get_spatial_axes_dot() called with NULL inboard link");
if (!outboard)
throw std::runtime_error("SphericalJoint::get_spatial_axes_dot() called with NULL outboard link");
// get q, _q_tare, and qd
const VectorN& q = this->q;
const VectorN& q_tare = this->_q_tare;
const VectorN& qd = this->qd;
// get the two transformed axes
Real c1 = std::cos(q[DOF_1]+q_tare[DOF_1]);
Real c2 = std::cos(q[DOF_2]+q_tare[DOF_2]);
Real s1 = std::sin(q[DOF_1]+q_tare[DOF_1]);
Real s2 = std::sin(q[DOF_2]+q_tare[DOF_2]);
Real qd1 = qd[DOF_1];
Real qd2 = qd[DOF_2];
// form the time derivatives of the non-constant spatial axes (untransformed)
Vector3 uu2(0, -s1*qd1, c1*qd1);
Vector3 uu3(c2*qd2, -c2*c1*qd1 + s2*s1*qd2, -c1*s2*qd2 - s1*c2*qd1);
// transform the axes into outer link coordinates
Vector3 u2 = _R * uu2;
Vector3 u3 = _R * uu3;
// get the outboard link's joint to com vector in outer link coordinates
const Vector3& p = outboard->get_inner_joint_data(inboard).joint_to_com_vec_of;
// update the spatial axis in link coordinates; note that third column of spatial axis
// derivative set to zero in constructor and is never modified
SVector6 si2, si3;
si2.set_upper(u2);
si3.set_upper(u3);
si2.set_lower(Vector3::cross(u2, p));
si3.set_lower(Vector3::cross(u3, p));
_si_dot.set_column(eAxis2, si2);
_si_dot.set_column(eAxis3, si3);
// transform to global coordinates
SpatialTransform X_0_i = outboard->get_spatial_transform_link_to_global();
X_0_i.transform(_si_dot, _s0_dot);
return (rftype == eLink) ? _si_dot : _s0_dot;
}
/// 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!");
}
}
}
