// Using the method of sbrel2cart3() but with 6 J*v multiplies, this gives
// the full 6xnu "Frame Jacobian" for one body for a specified frame on that
// body (only the origin, not the orientation, matters).
// This should produce the same result as the built-in calcFrameJacobian()
// method.
void sbrel2cart4(const State& state,
const SimbodyMatterSubsystem& matter,
MobilizedBodyIndex bodyIx,
const Vec3& p_BS, // point in body frame
RowVector_<SpatialVec>& dVdu) // V is [w,v] in G
{
const int nu = state.getNU();
const int nb = matter.getNumBodies(); // includes ground
const MobilizedBody& mobod = matter.getMobilizedBody(bodyIx);
const Vec3 p_BS_G = mobod.expressVectorInGroundFrame(state, p_BS);
// Calculate J=dVdu where V is spatial velocity of p_BS.
// (This is six rows of J.)
dVdu.resize(nu);
Vector_<SpatialVec> F(nb, SpatialVec(Vec3(0)));
SpatialVec& Fb = F[bodyIx]; // the only one we'll change
Vector col(nu); // temporary to hold column of J^T
// Rotational part.
for (int i=0; i < 3; ++i) {
Fb[0][i] = 1;
matter.multiplyBySystemJacobianTranspose(state,F,col);
for (int r=0; r < nu; ++r) dVdu[r][0][i] = col[r];
Fb[0][i] = 0;
}
// Translational part.
for (int i=0; i < 3; ++i) {
Fb[1][i] = 1;
Fb[0] = p_BS_G % Fb[1]; // r X F
matter.multiplyBySystemJacobianTranspose(state,F,col);
for (int r=0; r < nu; ++r) dVdu[r][1][i] = col[r];
Fb[1][i] = 0;
}
}
// Another way to calculate exactly what sbrel2cart() does -- can we do it
// faster using f=J^T*F rather than V=J*u?
void sbrel2cart2(const State& state,
const SimbodyMatterSubsystem& matter,
MobilizedBodyIndex bodyIx,
const Vec3& p_BS, // point in body frame
RowVector_<Vec3>& dvdu) // v is dS/dt in G
{
const int nu = state.getNU();
const int nb = matter.getNumBodies(); // includes ground
const MobilizedBody& mobod = matter.getMobilizedBody(bodyIx);
const Vec3 p_BS_G = mobod.expressVectorInGroundFrame(state, p_BS);
// Calculate J=dVdu where V is spatial velocity of body origin.
// (This is one row of J.)
Matrix Jt(nu, 6); // a column of Jt but with scalar elements
Vector_<SpatialVec> F(nb, SpatialVec(Vec3(0)));
SpatialVec& Fb = F[bodyIx]; // the only one we'll change
for (int which=0; which < 2; ++which) { // moment, force
for (int i=0; i < 3; ++i) {
Fb[which][i] = 1;
VectorView col = Jt(3*which + i);
matter.multiplyBySystemJacobianTranspose(state,F,col);
Fb[which][i] = 0;
}
}
Row<2,Mat33> dvdV( -crossMat(p_BS_G), Mat33(1) );
dvdu.resize(nu);
for (int i=0; i < nu; ++i) {
const RowVectorView r = Jt[i];
SpatialVec V(Vec3::getAs(&r[0]), Vec3::getAs(&r[3]));
dvdu[i] = dvdV * V; // or J[i][0] % p_BS_G + J[i][1]
}
}
