/usr/include/choreonoid-1.1/cnoid/src/Body/ForwardDynamicsABM.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 | /**
\file
\author Shin'ichiro Nakaoka
*/
#ifndef CNOID_BODY_FORWARD_DYNAMICS_ABM_H_INCLUDED
#define CNOID_BODY_FORWARD_DYNAMICS_ABM_H_INCLUDED
#include "ForwardDynamics.h"
#include <boost/intrusive_ptr.hpp>
#include <vector>
#include "exportdecl.h"
namespace cnoid
{
class LinkTraverse;
class AccelSensor;
class ForceSensor;
/**
Forward dynamics calculation using Featherstone's Articulated Body Method (ABM)
*/
class CNOID_EXPORT ForwardDynamicsABM : public ForwardDynamics {
public:
ForwardDynamicsABM(BodyPtr body);
~ForwardDynamicsABM();
virtual void initialize();
virtual void calcNextState();
private:
void calcMotionWithEulerMethod();
void integrateRungeKuttaOneStep(double r, double dt);
void calcMotionWithRungeKuttaMethod();
/**
compute position/orientation/velocity
*/
void calcABMPhase1();
/**
compute articulated inertia
*/
void calcABMPhase2();
void calcABMPhase2Part1();
void calcABMPhase2Part2();
/**
compute joint acceleration/spatial acceleration
*/
void calcABMPhase3();
inline void calcABMFirstHalf();
inline void calcABMLastHalf();
void updateForceSensors();
void updateForceSensor(ForceSensor* sensor);
// 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;
};
};
#endif
|