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

Merge pull request #675 from erwincoumans/master

Browse Source 
remove debug printf's, fix an issue introduced in a previous commit to btMultiJointMotor etc.
This commit is contained in:
erwincoumans
2016-06-27 16:22:59 -07:00
committed by GitHub
parent ed9be25570 178dd54ca5
commit eae90eb201
2 changed files with 5 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -25,7 +25,7 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(0.1),
m_kd(1.),
m_kp(0)
{
@@ -56,7 +56,7 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int li
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(0.1),
m_kd(1.),
m_kp(0)
{
btAssert(linkDoF < body->getLink(link).m_dofCount);
@@ -125,13 +125,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + m_kd * velocityError;
printf("m_kd = %f\n", m_kd);
printf("m_kp = %f\n", m_kp);
printf("m_desiredVelocity = %f\n", m_desiredVelocity);
printf("m_desiredPosition = %f\n", m_desiredPosition);
printf("m_maxAppliedImpulse = %f\n", m_maxAppliedImpulse);
printf("rhs = %f\n", rhs);
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,rhs);

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

@@ -45,13 +45,13 @@ public:
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 0.1f)
virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
{
m_desiredVelocity = velTarget;
m_kd = kd;
}
virtual void setPositionTarget(btScalar posTarget, btScalar kp = 0.1f)
virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
{
m_desiredPosition = posTarget;
m_kp = kp;