dali-physics/third-party/bullet3/src/BulletInverseDynamics/MultiBodyTree.hpp

   1 #ifndef MULTIBODYTREE_HPP_
   2 #define MULTIBODYTREE_HPP_
   3
   4 #include "IDConfig.hpp"
   5 #include "IDMath.hpp"
   6
   7 namespace btInverseDynamics
   8 {
   9 /// Enumeration of supported joint types
  10 enum JointType
  11 {
  12         /// no degree of freedom, moves with parent
  13         FIXED = 0,
  14         /// one rotational degree of freedom relative to parent
  15         REVOLUTE,
  16         /// one translational degree of freedom relative to parent
  17         PRISMATIC,
  18         /// six degrees of freedom relative to parent
  19         FLOATING,
  20         /// three degrees of freedom, relative to parent
  21         SPHERICAL
  22 };
  23
  24 /// Interface class for calculating inverse dynamics for tree structured
  25 /// multibody systems
  26 ///
  27 /// Note on degrees of freedom
  28 /// The q vector contains the generalized coordinate set defining the tree's configuration.
  29 /// Every joint adds elements that define the corresponding link's frame pose relative to
  30 /// its parent. For the joint types that is:
  31 ///     - FIXED:         none
  32 ///     - REVOLUTE:  angle of rotation [rad]
  33 ///     - PRISMATIC: displacement [m]
  34 ///     - FLOATING:  Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m]
  35 ///                              (in that order)
  36 /// - SPHERICAL: Euler x-y-z angles [rad]
  37 /// The u vector contains the generalized speeds, which are
  38 ///     - FIXED:         none
  39 ///     - REVOLUTE:  time derivative of angle of rotation [rad/s]
  40 ///     - PRISMATIC: time derivative of displacement [m/s]
  41 ///     - FLOATING:  angular velocity [rad/s] (*not* time derivative of rpy angles)
  42 ///                              and time derivative of displacement in parent frame [m/s]
  43 //      - SPHERICAL:  angular velocity [rad/s]
  44 ///
  45 /// The q and u vectors are obtained by stacking contributions of all bodies in one
  46 /// vector in the order of body indices.
  47 ///
  48 /// Note on generalized forces: analogous to u, i.e.,
  49 ///      - FIXED:        none
  50 ///      - REVOLUTE:  moment [Nm], about joint axis
  51 ///      - PRISMATIC: force  [N], along joint axis
  52 ///      - FLOATING:  moment vector [Nm] and force vector [N], both in body-fixed frame
  53 ///                               (in that order)
  54 ///  - SPHERICAL: moment vector [Nm]
  55 /// TODO - force element interface (friction, springs, dampers, etc)
  56 ///       - gears and motor inertia
  57 class MultiBodyTree
  58 {
  59 public:
  60         ID_DECLARE_ALIGNED_ALLOCATOR();
  61         /// The contructor.
  62         /// Initialization & allocation is via addBody and buildSystem calls.
  63         MultiBodyTree();
  64         /// the destructor. This also deallocates all memory
  65         ~MultiBodyTree();
  66
  67         /// Add body to the system. this allocates memory and not real-time safe.
  68         /// This only adds the data to an initial cache. After all bodies have been
  69         /// added,
  70         /// the system is setup using the buildSystem call
  71         /// @param body_index index of the body to be added. Must >=0, <number of bodies,
  72         ///             and index of parent must be < index of body
  73         /// @param parent_index index of the parent body
  74         ///             The root of the tree has index 0 and its parent (the world frame)
  75         ///             is assigned index -1
  76         ///             the rotation and translation relative to the parent are taken as
  77         ///             pose of the root body relative to the world frame. Other parameters
  78         ///             are ignored
  79         /// @param JointType type of joint connecting the body to the parent
  80         /// @param mass the mass of the body
  81         /// @param body_r_body_com the center of mass of the body relative to and
  82         /// described in
  83         ///             the body fixed frame, which is located in the joint axis connecting
  84         /// the body to its parent
  85         /// @param body_I_body the moment of inertia of the body w.r.t the body-fixed
  86         /// frame
  87         ///             (ie, the reference point is the origin of the body-fixed frame and
  88         /// the matrix is written
  89         ///              w.r.t. those unit vectors)
  90         /// @param parent_r_parent_body_ref position of joint relative to the parent
  91         /// body's reference frame
  92         ///             for q=0, written in the parent bodies reference frame
  93         /// @param body_axis_of_motion translation/rotation axis in body-fixed frame.
  94         ///             Ignored for joints that are not revolute or prismatic.
  95         ///             must be a unit vector.
  96         /// @param body_T_parent_ref transform matrix from parent to body reference
  97         /// frame for q=0.
  98         ///             This is the matrix transforming a vector represented in the
  99         /// parent's reference frame into one represented
 100         ///             in this body's reference frame.
 101         ///             ie, if parent_vec is a vector in R^3 whose components are w.r.t to
 102         /// the parent's reference frame,
 103         ///             then the same vector written w.r.t. this body's frame (for q=0) is
 104         /// given by
 105         ///             body_vec = parent_R_body_ref * parent_vec
 106         /// @param user_ptr pointer to user data
 107         /// @param user_int pointer to user integer
 108         /// @return 0 on success, -1 on error
 109         int addBody(int body_index, int parent_index, JointType joint_type,
 110                                 const vec3& parent_r_parent_body_ref, const mat33& body_T_parent_ref,
 111                                 const vec3& body_axis_of_motion, idScalar mass, const vec3& body_r_body_com,
 112                                 const mat33& body_I_body, const int user_int, void* user_ptr);
 113         /// set policy for invalid mass properties
 114         /// @param flag if true, invalid mass properties are accepted,
 115         ///             the default is false
 116         void setAcceptInvalidMassParameters(bool flag);
 117         /// @return the mass properties policy flag
 118         bool getAcceptInvalidMassProperties() const;
 119         /// build internal data structures
 120         /// call this after all bodies have been added via addBody
 121         /// @return 0 on success, -1 on error
 122         int finalize();
 123         /// pretty print ascii description of tree to stdout
 124         void printTree();
 125         /// print tree data to stdout
 126         void printTreeData();
 127         /// Calculate joint forces for given generalized state & derivatives.
 128         /// This also updates kinematic terms computed in calculateKinematics.
 129         /// If gravity is not set to zero, acceleration terms will contain
 130         /// gravitational acceleration.
 131         /// @param q generalized coordinates
 132         /// @param u generalized velocities. In the general case, u=T(q)*dot(q) and dim(q)>=dim(u)
 133         /// @param dot_u time derivative of u
 134         /// @param joint_forces this is where the resulting joint forces will be
 135         ///             stored. dim(joint_forces) = dim(u)
 136         /// @return 0 on success, -1 on error
 137         int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u,
 138                                                                  vecx* joint_forces);
 139         /// Calculate joint space mass matrix
 140         /// @param q generalized coordinates
 141         /// @param initialize_matrix if true, initialize mass matrix with zero.
 142         ///             If mass_matrix is initialized to zero externally and only used
 143         ///             for mass matrix computations for the same system, it is safe to
 144         ///             set this to false.
 145         /// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix
 146         ///             is also populated, otherwise not.
 147         /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q))
 148         /// @return -1 on error, 0 on success
 149         int calculateMassMatrix(const vecx& q, const bool update_kinematics,
 150                                                         const bool initialize_matrix, const bool set_lower_triangular_matrix,
 151                                                         matxx* mass_matrix);
 152
 153         /// Calculate joint space mass matrix.
 154         /// This version will update kinematics, initialize all mass_matrix elements to zero and
 155         /// populate all mass matrix entries.
 156         /// @param q generalized coordinates
 157         /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q))
 158         /// @return -1 on error, 0 on success
 159         int calculateMassMatrix(const vecx& q, matxx* mass_matrix);
 160
 161         /// Calculates kinematics also calculated in calculateInverseDynamics,
 162         /// but not dynamics.
 163         /// This function ensures that correct accelerations are computed that do not
 164         /// contain gravitational acceleration terms.
 165         /// Does not calculate Jacobians, but only vector quantities (positions, velocities & accelerations)
 166         int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u);
 167         /// Calculate position kinematics
 168         int calculatePositionKinematics(const vecx& q);
 169         /// Calculate position and velocity kinematics
 170         int calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u);
 171
 172 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
 173         /// Calculate Jacobians (dvel/du), as well as velocity-dependent accelearation components
 174         /// d(Jacobian)/dt*u
 175         /// This function assumes that calculateInverseDynamics was called, or calculateKinematics,
 176         /// or calculatePositionAndVelocityKinematics
 177         int calculateJacobians(const vecx& q, const vecx& u);
 178         /// Calculate Jacobians (dvel/du)
 179         /// This function assumes that calculateInverseDynamics was called, or
 180         /// one of the calculateKineamtics functions
 181         int calculateJacobians(const vecx& q);
 182 #endif  // BT_ID_HAVE_MAT3X
 183
 184         /// set gravitational acceleration
 185         /// the default is [0;0;-9.8] in the world frame
 186         /// @param gravity the gravitational acceleration in world frame
 187         /// @return 0 on success, -1 on error
 188         int setGravityInWorldFrame(const vec3& gravity);
 189         /// returns number of bodies in tree
 190         int numBodies() const;
 191         /// returns number of mechanical degrees of freedom (dimension of q-vector)
 192         int numDoFs() const;
 193         /// get origin of a body-fixed frame, represented in world frame
 194         /// @param body_index index for frame/body
 195         /// @param world_origin pointer for return data
 196         /// @return 0 on success, -1 on error
 197         int getBodyOrigin(const int body_index, vec3* world_origin) const;
 198         /// get center of mass of a body, represented in world frame
 199         /// @param body_index index for frame/body
 200         /// @param world_com pointer for return data
 201         /// @return 0 on success, -1 on error
 202         int getBodyCoM(const int body_index, vec3* world_com) const;
 203         /// get transform from of a body-fixed frame to the world frame
 204         /// @param body_index index for frame/body
 205         /// @param world_T_body pointer for return data
 206         /// @return 0 on success, -1 on error
 207         int getBodyTransform(const int body_index, mat33* world_T_body) const;
 208         /// get absolute angular velocity for a body, represented in the world frame
 209         /// @param body_index index for frame/body
 210         /// @param world_omega pointer for return data
 211         /// @return 0 on success, -1 on error
 212         int getBodyAngularVelocity(const int body_index, vec3* world_omega) const;
 213         /// get linear velocity of a body, represented in world frame
 214         /// @param body_index index for frame/body
 215         /// @param world_velocity pointer for return data
 216         /// @return 0 on success, -1 on error
 217         int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const;
 218         /// get linear velocity of a body's CoM, represented in world frame
 219         /// (not required for inverse dynamics, provided for convenience)
 220         /// @param body_index index for frame/body
 221         /// @param world_vel_com pointer for return data
 222         /// @return 0 on success, -1 on error
 223         int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const;
 224         /// get origin of a body-fixed frame, represented in world frame
 225         /// @param body_index index for frame/body
 226         /// @param world_origin pointer for return data
 227         /// @return 0 on success, -1 on error
 228         int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const;
 229         /// get origin of a body-fixed frame, represented in world frame
 230         /// NOTE: this will include the gravitational acceleration, so the actual acceleration is
 231         /// obtainened by setting gravitational acceleration to zero, or subtracting it.
 232         /// @param body_index index for frame/body
 233         /// @param world_origin pointer for return data
 234         /// @return 0 on success, -1 on error
 235         int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
 236
 237 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
 238         // get translational jacobian, in world frame (dworld_velocity/du)
 239         int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const;
 240         // get rotational jacobian, in world frame (dworld_omega/du)
 241         int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
 242         // get product of translational jacobian derivative * generatlized velocities
 243         int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const;
 244         // get product of rotational jacobian derivative * generatlized velocities
 245         int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
 246 #endif  // BT_ID_HAVE_MAT3X
 247
 248         /// returns the (internal) index of body
 249         /// @param body_index is the index of a body
 250         /// @param parent_index pointer to where parent index will be stored
 251         /// @return 0 on success, -1 on error
 252         int getParentIndex(const int body_index, int* parent_index) const;
 253         /// get joint type
 254         /// @param body_index index of the body
 255         /// @param joint_type the corresponding joint type
 256         /// @return 0 on success, -1 on failure
 257         int getJointType(const int body_index, JointType* joint_type) const;
 258         /// get joint type as string
 259         /// @param body_index index of the body
 260         /// @param joint_type string naming the corresponding joint type
 261         /// @return 0 on success, -1 on failure
 262         int getJointTypeStr(const int body_index, const char** joint_type) const;
 263         /// get offset translation to parent body (see addBody)
 264         /// @param body_index index of the body
 265         /// @param r the offset translation (see above)
 266         /// @return 0 on success, -1 on failure
 267         int getParentRParentBodyRef(const int body_index, vec3* r) const;
 268         /// get offset rotation to parent body (see addBody)
 269         /// @param body_index index of the body
 270         /// @param T the transform (see above)
 271         /// @return 0 on success, -1 on failure
 272         int getBodyTParentRef(const int body_index, mat33* T) const;
 273         /// get axis of motion (see addBody)
 274         /// @param body_index index of the body
 275         /// @param axis the axis (see above)
 276         /// @return 0 on success, -1 on failure
 277         int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
 278         /// get offset for degrees of freedom of this body into the q-vector
 279         /// @param body_index index of the body
 280         /// @param q_offset offset the q vector
 281         /// @return -1 on error, 0 on success
 282         int getDoFOffset(const int body_index, int* q_offset) const;
 283         /// get user integer. not used by the library.
 284         /// @param body_index index of the body
 285         /// @param user_int   the user integer
 286         /// @return 0 on success, -1 on error
 287         int getUserInt(const int body_index, int* user_int) const;
 288         /// get user pointer. not used by the library.
 289         /// @param body_index index of the body
 290         /// @param user_ptr   the user pointer
 291         /// @return 0 on success, -1 on error
 292         int getUserPtr(const int body_index, void** user_ptr) const;
 293         /// set user integer. not used by the library.
 294         /// @param body_index index of the body
 295         /// @param user_int   the user integer
 296         /// @return 0 on success, -1 on error
 297         int setUserInt(const int body_index, const int user_int);
 298         /// set user pointer. not used by the library.
 299         /// @param body_index index of the body
 300         /// @param user_ptr   the user pointer
 301         /// @return 0 on success, -1 on error
 302         int setUserPtr(const int body_index, void* const user_ptr);
 303         /// set mass for a body
 304         /// @param body_index index of the body
 305         /// @param mass the mass to set
 306         /// @return 0 on success, -1 on failure
 307         int setBodyMass(const int body_index, const idScalar mass);
 308         /// set first moment of mass for a body
 309         /// (mass * center of mass, in body fixed frame, relative to joint)
 310         /// @param body_index index of the body
 311         /// @param first_mass_moment the vector to set
 312         /// @return 0 on success, -1 on failure
 313         int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment);
 314         /// set second moment of mass for a body
 315         /// (moment of inertia, in body fixed frame, relative to joint)
 316         /// @param body_index index of the body
 317         /// @param second_mass_moment the inertia matrix
 318         /// @return 0 on success, -1 on failure
 319         int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment);
 320         /// get mass for a body
 321         /// @param body_index index of the body
 322         /// @param mass the mass
 323         /// @return 0 on success, -1 on failure
 324         int getBodyMass(const int body_index, idScalar* mass) const;
 325         /// get first moment of mass for a body
 326         /// (mass * center of mass, in body fixed frame, relative to joint)
 327         /// @param body_index index of the body
 328         /// @param first_moment the vector
 329         /// @return 0 on success, -1 on failure
 330         int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const;
 331         /// get second moment of mass for a body
 332         /// (moment of inertia, in body fixed frame, relative to joint)
 333         /// @param body_index index of the body
 334         /// @param second_mass_moment the inertia matrix
 335         /// @return 0 on success, -1 on failure
 336         int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const;
 337         /// set all user forces and moments to zero
 338         void clearAllUserForcesAndMoments();
 339         /// Add an external force to a body, acting at the origin of the body-fixed frame.
 340         /// Calls to addUserForce are cumulative. Set the user force and moment to zero
 341         /// via clearAllUserForcesAndMoments()
 342         /// @param body_force the force represented in the body-fixed frame of reference
 343         /// @return 0 on success, -1 on error
 344         int addUserForce(const int body_index, const vec3& body_force);
 345         /// Add an external moment to a body.
 346         /// Calls to addUserMoment are cumulative. Set the user force and moment to zero
 347         /// via clearAllUserForcesAndMoments()
 348         /// @param body_moment the moment represented in the body-fixed frame of reference
 349         /// @return 0 on success, -1 on error
 350         int addUserMoment(const int body_index, const vec3& body_moment);
 351
 352 private:
 353         // flag indicating if system has been initialized
 354         bool m_is_finalized;
 355         // flag indicating if mass properties are physically valid
 356         bool m_mass_parameters_are_valid;
 357         // flag defining if unphysical mass parameters are accepted
 358         bool m_accept_invalid_mass_parameters;
 359         // This struct implements the inverse dynamics calculations
 360         class MultiBodyImpl;
 361         MultiBodyImpl* m_impl;
 362         // cache data structure for initialization
 363         class InitCache;
 364         InitCache* m_init_cache;
 365 };
 366 }  // namespace btInverseDynamics
 367 #endif  // MULTIBODYTREE_HPP_