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

expose gear erp/relative position target to C-API/pybullet

Browse Source 
finish much better C++ vrSyncPlugin, running in-the-loop with the physics at high frequency, see also vr_kuka_setup_vrSyncPlugin.py
This commit is contained in:
erwincoumans
2017-09-26 19:54:36 -07:00
parent b1f8eb74a4
commit 8a265b8af2
12 changed files with 301 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -185,7 +185,9 @@ public:
virtual void setGearRatio(btScalar ratio) {}
virtual void setGearAuxLink(int gearAuxLink) {}
virtual void setRelativePositionTarget(btScalar relPosTarget){}
virtual void setErp(btScalar erp){}
};

21
src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
View File

@@ -23,7 +23,9 @@ subject to the following restrictions:
btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false),
m_gearRatio(1),
m_gearAuxLink(-1)
m_gearAuxLink(-1),
m_erp(0),
m_relativePositionTarget(0)
{
}
@@ -107,9 +109,9 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
jacobianA(0)[offsetA] = 1;
jacobianB(0)[offsetB] = m_gearRatio;
const btScalar posError = 0;
btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
btScalar erp = infoGlobal.m_erp;
btScalar kp = 1;
btScalar kd = 1;
int numRows = getNumRows();
@@ -129,10 +131,15 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
}
currentVelocity += auxVel;
//btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
//btScalar velocityError = (m_desiredVelocity - currentVelocity);
if (m_erp!=0)
{
btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof];
btScalar diff = currentPositionB+currentPositionA;
btScalar desiredPositionDiff = this->m_relativePositionTarget;
posError = -m_erp*(desiredPositionDiff - diff);
}
btScalar desiredRelativeVelocity = auxVel;
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity);

15
src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h
View File

@@ -28,10 +28,12 @@ protected:
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btMatrix3x3 m_frameInA;
btMatrix3x3 m_frameInB;
btMatrix3x3 m_frameInA;
btMatrix3x3 m_frameInB;
btScalar m_gearRatio;
int m_gearAuxLink;
btScalar m_erp;
btScalar m_relativePositionTarget;
public:
@@ -102,7 +104,14 @@ public:
{
m_gearAuxLink = gearAuxLink;
}
virtual void setRelativePositionTarget(btScalar relPosTarget)
{
m_relativePositionTarget = relPosTarget;
}
virtual void setErp(btScalar erp)
{
m_erp = erp;
}
};
#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H