diff --git a/data/racecar/racecar_differential.urdf b/data/racecar/racecar_differential.urdf index 8f5f37519..c4f6ee42e 100644 --- a/data/racecar/racecar_differential.urdf +++ b/data/racecar/racecar_differential.urdf @@ -20,12 +20,12 @@ - + - + @@ -33,14 +33,14 @@ - + - + @@ -66,14 +66,14 @@ - + - + @@ -163,14 +163,14 @@ - + - + @@ -207,14 +207,14 @@ - + - + @@ -533,7 +533,7 @@ - + @@ -573,7 +573,7 @@ - + @@ -612,7 +612,7 @@ - + @@ -650,7 +650,7 @@ - + @@ -686,7 +686,7 @@ - + diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 8f743f08d..cb3e69de4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1949,6 +1949,20 @@ int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHa return 0; } +int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_CONSTRAINT); + b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT); + + command->m_updateFlags |=USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK; + command->m_userConstraintArguments.m_gearAuxLink = gearAuxLink; + + return 0; +} + + b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index efa45d495..da7bdac42 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -107,7 +107,7 @@ int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHan int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]); int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce); int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio); - +int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink); b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 04610f6cd..1c07479cc 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6450,6 +6450,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio); } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) + { + userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink); + } } if (userConstraintPtr->m_rbConstraint) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 9658d6e6c..103dd5f96 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -645,6 +645,8 @@ enum EnumUserConstraintFlags USER_CONSTRAINT_CHANGE_MAX_FORCE=32, USER_CONSTRAINT_REQUEST_INFO=64, USER_CONSTRAINT_CHANGE_GEAR_RATIO=128, + USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256, + }; enum EnumBodyChangeFlags diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 882db7a24..eaf3de675 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -235,6 +235,7 @@ struct b3UserConstraint double m_maxAppliedForce; int m_userConstraintUniqueId; double m_gearRatio; + int m_gearAuxLink; }; diff --git a/examples/pybullet/examples/racecar_differential.py b/examples/pybullet/examples/racecar_differential.py index 215f7667a..9abab35df 100644 --- a/examples/pybullet/examples/racecar_differential.py +++ b/examples/pybullet/examples/racecar_differential.py @@ -12,10 +12,10 @@ useRealTimeSim = 1 #for video recording (works best on Mac and Linux, not well on Windows) #p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4") p.setRealTimeSimulation(useRealTimeSim) # either this -#p.loadURDF("plane.urdf") -p.loadSDF("stadium.sdf") +p.loadURDF("plane.urdf") +#p.loadSDF("stadium.sdf") -car = p.loadURDF("racecar/racecar_differential.urdf")#, [0,0,2],useFixedBase=True) +car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True) for i in range (p.getNumJoints(car)): print (p.getJointInfo(car,i)) for wheel in range(p.getNumJoints(car)): @@ -26,7 +26,6 @@ wheels = [8,15] print("----------------") #p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10) - c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=1, maxForce=10000) @@ -39,12 +38,17 @@ p.changeConstraint(c,gearRatio=-1, maxForce=10000) c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=1, maxForce=10000) -c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) -p.changeConstraint(c,gearRatio=-1, maxForce=10000) c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=-1, maxForce=10000) +c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, maxForce=10000) + +c = p.createConstraint(car,1,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15, maxForce=10000) +c = p.createConstraint(car,3,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000) steering = [0,2] diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index c690d128a..056838df8 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4804,11 +4804,12 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds) { - static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "physicsClientId", NULL}; + static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "gearAuxLink", "physicsClientId", NULL}; int userConstraintUniqueId = -1; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int statusType; + int gearAuxLink = -1; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; PyObject* jointChildPivotObj = 0; @@ -4817,7 +4818,7 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P double jointChildFrameOrn[4]; double maxForce = -1; double gearRatio = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddii", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &gearAuxLink, &physicsClientId)) { return NULL; } @@ -4847,6 +4848,10 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P { b3InitChangeUserConstraintSetGearRatio(commandHandle,gearRatio); } + if (gearAuxLink>=0) + { + b3InitChangeUserConstraintSetGearAuxLink(commandHandle,gearAuxLink); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); Py_INCREF(Py_None); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 27e1ccc05..8c28bbf4c 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -184,6 +184,9 @@ public: virtual void debugDraw(class btIDebugDraw* drawer)=0; virtual void setGearRatio(btScalar ratio) {} + virtual void setGearAuxLink(int gearAuxLink) {} + + }; #endif //BT_MULTIBODY_CONSTRAINT_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp index 2e242b1f7..3fdd51815 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -22,7 +22,8 @@ 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_gearRatio(1), + m_gearAuxLink(-1) { } @@ -121,11 +122,18 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& int dof = 0; btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; - + btScalar auxVel = 0; + + if (m_gearAuxLink>=0) + { + auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof]; + } + currentVelocity += auxVel; + //btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; //btScalar velocityError = (m_desiredVelocity - currentVelocity); - btScalar desiredRelativeVelocity = 0; + btScalar desiredRelativeVelocity = auxVel; fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h index 947166ae7..711a73e46 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h @@ -31,10 +31,11 @@ protected: btMatrix3x3 m_frameInA; btMatrix3x3 m_frameInB; btScalar m_gearRatio; + int m_gearAuxLink; public: - btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + //btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); virtual ~btMultiBodyGearConstraint(); @@ -97,6 +98,10 @@ public: { m_gearRatio = gearRatio; } + virtual void setGearAuxLink(int gearAuxLink) + { + m_gearAuxLink = gearAuxLink; + } };