dali-physics/third-party/bullet3/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp

   1 // The structs and classes defined here provide a basic inverse fynamics implementation used
   2 // by MultiBodyTree
   3 // User interaction should be through MultiBodyTree
   4
   5 #ifndef MULTI_BODY_REFERENCE_IMPL_HPP_
   6 #define MULTI_BODY_REFERENCE_IMPL_HPP_
   7
   8 #include "../IDConfig.hpp"
   9 #include "../MultiBodyTree.hpp"
  10
  11 namespace btInverseDynamics
  12 {
  13 /// Structure for for rigid body mass properties, connectivity and kinematic state
  14 /// all vectors and matrices are in body-fixed frame, if not indicated otherwise.
  15 /// The body-fixed frame is located in the joint connecting the body to its parent.
  16 struct RigidBody
  17 {
  18         ID_DECLARE_ALIGNED_ALLOCATOR();
  19         // 1 Inertial properties
  20         /// Mass
  21         idScalar m_mass;
  22         /// Mass times center of gravity in body-fixed frame
  23         vec3 m_body_mass_com;
  24         /// Moment of inertia w.r.t. body-fixed frame
  25         mat33 m_body_I_body;
  26
  27         // 2 dynamic properties
  28         /// Left-hand side of the body equation of motion, translational part
  29         vec3 m_eom_lhs_translational;
  30         /// Left-hand side of the body equation of motion, rotational part
  31         vec3 m_eom_lhs_rotational;
  32         /// Force acting at the joint when the body is cut from its parent;
  33         /// includes impressed joint force in J_JT direction,
  34         /// as well as constraint force,
  35         /// in body-fixed frame
  36         vec3 m_force_at_joint;
  37         /// Moment acting at the joint when the body is cut from its parent;
  38         /// includes impressed joint moment in J_JR direction, and constraint moment
  39         /// in body-fixed frame
  40         vec3 m_moment_at_joint;
  41         /// external (user provided) force acting at the body-fixed frame's origin, written in that
  42         /// frame
  43         vec3 m_body_force_user;
  44         /// external (user provided) moment acting at the body-fixed frame's origin, written in that
  45         /// frame
  46         vec3 m_body_moment_user;
  47         // 3 absolute kinematic properties
  48         /// Position of body-fixed frame relative to world frame
  49         /// this is currently only for debugging purposes
  50         vec3 m_body_pos;
  51         /// Absolute velocity of body-fixed frame
  52         vec3 m_body_vel;
  53         /// Absolute acceleration of body-fixed frame
  54         /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational
  55         /// acceleration!
  56         vec3 m_body_acc;
  57         /// Absolute angular velocity
  58         vec3 m_body_ang_vel;
  59         /// Absolute angular acceleration
  60         /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational
  61         /// acceleration!
  62         vec3 m_body_ang_acc;
  63
  64         // 4 relative kinematic properties.
  65         // these are in the parent body frame
  66         /// Transform from world to body-fixed frame;
  67         /// this is currently only for debugging purposes
  68         mat33 m_body_T_world;
  69         /// Transform from parent to body-fixed frame
  70         mat33 m_body_T_parent;
  71         /// Vector from parent to child frame in parent frame
  72         vec3 m_parent_pos_parent_body;
  73         /// Relative angular velocity
  74         vec3 m_body_ang_vel_rel;
  75         /// Relative linear velocity
  76         vec3 m_parent_vel_rel;
  77         /// Relative angular acceleration
  78         vec3 m_body_ang_acc_rel;
  79         /// Relative linear acceleration
  80         vec3 m_parent_acc_rel;
  81
  82         // 5 Data describing the joint type and geometry
  83         /// Type of joint
  84         JointType m_joint_type;
  85         /// Position of joint frame (body-fixed frame at q=0) relative to the parent frame
  86         /// Components are in body-fixed frame of the parent
  87         vec3 m_parent_pos_parent_body_ref;
  88         /// Orientation of joint frame (body-fixed frame at q=0) relative to the parent frame
  89         mat33 m_body_T_parent_ref;
  90         /// Joint rotational Jacobian, ie, the partial derivative of the body-fixed frames absolute
  91         /// angular velocity w.r.t. the generalized velocity of this body's relative degree of freedom.
  92         /// For revolute joints this is the joint axis, for prismatic joints it is a null matrix.
  93         /// (NOTE: dimensions will have to be dynamic for additional joint types!)
  94         vec3 m_Jac_JR;
  95         /// Joint translational Jacobian, ie, the partial derivative of the body-fixed frames absolute
  96         /// linear velocity w.r.t. the generalized velocity of this body's relative degree of freedom.
  97         /// For prismatic joints this is the joint axis, for revolute joints it is a null matrix.
  98         /// (NOTE: dimensions might have to be dynamic for additional joint types!)
  99         vec3 m_Jac_JT;
 100         /// m_Jac_JT in the parent frame, it, m_body_T_parent_ref.transpose()*m_Jac_JT
 101         vec3 m_parent_Jac_JT;
 102         /// Start of index range for the position degree(s) of freedom describing this body's motion
 103         /// relative to
 104         /// its parent. The indices are wrt the multibody system's q-vector of generalized coordinates.
 105         int m_q_index;
 106
 107         // 6 Scratch data for mass matrix computation using "composite rigid body algorithm"
 108         /// mass of the subtree rooted in this body
 109         idScalar m_subtree_mass;
 110         /// center of mass * mass for subtree rooted in this body, in body-fixed frame
 111         vec3 m_body_subtree_mass_com;
 112         /// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame
 113         mat33 m_body_subtree_I_body;
 114
 115 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
 116         /// translational jacobian in body-fixed frame d(m_body_vel)/du
 117         mat3x m_body_Jac_T;
 118         /// rotationsl jacobian in body-fixed frame d(m_body_ang_vel)/du
 119         mat3x m_body_Jac_R;
 120         /// components of linear acceleration depending on u
 121         /// (same as is d(m_Jac_T)/dt*u)
 122         vec3 m_body_dot_Jac_T_u;
 123         /// components of angular acceleration depending on u
 124         /// (same as is d(m_Jac_T)/dt*u)
 125         vec3 m_body_dot_Jac_R_u;
 126 #endif
 127 };
 128
 129 /// The MBS implements a tree structured multibody system
 130 class MultiBodyTree::MultiBodyImpl
 131 {
 132         friend class MultiBodyTree;
 133
 134 public:
 135         ID_DECLARE_ALIGNED_ALLOCATOR();
 136
 137         enum KinUpdateType
 138         {
 139                 POSITION_ONLY,
 140                 POSITION_VELOCITY,
 141                 POSITION_VELOCITY_ACCELERATION
 142         };
 143
 144         /// constructor
 145         /// @param num_bodies the number of bodies in the system
 146         /// @param num_dofs number of degrees of freedom in the system
 147         MultiBodyImpl(int num_bodies_, int num_dofs_);
 148
 149         /// \copydoc MultiBodyTree::calculateInverseDynamics
 150         int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u,
 151                                                                  vecx* joint_forces);
 152         ///\copydoc MultiBodyTree::calculateMassMatrix
 153         int calculateMassMatrix(const vecx& q, const bool update_kinematics,
 154                                                         const bool initialize_matrix, const bool set_lower_triangular_matrix,
 155                                                         matxx* mass_matrix);
 156         /// calculate kinematics (vector quantities)
 157         /// Depending on type, update positions only, positions & velocities, or positions, velocities
 158         /// and accelerations.
 159         int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u, const KinUpdateType type);
 160 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
 161         /// calculate jacobians and (if type == POSITION_VELOCITY), also velocity-dependent accelration terms.
 162         int calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type);
 163         /// \copydoc MultiBodyTree::getBodyDotJacobianTransU
 164         int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const;
 165         /// \copydoc MultiBodyTree::getBodyDotJacobianRotU
 166         int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
 167         /// \copydoc MultiBodyTree::getBodyJacobianTrans
 168         int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const;
 169         /// \copydoc MultiBodyTree::getBodyJacobianRot
 170         int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
 171         /// Add relative Jacobian component from motion relative to parent body
 172         /// @param body the body to add the Jacobian component for
 173         void addRelativeJacobianComponent(RigidBody& body);
 174 #endif
 175         /// generate additional index sets from the parent_index array
 176         /// @return -1 on error, 0 on success
 177         int generateIndexSets();
 178         /// set gravity acceleration in world frame
 179         /// @param gravity gravity vector in the world frame
 180         /// @return 0 on success, -1 on error
 181         int setGravityInWorldFrame(const vec3& gravity);
 182         /// pretty print tree
 183         void printTree();
 184         /// print tree data
 185         void printTreeData();
 186         /// initialize fixed data
 187         void calculateStaticData();
 188         /// \copydoc MultiBodyTree::getBodyFrame
 189         int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const;
 190         /// \copydoc MultiBodyTree::getParentIndex
 191         int getParentIndex(const int body_index, int* m_parent_index);
 192         /// \copydoc MultiBodyTree::getJointType
 193         int getJointType(const int body_index, JointType* joint_type) const;
 194         /// \copydoc MultiBodyTree::getJointTypeStr
 195         int getJointTypeStr(const int body_index, const char** joint_type) const;
 196         /// \copydoc MultiBodyTree::getParentRParentBodyRef
 197         int getParentRParentBodyRef(const int body_index, vec3* r) const;
 198         /// \copydoc MultiBodyTree::getBodyTParentRef
 199         int getBodyTParentRef(const int body_index, mat33* T) const;
 200         /// \copydoc MultiBodyTree::getBodyAxisOfMotion
 201         int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
 202         /// \copydoc MultiBodyTree:getDoFOffset
 203         int getDoFOffset(const int body_index, int* q_index) const;
 204         /// \copydoc MultiBodyTree::getBodyOrigin
 205         int getBodyOrigin(const int body_index, vec3* world_origin) const;
 206         /// \copydoc MultiBodyTree::getBodyCoM
 207         int getBodyCoM(const int body_index, vec3* world_com) const;
 208         /// \copydoc MultiBodyTree::getBodyTransform
 209         int getBodyTransform(const int body_index, mat33* world_T_body) const;
 210         /// \copydoc MultiBodyTree::getBodyAngularVelocity
 211         int getBodyAngularVelocity(const int body_index, vec3* world_omega) const;
 212         /// \copydoc MultiBodyTree::getBodyLinearVelocity
 213         int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const;
 214         /// \copydoc MultiBodyTree::getBodyLinearVelocityCoM
 215         int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const;
 216         /// \copydoc MultiBodyTree::getBodyAngularAcceleration
 217         int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const;
 218         /// \copydoc MultiBodyTree::getBodyLinearAcceleration
 219         int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
 220         /// \copydoc MultiBodyTree::getUserInt
 221         int getUserInt(const int body_index, int* user_int) const;
 222         /// \copydoc MultiBodyTree::getUserPtr
 223         int getUserPtr(const int body_index, void** user_ptr) const;
 224         /// \copydoc MultiBodyTree::setUserInt
 225         int setUserInt(const int body_index, const int user_int);
 226         /// \copydoc MultiBodyTree::setUserPtr
 227         int setUserPtr(const int body_index, void* const user_ptr);
 228         ///\copydoc MultiBodytTree::setBodyMass
 229         int setBodyMass(const int body_index, const idScalar mass);
 230         ///\copydoc MultiBodytTree::setBodyFirstMassMoment
 231         int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment);
 232         ///\copydoc MultiBodytTree::setBodySecondMassMoment
 233         int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment);
 234         ///\copydoc MultiBodytTree::getBodyMass
 235         int getBodyMass(const int body_index, idScalar* mass) const;
 236         ///\copydoc MultiBodytTree::getBodyFirstMassMoment
 237         int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const;
 238         ///\copydoc MultiBodytTree::getBodySecondMassMoment
 239         int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const;
 240         /// \copydoc MultiBodyTree::clearAllUserForcesAndMoments
 241         void clearAllUserForcesAndMoments();
 242         /// \copydoc MultiBodyTree::addUserForce
 243         int addUserForce(const int body_index, const vec3& body_force);
 244         /// \copydoc MultiBodyTree::addUserMoment
 245         int addUserMoment(const int body_index, const vec3& body_moment);
 246
 247 private:
 248         // debug function. print tree structure to stdout
 249         void printTree(int index, int indentation);
 250         // get string representation of JointType (for debugging)
 251         const char* jointTypeToString(const JointType& type) const;
 252         // get number of degrees of freedom from joint type
 253         int bodyNumDoFs(const JointType& type) const;
 254         // number of bodies in the system
 255         int m_num_bodies;
 256         // number of degrees of freedom
 257         int m_num_dofs;
 258         // Gravitational acceleration (in world frame)
 259         vec3 m_world_gravity;
 260         // vector of bodies in the system
 261         // body 0 is used as an environment body and is allways fixed.
 262         // The bodies are ordered such that a parent body always has an index
 263         // smaller than its child.
 264         idArray<RigidBody>::type m_body_list;
 265         // Parent_index[i] is the index for i's parent body in body_list.
 266         // This fully describes the tree.
 267         idArray<int>::type m_parent_index;
 268         // child_indices[i] contains a vector of indices of
 269         // all children of the i-th body
 270         idArray<idArray<int>::type>::type m_child_indices;
 271         // Indices of rotary joints
 272         idArray<int>::type m_body_revolute_list;
 273         // Indices of prismatic joints
 274         idArray<int>::type m_body_prismatic_list;
 275         // Indices of floating joints
 276         idArray<int>::type m_body_floating_list;
 277         // Indices of spherical joints
 278         idArray<int>::type m_body_spherical_list;
 279         // a user-provided integer
 280         idArray<int>::type m_user_int;
 281         // a user-provided pointer
 282         idArray<void*>::type m_user_ptr;
 283 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
 284         mat3x m_m3x;
 285 #endif
 286 };
 287 }  // namespace btInverseDynamics
 288 #endif