From cb556f95254e961094564ddabd10e570be1d06ba Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 00:51:42 +0100 Subject: [PATCH] dirty changes - stabilization hacks --- .../BulletMultiBodyDemos.cpp | 2 +- .../Featherstone/btMultiBody.cpp | 170 +++++++++++++++--- .../Featherstone/btMultiBodyConstraint.cpp | 4 +- .../Featherstone/btMultiBodyJointMotor.cpp | 6 +- .../Featherstone/btMultiBodyJointMotor.h | 2 +- .../Featherstone/btMultiBodyLink.h | 28 ++- 6 files changed, 182 insertions(+), 30 deletions(-) diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp index 1036809f1..368953eb2 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp @@ -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); } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index a25a9c988..3fb41f89b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -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,33 +1165,76 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, rot_from_world[0] = rot_from_parent[0]; - for (int i = 0; i < num_links; ++i) { + // + 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]; - - // 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 + + 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 + } ////////////////////////////////////////////////////////////// - // now set vhat_i to its true value by doing + //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 - 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; + 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]); + spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]); // calculate zhat_i^A // @@ -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; @@ -1369,13 +1429,14 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, { 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); + 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]; + 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]; @@ -1411,7 +1472,68 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, ///////////////// // Final step: add the accelerations (times dt) to the velocities. - applyDeltaVeeMultiDof(output, dt); + 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 diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 1439755be..609f92d63 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -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; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index f612aa025..527f2c011 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -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 diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index 61ee4663b..551813807 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -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; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index d89942747..7566bda53 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -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 + 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) @@ -342,7 +366,9 @@ struct btMultibodyLink // m_eVector is constant, but depends on the joint type // prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.) // revolute: vector from parent's COM to the pivot point, in PARENT's frame. - btVector3 m_eVector; + btVector3 m_eVector; + + btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity; enum eFeatherstoneJointType {