/**-------------------------------------------------------------------------------
Sensors
@brief
@param simulation
@return
---------------------------------------------------------------------------------*/
Sensors::Sensors(Simulation* simulation)
{
if (Config::Instance().getShowCOG())
{
// create center-of-gravity (COG) sensor
SensorVectors* s = new SensorCOG(simulation);
boost::shared_ptr<SensorVectors> cog(
new SensorDecoratorCross(s, Ogre::ColourValue(0, 0, 1), Config::Instance().getDrawingScaleCOG()));
mSensors.push_back(cog);
}
else
{
boost::shared_ptr<SensorVectors> cog(new SensorCOG(simulation));
mSensors.push_back(cog);
}
// create center-of-pressure (COP) sensor
if (Config::Instance().getShowCOP())
{
SensorVectors* s = new SensorCOP(simulation);
boost::shared_ptr<SensorVectors> cop(
new SensorDecoratorCross(s, Ogre::ColourValue(0, 1, 0), Config::Instance().getDrawingScaleCOP()));
mSensors.push_back(cop);
}
else
{
boost::shared_ptr<SensorVectors> cop(new SensorCOP(simulation));
mSensors.push_back(cop);
}
// create (Zero-moment-point) ZMP sensor
if (Config::Instance().getShowZMP())
{
SensorVectors* s = new SensorZMP(simulation);
boost::shared_ptr<SensorVectors> zmp(
new SensorDecoratorCross(s, Ogre::ColourValue(1, 0, 1), Config::Instance().getDrawingScaleZMP()));
mSensors.push_back(zmp);
}
else
{
boost::shared_ptr<SensorVectors> zmp(new SensorZMP(simulation));
mSensors.push_back(zmp);
}
// create (Foot-rotation-index) FRI sensor
if (Config::Instance().getShowFRI())
{
SensorVectors* s = new SensorFRI(simulation);
boost::shared_ptr<SensorVectors> fri(
new SensorDecoratorCross(s, Ogre::ColourValue(0.42, 0.5624, 0.8492), Config::Instance().getDrawingScaleFRI()));
mSensors.push_back(fri);
}
else
{
boost::shared_ptr<SensorVectors> fri(new SensorFRI(simulation));
mSensors.push_back(fri);
}
}
Vector& computeZmp (Vector& zmp, int time)
{
double fnormal = 0;
double sumZmpx = 0;
double sumZmpy = 0;
double sumZmpz = 0;
zmp.resize (3);
for (unsigned int i=0; i<footNumber; ++i) {
const Vector& f = forcesSIN_ [i]->access (time);
// Check that force is of dimension 6
if (f.size () != 6) {
zmp.fill (0.);
return zmp;
}
const MatrixHomogeneous& M = sensorPositionsSIN_ [i]->access (time);
double fx = M (0,0) * f (0) + M(0,1) * f (1) + M (0,2) * f (2);
double fy = M (1,0) * f (0) + M(1,1) * f (1) + M (1,2) * f (2);
double fz = M (2,0) * f (0) + M(2,1) * f (1) + M (2,2) * f (2);
if (fz > 0) {
double Mx = M (0,0)*f(3) + M (0,1)*f(4) + M (0,2)*f(5)
+ M (1,3)*fz - M (2,3)*fy;
double My = M (1,0)*f(3) + M (1,1)*f(4) + M (1,2)*f(5)
+ M (2,3)*fx - M (0,3)*fz;
fnormal += fz;
sumZmpx -= My;
sumZmpy += Mx;
sumZmpz += fz * M (2,3);
}
}
if (fnormal != 0) {
zmp (0) = sumZmpx / fnormal;
zmp (1) = sumZmpy / fnormal;
zmp (2) = sumZmpz / fnormal;
} else {
zmp.fill (0.);
}
return zmp;
}
