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