diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h | 466 |
1 files changed, 466 insertions, 0 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h new file mode 100644 index 00000000000..7177bebbff5 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h @@ -0,0 +1,466 @@ +/* + * PURPOSE: + * Class representing an articulated rigid body. Stores the body's + * current state, allows forces and torques to be set, handles + * timestepping and implements Featherstone's algorithm. + * + * COPYRIGHT: + * Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013 + * Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) + + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + + */ + + +#ifndef BT_MULTIBODY_H +#define BT_MULTIBODY_H + +#include "LinearMath/btScalar.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" +#include "LinearMath/btMatrix3x3.h" +#include "LinearMath/btAlignedObjectArray.h" + + +#include "btMultiBodyLink.h" +class btMultiBodyLinkCollider; + +class btMultiBody +{ +public: + + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + // + // initialization + // + + btMultiBody(int n_links, // NOT including the base + btScalar mass, // mass of base + const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal + bool fixed_base_, // whether the base is fixed (true) or can move (false) + bool can_sleep_); + + ~btMultiBody(); + + void setupPrismatic(int i, // 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, // in my frame; assumed diagonal + int parent, + const btQuaternion &rot_parent_to_this, // rotate points in parent frame to my frame. + const btVector3 &joint_axis, // in my frame + const btVector3 &r_vector_when_q_zero, // vector from parent COM to my COM, in my frame, when q = 0. + bool disableParentCollision=false + ); + + void setupRevolute(int i, // 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &zero_rot_parent_to_this, // rotate points in parent frame to this frame, when q = 0 + const btVector3 &joint_axis, // in my frame + const btVector3 &parent_axis_position, // vector from parent COM to joint axis, in PARENT frame + const btVector3 &my_axis_position, // vector from joint axis to my COM, in MY frame + bool disableParentCollision=false); + + const btMultibodyLink& getLink(int index) const + { + return links[index]; + } + + btMultibodyLink& getLink(int index) + { + return links[index]; + } + + + void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base + { + m_baseCollider = collider; + } + const btMultiBodyLinkCollider* getBaseCollider() const + { + return m_baseCollider; + } + btMultiBodyLinkCollider* getBaseCollider() + { + return m_baseCollider; + } + + // + // get parent + // input: link num from 0 to num_links-1 + // output: link num from 0 to num_links-1, OR -1 to mean the base. + // + int getParent(int link_num) const; + + + // + // get number of links, masses, moments of inertia + // + + int getNumLinks() const { return links.size(); } + btScalar getBaseMass() const { return base_mass; } + const btVector3 & getBaseInertia() const { return base_inertia; } + btScalar getLinkMass(int i) const; + const btVector3 & getLinkInertia(int i) const; + + + // + // change mass (incomplete: can only change base mass and inertia at present) + // + + void setBaseMass(btScalar mass) { base_mass = mass; } + void setBaseInertia(const btVector3 &inertia) { base_inertia = inertia; } + + + // + // get/set pos/vel/rot/omega for the base link + // + + const btVector3 & getBasePos() const { return base_pos; } // in world frame + const btVector3 getBaseVel() const + { + return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]); + } // in world frame + const btQuaternion & getWorldToBaseRot() const + { + return base_quat; + } // rotates world vectors into base frame + btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); } // in world frame + + void setBasePos(const btVector3 &pos) + { + base_pos = pos; + } + void setBaseVel(const btVector3 &vel) + { + + m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2]; + } + void setWorldToBaseRot(const btQuaternion &rot) + { + base_quat = rot; + } + void setBaseOmega(const btVector3 &omega) + { + m_real_buf[0]=omega[0]; + m_real_buf[1]=omega[1]; + m_real_buf[2]=omega[2]; + } + + + // + // get/set pos/vel for child links (i = 0 to num_links-1) + // + + btScalar getJointPos(int i) const; + btScalar getJointVel(int i) const; + + void setJointPos(int i, btScalar q); + void setJointVel(int i, btScalar qdot); + + // + // direct access to velocities as a vector of 6 + num_links elements. + // (omega first, then v, then joint velocities.) + // + const btScalar * getVelocityVector() const + { + return &m_real_buf[0]; + } +/* btScalar * getVelocityVector() + { + return &real_buf[0]; + } + */ + + // + // get the frames of reference (positions and orientations) of the child links + // (i = 0 to num_links-1) + // + + const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords + const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. + + + // + // transform vectors in local frame of link i to world frame (or vice versa) + // + btVector3 localPosToWorld(int i, const btVector3 &vec) const; + btVector3 localDirToWorld(int i, const btVector3 &vec) const; + btVector3 worldPosToLocal(int i, const btVector3 &vec) const; + btVector3 worldDirToLocal(int i, const btVector3 &vec) const; + + + // + // calculate kinetic energy and angular momentum + // useful for debugging. + // + + btScalar getKineticEnergy() const; + btVector3 getAngularMomentum() const; + + + // + // set external forces and torques. Note all external forces/torques are given in the WORLD frame. + // + + void clearForcesAndTorques(); + void clearVelocities(); + + void addBaseForce(const btVector3 &f) + { + base_force += f; + } + void addBaseTorque(const btVector3 &t) { base_torque += t; } + void addLinkForce(int i, const btVector3 &f); + void addLinkTorque(int i, const btVector3 &t); + void addJointTorque(int i, btScalar Q); + + const btVector3 & getBaseForce() const { return base_force; } + const btVector3 & getBaseTorque() const { return base_torque; } + const btVector3 & getLinkForce(int i) const; + const btVector3 & getLinkTorque(int i) const; + btScalar getJointTorque(int i) const; + + + // + // dynamics routines. + // + + // timestep the velocities (given the external forces/torques set using addBaseForce etc). + // also sets up caches for calcAccelerationDeltas. + // + // Note: the caller must provide three vectors which are used as + // temporary scratch space. The idea here is to reduce dynamic + // memory allocation: the same scratch vectors can be re-used + // again and again for different Multibodies, instead of each + // btMultiBody allocating (and then deallocating) their own + // individual scratch buffers. This gives a considerable speed + // improvement, at least on Windows (where dynamic memory + // allocation appears to be fairly slow). + // + void stepVelocities(btScalar dt, + btAlignedObjectArray<btScalar> &scratch_r, + btAlignedObjectArray<btVector3> &scratch_v, + btAlignedObjectArray<btMatrix3x3> &scratch_m); + + // calcAccelerationDeltas + // input: force vector (in same format as jacobian, i.e.: + // 3 torque values, 3 force values, num_links joint torque values) + // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values + // (existing contents of output array are replaced) + // stepVelocities must have been called first. + void calcAccelerationDeltas(const btScalar *force, btScalar *output, + btAlignedObjectArray<btScalar> &scratch_r, + btAlignedObjectArray<btVector3> &scratch_v) const; + + // apply a delta-vee directly. used in sequential impulses code. + void applyDeltaVee(const btScalar * delta_vee) + { + + for (int i = 0; i < 6 + getNumLinks(); ++i) + { + m_real_buf[i] += delta_vee[i]; + } + + } + void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) + { + btScalar sum = 0; + for (int i = 0; i < 6 + getNumLinks(); ++i) + { + sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; + } + btScalar l = btSqrt(sum); + /* + static btScalar maxl = -1e30f; + if (l>maxl) + { + maxl=l; + // printf("maxl=%f\n",maxl); + } + */ + if (l>m_maxAppliedImpulse) + { +// printf("exceeds 100: l=%f\n",maxl); + multiplier *= m_maxAppliedImpulse/l; + } + + for (int i = 0; i < 6 + getNumLinks(); ++i) + { + sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; + m_real_buf[i] += delta_vee[i] * multiplier; + } + } + + // timestep the positions (given current velocities). + void stepPositions(btScalar dt); + + + // + // contacts + // + + // This routine fills out a contact constraint jacobian for this body. + // the 'normal' supplied must be -n for body1 or +n for body2 of the contact. + // 'normal' & 'contact_point' are both given in world coordinates. + void fillContactJacobian(int link, + const btVector3 &contact_point, + const btVector3 &normal, + btScalar *jac, + btAlignedObjectArray<btScalar> &scratch_r, + btAlignedObjectArray<btVector3> &scratch_v, + btAlignedObjectArray<btMatrix3x3> &scratch_m) const; + + + // + // sleeping + // + void setCanSleep(bool canSleep) + { + can_sleep = canSleep; + } + + bool isAwake() const { return awake; } + void wakeUp(); + void goToSleep(); + void checkMotionAndSleepIfRequired(btScalar timestep); + + bool hasFixedBase() const + { + return fixed_base; + } + + int getCompanionId() const + { + return m_companionId; + } + void setCompanionId(int id) + { + //printf("for %p setCompanionId(%d)\n",this, id); + m_companionId = id; + } + + void setNumLinks(int numLinks)//careful: when changing the number of links, make sure to re-initialize or update existing links + { + links.resize(numLinks); + } + + btScalar getLinearDamping() const + { + return m_linearDamping; + } + void setLinearDamping( btScalar damp) + { + m_linearDamping = damp; + } + btScalar getAngularDamping() const + { + return m_angularDamping; + } + + bool getUseGyroTerm() const + { + return m_useGyroTerm; + } + void setUseGyroTerm(bool useGyro) + { + m_useGyroTerm = useGyro; + } + btScalar getMaxAppliedImpulse() const + { + return m_maxAppliedImpulse; + } + void setMaxAppliedImpulse(btScalar maxImp) + { + m_maxAppliedImpulse = maxImp; + } + + void setHasSelfCollision(bool hasSelfCollision) + { + m_hasSelfCollision = hasSelfCollision; + } + bool hasSelfCollision() const + { + return m_hasSelfCollision; + } + +private: + btMultiBody(const btMultiBody &); // not implemented + void operator=(const btMultiBody &); // not implemented + + void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; + + void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const; + + +private: + + btMultiBodyLinkCollider* m_baseCollider;//can be NULL + + btVector3 base_pos; // position of COM of base (world frame) + btQuaternion base_quat; // rotates world points into base frame + + btScalar base_mass; // mass of the base + btVector3 base_inertia; // inertia of the base (in local frame; diagonal) + + btVector3 base_force; // external force applied to base. World frame. + btVector3 base_torque; // external torque applied to base. World frame. + + btAlignedObjectArray<btMultibodyLink> links; // array of links, excluding the base. index from 0 to num_links-1. + btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders; + + // + // real_buf: + // offset size array + // 0 6 + num_links v (base_omega; base_vel; joint_vels) + // 6+num_links num_links D + // + // vector_buf: + // offset size array + // 0 num_links h_top + // num_links num_links h_bottom + // + // matrix_buf: + // offset size array + // 0 num_links+1 rot_from_parent + // + + btAlignedObjectArray<btScalar> m_real_buf; + btAlignedObjectArray<btVector3> vector_buf; + btAlignedObjectArray<btMatrix3x3> matrix_buf; + + //std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu; + + btMatrix3x3 cached_inertia_top_left; + btMatrix3x3 cached_inertia_top_right; + btMatrix3x3 cached_inertia_lower_left; + btMatrix3x3 cached_inertia_lower_right; + + bool fixed_base; + + // Sleep parameters. + bool awake; + bool can_sleep; + btScalar sleep_timer; + + int m_companionId; + btScalar m_linearDamping; + btScalar m_angularDamping; + bool m_useGyroTerm; + btScalar m_maxAppliedImpulse; + bool m_hasSelfCollision; +}; + +#endif |