diff options
author | Rémi Verschelde <rverschelde@gmail.com> | 2022-03-09 21:15:53 +0100 |
---|---|---|
committer | Rémi Verschelde <rverschelde@gmail.com> | 2022-03-09 21:45:47 +0100 |
commit | 3d7f1555865a981b7144becfc58d3f3f34362f5f (patch) | |
tree | d92912c6d700468b3330148b9179026b9f4efcb4 /thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp | |
parent | 33c907f9f5b3ec1a43d0251d7cac80da49b5b658 (diff) |
Remove unused Bullet module and thirdparty code
It has been disabled in `master` since one year (#45852) and our plan
is for Bullet, and possibly other thirdparty physics engines, to be
implemented via GDExtension so that they can be selected by the users
who need them.
Diffstat (limited to 'thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp')
-rw-r--r-- | thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp | 367 |
1 files changed, 0 insertions, 367 deletions
diff --git a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp deleted file mode 100644 index 7b852f976c..0000000000 --- a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp +++ /dev/null @@ -1,367 +0,0 @@ -#ifndef MULTIBODYTREE_HPP_ -#define MULTIBODYTREE_HPP_ - -#include "IDConfig.hpp" -#include "IDMath.hpp" - -namespace btInverseDynamics -{ -/// Enumeration of supported joint types -enum JointType -{ - /// no degree of freedom, moves with parent - FIXED = 0, - /// one rotational degree of freedom relative to parent - REVOLUTE, - /// one translational degree of freedom relative to parent - PRISMATIC, - /// six degrees of freedom relative to parent - FLOATING, - /// three degrees of freedom, relative to parent - SPHERICAL -}; - -/// Interface class for calculating inverse dynamics for tree structured -/// multibody systems -/// -/// Note on degrees of freedom -/// The q vector contains the generalized coordinate set defining the tree's configuration. -/// Every joint adds elements that define the corresponding link's frame pose relative to -/// its parent. For the joint types that is: -/// - FIXED: none -/// - REVOLUTE: angle of rotation [rad] -/// - PRISMATIC: displacement [m] -/// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m] -/// (in that order) -/// - SPHERICAL: Euler x-y-z angles [rad] -/// The u vector contains the generalized speeds, which are -/// - FIXED: none -/// - REVOLUTE: time derivative of angle of rotation [rad/s] -/// - PRISMATIC: time derivative of displacement [m/s] -/// - FLOATING: angular velocity [rad/s] (*not* time derivative of rpy angles) -/// and time derivative of displacement in parent frame [m/s] -// - SPHERICAL: angular velocity [rad/s] -/// -/// The q and u vectors are obtained by stacking contributions of all bodies in one -/// vector in the order of body indices. -/// -/// Note on generalized forces: analogous to u, i.e., -/// - FIXED: none -/// - REVOLUTE: moment [Nm], about joint axis -/// - PRISMATIC: force [N], along joint axis -/// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame -/// (in that order) -/// - SPHERICAL: moment vector [Nm] -/// TODO - force element interface (friction, springs, dampers, etc) -/// - gears and motor inertia -class MultiBodyTree -{ -public: - ID_DECLARE_ALIGNED_ALLOCATOR(); - /// The contructor. - /// Initialization & allocation is via addBody and buildSystem calls. - MultiBodyTree(); - /// the destructor. This also deallocates all memory - ~MultiBodyTree(); - - /// Add body to the system. this allocates memory and not real-time safe. - /// This only adds the data to an initial cache. After all bodies have been - /// added, - /// the system is setup using the buildSystem call - /// @param body_index index of the body to be added. Must >=0, <number of bodies, - /// and index of parent must be < index of body - /// @param parent_index index of the parent body - /// The root of the tree has index 0 and its parent (the world frame) - /// is assigned index -1 - /// the rotation and translation relative to the parent are taken as - /// pose of the root body relative to the world frame. Other parameters - /// are ignored - /// @param JointType type of joint connecting the body to the parent - /// @param mass the mass of the body - /// @param body_r_body_com the center of mass of the body relative to and - /// described in - /// the body fixed frame, which is located in the joint axis connecting - /// the body to its parent - /// @param body_I_body the moment of inertia of the body w.r.t the body-fixed - /// frame - /// (ie, the reference point is the origin of the body-fixed frame and - /// the matrix is written - /// w.r.t. those unit vectors) - /// @param parent_r_parent_body_ref position of joint relative to the parent - /// body's reference frame - /// for q=0, written in the parent bodies reference frame - /// @param body_axis_of_motion translation/rotation axis in body-fixed frame. - /// Ignored for joints that are not revolute or prismatic. - /// must be a unit vector. - /// @param body_T_parent_ref transform matrix from parent to body reference - /// frame for q=0. - /// This is the matrix transforming a vector represented in the - /// parent's reference frame into one represented - /// in this body's reference frame. - /// ie, if parent_vec is a vector in R^3 whose components are w.r.t to - /// the parent's reference frame, - /// then the same vector written w.r.t. this body's frame (for q=0) is - /// given by - /// body_vec = parent_R_body_ref * parent_vec - /// @param user_ptr pointer to user data - /// @param user_int pointer to user integer - /// @return 0 on success, -1 on error - int addBody(int body_index, int parent_index, JointType joint_type, - const vec3& parent_r_parent_body_ref, const mat33& body_T_parent_ref, - const vec3& body_axis_of_motion, idScalar mass, const vec3& body_r_body_com, - const mat33& body_I_body, const int user_int, void* user_ptr); - /// set policy for invalid mass properties - /// @param flag if true, invalid mass properties are accepted, - /// the default is false - void setAcceptInvalidMassParameters(bool flag); - /// @return the mass properties policy flag - bool getAcceptInvalidMassProperties() const; - /// build internal data structures - /// call this after all bodies have been added via addBody - /// @return 0 on success, -1 on error - int finalize(); - /// pretty print ascii description of tree to stdout - void printTree(); - /// print tree data to stdout - void printTreeData(); - /// Calculate joint forces for given generalized state & derivatives. - /// This also updates kinematic terms computed in calculateKinematics. - /// If gravity is not set to zero, acceleration terms will contain - /// gravitational acceleration. - /// @param q generalized coordinates - /// @param u generalized velocities. In the general case, u=T(q)*dot(q) and dim(q)>=dim(u) - /// @param dot_u time derivative of u - /// @param joint_forces this is where the resulting joint forces will be - /// stored. dim(joint_forces) = dim(u) - /// @return 0 on success, -1 on error - int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, - vecx* joint_forces); - /// Calculate joint space mass matrix - /// @param q generalized coordinates - /// @param initialize_matrix if true, initialize mass matrix with zero. - /// If mass_matrix is initialized to zero externally and only used - /// for mass matrix computations for the same system, it is safe to - /// set this to false. - /// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix - /// is also populated, otherwise not. - /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) - /// @return -1 on error, 0 on success - int calculateMassMatrix(const vecx& q, const bool update_kinematics, - const bool initialize_matrix, const bool set_lower_triangular_matrix, - matxx* mass_matrix); - - /// Calculate joint space mass matrix. - /// This version will update kinematics, initialize all mass_matrix elements to zero and - /// populate all mass matrix entries. - /// @param q generalized coordinates - /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) - /// @return -1 on error, 0 on success - int calculateMassMatrix(const vecx& q, matxx* mass_matrix); - - /// Calculates kinematics also calculated in calculateInverseDynamics, - /// but not dynamics. - /// This function ensures that correct accelerations are computed that do not - /// contain gravitational acceleration terms. - /// Does not calculate Jacobians, but only vector quantities (positions, velocities & accelerations) - int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u); - /// Calculate position kinematics - int calculatePositionKinematics(const vecx& q); - /// Calculate position and velocity kinematics - int calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u); - -#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - /// Calculate Jacobians (dvel/du), as well as velocity-dependent accelearation components - /// d(Jacobian)/dt*u - /// This function assumes that calculateInverseDynamics was called, or calculateKinematics, - /// or calculatePositionAndVelocityKinematics - int calculateJacobians(const vecx& q, const vecx& u); - /// Calculate Jacobians (dvel/du) - /// This function assumes that calculateInverseDynamics was called, or - /// one of the calculateKineamtics functions - int calculateJacobians(const vecx& q); -#endif // BT_ID_HAVE_MAT3X - - /// set gravitational acceleration - /// the default is [0;0;-9.8] in the world frame - /// @param gravity the gravitational acceleration in world frame - /// @return 0 on success, -1 on error - int setGravityInWorldFrame(const vec3& gravity); - /// returns number of bodies in tree - int numBodies() const; - /// returns number of mechanical degrees of freedom (dimension of q-vector) - int numDoFs() const; - /// get origin of a body-fixed frame, represented in world frame - /// @param body_index index for frame/body - /// @param world_origin pointer for return data - /// @return 0 on success, -1 on error - int getBodyOrigin(const int body_index, vec3* world_origin) const; - /// get center of mass of a body, represented in world frame - /// @param body_index index for frame/body - /// @param world_com pointer for return data - /// @return 0 on success, -1 on error - int getBodyCoM(const int body_index, vec3* world_com) const; - /// get transform from of a body-fixed frame to the world frame - /// @param body_index index for frame/body - /// @param world_T_body pointer for return data - /// @return 0 on success, -1 on error - int getBodyTransform(const int body_index, mat33* world_T_body) const; - /// get absolute angular velocity for a body, represented in the world frame - /// @param body_index index for frame/body - /// @param world_omega pointer for return data - /// @return 0 on success, -1 on error - int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; - /// get linear velocity of a body, represented in world frame - /// @param body_index index for frame/body - /// @param world_velocity pointer for return data - /// @return 0 on success, -1 on error - int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; - /// get linear velocity of a body's CoM, represented in world frame - /// (not required for inverse dynamics, provided for convenience) - /// @param body_index index for frame/body - /// @param world_vel_com pointer for return data - /// @return 0 on success, -1 on error - int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; - /// get origin of a body-fixed frame, represented in world frame - /// @param body_index index for frame/body - /// @param world_origin pointer for return data - /// @return 0 on success, -1 on error - int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; - /// get origin of a body-fixed frame, represented in world frame - /// NOTE: this will include the gravitational acceleration, so the actual acceleration is - /// obtainened by setting gravitational acceleration to zero, or subtracting it. - /// @param body_index index for frame/body - /// @param world_origin pointer for return data - /// @return 0 on success, -1 on error - int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; - -#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) - // get translational jacobian, in world frame (dworld_velocity/du) - int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const; - // get rotational jacobian, in world frame (dworld_omega/du) - int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const; - // get product of translational jacobian derivative * generatlized velocities - int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const; - // get product of rotational jacobian derivative * generatlized velocities - int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const; -#endif // BT_ID_HAVE_MAT3X - - /// returns the (internal) index of body - /// @param body_index is the index of a body - /// @param parent_index pointer to where parent index will be stored - /// @return 0 on success, -1 on error - int getParentIndex(const int body_index, int* parent_index) const; - /// get joint type - /// @param body_index index of the body - /// @param joint_type the corresponding joint type - /// @return 0 on success, -1 on failure - int getJointType(const int body_index, JointType* joint_type) const; - /// get joint type as string - /// @param body_index index of the body - /// @param joint_type string naming the corresponding joint type - /// @return 0 on success, -1 on failure - int getJointTypeStr(const int body_index, const char** joint_type) const; - /// get offset translation to parent body (see addBody) - /// @param body_index index of the body - /// @param r the offset translation (see above) - /// @return 0 on success, -1 on failure - int getParentRParentBodyRef(const int body_index, vec3* r) const; - /// get offset rotation to parent body (see addBody) - /// @param body_index index of the body - /// @param T the transform (see above) - /// @return 0 on success, -1 on failure - int getBodyTParentRef(const int body_index, mat33* T) const; - /// get axis of motion (see addBody) - /// @param body_index index of the body - /// @param axis the axis (see above) - /// @return 0 on success, -1 on failure - int getBodyAxisOfMotion(const int body_index, vec3* axis) const; - /// get offset for degrees of freedom of this body into the q-vector - /// @param body_index index of the body - /// @param q_offset offset the q vector - /// @return -1 on error, 0 on success - int getDoFOffset(const int body_index, int* q_offset) const; - /// get user integer. not used by the library. - /// @param body_index index of the body - /// @param user_int the user integer - /// @return 0 on success, -1 on error - int getUserInt(const int body_index, int* user_int) const; - /// get user pointer. not used by the library. - /// @param body_index index of the body - /// @param user_ptr the user pointer - /// @return 0 on success, -1 on error - int getUserPtr(const int body_index, void** user_ptr) const; - /// set user integer. not used by the library. - /// @param body_index index of the body - /// @param user_int the user integer - /// @return 0 on success, -1 on error - int setUserInt(const int body_index, const int user_int); - /// set user pointer. not used by the library. - /// @param body_index index of the body - /// @param user_ptr the user pointer - /// @return 0 on success, -1 on error - int setUserPtr(const int body_index, void* const user_ptr); - /// set mass for a body - /// @param body_index index of the body - /// @param mass the mass to set - /// @return 0 on success, -1 on failure - int setBodyMass(const int body_index, const idScalar mass); - /// set first moment of mass for a body - /// (mass * center of mass, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param first_mass_moment the vector to set - /// @return 0 on success, -1 on failure - int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); - /// set second moment of mass for a body - /// (moment of inertia, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param second_mass_moment the inertia matrix - /// @return 0 on success, -1 on failure - int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); - /// get mass for a body - /// @param body_index index of the body - /// @param mass the mass - /// @return 0 on success, -1 on failure - int getBodyMass(const int body_index, idScalar* mass) const; - /// get first moment of mass for a body - /// (mass * center of mass, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param first_moment the vector - /// @return 0 on success, -1 on failure - int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; - /// get second moment of mass for a body - /// (moment of inertia, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param second_mass_moment the inertia matrix - /// @return 0 on success, -1 on failure - int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; - /// set all user forces and moments to zero - void clearAllUserForcesAndMoments(); - /// Add an external force to a body, acting at the origin of the body-fixed frame. - /// Calls to addUserForce are cumulative. Set the user force and moment to zero - /// via clearAllUserForcesAndMoments() - /// @param body_force the force represented in the body-fixed frame of reference - /// @return 0 on success, -1 on error - int addUserForce(const int body_index, const vec3& body_force); - /// Add an external moment to a body. - /// Calls to addUserMoment are cumulative. Set the user force and moment to zero - /// via clearAllUserForcesAndMoments() - /// @param body_moment the moment represented in the body-fixed frame of reference - /// @return 0 on success, -1 on error - int addUserMoment(const int body_index, const vec3& body_moment); - -private: - // flag indicating if system has been initialized - bool m_is_finalized; - // flag indicating if mass properties are physically valid - bool m_mass_parameters_are_valid; - // flag defining if unphysical mass parameters are accepted - bool m_accept_invalid_mass_parameters; - // This struct implements the inverse dynamics calculations - class MultiBodyImpl; - MultiBodyImpl* m_impl; - // cache data structure for initialization - class InitCache; - InitCache* m_init_cache; -}; -} // namespace btInverseDynamics -#endif // MULTIBODYTREE_HPP_ |