d830681674
compare joint feedback between multi body and rigid body. initial results are promising (not exactly the same, but reasonably close)
2356 lines
84 KiB
C++
2356 lines
84 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.
|
|
|
|
*/
|
|
|
|
|
|
#include "btMultiBody.h"
|
|
#include "btMultiBodyLink.h"
|
|
#include "btMultiBodyLinkCollider.h"
|
|
#include "btMultiBodyJointFeedback.h"
|
|
#include "LinearMath/btTransformUtil.h"
|
|
|
|
#include "Bullet3Common/b3Logging.h"
|
|
// #define INCLUDE_GYRO_TERM
|
|
|
|
///todo: determine if we need this option. If so, make a proper API, otherwise delete the global
|
|
bool gJointFeedbackInWorldSpace = false;
|
|
|
|
namespace {
|
|
const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
|
|
const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
|
|
}
|
|
|
|
namespace {
|
|
void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame
|
|
const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
|
|
const btVector3 &top_in, // top part of input vector
|
|
const btVector3 &bottom_in, // bottom part of input vector
|
|
btVector3 &top_out, // top part of output vector
|
|
btVector3 &bottom_out) // bottom part of output vector
|
|
{
|
|
top_out = rotation_matrix * top_in;
|
|
bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
|
|
}
|
|
|
|
void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
|
|
const btVector3 &displacement,
|
|
const btVector3 &top_in,
|
|
const btVector3 &bottom_in,
|
|
btVector3 &top_out,
|
|
btVector3 &bottom_out)
|
|
{
|
|
top_out = rotation_matrix.transpose() * top_in;
|
|
bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));
|
|
}
|
|
|
|
btScalar SpatialDotProduct(const btVector3 &a_top,
|
|
const btVector3 &a_bottom,
|
|
const btVector3 &b_top,
|
|
const btVector3 &b_bottom)
|
|
{
|
|
return a_bottom.dot(b_top) + a_top.dot(b_bottom);
|
|
}
|
|
|
|
void SpatialCrossProduct(const btVector3 &a_top,
|
|
const btVector3 &a_bottom,
|
|
const btVector3 &b_top,
|
|
const btVector3 &b_bottom,
|
|
btVector3 &top_out,
|
|
btVector3 &bottom_out)
|
|
{
|
|
top_out = a_top.cross(b_top);
|
|
bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
|
|
}
|
|
}
|
|
|
|
|
|
//
|
|
// Implementation of class btMultiBody
|
|
//
|
|
|
|
btMultiBody::btMultiBody(int n_links,
|
|
btScalar mass,
|
|
const btVector3 &inertia,
|
|
bool fixedBase,
|
|
bool canSleep,
|
|
bool multiDof)
|
|
:
|
|
m_baseCollider(0),
|
|
m_basePos(0,0,0),
|
|
m_baseQuat(0, 0, 0, 1),
|
|
m_baseMass(mass),
|
|
m_baseInertia(inertia),
|
|
|
|
m_fixedBase(fixedBase),
|
|
m_awake(true),
|
|
m_canSleep(canSleep),
|
|
m_sleepTimer(0),
|
|
|
|
m_linearDamping(0.04f),
|
|
m_angularDamping(0.04f),
|
|
m_useGyroTerm(true),
|
|
m_maxAppliedImpulse(1000.f),
|
|
m_maxCoordinateVelocity(100.f),
|
|
m_hasSelfCollision(true),
|
|
m_isMultiDof(multiDof),
|
|
__posUpdated(false),
|
|
m_dofCount(0),
|
|
m_posVarCnt(0),
|
|
m_useRK4(false),
|
|
m_useGlobalVelocities(false),
|
|
m_internalNeedsJointFeedback(false)
|
|
{
|
|
|
|
if(!m_isMultiDof)
|
|
{
|
|
m_vectorBuf.resize(2*n_links);
|
|
m_realBuf.resize(6 + 2*n_links);
|
|
m_posVarCnt = n_links;
|
|
}
|
|
|
|
m_links.resize(n_links);
|
|
m_matrixBuf.resize(n_links + 1);
|
|
|
|
|
|
m_baseForce.setValue(0, 0, 0);
|
|
m_baseTorque.setValue(0, 0, 0);
|
|
}
|
|
|
|
btMultiBody::~btMultiBody()
|
|
{
|
|
}
|
|
|
|
void btMultiBody::setupFixed(int i,
|
|
btScalar mass,
|
|
const btVector3 &inertia,
|
|
int parent,
|
|
const btQuaternion &rotParentToThis,
|
|
const btVector3 &parentComToThisPivotOffset,
|
|
const btVector3 &thisPivotToThisComOffset,
|
|
bool disableParentCollision)
|
|
{
|
|
btAssert(m_isMultiDof);
|
|
|
|
m_links[i].m_mass = mass;
|
|
m_links[i].m_inertiaLocal = inertia;
|
|
m_links[i].m_parent = parent;
|
|
m_links[i].m_zeroRotParentToThis = rotParentToThis;
|
|
m_links[i].m_dVector = thisPivotToThisComOffset;
|
|
m_links[i].m_eVector = parentComToThisPivotOffset;
|
|
|
|
m_links[i].m_jointType = btMultibodyLink::eFixed;
|
|
m_links[i].m_dofCount = 0;
|
|
m_links[i].m_posVarCount = 0;
|
|
|
|
if (disableParentCollision)
|
|
m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
|
|
//
|
|
m_links[i].updateCacheMultiDof();
|
|
|
|
//
|
|
//if(m_isMultiDof)
|
|
// resizeInternalMultiDofBuffers();
|
|
//
|
|
updateLinksDofOffsets();
|
|
|
|
}
|
|
|
|
|
|
void btMultiBody::setupPrismatic(int i,
|
|
btScalar mass,
|
|
const btVector3 &inertia,
|
|
int parent,
|
|
const btQuaternion &rotParentToThis,
|
|
const btVector3 &jointAxis,
|
|
const btVector3 &parentComToThisComOffset,
|
|
const btVector3 &thisPivotToThisComOffset,
|
|
bool disableParentCollision)
|
|
{
|
|
if(m_isMultiDof)
|
|
{
|
|
m_dofCount += 1;
|
|
m_posVarCnt += 1;
|
|
}
|
|
|
|
m_links[i].m_mass = mass;
|
|
m_links[i].m_inertiaLocal = inertia;
|
|
m_links[i].m_parent = parent;
|
|
m_links[i].m_zeroRotParentToThis = rotParentToThis;
|
|
m_links[i].setAxisTop(0, 0., 0., 0.);
|
|
m_links[i].setAxisBottom(0, jointAxis);
|
|
m_links[i].m_eVector = parentComToThisComOffset;
|
|
m_links[i].m_dVector = thisPivotToThisComOffset;
|
|
m_links[i].m_cachedRotParentToThis = rotParentToThis;
|
|
|
|
m_links[i].m_jointType = btMultibodyLink::ePrismatic;
|
|
m_links[i].m_dofCount = 1;
|
|
m_links[i].m_posVarCount = 1;
|
|
m_links[i].m_jointPos[0] = 0.f;
|
|
m_links[i].m_jointTorque[0] = 0.f;
|
|
|
|
if (disableParentCollision)
|
|
m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
|
|
//
|
|
if(m_isMultiDof)
|
|
m_links[i].updateCacheMultiDof();
|
|
else
|
|
m_links[i].updateCache();
|
|
//
|
|
if(m_isMultiDof)
|
|
updateLinksDofOffsets();
|
|
}
|
|
|
|
void btMultiBody::setupRevolute(int i,
|
|
btScalar mass,
|
|
const btVector3 &inertia,
|
|
int parent,
|
|
const btQuaternion &rotParentToThis,
|
|
const btVector3 &jointAxis,
|
|
const btVector3 &parentComToThisPivotOffset,
|
|
const btVector3 &thisPivotToThisComOffset,
|
|
bool disableParentCollision)
|
|
{
|
|
if(m_isMultiDof)
|
|
{
|
|
m_dofCount += 1;
|
|
m_posVarCnt += 1;
|
|
}
|
|
|
|
m_links[i].m_mass = mass;
|
|
m_links[i].m_inertiaLocal = inertia;
|
|
m_links[i].m_parent = parent;
|
|
m_links[i].m_zeroRotParentToThis = rotParentToThis;
|
|
m_links[i].setAxisTop(0, jointAxis);
|
|
m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
|
|
m_links[i].m_dVector = thisPivotToThisComOffset;
|
|
m_links[i].m_eVector = parentComToThisPivotOffset;
|
|
|
|
m_links[i].m_jointType = btMultibodyLink::eRevolute;
|
|
m_links[i].m_dofCount = 1;
|
|
m_links[i].m_posVarCount = 1;
|
|
m_links[i].m_jointPos[0] = 0.f;
|
|
m_links[i].m_jointTorque[0] = 0.f;
|
|
|
|
if (disableParentCollision)
|
|
m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
|
|
//
|
|
if(m_isMultiDof)
|
|
m_links[i].updateCacheMultiDof();
|
|
else
|
|
m_links[i].updateCache();
|
|
//
|
|
if(m_isMultiDof)
|
|
updateLinksDofOffsets();
|
|
}
|
|
|
|
|
|
|
|
void btMultiBody::setupSpherical(int i,
|
|
btScalar mass,
|
|
const btVector3 &inertia,
|
|
int parent,
|
|
const btQuaternion &rotParentToThis,
|
|
const btVector3 &parentComToThisPivotOffset,
|
|
const btVector3 &thisPivotToThisComOffset,
|
|
bool disableParentCollision)
|
|
{
|
|
btAssert(m_isMultiDof);
|
|
m_dofCount += 3;
|
|
m_posVarCnt += 4;
|
|
|
|
m_links[i].m_mass = mass;
|
|
m_links[i].m_inertiaLocal = inertia;
|
|
m_links[i].m_parent = parent;
|
|
m_links[i].m_zeroRotParentToThis = rotParentToThis;
|
|
m_links[i].m_dVector = thisPivotToThisComOffset;
|
|
m_links[i].m_eVector = parentComToThisPivotOffset;
|
|
|
|
m_links[i].m_jointType = btMultibodyLink::eSpherical;
|
|
m_links[i].m_dofCount = 3;
|
|
m_links[i].m_posVarCount = 4;
|
|
m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
|
|
m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
|
|
m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
|
|
m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
|
|
m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
|
|
m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
|
|
m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointPos[3] = 1.f;
|
|
m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
|
|
|
|
|
|
if (disableParentCollision)
|
|
m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
|
|
//
|
|
m_links[i].updateCacheMultiDof();
|
|
//
|
|
updateLinksDofOffsets();
|
|
}
|
|
|
|
void btMultiBody::setupPlanar(int i,
|
|
btScalar mass,
|
|
const btVector3 &inertia,
|
|
int parent,
|
|
const btQuaternion &rotParentToThis,
|
|
const btVector3 &rotationAxis,
|
|
const btVector3 &parentComToThisComOffset,
|
|
bool disableParentCollision)
|
|
{
|
|
btAssert(m_isMultiDof);
|
|
m_dofCount += 3;
|
|
m_posVarCnt += 3;
|
|
|
|
m_links[i].m_mass = mass;
|
|
m_links[i].m_inertiaLocal = inertia;
|
|
m_links[i].m_parent = parent;
|
|
m_links[i].m_zeroRotParentToThis = rotParentToThis;
|
|
m_links[i].m_dVector.setZero();
|
|
m_links[i].m_eVector = parentComToThisComOffset;
|
|
|
|
//
|
|
static btVector3 vecNonParallelToRotAxis(1, 0, 0);
|
|
if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
|
|
vecNonParallelToRotAxis.setValue(0, 1, 0);
|
|
//
|
|
|
|
m_links[i].m_jointType = btMultibodyLink::ePlanar;
|
|
m_links[i].m_dofCount = 3;
|
|
m_links[i].m_posVarCount = 3;
|
|
btVector3 n=rotationAxis.normalized();
|
|
m_links[i].setAxisTop(0, n[0],n[1],n[2]);
|
|
m_links[i].setAxisTop(1,0,0,0);
|
|
m_links[i].setAxisTop(2,0,0,0);
|
|
m_links[i].setAxisBottom(0,0,0,0);
|
|
btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
|
|
m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
|
|
cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
|
|
m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
|
|
m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
|
|
m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
|
|
|
|
if (disableParentCollision)
|
|
m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
|
|
//
|
|
m_links[i].updateCacheMultiDof();
|
|
//
|
|
updateLinksDofOffsets();
|
|
}
|
|
|
|
void btMultiBody::finalizeMultiDof()
|
|
{
|
|
btAssert(m_isMultiDof);
|
|
|
|
m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
|
|
m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
|
|
|
|
updateLinksDofOffsets();
|
|
}
|
|
|
|
int btMultiBody::getParent(int i) const
|
|
{
|
|
return m_links[i].m_parent;
|
|
}
|
|
|
|
btScalar btMultiBody::getLinkMass(int i) const
|
|
{
|
|
return m_links[i].m_mass;
|
|
}
|
|
|
|
const btVector3 & btMultiBody::getLinkInertia(int i) const
|
|
{
|
|
return m_links[i].m_inertiaLocal;
|
|
}
|
|
|
|
btScalar btMultiBody::getJointPos(int i) const
|
|
{
|
|
return m_links[i].m_jointPos[0];
|
|
}
|
|
|
|
btScalar btMultiBody::getJointVel(int i) const
|
|
{
|
|
return m_realBuf[6 + i];
|
|
}
|
|
|
|
btScalar * btMultiBody::getJointPosMultiDof(int i)
|
|
{
|
|
return &m_links[i].m_jointPos[0];
|
|
}
|
|
|
|
btScalar * btMultiBody::getJointVelMultiDof(int i)
|
|
{
|
|
return &m_realBuf[6 + m_links[i].m_dofOffset];
|
|
}
|
|
|
|
void btMultiBody::setJointPos(int i, btScalar q)
|
|
{
|
|
m_links[i].m_jointPos[0] = q;
|
|
m_links[i].updateCache();
|
|
}
|
|
|
|
void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
|
|
{
|
|
for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
|
|
m_links[i].m_jointPos[pos] = q[pos];
|
|
|
|
m_links[i].updateCacheMultiDof();
|
|
}
|
|
|
|
void btMultiBody::setJointVel(int i, btScalar qdot)
|
|
{
|
|
m_realBuf[6 + i] = qdot;
|
|
}
|
|
|
|
void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot)
|
|
{
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof];
|
|
}
|
|
|
|
const btVector3 & btMultiBody::getRVector(int i) const
|
|
{
|
|
return m_links[i].m_cachedRVector;
|
|
}
|
|
|
|
const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
|
|
{
|
|
return m_links[i].m_cachedRotParentToThis;
|
|
}
|
|
|
|
btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
|
|
{
|
|
btVector3 result = local_pos;
|
|
while (i != -1) {
|
|
// 'result' is in frame i. transform it to frame parent(i)
|
|
result += getRVector(i);
|
|
result = quatRotate(getParentToLocalRot(i).inverse(),result);
|
|
i = getParent(i);
|
|
}
|
|
|
|
// 'result' is now in the base frame. transform it to world frame
|
|
result = quatRotate(getWorldToBaseRot().inverse() ,result);
|
|
result += getBasePos();
|
|
|
|
return result;
|
|
}
|
|
|
|
btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
|
|
{
|
|
if (i == -1) {
|
|
// world to base
|
|
return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
|
|
} else {
|
|
// find position in parent frame, then transform to current frame
|
|
return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
|
|
}
|
|
}
|
|
|
|
btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
|
|
{
|
|
btVector3 result = local_dir;
|
|
while (i != -1) {
|
|
result = quatRotate(getParentToLocalRot(i).inverse() , result);
|
|
i = getParent(i);
|
|
}
|
|
result = quatRotate(getWorldToBaseRot().inverse() , result);
|
|
return result;
|
|
}
|
|
|
|
btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
|
|
{
|
|
if (i == -1) {
|
|
return quatRotate(getWorldToBaseRot(), world_dir);
|
|
} else {
|
|
return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir));
|
|
}
|
|
}
|
|
|
|
void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
|
|
{
|
|
int num_links = getNumLinks();
|
|
// Calculates the velocities of each link (and the base) in its local frame
|
|
omega[0] = quatRotate(m_baseQuat ,getBaseOmega());
|
|
vel[0] = quatRotate(m_baseQuat ,getBaseVel());
|
|
|
|
for (int i = 0; i < num_links; ++i) {
|
|
const int parent = m_links[i].m_parent;
|
|
|
|
// transform parent vel into this frame, store in omega[i+1], vel[i+1]
|
|
SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector,
|
|
omega[parent+1], vel[parent+1],
|
|
omega[i+1], vel[i+1]);
|
|
|
|
// now add qidot * shat_i
|
|
omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
|
|
vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
|
|
}
|
|
}
|
|
|
|
btScalar btMultiBody::getKineticEnergy() const
|
|
{
|
|
int num_links = getNumLinks();
|
|
// TODO: would be better not to allocate memory here
|
|
btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
|
|
btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
|
|
compTreeLinkVelocities(&omega[0], &vel[0]);
|
|
|
|
// we will do the factor of 0.5 at the end
|
|
btScalar result = m_baseMass * vel[0].dot(vel[0]);
|
|
result += omega[0].dot(m_baseInertia * omega[0]);
|
|
|
|
for (int i = 0; i < num_links; ++i) {
|
|
result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
|
|
result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]);
|
|
}
|
|
|
|
return 0.5f * result;
|
|
}
|
|
|
|
btVector3 btMultiBody::getAngularMomentum() const
|
|
{
|
|
int num_links = getNumLinks();
|
|
// TODO: would be better not to allocate memory here
|
|
btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
|
|
btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
|
|
btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1);
|
|
compTreeLinkVelocities(&omega[0], &vel[0]);
|
|
|
|
rot_from_world[0] = m_baseQuat;
|
|
btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0]));
|
|
|
|
for (int i = 0; i < num_links; ++i) {
|
|
rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1];
|
|
result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
|
|
void btMultiBody::clearForcesAndTorques()
|
|
{
|
|
m_baseForce.setValue(0, 0, 0);
|
|
m_baseTorque.setValue(0, 0, 0);
|
|
|
|
|
|
for (int i = 0; i < getNumLinks(); ++i) {
|
|
m_links[i].m_appliedForce.setValue(0, 0, 0);
|
|
m_links[i].m_appliedTorque.setValue(0, 0, 0);
|
|
m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f;
|
|
}
|
|
}
|
|
|
|
void btMultiBody::clearVelocities()
|
|
{
|
|
for (int i = 0; i < 6 + getNumLinks(); ++i)
|
|
{
|
|
m_realBuf[i] = 0.f;
|
|
}
|
|
}
|
|
void btMultiBody::addLinkForce(int i, const btVector3 &f)
|
|
{
|
|
m_links[i].m_appliedForce += f;
|
|
}
|
|
|
|
void btMultiBody::addLinkTorque(int i, const btVector3 &t)
|
|
{
|
|
m_links[i].m_appliedTorque += t;
|
|
}
|
|
|
|
void btMultiBody::addJointTorque(int i, btScalar Q)
|
|
{
|
|
m_links[i].m_jointTorque[0] += Q;
|
|
}
|
|
|
|
void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q)
|
|
{
|
|
m_links[i].m_jointTorque[dof] += Q;
|
|
}
|
|
|
|
void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q)
|
|
{
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
m_links[i].m_jointTorque[dof] = Q[dof];
|
|
}
|
|
|
|
const btVector3 & btMultiBody::getLinkForce(int i) const
|
|
{
|
|
return m_links[i].m_appliedForce;
|
|
}
|
|
|
|
const btVector3 & btMultiBody::getLinkTorque(int i) const
|
|
{
|
|
return m_links[i].m_appliedTorque;
|
|
}
|
|
|
|
btScalar btMultiBody::getJointTorque(int i) const
|
|
{
|
|
return m_links[i].m_jointTorque[0];
|
|
}
|
|
|
|
btScalar * btMultiBody::getJointTorqueMultiDof(int i)
|
|
{
|
|
return &m_links[i].m_jointTorque[0];
|
|
}
|
|
|
|
inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?
|
|
{
|
|
btVector3 row0 = btVector3(
|
|
v0.x() * v1.x(),
|
|
v0.x() * v1.y(),
|
|
v0.x() * v1.z());
|
|
btVector3 row1 = btVector3(
|
|
v0.y() * v1.x(),
|
|
v0.y() * v1.y(),
|
|
v0.y() * v1.z());
|
|
btVector3 row2 = btVector3(
|
|
v0.z() * v1.x(),
|
|
v0.z() * v1.y(),
|
|
v0.z() * v1.z());
|
|
|
|
btMatrix3x3 m(row0[0],row0[1],row0[2],
|
|
row1[0],row1[1],row1[2],
|
|
row2[0],row2[1],row2[2]);
|
|
return m;
|
|
}
|
|
|
|
#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
|
|
//
|
|
|
|
void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
|
btAlignedObjectArray<btScalar> &scratch_r,
|
|
btAlignedObjectArray<btVector3> &scratch_v,
|
|
btAlignedObjectArray<btMatrix3x3> &scratch_m)
|
|
{
|
|
// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
|
|
// and the base linear & angular accelerations.
|
|
|
|
// We apply damping forces in this routine as well as any external forces specified by the
|
|
// caller (via addBaseForce etc).
|
|
|
|
// output should point to an array of 6 + num_links reals.
|
|
// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
|
|
// num_links joint acceleration values.
|
|
|
|
m_internalNeedsJointFeedback = false;
|
|
|
|
int num_links = getNumLinks();
|
|
|
|
const btScalar DAMPING_K1_LINEAR = m_linearDamping;
|
|
const btScalar DAMPING_K2_LINEAR = m_linearDamping;
|
|
|
|
const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
|
|
const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
|
|
|
|
btVector3 base_vel = getBaseVel();
|
|
btVector3 base_omega = getBaseOmega();
|
|
|
|
// Temporary matrices/vectors -- use scratch space from caller
|
|
// so that we don't have to keep reallocating every frame
|
|
|
|
scratch_r.resize(2*m_dofCount + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
|
|
scratch_v.resize(8*num_links + 6);
|
|
scratch_m.resize(4*num_links + 4);
|
|
|
|
//btScalar * r_ptr = &scratch_r[0];
|
|
btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results
|
|
btVector3 * v_ptr = &scratch_v[0];
|
|
|
|
// vhat_i (top = angular, bottom = linear part)
|
|
btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;
|
|
v_ptr += num_links * 2 + 2;
|
|
//
|
|
// zhat_i^A
|
|
btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
|
|
v_ptr += num_links * 2 + 2;
|
|
//
|
|
// chat_i (note NOT defined for the base)
|
|
btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
|
|
v_ptr += num_links * 2;
|
|
//
|
|
// Ihat_i^A.
|
|
btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];
|
|
|
|
// Cached 3x3 rotation matrices from parent frame to this frame.
|
|
btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
|
|
btMatrix3x3 * rot_from_world = &scratch_m[0];
|
|
|
|
// hhat_i, ahat_i
|
|
// hhat is NOT stored for the base (but ahat is)
|
|
btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
|
|
btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
|
|
v_ptr += num_links * 2 + 2;
|
|
//
|
|
// Y_i, invD_i
|
|
btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
|
|
btScalar * Y = &scratch_r[0];
|
|
//
|
|
//aux variables
|
|
static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
|
|
static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
|
|
static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
|
|
static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
|
|
static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
|
|
static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
|
|
static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
|
|
static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
|
|
static btSpatialTransformationMatrix fromWorld;
|
|
fromWorld.m_trnVec.setZero();
|
|
/////////////////
|
|
|
|
// ptr to the joint accel part of the output
|
|
btScalar * joint_accel = output + 6;
|
|
|
|
// Start of the algorithm proper.
|
|
|
|
// First 'upward' loop.
|
|
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
|
|
|
|
rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
|
|
|
|
//create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
|
|
spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
|
|
|
|
if (m_fixedBase)
|
|
{
|
|
zeroAccSpatFrc[0].setZero();
|
|
}
|
|
else
|
|
{
|
|
//external forces
|
|
zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * m_baseTorque), -(rot_from_parent[0] * m_baseForce));
|
|
|
|
//adding damping terms (only)
|
|
btScalar linDampMult = 1., angDampMult = 1.;
|
|
zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()),
|
|
linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm()));
|
|
|
|
//
|
|
//p += vhat x Ihat vhat - done in a simpler way
|
|
if (m_useGyroTerm)
|
|
zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));
|
|
//
|
|
zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
|
|
}
|
|
|
|
|
|
//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
|
|
spatInertia[0].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0),
|
|
//
|
|
btMatrix3x3(m_baseMass, 0, 0,
|
|
0, m_baseMass, 0,
|
|
0, 0, m_baseMass),
|
|
//
|
|
btMatrix3x3(m_baseInertia[0], 0, 0,
|
|
0, m_baseInertia[1], 0,
|
|
0, 0, m_baseInertia[2])
|
|
);
|
|
|
|
rot_from_world[0] = rot_from_parent[0];
|
|
|
|
//
|
|
for (int i = 0; i < num_links; ++i) {
|
|
const int parent = m_links[i].m_parent;
|
|
rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
|
|
rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
|
|
|
|
fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
|
|
fromWorld.m_rotMat = rot_from_world[i+1];
|
|
fromParent.transform(spatVel[parent+1], spatVel[i+1]);
|
|
|
|
// now set vhat_i to its true value by doing
|
|
// vhat_i += qidot * shat_i
|
|
if(!m_useGlobalVelocities)
|
|
{
|
|
spatJointVel.setZero();
|
|
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
|
|
|
|
// remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
|
|
spatVel[i+1] += spatJointVel;
|
|
|
|
//
|
|
// vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
|
|
//spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
|
|
|
|
}
|
|
else
|
|
{
|
|
fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]);
|
|
fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
|
|
}
|
|
|
|
// we can now calculate chat_i
|
|
spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]);
|
|
|
|
// calculate zhat_i^A
|
|
//
|
|
//external forces
|
|
zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * m_links[i].m_appliedTorque), -(rot_from_world[i+1] * m_links[i].m_appliedForce));
|
|
|
|
if (0)
|
|
{
|
|
|
|
b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
|
|
i+1,
|
|
zeroAccSpatFrc[i+1].m_topVec[0],
|
|
zeroAccSpatFrc[i+1].m_topVec[1],
|
|
zeroAccSpatFrc[i+1].m_topVec[2],
|
|
|
|
zeroAccSpatFrc[i+1].m_bottomVec[0],
|
|
zeroAccSpatFrc[i+1].m_bottomVec[1],
|
|
zeroAccSpatFrc[i+1].m_bottomVec[2]);
|
|
}
|
|
//
|
|
//adding damping terms (only)
|
|
btScalar linDampMult = 1., angDampMult = 1.;
|
|
zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
|
|
linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
|
|
|
|
// calculate Ihat_i^A
|
|
//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
|
|
spatInertia[i+1].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0),
|
|
//
|
|
btMatrix3x3(m_links[i].m_mass, 0, 0,
|
|
0, m_links[i].m_mass, 0,
|
|
0, 0, m_links[i].m_mass),
|
|
//
|
|
btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
|
|
0, m_links[i].m_inertiaLocal[1], 0,
|
|
0, 0, m_links[i].m_inertiaLocal[2])
|
|
);
|
|
//
|
|
//p += vhat x Ihat vhat - done in a simpler way
|
|
if(m_useGyroTerm)
|
|
zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));
|
|
//
|
|
zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
|
|
//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
|
|
////clamp parent's omega
|
|
//btScalar parOmegaMod = temp.length();
|
|
//btScalar parOmegaModMax = 1000;
|
|
//if(parOmegaMod > parOmegaModMax)
|
|
// temp *= parOmegaModMax / parOmegaMod;
|
|
//zeroAccSpatFrc[i+1].addLinear(temp);
|
|
//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
|
|
//temp = spatCoriolisAcc[i].getLinear();
|
|
//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
|
|
|
|
|
|
|
|
//printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
|
|
//printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());
|
|
//printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
|
|
}
|
|
|
|
// 'Downward' loop.
|
|
// (part of TreeForwardDynamics in Mirtich.)
|
|
for (int i = num_links - 1; i >= 0; --i)
|
|
{
|
|
const int parent = m_links[i].m_parent;
|
|
fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
|
|
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
|
|
//
|
|
hDof = spatInertia[i+1] * m_links[i].m_axes[dof];
|
|
//
|
|
Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
|
|
- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
|
|
- spatCoriolisAcc[i].dot(hDof)
|
|
;
|
|
}
|
|
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
btScalar *D_row = &D[dof * m_links[i].m_dofCount];
|
|
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
|
{
|
|
btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
|
|
D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
|
|
}
|
|
}
|
|
|
|
btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
|
|
switch(m_links[i].m_jointType)
|
|
{
|
|
case btMultibodyLink::ePrismatic:
|
|
case btMultibodyLink::eRevolute:
|
|
{
|
|
invDi[0] = 1.0f / D[0];
|
|
break;
|
|
}
|
|
case btMultibodyLink::eSpherical:
|
|
case btMultibodyLink::ePlanar:
|
|
{
|
|
static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
|
|
static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
|
|
|
|
//unroll the loop?
|
|
for(int row = 0; row < 3; ++row)
|
|
{
|
|
for(int col = 0; col < 3; ++col)
|
|
{
|
|
invDi[row * 3 + col] = invD3x3[row][col];
|
|
}
|
|
}
|
|
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
|
|
}
|
|
}
|
|
|
|
//determine h*D^{-1}
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
spatForceVecTemps[dof].setZero();
|
|
|
|
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
|
{
|
|
btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
|
|
//
|
|
spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
|
|
}
|
|
}
|
|
|
|
dyadTemp = spatInertia[i+1];
|
|
|
|
//determine (h*D^{-1}) * h^{T}
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
|
|
//
|
|
dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
|
|
}
|
|
|
|
fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add);
|
|
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
invD_times_Y[dof] = 0.f;
|
|
|
|
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
|
{
|
|
invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
|
|
}
|
|
}
|
|
|
|
spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i];
|
|
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
|
|
//
|
|
spatForceVecTemps[0] += hDof * invD_times_Y[dof];
|
|
}
|
|
|
|
fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
|
|
|
|
zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
|
|
}
|
|
|
|
|
|
// Second 'upward' loop
|
|
// (part of TreeForwardDynamics in Mirtich)
|
|
|
|
if (m_fixedBase)
|
|
{
|
|
spatAcc[0].setZero();
|
|
}
|
|
else
|
|
{
|
|
if (num_links > 0)
|
|
{
|
|
m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
|
|
m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
|
|
m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
|
|
m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose();
|
|
|
|
}
|
|
|
|
solveImatrix(zeroAccSpatFrc[0], result);
|
|
spatAcc[0] = -result;
|
|
}
|
|
|
|
|
|
// now do the loop over the m_links
|
|
for (int i = 0; i < num_links; ++i)
|
|
{
|
|
// qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)
|
|
// a = apar + cor + Sqdd
|
|
//or
|
|
// qdd = D^{-1} * (Y - h^{T}*(apar+cor))
|
|
// a = apar + Sqdd
|
|
|
|
const int parent = m_links[i].m_parent;
|
|
fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
|
|
|
|
fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
|
|
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
|
|
//
|
|
Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
|
|
}
|
|
|
|
btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
|
|
//D^{-1} * (Y - h^{T}*apar)
|
|
mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
|
|
|
|
spatAcc[i+1] += spatCoriolisAcc[i];
|
|
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
|
|
|
|
if (m_links[i].m_jointFeedback)
|
|
{
|
|
|
|
|
|
m_internalNeedsJointFeedback = true;
|
|
//m_links[i].m_jointFeedback->m_spatialInertia = spatInertia[i+1];
|
|
if (gJointFeedbackInWorldSpace)
|
|
{
|
|
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
|
|
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
|
|
} else
|
|
{
|
|
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
|
|
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
// transform base accelerations back to the world frame.
|
|
btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
|
|
output[0] = omegadot_out[0];
|
|
output[1] = omegadot_out[1];
|
|
output[2] = omegadot_out[2];
|
|
|
|
btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
|
|
output[3] = vdot_out[0];
|
|
output[4] = vdot_out[1];
|
|
output[5] = vdot_out[2];
|
|
|
|
/////////////////
|
|
//printf("q = [");
|
|
//printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
|
|
//for(int link = 0; link < getNumLinks(); ++link)
|
|
// for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
|
|
// printf("%.6f ", m_links[link].m_jointPos[dof]);
|
|
//printf("]\n");
|
|
////
|
|
//printf("qd = [");
|
|
//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
|
|
// printf("%.6f ", m_realBuf[dof]);
|
|
//printf("]\n");
|
|
//printf("qdd = [");
|
|
//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
|
|
// printf("%.6f ", output[dof]);
|
|
//printf("]\n");
|
|
/////////////////
|
|
|
|
// Final step: add the accelerations (times dt) to the velocities.
|
|
if(dt > 0.)
|
|
applyDeltaVeeMultiDof(output, dt);
|
|
|
|
/////
|
|
//btScalar angularThres = 1;
|
|
//btScalar maxAngVel = 0.;
|
|
//bool scaleDown = 1.;
|
|
//for(int link = 0; link < m_links.size(); ++link)
|
|
//{
|
|
// if(spatVel[link+1].getAngular().length() > maxAngVel)
|
|
// {
|
|
// maxAngVel = spatVel[link+1].getAngular().length();
|
|
// scaleDown = angularThres / spatVel[link+1].getAngular().length();
|
|
// break;
|
|
// }
|
|
//}
|
|
|
|
//if(scaleDown != 1.)
|
|
//{
|
|
// for(int link = 0; link < m_links.size(); ++link)
|
|
// {
|
|
// if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)
|
|
// {
|
|
// for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
|
|
// getJointVelMultiDof(link)[dof] *= scaleDown;
|
|
// }
|
|
// }
|
|
//}
|
|
/////
|
|
|
|
/////////////////////
|
|
if(m_useGlobalVelocities)
|
|
{
|
|
for (int i = 0; i < num_links; ++i)
|
|
{
|
|
const int parent = m_links[i].m_parent;
|
|
//rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done
|
|
//rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done
|
|
|
|
fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
|
|
fromWorld.m_rotMat = rot_from_world[i+1];
|
|
|
|
// vhat_i = i_xhat_p(i) * vhat_p(i)
|
|
fromParent.transform(spatVel[parent+1], spatVel[i+1]);
|
|
//nice alternative below (using operator *) but it generates temps
|
|
/////////////////////////////////////////////////////////////
|
|
|
|
// now set vhat_i to its true value by doing
|
|
// vhat_i += qidot * shat_i
|
|
spatJointVel.setZero();
|
|
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
|
|
|
|
// remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
|
|
spatVel[i+1] += spatJointVel;
|
|
|
|
|
|
fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity);
|
|
fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
void btMultiBody::stepVelocities(btScalar dt,
|
|
btAlignedObjectArray<btScalar> &scratch_r,
|
|
btAlignedObjectArray<btVector3> &scratch_v,
|
|
btAlignedObjectArray<btMatrix3x3> &scratch_m)
|
|
{
|
|
// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
|
|
// and the base linear & angular accelerations.
|
|
|
|
// We apply damping forces in this routine as well as any external forces specified by the
|
|
// caller (via addBaseForce etc).
|
|
|
|
// output should point to an array of 6 + num_links reals.
|
|
// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
|
|
// num_links joint acceleration values.
|
|
|
|
int num_links = getNumLinks();
|
|
|
|
const btScalar DAMPING_K1_LINEAR = m_linearDamping;
|
|
const btScalar DAMPING_K2_LINEAR = m_linearDamping;
|
|
|
|
const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
|
|
const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
|
|
|
|
btVector3 base_vel = getBaseVel();
|
|
btVector3 base_omega = getBaseOmega();
|
|
|
|
// Temporary matrices/vectors -- use scratch space from caller
|
|
// so that we don't have to keep reallocating every frame
|
|
|
|
scratch_r.resize(2*num_links + 6);
|
|
scratch_v.resize(8*num_links + 6);
|
|
scratch_m.resize(4*num_links + 4);
|
|
|
|
btScalar * r_ptr = &scratch_r[0];
|
|
btScalar * output = &scratch_r[num_links]; // "output" holds the q_double_dot results
|
|
btVector3 * v_ptr = &scratch_v[0];
|
|
|
|
// vhat_i (top = angular, bottom = linear part)
|
|
btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
|
|
btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
|
|
|
|
// zhat_i^A
|
|
btVector3 * f_zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
|
|
btVector3 * f_zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
|
|
|
|
// chat_i (note NOT defined for the base)
|
|
btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
|
|
btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
|
|
|
|
// top left, top right and bottom left blocks of Ihat_i^A.
|
|
// bottom right block = transpose of top left block and is not stored.
|
|
// Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
|
|
btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
|
|
btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
|
|
btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
|
|
|
|
// Cached 3x3 rotation matrices from parent frame to this frame.
|
|
btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
|
|
btMatrix3x3 * rot_from_world = &scratch_m[0];
|
|
|
|
// hhat_i, ahat_i
|
|
// hhat is NOT stored for the base (but ahat is)
|
|
btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0;
|
|
btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0;
|
|
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
|
|
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
|
|
|
|
// Y_i, D_i
|
|
btScalar * Y1 = r_ptr; r_ptr += num_links;
|
|
btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
|
|
|
|
// ptr to the joint accel part of the output
|
|
btScalar * joint_accel = output + 6;
|
|
|
|
|
|
// Start of the algorithm proper.
|
|
|
|
// First 'upward' loop.
|
|
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
|
|
|
|
rot_from_parent[0] = btMatrix3x3(m_baseQuat);
|
|
|
|
vel_top_angular[0] = rot_from_parent[0] * base_omega;
|
|
vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
|
|
|
|
if (m_fixedBase) {
|
|
f_zero_acc_top_angular[0] = f_zero_acc_bottom_linear[0] = btVector3(0,0,0);
|
|
} else {
|
|
f_zero_acc_top_angular[0] = - (rot_from_parent[0] * (m_baseForce
|
|
- m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
|
|
|
|
f_zero_acc_bottom_linear[0] =
|
|
- (rot_from_parent[0] * m_baseTorque);
|
|
|
|
if (m_useGyroTerm)
|
|
f_zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
|
|
|
|
f_zero_acc_bottom_linear[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
|
|
|
|
}
|
|
|
|
|
|
|
|
inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
|
|
|
|
|
|
inertia_top_right[0].setValue(m_baseMass, 0, 0,
|
|
0, m_baseMass, 0,
|
|
0, 0, m_baseMass);
|
|
inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0,
|
|
0, m_baseInertia[1], 0,
|
|
0, 0, m_baseInertia[2]);
|
|
|
|
rot_from_world[0] = rot_from_parent[0];
|
|
|
|
for (int i = 0; i < num_links; ++i) {
|
|
const int parent = m_links[i].m_parent;
|
|
rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
|
|
|
|
|
|
rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
|
|
|
|
// vhat_i = i_xhat_p(i) * vhat_p(i)
|
|
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
|
vel_top_angular[parent+1], vel_bottom_linear[parent+1],
|
|
vel_top_angular[i+1], vel_bottom_linear[i+1]);
|
|
|
|
// we can now calculate chat_i
|
|
// remember vhat_i is really vhat_p(i) (but in current frame) at this point
|
|
coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(m_links[i].m_cachedRVector))
|
|
+ 2 * vel_top_angular[i+1].cross(m_links[i].getAxisBottom(0)) * getJointVel(i);
|
|
if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
|
|
coriolis_top_angular[i] = vel_top_angular[i+1].cross(m_links[i].getAxisTop(0)) * getJointVel(i);
|
|
coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * m_links[i].getAxisTop(0).cross(m_links[i].getAxisBottom(0));
|
|
} else {
|
|
coriolis_top_angular[i] = btVector3(0,0,0);
|
|
}
|
|
|
|
// now set vhat_i to its true value by doing
|
|
// vhat_i += qidot * shat_i
|
|
vel_top_angular[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
|
|
vel_bottom_linear[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
|
|
|
|
// calculate zhat_i^A
|
|
f_zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (m_links[i].m_appliedForce));
|
|
f_zero_acc_top_angular[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
|
|
|
|
f_zero_acc_bottom_linear[i+1] =
|
|
- (rot_from_world[i+1] * m_links[i].m_appliedTorque);
|
|
if (m_useGyroTerm)
|
|
{
|
|
f_zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
|
|
}
|
|
|
|
f_zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
|
|
|
|
// calculate Ihat_i^A
|
|
inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
|
|
inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0,
|
|
0, m_links[i].m_mass, 0,
|
|
0, 0, m_links[i].m_mass);
|
|
inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0,
|
|
0, m_links[i].m_inertiaLocal[1], 0,
|
|
0, 0, m_links[i].m_inertiaLocal[2]);
|
|
}
|
|
|
|
|
|
// 'Downward' loop.
|
|
// (part of TreeForwardDynamics in Mirtich.)
|
|
for (int i = num_links - 1; i >= 0; --i) {
|
|
|
|
h_top[i] = inertia_top_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_right[i+1] * m_links[i].getAxisBottom(0);
|
|
h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0);
|
|
btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]);
|
|
D[i] = val;
|
|
//Y1 = joint torque - zero acceleration force - coriolis force
|
|
Y1[i] = m_links[i].m_jointTorque[0]
|
|
- SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), f_zero_acc_top_angular[i+1], f_zero_acc_bottom_linear[i+1])
|
|
- SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
|
|
|
|
const int parent = m_links[i].m_parent;
|
|
|
|
btAssert(D[i]!=0.f);
|
|
// Ip += pXi * (Ii - hi hi' / Di) * iXp
|
|
const btScalar one_over_di = 1.0f / D[i];
|
|
|
|
|
|
|
|
|
|
const btMatrix3x3 TL = inertia_top_left[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]);
|
|
const btMatrix3x3 TR = inertia_top_right[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]);
|
|
const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]);
|
|
|
|
|
|
btMatrix3x3 r_cross;
|
|
r_cross.setValue(
|
|
0, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1],
|
|
m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0],
|
|
-m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[0], 0);
|
|
|
|
inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
|
|
inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
|
|
inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
|
|
(r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];
|
|
|
|
|
|
// Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
|
|
btVector3 in_top, in_bottom, out_top, out_bottom;
|
|
const btScalar Y_over_D = Y1[i] * one_over_di;
|
|
in_top = f_zero_acc_top_angular[i+1]
|
|
+ inertia_top_left[i+1] * coriolis_top_angular[i]
|
|
+ inertia_top_right[i+1] * coriolis_bottom_linear[i]
|
|
+ Y_over_D * h_top[i];
|
|
in_bottom = f_zero_acc_bottom_linear[i+1]
|
|
+ inertia_bottom_left[i+1] * coriolis_top_angular[i]
|
|
+ inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
|
|
+ Y_over_D * h_bottom[i];
|
|
InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
|
in_top, in_bottom, out_top, out_bottom);
|
|
f_zero_acc_top_angular[parent+1] += out_top;
|
|
f_zero_acc_bottom_linear[parent+1] += out_bottom;
|
|
}
|
|
|
|
|
|
// Second 'upward' loop
|
|
// (part of TreeForwardDynamics in Mirtich)
|
|
|
|
if (m_fixedBase)
|
|
{
|
|
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
|
|
}
|
|
else
|
|
{
|
|
if (num_links > 0)
|
|
{
|
|
//Matrix<btScalar, 6, 6> Imatrix;
|
|
//Imatrix.block<3,3>(0,0) = inertia_top_left[0];
|
|
//Imatrix.block<3,3>(3,0) = inertia_bottom_left[0];
|
|
//Imatrix.block<3,3>(0,3) = inertia_top_right[0];
|
|
//Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose();
|
|
//cached_imatrix_lu.reset(new Eigen::LU<Matrix<btScalar, 6, 6> >(Imatrix)); // TODO: Avoid memory allocation here?
|
|
|
|
m_cachedInertiaTopLeft = inertia_top_left[0];
|
|
m_cachedInertiaTopRight = inertia_top_right[0];
|
|
m_cachedInertiaLowerLeft = inertia_bottom_left[0];
|
|
m_cachedInertiaLowerRight= inertia_top_left[0].transpose();
|
|
|
|
}
|
|
btVector3 rhs_top (f_zero_acc_top_angular[0][0], f_zero_acc_top_angular[0][1], f_zero_acc_top_angular[0][2]);
|
|
btVector3 rhs_bot (f_zero_acc_bottom_linear[0][0], f_zero_acc_bottom_linear[0][1], f_zero_acc_bottom_linear[0][2]);
|
|
float result[6];
|
|
|
|
solveImatrix(rhs_top, rhs_bot, result);
|
|
// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
|
|
for (int i = 0; i < 3; ++i) {
|
|
accel_top[0][i] = -result[i];
|
|
accel_bottom[0][i] = -result[i+3];
|
|
}
|
|
|
|
}
|
|
|
|
// now do the loop over the m_links
|
|
for (int i = 0; i < num_links; ++i)
|
|
{
|
|
//acceleration of the child link = acceleration of the parent transformed into child frame +
|
|
// + coriolis acceleration
|
|
// + joint acceleration
|
|
const int parent = m_links[i].m_parent;
|
|
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
|
accel_top[parent+1], accel_bottom[parent+1],
|
|
accel_top[i+1], accel_bottom[i+1]);
|
|
|
|
|
|
joint_accel[i] = (Y1[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
|
|
accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * m_links[i].getAxisTop(0);
|
|
accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * m_links[i].getAxisBottom(0);
|
|
}
|
|
|
|
// transform base accelerations back to the world frame.
|
|
btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
|
|
output[0] = omegadot_out[0];
|
|
output[1] = omegadot_out[1];
|
|
output[2] = omegadot_out[2];
|
|
|
|
btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
|
|
output[3] = vdot_out[0];
|
|
output[4] = vdot_out[1];
|
|
output[5] = vdot_out[2];
|
|
|
|
/////////////////
|
|
//printf("q = [");
|
|
//printf("%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
|
|
//for(int link = 0; link < getNumLinks(); ++link)
|
|
// printf("%.2f ", m_links[link].m_jointPos[0]);
|
|
//printf("]\n");
|
|
////
|
|
//printf("qd = [");
|
|
//for(int dof = 0; dof < getNumLinks() + 6; ++dof)
|
|
// printf("%.2f ", m_realBuf[dof]);
|
|
//printf("]\n");
|
|
////
|
|
//printf("qdd = [");
|
|
//for(int dof = 0; dof < getNumLinks() + 6; ++dof)
|
|
// printf("%.2f ", output[dof]);
|
|
//printf("]\n");
|
|
/////////////////
|
|
|
|
// Final step: add the accelerations (times dt) to the velocities.
|
|
//todo: do we already need to update the velocities, or can we move this into the constraint solver?
|
|
//the gravity (and other forces) will cause an undesired bounce/restitution effect when using this approach
|
|
applyDeltaVee(output, dt);
|
|
|
|
|
|
}
|
|
|
|
void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
|
|
{
|
|
int num_links = getNumLinks();
|
|
///solve I * x = rhs, so the result = invI * rhs
|
|
if (num_links == 0)
|
|
{
|
|
// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
|
|
result[0] = rhs_bot[0] / m_baseInertia[0];
|
|
result[1] = rhs_bot[1] / m_baseInertia[1];
|
|
result[2] = rhs_bot[2] / m_baseInertia[2];
|
|
result[3] = rhs_top[0] / m_baseMass;
|
|
result[4] = rhs_top[1] / m_baseMass;
|
|
result[5] = rhs_top[2] / m_baseMass;
|
|
} else
|
|
{
|
|
/// Special routine for calculating the inverse of a spatial inertia matrix
|
|
///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
|
|
btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
|
|
btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
|
|
btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
|
|
tmp = invIupper_right * m_cachedInertiaLowerRight;
|
|
btMatrix3x3 invI_upper_left = (tmp * Binv);
|
|
btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
|
|
tmp = m_cachedInertiaTopLeft * invI_upper_left;
|
|
tmp[0][0]-= 1.0;
|
|
tmp[1][1]-= 1.0;
|
|
tmp[2][2]-= 1.0;
|
|
btMatrix3x3 invI_lower_left = (Binv * tmp);
|
|
|
|
//multiply result = invI * rhs
|
|
{
|
|
btVector3 vtop = invI_upper_left*rhs_top;
|
|
btVector3 tmp;
|
|
tmp = invIupper_right * rhs_bot;
|
|
vtop += tmp;
|
|
btVector3 vbot = invI_lower_left*rhs_top;
|
|
tmp = invI_lower_right * rhs_bot;
|
|
vbot += tmp;
|
|
result[0] = vtop[0];
|
|
result[1] = vtop[1];
|
|
result[2] = vtop[2];
|
|
result[3] = vbot[0];
|
|
result[4] = vbot[1];
|
|
result[5] = vbot[2];
|
|
}
|
|
|
|
}
|
|
}
|
|
void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
|
|
{
|
|
int num_links = getNumLinks();
|
|
///solve I * x = rhs, so the result = invI * rhs
|
|
if (num_links == 0)
|
|
{
|
|
// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
|
|
result.setAngular(rhs.getAngular() / m_baseInertia);
|
|
result.setLinear(rhs.getLinear() / m_baseMass);
|
|
} else
|
|
{
|
|
/// Special routine for calculating the inverse of a spatial inertia matrix
|
|
///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
|
|
btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
|
|
btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
|
|
btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
|
|
tmp = invIupper_right * m_cachedInertiaLowerRight;
|
|
btMatrix3x3 invI_upper_left = (tmp * Binv);
|
|
btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
|
|
tmp = m_cachedInertiaTopLeft * invI_upper_left;
|
|
tmp[0][0]-= 1.0;
|
|
tmp[1][1]-= 1.0;
|
|
tmp[2][2]-= 1.0;
|
|
btMatrix3x3 invI_lower_left = (Binv * tmp);
|
|
|
|
//multiply result = invI * rhs
|
|
{
|
|
btVector3 vtop = invI_upper_left*rhs.getLinear();
|
|
btVector3 tmp;
|
|
tmp = invIupper_right * rhs.getAngular();
|
|
vtop += tmp;
|
|
btVector3 vbot = invI_lower_left*rhs.getLinear();
|
|
tmp = invI_lower_right * rhs.getAngular();
|
|
vbot += tmp;
|
|
result.setVector(vtop, vbot);
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
|
|
{
|
|
for (int row = 0; row < rowsA; row++)
|
|
{
|
|
for (int col = 0; col < colsB; col++)
|
|
{
|
|
pC[row * colsB + col] = 0.f;
|
|
for (int inner = 0; inner < rowsB; inner++)
|
|
{
|
|
pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
|
|
btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
|
|
{
|
|
// Temporary matrices/vectors -- use scratch space from caller
|
|
// so that we don't have to keep reallocating every frame
|
|
|
|
|
|
int num_links = getNumLinks();
|
|
scratch_r.resize(m_dofCount);
|
|
scratch_v.resize(4*num_links + 4);
|
|
|
|
btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0;
|
|
btVector3 * v_ptr = &scratch_v[0];
|
|
|
|
// zhat_i^A (scratch space)
|
|
btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
|
|
v_ptr += num_links * 2 + 2;
|
|
|
|
// rot_from_parent (cached from calcAccelerations)
|
|
const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
|
|
|
|
// hhat (cached), accel (scratch)
|
|
// hhat is NOT stored for the base (but ahat is)
|
|
const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
|
|
btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
|
|
v_ptr += num_links * 2 + 2;
|
|
|
|
// Y_i (scratch), invD_i (cached)
|
|
const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
|
|
btScalar * Y = r_ptr;
|
|
////////////////
|
|
//aux variables
|
|
static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
|
|
static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
|
|
static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
|
|
static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
|
|
static btSpatialTransformationMatrix fromParent;
|
|
/////////////////
|
|
|
|
// First 'upward' loop.
|
|
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
|
|
|
|
// Fill in zero_acc
|
|
// -- set to force/torque on the base, zero otherwise
|
|
if (m_fixedBase)
|
|
{
|
|
zeroAccSpatFrc[0].setZero();
|
|
} else
|
|
{
|
|
//test forces
|
|
fromParent.m_rotMat = rot_from_parent[0];
|
|
fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]);
|
|
}
|
|
for (int i = 0; i < num_links; ++i)
|
|
{
|
|
zeroAccSpatFrc[i+1].setZero();
|
|
}
|
|
|
|
// 'Downward' loop.
|
|
// (part of TreeForwardDynamics in Mirtich.)
|
|
for (int i = num_links - 1; i >= 0; --i)
|
|
{
|
|
const int parent = m_links[i].m_parent;
|
|
fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
|
|
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
|
|
- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
|
|
;
|
|
}
|
|
|
|
btVector3 in_top, in_bottom, out_top, out_bottom;
|
|
const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
|
|
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
invD_times_Y[dof] = 0.f;
|
|
|
|
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
|
{
|
|
invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
|
|
}
|
|
}
|
|
|
|
// Zp += pXi * (Zi + hi*Yi/Di)
|
|
spatForceVecTemps[0] = zeroAccSpatFrc[i+1];
|
|
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
|
|
//
|
|
spatForceVecTemps[0] += hDof * invD_times_Y[dof];
|
|
}
|
|
|
|
|
|
fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
|
|
|
|
zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
|
|
}
|
|
|
|
// ptr to the joint accel part of the output
|
|
btScalar * joint_accel = output + 6;
|
|
|
|
|
|
// Second 'upward' loop
|
|
// (part of TreeForwardDynamics in Mirtich)
|
|
|
|
if (m_fixedBase)
|
|
{
|
|
spatAcc[0].setZero();
|
|
}
|
|
else
|
|
{
|
|
solveImatrix(zeroAccSpatFrc[0], result);
|
|
spatAcc[0] = -result;
|
|
|
|
}
|
|
|
|
// now do the loop over the m_links
|
|
for (int i = 0; i < num_links; ++i)
|
|
{
|
|
const int parent = m_links[i].m_parent;
|
|
fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
|
|
|
|
fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
|
|
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
{
|
|
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
|
|
//
|
|
Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
|
|
}
|
|
|
|
const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
|
|
mulMatrix(const_cast<btScalar*>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
|
|
|
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
|
|
}
|
|
|
|
// transform base accelerations back to the world frame.
|
|
btVector3 omegadot_out;
|
|
omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
|
|
output[0] = omegadot_out[0];
|
|
output[1] = omegadot_out[1];
|
|
output[2] = omegadot_out[2];
|
|
|
|
btVector3 vdot_out;
|
|
vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();
|
|
output[3] = vdot_out[0];
|
|
output[4] = vdot_out[1];
|
|
output[5] = vdot_out[2];
|
|
|
|
/////////////////
|
|
//printf("delta = [");
|
|
//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
|
|
// printf("%.2f ", output[dof]);
|
|
//printf("]\n");
|
|
/////////////////
|
|
}
|
|
|
|
|
|
void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output,
|
|
btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
|
|
{
|
|
// Temporary matrices/vectors -- use scratch space from caller
|
|
// so that we don't have to keep reallocating every frame
|
|
int num_links = getNumLinks();
|
|
scratch_r.resize(num_links);
|
|
scratch_v.resize(4*num_links + 4);
|
|
|
|
btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0];
|
|
btVector3 * v_ptr = &scratch_v[0];
|
|
|
|
// zhat_i^A (scratch space)
|
|
btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
|
|
btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
|
|
|
|
// rot_from_parent (cached from calcAccelerations)
|
|
const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
|
|
|
|
// hhat (cached), accel (scratch)
|
|
const btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0;
|
|
const btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0;
|
|
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
|
|
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
|
|
|
|
// Y_i (scratch), D_i (cached)
|
|
btScalar * Y = r_ptr; r_ptr += num_links;
|
|
const btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
|
|
|
|
btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size());
|
|
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
|
|
|
|
|
|
|
|
// First 'upward' loop.
|
|
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
|
|
|
|
btVector3 input_force(force[3],force[4],force[5]);
|
|
btVector3 input_torque(force[0],force[1],force[2]);
|
|
|
|
// Fill in zero_acc
|
|
// -- set to force/torque on the base, zero otherwise
|
|
if (m_fixedBase)
|
|
{
|
|
zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
|
|
} else
|
|
{
|
|
zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force);
|
|
zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque);
|
|
}
|
|
for (int i = 0; i < num_links; ++i)
|
|
{
|
|
zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0);
|
|
}
|
|
|
|
// 'Downward' loop.
|
|
for (int i = num_links - 1; i >= 0; --i)
|
|
{
|
|
// btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
|
|
Y[i] = - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
|
|
Y[i] += force[6 + i]; // add joint torque
|
|
|
|
const int parent = m_links[i].m_parent;
|
|
|
|
// Zp += pXi * (Zi + hi*Yi/Di)
|
|
btVector3 in_top, in_bottom, out_top, out_bottom;
|
|
const btScalar Y_over_D = Y[i] / D[i];
|
|
in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i];
|
|
in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i];
|
|
InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
|
in_top, in_bottom, out_top, out_bottom);
|
|
zero_acc_top_angular[parent+1] += out_top;
|
|
zero_acc_bottom_linear[parent+1] += out_bottom;
|
|
}
|
|
|
|
// ptr to the joint accel part of the output
|
|
btScalar * joint_accel = output + 6;
|
|
|
|
// Second 'upward' loop
|
|
if (m_fixedBase)
|
|
{
|
|
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
|
|
} else
|
|
{
|
|
btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
|
|
btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
|
|
|
|
float result[6];
|
|
solveImatrix(rhs_top,rhs_bot, result);
|
|
// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
|
|
|
|
for (int i = 0; i < 3; ++i) {
|
|
accel_top[0][i] = -result[i];
|
|
accel_bottom[0][i] = -result[i+3];
|
|
}
|
|
|
|
}
|
|
|
|
// now do the loop over the m_links
|
|
for (int i = 0; i < num_links; ++i) {
|
|
const int parent = m_links[i].m_parent;
|
|
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
|
accel_top[parent+1], accel_bottom[parent+1],
|
|
accel_top[i+1], accel_bottom[i+1]);
|
|
btScalar Y_minus_hT_a = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1]));
|
|
joint_accel[i] = Y_minus_hT_a / D[i];
|
|
accel_top[i+1] += joint_accel[i] * m_links[i].getAxisTop(0);
|
|
accel_bottom[i+1] += joint_accel[i] * m_links[i].getAxisBottom(0);
|
|
}
|
|
|
|
// transform base accelerations back to the world frame.
|
|
btVector3 omegadot_out;
|
|
omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
|
|
output[0] = omegadot_out[0];
|
|
output[1] = omegadot_out[1];
|
|
output[2] = omegadot_out[2];
|
|
|
|
btVector3 vdot_out;
|
|
vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
|
|
|
|
output[3] = vdot_out[0];
|
|
output[4] = vdot_out[1];
|
|
output[5] = vdot_out[2];
|
|
|
|
|
|
/////////////////
|
|
/*
|
|
int ndof = getNumLinks() + 6;
|
|
printf("test force(impulse) (%d) = [\n",ndof);
|
|
|
|
for (int i=0;i<ndof;i++)
|
|
{
|
|
printf("%.2f ", force[i]);
|
|
printf("]\n");
|
|
}
|
|
|
|
printf("delta(%d) = [",ndof);
|
|
for(int dof = 0; dof < getNumLinks() + 6; ++dof)
|
|
printf("%.2f ", output[dof]);
|
|
printf("]\n");
|
|
/////////////////
|
|
*/
|
|
|
|
//int dummy = 0;
|
|
}
|
|
|
|
void btMultiBody::stepPositions(btScalar dt)
|
|
{
|
|
int num_links = getNumLinks();
|
|
// step position by adding dt * velocity
|
|
btVector3 v = getBaseVel();
|
|
m_basePos += dt * v;
|
|
|
|
// "exponential map" method for the rotation
|
|
btVector3 base_omega = getBaseOmega();
|
|
const btScalar omega_norm = base_omega.norm();
|
|
const btScalar omega_times_dt = omega_norm * dt;
|
|
const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156
|
|
if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE)
|
|
{
|
|
const btScalar xsq = omega_times_dt * omega_times_dt; // |omega|^2 * dt^2
|
|
const btScalar sin_term = dt * (xsq / 48.0f - 0.5f); // -sin(0.5*dt*|omega|) / |omega|
|
|
const btScalar cos_term = 1.0f - xsq / 8.0f; // cos(0.5*dt*|omega|)
|
|
m_baseQuat = m_baseQuat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term);
|
|
} else
|
|
{
|
|
m_baseQuat = m_baseQuat * btQuaternion(base_omega / omega_norm,-omega_times_dt);
|
|
}
|
|
|
|
// Make sure the quaternion represents a valid rotation.
|
|
// (Not strictly necessary, but helps prevent any round-off errors from building up.)
|
|
m_baseQuat.normalize();
|
|
|
|
// Finally we can update m_jointPos for each of the m_links
|
|
for (int i = 0; i < num_links; ++i)
|
|
{
|
|
float jointVel = getJointVel(i);
|
|
m_links[i].m_jointPos[0] += dt * jointVel;
|
|
m_links[i].updateCache();
|
|
}
|
|
}
|
|
|
|
void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
|
|
{
|
|
int num_links = getNumLinks();
|
|
// step position by adding dt * velocity
|
|
//btVector3 v = getBaseVel();
|
|
//m_basePos += dt * v;
|
|
//
|
|
btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
|
|
btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
|
|
//
|
|
pBasePos[0] += dt * pBaseVel[0];
|
|
pBasePos[1] += dt * pBaseVel[1];
|
|
pBasePos[2] += dt * pBaseVel[2];
|
|
|
|
///////////////////////////////
|
|
//local functor for quaternion integration (to avoid error prone redundancy)
|
|
struct
|
|
{
|
|
//"exponential map" based on btTransformUtil::integrateTransform(..)
|
|
void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
|
|
{
|
|
//baseBody => quat is alias and omega is global coor
|
|
//!baseBody => quat is alibi and omega is local coor
|
|
|
|
btVector3 axis;
|
|
btVector3 angvel;
|
|
|
|
if(!baseBody)
|
|
angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok
|
|
else
|
|
angvel = omega;
|
|
|
|
btScalar fAngle = angvel.length();
|
|
//limit the angular motion
|
|
if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
|
|
{
|
|
fAngle = btScalar(0.5)*SIMD_HALF_PI / dt;
|
|
}
|
|
|
|
if ( fAngle < btScalar(0.001) )
|
|
{
|
|
// use Taylor's expansions of sync function
|
|
axis = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle );
|
|
}
|
|
else
|
|
{
|
|
// sync(fAngle) = sin(c*fAngle)/t
|
|
axis = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle );
|
|
}
|
|
|
|
if(!baseBody)
|
|
quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat;
|
|
else
|
|
quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) ));
|
|
//equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
|
|
|
|
quat.normalize();
|
|
}
|
|
} pQuatUpdateFun;
|
|
///////////////////////////////
|
|
|
|
//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
|
|
//
|
|
btScalar *pBaseQuat = pq ? pq : m_baseQuat;
|
|
btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
|
|
//
|
|
static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
|
|
static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
|
|
pQuatUpdateFun(baseOmega, baseQuat, true, dt);
|
|
pBaseQuat[0] = baseQuat.x();
|
|
pBaseQuat[1] = baseQuat.y();
|
|
pBaseQuat[2] = baseQuat.z();
|
|
pBaseQuat[3] = baseQuat.w();
|
|
|
|
|
|
//printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
|
|
//printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
|
|
//printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
|
|
|
|
if(pq)
|
|
pq += 7;
|
|
if(pqd)
|
|
pqd += 6;
|
|
|
|
// Finally we can update m_jointPos for each of the m_links
|
|
for (int i = 0; i < num_links; ++i)
|
|
{
|
|
btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);
|
|
btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
|
|
|
|
switch(m_links[i].m_jointType)
|
|
{
|
|
case btMultibodyLink::ePrismatic:
|
|
case btMultibodyLink::eRevolute:
|
|
{
|
|
btScalar jointVel = pJointVel[0];
|
|
pJointPos[0] += dt * jointVel;
|
|
break;
|
|
}
|
|
case btMultibodyLink::eSpherical:
|
|
{
|
|
static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
|
|
static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
|
|
pQuatUpdateFun(jointVel, jointOri, false, dt);
|
|
pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
|
|
break;
|
|
}
|
|
case btMultibodyLink::ePlanar:
|
|
{
|
|
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
|
|
|
|
btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
|
|
btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
|
|
pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
|
|
pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
|
|
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
}
|
|
|
|
}
|
|
|
|
m_links[i].updateCacheMultiDof(pq);
|
|
|
|
if(pq)
|
|
pq += m_links[i].m_posVarCount;
|
|
if(pqd)
|
|
pqd += m_links[i].m_dofCount;
|
|
}
|
|
}
|
|
|
|
void btMultiBody::filConstraintJacobianMultiDof(int link,
|
|
const btVector3 &contact_point,
|
|
const btVector3 &normal_ang,
|
|
const btVector3 &normal_lin,
|
|
btScalar *jac,
|
|
btAlignedObjectArray<btScalar> &scratch_r,
|
|
btAlignedObjectArray<btVector3> &scratch_v,
|
|
btAlignedObjectArray<btMatrix3x3> &scratch_m) const
|
|
{
|
|
// temporary space
|
|
int num_links = getNumLinks();
|
|
int m_dofCount = getNumDofs();
|
|
scratch_v.resize(3*num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
|
|
scratch_m.resize(num_links + 1);
|
|
|
|
btVector3 * v_ptr = &scratch_v[0];
|
|
btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
|
|
btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
|
|
btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
|
|
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
|
|
|
|
scratch_r.resize(m_dofCount);
|
|
btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
|
|
|
|
btMatrix3x3 * rot_from_world = &scratch_m[0];
|
|
|
|
const btVector3 p_minus_com_world = contact_point - m_basePos;
|
|
const btVector3 &normal_lin_world = normal_lin; //convenience
|
|
const btVector3 &normal_ang_world = normal_ang;
|
|
|
|
rot_from_world[0] = btMatrix3x3(m_baseQuat);
|
|
|
|
// omega coeffients first.
|
|
btVector3 omega_coeffs_world;
|
|
omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
|
|
jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
|
|
jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
|
|
jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
|
|
// then v coefficients
|
|
jac[3] = normal_lin_world[0];
|
|
jac[4] = normal_lin_world[1];
|
|
jac[5] = normal_lin_world[2];
|
|
|
|
//create link-local versions of p_minus_com and normal
|
|
p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
|
|
n_local_lin[0] = rot_from_world[0] * normal_lin_world;
|
|
n_local_ang[0] = rot_from_world[0] * normal_ang_world;
|
|
|
|
// Set remaining jac values to zero for now.
|
|
for (int i = 6; i < 6 + m_dofCount; ++i)
|
|
{
|
|
jac[i] = 0;
|
|
}
|
|
|
|
// Qdot coefficients, if necessary.
|
|
if (num_links > 0 && link > -1) {
|
|
|
|
// TODO: speed this up -- don't calculate for m_links we don't need.
|
|
// (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
|
|
// which is resulting in repeated work being done...)
|
|
|
|
// calculate required normals & positions in the local frames.
|
|
for (int i = 0; i < num_links; ++i) {
|
|
|
|
// transform to local frame
|
|
const int parent = m_links[i].m_parent;
|
|
const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
|
|
rot_from_world[i+1] = mtx * rot_from_world[parent+1];
|
|
|
|
n_local_lin[i+1] = mtx * n_local_lin[parent+1];
|
|
n_local_ang[i+1] = mtx * n_local_ang[parent+1];
|
|
p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector;
|
|
|
|
// calculate the jacobian entry
|
|
switch(m_links[i].m_jointType)
|
|
{
|
|
case btMultibodyLink::eRevolute:
|
|
{
|
|
results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
|
|
results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
|
|
break;
|
|
}
|
|
case btMultibodyLink::ePrismatic:
|
|
{
|
|
results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
|
|
break;
|
|
}
|
|
case btMultibodyLink::eSpherical:
|
|
{
|
|
results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
|
|
results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1));
|
|
results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2));
|
|
|
|
results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
|
|
results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1));
|
|
results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2));
|
|
|
|
break;
|
|
}
|
|
case btMultibodyLink::ePlanar:
|
|
{
|
|
results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
|
|
results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
|
|
results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
|
|
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
// Now copy through to output.
|
|
//printf("jac[%d] = ", link);
|
|
while (link != -1)
|
|
{
|
|
for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
|
|
{
|
|
jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
|
|
//printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
|
|
}
|
|
|
|
link = m_links[link].m_parent;
|
|
}
|
|
//printf("]\n");
|
|
}
|
|
}
|
|
|
|
void btMultiBody::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
|
|
{
|
|
// temporary space
|
|
int num_links = getNumLinks();
|
|
scratch_v.resize(2*num_links + 2);
|
|
scratch_m.resize(num_links + 1);
|
|
|
|
btVector3 * v_ptr = &scratch_v[0];
|
|
btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
|
|
btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
|
|
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
|
|
|
|
scratch_r.resize(num_links);
|
|
btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
|
|
|
|
btMatrix3x3 * rot_from_world = &scratch_m[0];
|
|
|
|
const btVector3 p_minus_com_world = contact_point - m_basePos;
|
|
|
|
rot_from_world[0] = btMatrix3x3(m_baseQuat);
|
|
|
|
p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
|
|
n_local[0] = rot_from_world[0] * normal;
|
|
|
|
// omega coeffients first.
|
|
if (this->m_fixedBase)
|
|
{
|
|
for (int i=0;i<6;i++)
|
|
{
|
|
jac[i]=0;
|
|
}
|
|
} else
|
|
{
|
|
btVector3 omega_coeffs;
|
|
|
|
omega_coeffs = p_minus_com_world.cross(normal);
|
|
jac[0] = omega_coeffs[0];
|
|
jac[1] = omega_coeffs[1];
|
|
jac[2] = omega_coeffs[2];
|
|
// then v coefficients
|
|
jac[3] = normal[0];
|
|
jac[4] = normal[1];
|
|
jac[5] = normal[2];
|
|
}
|
|
// Set remaining jac values to zero for now.
|
|
for (int i = 6; i < 6 + num_links; ++i) {
|
|
jac[i] = 0;
|
|
}
|
|
|
|
// Qdot coefficients, if necessary.
|
|
if (num_links > 0 && link > -1) {
|
|
|
|
// TODO: speed this up -- don't calculate for m_links we don't need.
|
|
// (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
|
|
// which is resulting in repeated work being done...)
|
|
|
|
// calculate required normals & positions in the local frames.
|
|
for (int i = 0; i < num_links; ++i) {
|
|
|
|
// transform to local frame
|
|
const int parent = m_links[i].m_parent;
|
|
const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
|
|
rot_from_world[i+1] = mtx * rot_from_world[parent+1];
|
|
|
|
n_local[i+1] = mtx * n_local[parent+1];
|
|
p_minus_com[i+1] = mtx * p_minus_com[parent+1] - m_links[i].m_cachedRVector;
|
|
|
|
// calculate the jacobian entry
|
|
if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
|
|
results[i] = n_local[i+1].dot( m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0) );
|
|
} else {
|
|
results[i] = n_local[i+1].dot( m_links[i].getAxisBottom(0) );
|
|
}
|
|
}
|
|
|
|
// Now copy through to output.
|
|
//printf("jac[%d] = ", link);
|
|
while (link != -1)
|
|
{
|
|
jac[6 + link] = results[link];
|
|
//printf("%.2f\t", jac[6 + link]);
|
|
link = m_links[link].m_parent;
|
|
}
|
|
//printf("]\n");
|
|
}
|
|
}
|
|
|
|
void btMultiBody::wakeUp()
|
|
{
|
|
m_awake = true;
|
|
}
|
|
|
|
void btMultiBody::goToSleep()
|
|
{
|
|
m_awake = false;
|
|
}
|
|
|
|
void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
|
|
{
|
|
int num_links = getNumLinks();
|
|
extern bool gDisableDeactivation;
|
|
if (!m_canSleep || gDisableDeactivation)
|
|
{
|
|
m_awake = true;
|
|
m_sleepTimer = 0;
|
|
return;
|
|
}
|
|
|
|
// motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
|
|
btScalar motion = 0;
|
|
if(m_isMultiDof)
|
|
{
|
|
for (int i = 0; i < 6 + m_dofCount; ++i)
|
|
motion += m_realBuf[i] * m_realBuf[i];
|
|
}
|
|
else
|
|
{
|
|
for (int i = 0; i < 6 + num_links; ++i)
|
|
motion += m_realBuf[i] * m_realBuf[i];
|
|
}
|
|
|
|
|
|
if (motion < SLEEP_EPSILON) {
|
|
m_sleepTimer += timestep;
|
|
if (m_sleepTimer > SLEEP_TIMEOUT) {
|
|
goToSleep();
|
|
}
|
|
} else {
|
|
m_sleepTimer = 0;
|
|
if (!m_awake)
|
|
wakeUp();
|
|
}
|
|
}
|
|
|
|
|
|
void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
|
|
{
|
|
|
|
int num_links = getNumLinks();
|
|
|
|
// Cached 3x3 rotation matrices from parent frame to this frame.
|
|
btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0];
|
|
|
|
rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
|
|
|
|
for (int i = 0; i < num_links; ++i)
|
|
{
|
|
const int parent = m_links[i].m_parent;
|
|
rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
|
|
}
|
|
|
|
int nLinks = getNumLinks();
|
|
///base + num m_links
|
|
world_to_local.resize(nLinks+1);
|
|
local_origin.resize(nLinks+1);
|
|
|
|
world_to_local[0] = getWorldToBaseRot();
|
|
local_origin[0] = getBasePos();
|
|
|
|
for (int k=0;k<getNumLinks();k++)
|
|
{
|
|
const int parent = getParent(k);
|
|
world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
|
|
local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
|
|
}
|
|
|
|
for (int link=0;link<getNumLinks();link++)
|
|
{
|
|
int index = link+1;
|
|
|
|
btVector3 posr = local_origin[index];
|
|
btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
|
|
btTransform tr;
|
|
tr.setIdentity();
|
|
tr.setOrigin(posr);
|
|
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
|
getLink(link).m_cachedWorldTransform = tr;
|
|
|
|
}
|
|
|
|
}
|
|
|