Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00014 #ifndef OPENHRP_FORWARD_DYNAMICS_H_INCLUDED
00015 #define OPENHRP_FORWARD_DYNAMICS_H_INCLUDED
00016
00017 #include "Body.h"
00018
00019 #include <boost/shared_ptr.hpp>
00020 #include <boost/intrusive_ptr.hpp>
00021 #include <hrpUtil/Eigen3d.h>
00022
00023 #include "Config.h"
00024
00025
00026 namespace hrp
00027 {
00028 class AccelSensor;
00029
00035 class HRPMODEL_API ForwardDynamics {
00036
00037 public:
00038 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00039
00040 ForwardDynamics(BodyPtr body);
00041 virtual ~ForwardDynamics();
00042
00043 void setGravityAcceleration(const Vector3& g);
00044 void setEulerMethod();
00045 void setRungeKuttaMethod();
00046 void setTimeStep(double timeStep);
00047 void enableSensors(bool on);
00048
00049 virtual void initialize() = 0;
00050 virtual void calcNextState() = 0;
00051
00052 protected:
00053
00054 virtual void initializeSensors();
00055 virtual void updateSensorsFinal();
00056
00067 static void SE3exp(Vector3& out_p, Matrix33& out_R,
00068 const Vector3& p0, const Matrix33& R0,
00069 const Vector3& w, const Vector3& vo, double dt);
00070
00071 BodyPtr body;
00072 Vector3 g;
00073 double timeStep;
00074 bool sensorsEnabled;
00075
00076 enum { EULER_METHOD, RUNGEKUTTA_METHOD } integrationMode;
00077
00078 private:
00079
00080 void updateAccelSensor(AccelSensor* sensor);
00081 void initializeAccelSensors();
00082
00083
00084
00085 typedef Eigen::Matrix2d matrix22;
00086 typedef Eigen::Vector2d vector2;
00087 matrix22 A;
00088 vector2 B;
00089
00090 };
00091
00092 typedef boost::shared_ptr<ForwardDynamics> ForwardDynamicsPtr;
00093
00094 };
00095
00096 #endif