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

add experimental pr2-gripper support in VR physics server

Browse Source 
add setErp to btMultiBodyJointMotor
This commit is contained in:
erwin coumans
2016-09-09 14:30:37 -07:00
parent f72982306e
commit 3c706306cd
7 changed files with 234 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -26,7 +26,8 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(1.),
m_kp(0)
m_kp(0),
m_erp(1)
{
m_maxAppliedImpulse = maxMotorImpulse;
@@ -123,7 +124,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;

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

@@ -29,6 +29,7 @@ protected:
btScalar m_desiredPosition;
btScalar m_kd;
btScalar m_kp;
btScalar m_erp;
public:
@@ -57,6 +58,14 @@ public:
m_kp = kp;
}
virtual void setErp(btScalar erp)
{
m_erp = erp;
}
virtual btScalar getErp() const
{
return m_erp;
}
virtual void debugDraw(class btIDebugDraw* drawer)
{