// 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] } }