Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

dirty changes - stabilization hacks

Browse Source
This commit is contained in:
kubas
2014-01-09 00:51:42 +01:00
parent 96ff69276f
commit cb556f9525
6 changed files with 182 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp
View File

@@ -396,7 +396,7 @@ btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDyn
{
if (1)
{
btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,500000);
btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,0,500000);
world->addMultiBodyConstraint(con);
}

132
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -1111,6 +1111,8 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
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
@@ -1136,7 +1138,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * m_baseTorque), -(rot_from_parent[0] * m_baseForce));
//adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 10.;
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()));
@@ -1163,31 +1165,74 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
rot_from_world[0] = rot_from_parent[0];
//
bool useGlobalVelocities = false;
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);
fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
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];
////clamp parent's omega
//btScalar parOmegaMod = spatVel[parent+1].getAngular().length();
//btScalar parOmegaModMax = 0.1;
//if(parOmegaMod > parOmegaModMax)
//{
// //btSpatialMotionVector clampedParVel(spatVel[parent+1].getAngular() * parOmegaModMax / parOmegaMod, spatVel[parent+1].getLinear());
// btSpatialMotionVector clampedParVel; clampedParVel = spatVel[parent+1] * (parOmegaModMax / parOmegaMod);
// fromParent.transform(clampedParVel, spatVel[i+1]);
// spatVel[parent+1] *= (parOmegaModMax / parOmegaMod);
//}
//else
{
// 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
}
//////////////////////////////////////////////////////////////
//if(m_links[i].m_jointType == btMultibodyLink::eRevolute || m_links[i].m_jointType == btMultibodyLink::eSpherical)
//{
// btScalar mod2 = 0;
// for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
// mod2 += getJointVelMultiDof(i)[dof]*getJointVelMultiDof(i)[dof];
// btScalar angvel = sqrt(mod2);
// btScalar maxAngVel = 6;//SIMD_HALF_PI * 0.075;
// btScalar step = 1; //dt
// if (angvel*step > maxAngVel)
// {
// btScalar * qd = getJointVelMultiDof(i);
// for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
// qd[dof] *= (maxAngVel/step) /angvel;
// }
//}
// now set vhat_i to its true value by doing
// vhat_i += qidot * shat_i
if(!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]);
@@ -1197,7 +1242,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * m_links[i].m_appliedTorque), -(rot_from_world[i+1] * m_links[i].m_appliedForce));
//
//adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 10.;
btScalar linDampMult = 1., angDampMult = 1.;
zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertia * 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()));
@@ -1219,6 +1264,15 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_baseInertia * 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());
//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());
@@ -1360,6 +1414,12 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
// 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;
@@ -1373,6 +1433,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
}
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];
@@ -1412,6 +1473,67 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
// Final step: add the accelerations (times dt) to the velocities.
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(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);
}
}
}
#endif

4
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@@ -197,7 +197,9 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
}
for (int i = 6; i < ndofA ; ++i)
printf("%.4f ", multiBodyA->getVelocityVector()[i]);
printf("\nrel_vel = %.4f\n------------\n", rel_vel);
constraintRow.m_friction = 0.f;
constraintRow.m_appliedImpulse = 0.f;

6
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
View File

@@ -21,11 +21,13 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
//:btMultiBodyConstraint(body,0,link,-1,1,true),
:btMultiBodyConstraint(body,body,link,link,1,true),
m_desiredVelocity(desiredVelocity)
{
btAssert(linkDoF < body->getLink(link).m_dofCount);
m_maxAppliedImpulse = maxMotorImpulse;
// the data.m_jacobians never change, so may as well
// initialize them here
@@ -33,7 +35,7 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link);
unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link);
// row 0: the lower bound
// row 0: the lower bound

2
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
View File

@@ -30,7 +30,7 @@ protected:
public:
btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
virtual ~btMultiBodyJointMotor();
virtual int getIslandIdA() const;

26
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -133,6 +133,7 @@ namespace {
//
btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; }
btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); }
btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); }
btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); }
@@ -256,6 +257,29 @@ namespace {
}
}
template<typename SpatialVectorType>
void transformInverseRotationOnly( const SpatialVectorType &inVec,
SpatialVectorType &outVec,
eOutputOperation outOp = None)
{
if(outOp == None)
{
outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec;
}
else if(outOp == Add)
{
outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec;
}
else if(outOp == Subtract)
{
outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec;
}
}
void transformInverse( const btSymmetricSpatialDyad &inMat,
btSymmetricSpatialDyad &outMat,
eOutputOperation outOp = None)
@@ -344,6 +368,8 @@ struct btMultibodyLink
// revolute: vector from parent's COM to the pivot point, in PARENT's frame.
btVector3 m_eVector;
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
enum eFeatherstoneJointType
{
eRevolute = 0,