// 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; } }
// This is an imitation of SD/FAST's sdrel2cart() subroutine. We // are given a station point S fixed to a body B. S is given by // the constant vector p_BS from B's origin Bo to point S, expressed // in B's frame. Denote the position of S in the ground frame G // p_GS = p_GB + p_BS_G, where p_BS_G=R_GB*p_BS is the vector p_BS // reexpressed in G. The velocity of S in G is v_GS = d/dt p_GS, taken // in G. So v_GS = v_GB + w_GB X p_BS_G = v_GB - p_BS_G % w_GB. // // We would like to obtain the partial velocity of S with respect to each of // the generalized speeds u, taken in the Ground frame, that is, // JS=d v_GS / du. (JS is a 3xnu matrix, or a single row of Vec3s.) We have // a method that can calculate J=d V_GB / du where V_GB=[w_GB;v_GB] is the // spatial velocity of B at its origin. So we need to calculate // d v_GS d v_GS d V_GB // JS = ------ = ------ * ------ = // du d V_GB du // // = [ -px | eye(3) ] * J // where px is the cross product matrix of p_BS_G and eye(3) is a 3x3 // identity matrix. // // This function should produce the same result as the SimbodyMatterSubsystem // method calcStationJacobian(). void sbrel2cart(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 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. Vector_<SpatialVec> J(nu); J = SpatialVec(Vec3(0), Vec3(0)); // or J.setToZero(); Vector u(nu); u = 0; Vector_<SpatialVec> Ju(nu); // d allV / d ui for (int i=0; i < nu; ++i) { u[i] = 1; matter.multiplyBySystemJacobian(state,u,Ju); u[i] = 0; J[i] = Ju[bodyIx]; // pick out the body of interest } Row<2,Mat33> dvdV( -crossMat(p_BS_G), Mat33(1) ); dvdu.resize(nu); for (int i=0; i < nu; ++i) dvdu[i] = dvdV * J[i]; // or J[i][0] % p_BS_G + J[i][1] }
// 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] } }
void ObservedPointFitter::findUpstreamBodies(MobilizedBodyIndex currentBodyIx, const Array_<int> numStations, const SimbodyMatterSubsystem& matter, Array_<MobilizedBodyIndex>& bodyIxs, int requiredStations) { const MobilizedBody& currentBody = matter.getMobilizedBody(currentBodyIx); if (currentBody.isGround()) return; MobilizedBodyIndex parentIx = currentBody.getParentMobilizedBody().getMobilizedBodyIndex(); requiredStations -= numStations[parentIx]; if (requiredStations > 0) findUpstreamBodies(parentIx, numStations, matter, bodyIxs, requiredStations); bodyIxs.push_back(parentIx); }