// 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]
}
// 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]
}
}
// Constructor from just gravity magnitude; use negative of System "up"
// direction as the down direction here.
GravityImpl(const SimbodyMatterSubsystem& matter,
Real magnitude,
Real zeroHeight)
: matter(matter)
{ // Invoke the general constructor using system up direction.
new (this) GravityImpl(matter, -matter.getSystem().getUpDirection(),
magnitude, zeroHeight);
}
// Constructor from a direction and magnitude.
GravityImpl(const SimbodyMatterSubsystem& matter,
const UnitVec3& direction,
Real magnitude,
Real zeroHeight)
: matter(matter), defDirection(direction), defMagnitude(magnitude),
defZeroHeight(zeroHeight),
defMobodIsImmune(matter.getNumBodies(), false),
numEvaluations(0)
{ defMobodIsImmune.front() = true; } // Ground is always immune
// Constructor from a gravity vector, which might have zero magnitude.
// In that case we'll negate the default up direction in the System as the
// default direction. That would only be noticeable if the magnitude were
// later increased without giving a new direction.
GravityImpl(const SimbodyMatterSubsystem& matter,
const Vec3& gravityVec,
Real zeroHeight)
: matter(matter)
{ // Invoke the general constructor using system up direction.
new (this) GravityImpl(matter, -matter.getSystem().getUpDirection(),
gravityVec.norm(), zeroHeight);
if (defMagnitude > 0)
defDirection=UnitVec3(gravityVec/defMagnitude,true);
}
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);
}