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