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), :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity), m_desiredVelocity(desiredVelocity),
m_desiredPosition(0), m_desiredPosition(0),
m_kd(0.1), m_kd(1.),
m_kp(0) 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), :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity), m_desiredVelocity(desiredVelocity),
m_desiredPosition(0), m_desiredPosition(0),
m_kd(0.1), m_kd(1.),
m_kp(0) m_kp(0)
{ {
btAssert(linkDoF < body->getLink(link).m_dofCount); 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 currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity); btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + m_kd * velocityError; btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+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);
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,rhs); 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, btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal); 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_desiredVelocity = velTarget;
m_kd = kd; 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_desiredPosition = posTarget;
m_kp = kp; m_kp = kp;