openhrp3: ForwardDynamicsCBM.h Source File

Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
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                    Elements of the motion equation
00062                    
00063                   |     |     |   | dv         |   | b1 |   | 0  |   | totalfext      |
00064                   | M11 | M12 |   | dw         |   |    |   | 0  |   | totaltauext    |
00065                   |     |     | * |ddq (unkown)| + |    | + | d1 | = | u (known)      |
00066                   |-----+-----|   |------------|   |----|   |----|   |----------------|
00067                   | M21 | M22 |   | given ddq  |   | b2 |   | d2 |   | u2             |
00068                 
00069                          |fext  |
00070             d1 = trans(s)|      |
00071                          |tauext|
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                 //int rootDof; // dof of dv and dw (0 or 6)
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                 // buffers for the unit vector method
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                 // Buffers for the Runge Kutta Method
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