ForwardDynamicsCBM.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
14 #ifndef OPENHRP_FORWARD_DYNAMICS_MM_H_INCLUDED
15 #define OPENHRP_FORWARD_DYNAMICS_MM_H_INCLUDED
16 
17 #include <vector>
18 #include <boost/shared_ptr.hpp>
19 #include <boost/intrusive_ptr.hpp>
20 #include <hrpUtil/Eigen3d.h>
21 #include "ForwardDynamics.h"
22 #include "Config.h"
23 
24 namespace hrp
25 {
26  class Link;
27 
28  class Body;
29  typedef boost::shared_ptr<Body> BodyPtr;
30 
31  class LinkTraverse;
32  class AccelSensor;
33  class ForceSensor;
34 
42  class HRPMODEL_API ForwardDynamicsMM : public ForwardDynamics {
43 
44  public:
45 
46  ForwardDynamicsMM(BodyPtr body);
48 
49  virtual void initialize();
50  virtual void calcNextState();
51 
52  void initializeAccelSolver();
53  void solveUnknownAccels(const Vector3& fext, const Vector3& tauext);
54  void solveUnknownAccels(Link* link, const Vector3& fext, const Vector3& tauext, const Vector3& rootfext, const Vector3& roottauext);
55  void sumExternalForces();
56  void solveUnknownAccels();
57 
58  private:
59 
60  /*
61  Elements of the motion equation
62 
63  | | | | dv | | b1 | | 0 | | totalfext |
64  | M11 | M12 | | dw | | | | 0 | | totaltauext |
65  | | | * |ddq (unkown)| + | | + | d1 | = | u (known) |
66  |-----+-----| |------------| |----| |----| |----------------|
67  | M21 | M22 | | given ddq | | b2 | | d2 | | u2 |
68 
69  |fext |
70  d1 = trans(s)| |
71  |tauext|
72 
73  */
74 
80 
81  std::vector<Link*> torqueModeJoints;
82  std::vector<Link*> highGainModeJoints;
83 
84  //int rootDof; // dof of dv and dw (0 or 6)
87 
89 
97 
100 
107 
110 
112 
113  // buffers for the unit vector method
118 
122  };
123 
124  std::vector<ForceSensorInfo> forceSensorInfo;
125 
126  // Buffers for the Runge Kutta Method
131  std::vector<double> q0;
132  std::vector<double> dq0;
133 
138  std::vector<double> dq;
139  std::vector<double> ddq;
140 
141  virtual void initializeSensors();
142 
143  void calcMotionWithEulerMethod();
144  void calcMotionWithRungeKuttaMethod();
145  void integrateRungeKuttaOneStep(double r, double dt);
146  void preserveHighGainModeJointState();
147  void calcPositionAndVelocityFK();
148  void calcMassMatrix();
149  void setColumnOfMassMatrix(dmatrix& M, int column);
150  void calcInverseDynamics(Link* link, Vector3& out_f, Vector3& out_tau);
151  void calcd1(Link* link, Vector3& out_f, Vector3& out_tau);
152  inline void calcAccelFKandForceSensorValues();
153  void calcAccelFKandForceSensorValues(Link* link, Vector3& out_f, Vector3& out_tau);
154  void updateForceSensorInfo(Link* link, bool hasSensorsAbove);
155  };
156 
157  typedef boost::shared_ptr<ForwardDynamicsMM> ForwardDynamicsMMPtr;
158 
159 };
160 
161 #endif
boost::shared_ptr< ForwardDynamicsMM > ForwardDynamicsMMPtr
Eigen::MatrixXd dmatrix
Definition: EigenTypes.h:13
std::vector< double > dq0
Eigen::VectorXd dvector
Definition: EigenTypes.h:14
std::vector< ForceSensorInfo > forceSensorInfo
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
std::vector< Link * > highGainModeJoints
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
std::vector< double > ddq
std::vector< double > q0
std::vector< Link * > torqueModeJoints
std::vector< double > dq


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:03