libcnoid-dev 1.1.0+dfsg-6.1+b4 / usr / include / choreonoid-1.1 / cnoid / src / Body / ForwardDynamicsCBM.h

This file is indexed.

/usr/include/choreonoid-1.1/cnoid/src/Body/ForwardDynamicsCBM.h is in libcnoid-dev 1.1.0+dfsg-6.1+b4.

This file is owned by root:root, with mode 0o644.

The actual contents of the file can be viewed below. 

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
/**
   \file
   \author Shin'ichiro Nakaoka
*/

#ifndef CNOID_BODY_FORWARD_DYNAMICS_MM_H_INCLUDED
#define CNOID_BODY_FORWARD_DYNAMICS_MM_H_INCLUDED

#include "ForwardDynamics.h"
#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/intrusive_ptr.hpp>
#include "exportdecl.h"

namespace cnoid
{
    class Link;
	
    class Body;
    typedef boost::intrusive_ptr<Body> BodyPtr;

    class LinkTraverse;
    class AccelSensor;
    class ForceSensor;

    /**
       The ForwardDynamicsMM class calculates the forward dynamics using
       the motion equation based on the generalized mass matrix.
       The class is mainly used for a body that has high-gain mode joints.
       If all the joints of a body are the torque mode, the ForwardDynamicsABM,
       which uses the articulated body method, is more efficient.
    */
    class CNOID_EXPORT ForwardDynamicsMM : public ForwardDynamics {

      public:
        
        ForwardDynamicsMM(BodyPtr body);
        ~ForwardDynamicsMM();

        virtual void initialize();
        virtual void calcNextState();

        void initializeAccelSolver();
        void solveUnknownAccels(const Vector3& fext, const Vector3& tauext);
        void solveUnknownAccels(Link* link, const Vector3& fext, const Vector3& tauext, const Vector3& rootfext, const Vector3& roottauext);

      private:
        
        /*
          Elements of the motion equation
		   
          |     |     |   | dv         |   | b1 |   | 0  |   | totalfext      |
          | M11 | M12 |   | dw         |   |    |   | 0  |   | totaltauext    |
          |     |     | * |ddq (unkown)| + |    | + | d1 | = | u (known)      |
          |-----+-----|   |------------|   |----|   |----|   |----------------|
          | M21 | M22 |   | given ddq  |   | b2 |   | d2 |   | u2             |
		
          |fext  |
          d1 = trans(s)|      |
          |tauext|

        */
		
        MatrixXd M11;
        MatrixXd M12;
        MatrixXd b1;
        VectorXd d1;
        VectorXd c1;

        std::vector<Link*> torqueModeJoints;
        std::vector<Link*> highGainModeJoints;

        //int rootDof; // dof of dv and dw (0 or 6)
        int unknown_rootDof;
        int given_rootDof;

        bool isNoUnknownAccelMode;

        VectorXd qGiven;
        VectorXd dqGiven;
        VectorXd ddqGiven;
        Vector3 pGiven;
        Matrix3 RGiven;
        Vector3 voGiven;
        Vector3 wGiven;

        bool accelSolverInitialized;
        bool ddqGivenCopied;

        VectorXd qGivenPrev;
        VectorXd dqGivenPrev;
        Vector3 pGivenPrev;
        Matrix3 RGivenPrev;
        Vector3 voGivenPrev;
        Vector3 wGivenPrev;

        Vector3 fextTotal;
        Vector3 tauextTotal;

        Vector3 root_w_x_v;

        // buffers for the unit vector method
        VectorXd ddqorg;
        VectorXd uorg;
        Vector3 dvoorg;
        Vector3 dworg;
		
        struct ForceSensorInfo {
            ForceSensor* sensor;
            bool hasSensorsAbove;
        };

        std::vector<ForceSensorInfo> forceSensorInfo;

        // Buffers for the Runge Kutta Method
        Vector3 p0;
        Matrix3 R0;
        Vector3 vo0;
        Vector3 w0;
        std::vector<double> q0;
        std::vector<double> dq0;
		
        Vector3 vo;
        Vector3 w;
        Vector3 dvo;
        Vector3 dw;
        std::vector<double> dq;
        std::vector<double> ddq;

        virtual void initializeSensors();

        void calcMotionWithEulerMethod();
        void calcMotionWithRungeKuttaMethod();
        void integrateRungeKuttaOneStep(double r, double dt);
        void preserveHighGainModeJointState();
        void calcPositionAndVelocityFK();
        void calcMassMatrix();
        void setColumnOfMassMatrix(MatrixXd& M, int column);
        void calcInverseDynamics(Link* link, Vector3& out_f, Vector3& out_tau);
        void calcd1(Link* link, Vector3& out_f, Vector3& out_tau);
        void sumExternalForces();
        inline void solveUnknownAccels();
        inline void calcAccelFKandForceSensorValues();
        void calcAccelFKandForceSensorValues(Link* link, Vector3& out_f, Vector3& out_tau);
        void updateForceSensorInfo(Link* link, bool hasSensorsAbove);
    };

    typedef boost::shared_ptr<ForwardDynamicsMM> ForwardDynamicsMMPtr;
	
};

#endif

APT Browse - Built by Thomas Orozco.