doxy-ocra-wbi-plugins/html/ZmpController_8h_source.html

00001
00034 #ifndef _ZMPCONTROLLER_
00035 #define _ZMPCONTROLLER_
00036
00037 #include <ocra-icub/Utilities.h>
00038 #include <ocra/util/ErrorsHelper.h>
00039 #include <ocra/util/EigenUtilities.h>
00040 #include <ocra/control/TaskState.h>
00041 #include <ocra-recipes/TaskConnection.h>
00042 #include <Eigen/Dense>
00043 #include <vector>
00044
00045 struct ZmpControllerParams {
00049     double kfx;
00053     double kfy;
00054     double kdx;
00055     double kdy;
00059     double m;
00063     double cz;
00067     double g;
00071     double controllerPeriod;
00072
00073     ZmpControllerParams(double kfx,
00074                         double kfy,
00075                         double kdx,
00076                         double kdy,
00077                         double m,
00078                         double cz,
00079                         double g,
00080                         double controllerPeriod):
00081     kfx(kfx),
00082     kfy(kfy),
00083     kdx(kdx),
00084     kdy(kdy),
00085     m(m),
00086     cz(cz),
00087     g(g),
00088     controllerPeriod(controllerPeriod){}
00089 };
00090
00091 enum FOOT {
00092     LEFT_FOOT,
00093     RIGHT_FOOT
00094 };
00095
00096 class ZmpController
00097 {
00098 public:
00099
00103     ZmpController(const int period,
00104                   std::shared_ptr<ocra::Model> modelPtr,
00105                   std::shared_ptr<ZmpControllerParams> parameters);
00106
00107     virtual ~ZmpController();
00108
00136     bool computeFootZMP(FOOT whichFoot,
00137                         Eigen::VectorXd wrench,
00138                         Eigen::Vector2d &footZMP,
00139                         Eigen::VectorXd &wrenchInWorldRef,
00140                         const double tolerance=1e-3);
00141
00168     bool computeGlobalZMPFromSensors(Eigen::VectorXd rawLeftFootWrench,
00169                                      Eigen::VectorXd rawRightFootWrench,
00170                                      Eigen::Vector2d &globalZMP);
00171
00178     void getFTSensorAdjointMatrix(FOOT whichFoot, Eigen::MatrixXd &T, Eigen::Vector3d &sensorPosition);
00179
00180     void getLeftFootPosition(Eigen::Vector3d &leftFootPosition);
00181
00182     void getRightFootPosition(Eigen::Vector3d &rightFootPosition);
00183
00184
00185
00193     bool computehd(Eigen::Vector2d p, Eigen::Vector2d pd, Eigen::Vector2d &dhd);
00194
00195     void computehdd(Eigen::Vector3d comPosition, Eigen::Vector2d globalZMP, Eigen::Vector2d &ddh);
00196
00197     void computeh(Eigen::Vector2d prevComPosition, Eigen::Vector2d prevComVel, Eigen::Vector2d &intComPosition);
00198
00199     ocra::TaskState createDesiredState(Eigen::Vector2d comRefPosition, Eigen::Vector2d comRefVelocity, Eigen::Vector2d comRefAcceleration);
00200 private:
00201     std::shared_ptr<ZmpControllerParams> _params;
00202     std::shared_ptr<ocra::Model> _model;
00203
00204 };
00205
00206
00207 #endif