Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00014 #ifndef OPENHRP_FORWARD_DYNAMICS_MM_H_INCLUDED
00015 #define OPENHRP_FORWARD_DYNAMICS_MM_H_INCLUDED
00016
00017 #include <vector>
00018 #include <boost/shared_ptr.hpp>
00019 #include <boost/intrusive_ptr.hpp>
00020 #include <hrpUtil/Eigen3d.h>
00021 #include "ForwardDynamics.h"
00022 #include "Config.h"
00023
00024 namespace hrp
00025 {
00026 class Link;
00027
00028 class Body;
00029 typedef boost::shared_ptr<Body> BodyPtr;
00030
00031 class LinkTraverse;
00032 class AccelSensor;
00033 class ForceSensor;
00034
00042 class HRPMODEL_API ForwardDynamicsMM : public ForwardDynamics {
00043
00044 public:
00045
00046 ForwardDynamicsMM(BodyPtr body);
00047 ~ForwardDynamicsMM();
00048
00049 virtual void initialize();
00050 virtual void calcNextState();
00051
00052 void initializeAccelSolver();
00053 void solveUnknownAccels(const Vector3& fext, const Vector3& tauext);
00054 void solveUnknownAccels(Link* link, const Vector3& fext, const Vector3& tauext, const Vector3& rootfext, const Vector3& roottauext);
00055 void sumExternalForces();
00056 void solveUnknownAccels();
00057
00058 private:
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075 dmatrix M11;
00076 dmatrix M12;
00077 dmatrix b1;
00078 dmatrix d1;
00079 dvector c1;
00080
00081 std::vector<Link*> torqueModeJoints;
00082 std::vector<Link*> highGainModeJoints;
00083
00084
00085 int unknown_rootDof;
00086 int given_rootDof;
00087
00088 bool isNoUnknownAccelMode;
00089
00090 dvector qGiven;
00091 dvector dqGiven;
00092 dvector ddqGiven;
00093 Vector3 pGiven;
00094 Matrix33 RGiven;
00095 Vector3 voGiven;
00096 Vector3 wGiven;
00097
00098 bool accelSolverInitialized;
00099 bool ddqGivenCopied;
00100
00101 dvector qGivenPrev;
00102 dvector dqGivenPrev;
00103 Vector3 pGivenPrev;
00104 Matrix33 RGivenPrev;
00105 Vector3 voGivenPrev;
00106 Vector3 wGivenPrev;
00107
00108 Vector3 fextTotal;
00109 Vector3 tauextTotal;
00110
00111 Vector3 root_w_x_v;
00112
00113
00114 dvector ddqorg;
00115 dvector uorg;
00116 Vector3 dvoorg;
00117 Vector3 dworg;
00118
00119 struct ForceSensorInfo {
00120 ForceSensor* sensor;
00121 bool hasSensorsAbove;
00122 };
00123
00124 std::vector<ForceSensorInfo> forceSensorInfo;
00125
00126
00127 Vector3 p0;
00128 Matrix33 R0;
00129 Vector3 vo0;
00130 Vector3 w0;
00131 std::vector<double> q0;
00132 std::vector<double> dq0;
00133
00134 Vector3 vo;
00135 Vector3 w;
00136 Vector3 dvo;
00137 Vector3 dw;
00138 std::vector<double> dq;
00139 std::vector<double> ddq;
00140
00141 virtual void initializeSensors();
00142
00143 void calcMotionWithEulerMethod();
00144 void calcMotionWithRungeKuttaMethod();
00145 void integrateRungeKuttaOneStep(double r, double dt);
00146 void preserveHighGainModeJointState();
00147 void calcPositionAndVelocityFK();
00148 void calcMassMatrix();
00149 void setColumnOfMassMatrix(dmatrix& M, int column);
00150 void calcInverseDynamics(Link* link, Vector3& out_f, Vector3& out_tau);
00151 void calcd1(Link* link, Vector3& out_f, Vector3& out_tau);
00152 inline void calcAccelFKandForceSensorValues();
00153 void calcAccelFKandForceSensorValues(Link* link, Vector3& out_f, Vector3& out_tau);
00154 void updateForceSensorInfo(Link* link, bool hasSensorsAbove);
00155 };
00156
00157 typedef boost::shared_ptr<ForwardDynamicsMM> ForwardDynamicsMMPtr;
00158
00159 };
00160
00161 #endif