// goal = 1/2 sum( wi * ai^2 ) / sum(wi) for WRMS
// ai == rotation angle between sensor and observation (-Pi:Pi)
int OrientationSensors::calcGoal(const State& state, Real& goal) const {
const SimbodyMatterSubsystem& matter = getMatterSubsystem();
goal = 0;
// Loop over each body that has one or more active osensors.
Real wtot = 0;
PerBodyOSensors::const_iterator bodyp = bodiesWithOSensors.begin();
for (; bodyp != bodiesWithOSensors.end(); ++bodyp) {
const MobilizedBodyIndex mobodIx = bodyp->first;
const Array_<OSensorIx>& bodyOSensors = bodyp->second;
const MobilizedBody& mobod = matter.getMobilizedBody(mobodIx);
const Rotation& R_GB = mobod.getBodyRotation(state);
assert(bodyOSensors.size());
// Loop over each osensor on this body.
for (unsigned m=0; m < bodyOSensors.size(); ++m) {
const OSensorIx mx = bodyOSensors[m];
const OSensor& osensor = osensors[mx];
assert(osensor.bodyB == mobodIx); // better be on this body!
const Rotation& R_GO = observations[getObservationIxForOSensor(mx)];
if (R_GO.isFinite()) { // skip NaNs
const Rotation R_GS = R_GB * osensor.orientationInB;
const Rotation R_SO = ~R_GS*R_GO; // error, in S
const Vec4 aa_SO = R_SO.convertRotationToAngleAxis();
goal += osensor.weight * square(aa_SO[0]);
wtot += osensor.weight;
}
}
}
goal /= (2*wtot);
return 0;
}
// dgoal/dq = sum( wi * ai * dai/dq ) / sum(wi)
// This calculation is modeled after Peter Eastman's gradient implementation
// in ObservedPointFitter. It treats each osensor orientation error as a
// potential energy function whose negative spatial gradient would be a spatial
// force F. We can then use Simbody's spatial force-to-generalized force method
// (using -F instead of F) to obtain the gradient in internal coordinates.
int OrientationSensors::
calcGoalGradient(const State& state, Vector& gradient) const {
const int np = getNumFreeQs();
assert(gradient.size() == np);
const SimbodyMatterSubsystem& matter = getMatterSubsystem();
Vector_<SpatialVec> dEdR(matter.getNumBodies());
dEdR = SpatialVec(Vec3(0), Vec3(0));
// Loop over each body that has one or more active osensors.
Real wtot = 0;
PerBodyOSensors::const_iterator bodyp = bodiesWithOSensors.begin();
for (; bodyp != bodiesWithOSensors.end(); ++bodyp) {
const MobilizedBodyIndex mobodIx = bodyp->first;
const Array_<OSensorIx>& bodyOSensors = bodyp->second;
const MobilizedBody& mobod = matter.getMobilizedBody(mobodIx);
const Rotation& R_GB = mobod.getBodyRotation(state);
assert(bodyOSensors.size());
// Loop over each osensor on this body.
for (unsigned m=0; m < bodyOSensors.size(); ++m) {
const OSensorIx mx = bodyOSensors[m];
const OSensor& osensor = osensors[mx];
assert(osensor.bodyB == mobodIx); // better be on this body!
const Rotation& R_GO = observations[getObservationIxForOSensor(mx)];
if (R_GO.isFinite()) { // skip NaNs
const Rotation R_GS = R_GB * osensor.orientationInB;
const Rotation R_SO = ~R_GS*R_GO; // error, in S
const Vec4 aa_SO = R_SO.convertRotationToAngleAxis();
const Vec3 trq_S = -osensor.weight * aa_SO[0]
* aa_SO.getSubVec<3>(1);
const Vec3 trq_G = R_GS * trq_S;
mobod.applyBodyTorque(state, trq_G, dEdR);
wtot += osensor.weight;
}
}
}
// Convert spatial forces dEdR to generalized forces dEdU.
Vector dEdU;
matter.multiplyBySystemJacobianTranspose(state, dEdR, dEdU);
dEdU /= wtot;
const int nq = state.getNQ();
if (np == nq) // gradient is full length
matter.multiplyByNInv(state, true, dEdU, gradient);
else { // calculate full gradient; extract the relevant parts
Vector fullGradient(nq);
matter.multiplyByNInv(state, true, dEdU, fullGradient);
for (Assembler::FreeQIndex fx(0); fx < np; ++fx)
gradient[fx] = fullGradient[getQIndexOfFreeQ(fx)];
}
return 0;
}