467 lines
14 KiB
C
467 lines
14 KiB
C
|
/*
|
||
|
|
* 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
|