summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp')
-rw-r--r--thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp101
1 files changed, 45 insertions, 56 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
index 2a70ea97e5..5c816c4987 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
@@ -20,22 +20,18 @@ subject to the following restrictions:
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
-
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
- :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
- m_desiredVelocity(desiredVelocity),
- m_desiredPosition(0),
- m_kd(1.),
- m_kp(0),
- m_erp(1),
- m_rhsClamp(SIMD_INFINITY)
+ : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true),
+ m_desiredVelocity(desiredVelocity),
+ m_desiredPosition(0),
+ m_kd(1.),
+ m_kp(0),
+ m_erp(1),
+ m_rhsClamp(SIMD_INFINITY)
{
-
m_maxAppliedImpulse = maxMotorImpulse;
// the data.m_jacobians never change, so may as well
- // initialize them here
-
-
+ // initialize them here
}
void btMultiBodyJointMotor::finalizeMultiDof()
@@ -55,18 +51,17 @@ void btMultiBodyJointMotor::finalizeMultiDof()
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
//:btMultiBodyConstraint(body,0,link,-1,1,true),
- :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
- m_desiredVelocity(desiredVelocity),
- m_desiredPosition(0),
- m_kd(1.),
- m_kp(0),
- m_erp(1),
- m_rhsClamp(SIMD_INFINITY)
+ : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true),
+ m_desiredVelocity(desiredVelocity),
+ m_desiredPosition(0),
+ m_kd(1.),
+ m_kp(0),
+ m_erp(1),
+ m_rhsClamp(SIMD_INFINITY)
{
btAssert(linkDoF < body->getLink(link).m_dofCount);
m_maxAppliedImpulse = maxMotorImpulse;
-
}
btMultiBodyJointMotor::~btMultiBodyJointMotor()
{
@@ -108,76 +103,74 @@ int btMultiBodyJointMotor::getIslandIdB() const
return -1;
}
-
void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
- btMultiBodyJacobianData& data,
- const btContactSolverInfo& infoGlobal)
+ btMultiBodyJacobianData& data,
+ const btContactSolverInfo& infoGlobal)
{
- // only positions need to be updated -- data.m_jacobians and force
- // directions were set in the ctor and never change.
-
+ // only positions need to be updated -- data.m_jacobians and force
+ // directions were set in the ctor and never change.
+
if (m_numDofsFinalized != m_jacSizeBoth)
{
- finalizeMultiDof();
+ finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
- if (m_maxAppliedImpulse==0.f)
+ if (m_maxAppliedImpulse == 0.f)
return;
const btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
- for (int row=0;row<getNumRows();row++)
+ for (int row = 0; row < getNumRows(); row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
- int dof = 0;
- btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
- btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
- btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
-
- btScalar velocityError = (m_desiredVelocity - currentVelocity);
- btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;
- if (rhs>m_rhsClamp)
+ int dof = 0;
+ btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+ btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+ btScalar positionStabiliationTerm = m_erp * (m_desiredPosition - currentPosition) / infoGlobal.m_timeStep;
+
+ btScalar velocityError = (m_desiredVelocity - currentVelocity);
+ btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity + m_kd * velocityError;
+ if (rhs > m_rhsClamp)
{
- rhs=m_rhsClamp;
+ rhs = m_rhsClamp;
}
- if (rhs<-m_rhsClamp)
+ if (rhs < -m_rhsClamp)
{
- rhs=-m_rhsClamp;
+ rhs = -m_rhsClamp;
}
-
-
- fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,rhs);
+
+ fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, rhs);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
{
//expect either prismatic or revolute joint type for now
- btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+ btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
- btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
- constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
- constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
-
+ btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+ constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
+ constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
+
break;
}
case btMultibodyLink::ePrismatic:
{
- btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
- constraintRow.m_contactNormal1=prismaticAxisInWorld;
- constraintRow.m_contactNormal2=-prismaticAxisInWorld;
+ btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+ constraintRow.m_contactNormal1 = prismaticAxisInWorld;
+ constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
-
+
break;
}
default:
@@ -185,10 +178,6 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
btAssert(0);
}
};
-
}
-
}
-
}
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-13 02:19:01 +0000