diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 2e335a8ce..7c02f71c0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1704,7 +1704,18 @@ int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHan return 0; } +int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio) +{ + 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_RATIO; + command->m_userConstraintArguments.m_gearRatio = gearRatio; + return 0; +} b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 5fd7dd61c..aefdf469e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -102,7 +102,9 @@ b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHa int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]); int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]); int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce); - +int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio); + + b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); int b3GetNumUserConstraints(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c89f4af21..85b75acbb 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5877,7 +5877,48 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } + } + else + { + InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; + + if (parentBody && childBody) + { + if (parentBody->m_rigidBody) + { + if (clientCmd.m_userConstraintArguments.m_jointType == eGearType) + { + btRigidBody* childRb = childBody->m_rigidBody; + if (childRb) + { + btVector3 axisA(clientCmd.m_userConstraintArguments.m_jointAxis[0], + clientCmd.m_userConstraintArguments.m_jointAxis[1], + clientCmd.m_userConstraintArguments.m_jointAxis[2]); + + //for now we use the same local axis for both objects + btVector3 axisB(clientCmd.m_userConstraintArguments.m_jointAxis[0], + clientCmd.m_userConstraintArguments.m_jointAxis[1], + clientCmd.m_userConstraintArguments.m_jointAxis[2]); + + btScalar ratio=1; + btGearConstraint* gear = new btGearConstraint(*parentBody->m_rigidBody,*childRb, axisA,axisB,ratio); + m_data->m_dynamicsWorld->addConstraint(gear,true); + + InteralUserConstraintData userConstraintData; + userConstraintData.m_rbConstraint = gear; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + } + } } + } } if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT) @@ -5921,7 +5962,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } if (userConstraintPtr->m_rbConstraint) { - //todo + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) + { + if (userConstraintPtr->m_rbConstraint->getObjectType()==GEAR_CONSTRAINT_TYPE) + { + btGearConstraint* gear = (btGearConstraint*) userConstraintPtr->m_rbConstraint; + gear->setRatio(clientCmd.m_userConstraintArguments.m_gearRatio); + } + } } serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidChange; @@ -5944,7 +5992,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } if (userConstraintPtr->m_rbConstraint) { - + m_data->m_dynamicsWorld->removeConstraint(userConstraintPtr->m_rbConstraint); + delete userConstraintPtr->m_rbConstraint; + m_data->m_userConstraints.remove(userConstraintUidRemove); } serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove; serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 0ef7a2b68..d6f675fa9 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -627,7 +627,7 @@ enum EnumUserConstraintFlags USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16, USER_CONSTRAINT_CHANGE_MAX_FORCE=32, USER_CONSTRAINT_REQUEST_INFO=64, - + USER_CONSTRAINT_CHANGE_GEAR_RATIO=128, }; enum EnumBodyChangeFlags diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index e5b288dbb..d6cc19a56 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -184,6 +184,7 @@ enum JointType { ePlanarType = 3, eFixedType = 4, ePoint2PointType = 5, + eGearType=6 }; @@ -227,6 +228,8 @@ struct b3UserConstraint int m_jointType; double m_maxAppliedForce; int m_userConstraintUniqueId; + double m_gearRatio; + }; struct b3BodyInfo diff --git a/examples/pybullet/examples/mimicJointConstraint.py b/examples/pybullet/examples/mimicJointConstraint.py new file mode 100644 index 000000000..a92edc130 --- /dev/null +++ b/examples/pybullet/examples/mimicJointConstraint.py @@ -0,0 +1,16 @@ +#mimic joint currently only works for 'maximal coordinate' rigid bodies +#one way to use it would be to attach maximal coordinate rigid bodies to multibody links using +#fixed constraints + +import pybullet as p +import time +p.connect(p.GUI) +wheelA = p.loadURDF("wheel.urdf",[0,0,0],useMaximalCoordinates=1) +wheelB = p.loadURDF("wheel.urdf",[0,0,1],useMaximalCoordinates=1) +c = p.createConstraint(wheelA,-1,wheelB,-1,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-0.1) +p.setRealTimeSimulation(1) +while(1): + time.sleep(0.01) +p.removeConstraint(c) + \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f4b14b62c..0c7ae9f0e 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4608,7 +4608,7 @@ 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", "physicsClientId", NULL}; + static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "physicsClientId", NULL}; int userConstraintUniqueId = -1; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; @@ -4620,8 +4620,8 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P double jointChildPivot[3]; double jointChildFrameOrn[4]; double maxForce = -1; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOdi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &physicsClientId)) + double gearRatio = 0; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &physicsClientId)) { return NULL; } @@ -4647,7 +4647,10 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P { b3InitChangeUserConstraintSetMaxForce(commandHandle, maxForce); } - + if (gearRatio!=0) + { + b3InitChangeUserConstraintSetGearRatio(commandHandle,gearRatio); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); Py_INCREF(Py_None); @@ -6769,6 +6772,7 @@ initpybullet(void) PyModule_AddIntConstant(m, "JOINT_PLANAR", ePlanarType); // user read PyModule_AddIntConstant(m, "JOINT_FIXED", eFixedType); // user read PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType); // user read + PyModule_AddIntConstant(m, "JOINT_GEAR", eGearType); // user read PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read