diff --git a/docs/pybullet_quickstartguide.pdf b/docs/pybullet_quickstartguide.pdf index 8f49ec179..b2ad0f70f 100644 Binary files a/docs/pybullet_quickstartguide.pdf and b/docs/pybullet_quickstartguide.pdf differ diff --git a/examples/Benchmarks/BenchmarkDemo.cpp b/examples/Benchmarks/BenchmarkDemo.cpp index fdacc0347..d9a1b3ef0 100644 --- a/examples/Benchmarks/BenchmarkDemo.cpp +++ b/examples/Benchmarks/BenchmarkDemo.cpp @@ -210,9 +210,13 @@ public: { for ( int i = iBegin; i < iEnd; ++i ) { + btCollisionWorld::ClosestRayResultCallback cb(source[i], dest[i]); - cw->rayTest (source[i], dest[i], cb); + { + BT_PROFILE("cw->rayTest"); + cw->rayTest(source[i], dest[i], cb); + } if (cb.hasHit ()) { hit[i] = cb.m_hitPointWorld; @@ -241,6 +245,8 @@ public: void cast (btCollisionWorld* cw, bool multiThreading = false) { + BT_PROFILE("cast"); + #ifdef USE_BT_CLOCK frame_timer.reset (); #endif //USE_BT_CLOCK diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 40e60c3bb..913fe086b 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -189,6 +189,11 @@ struct b3BodyInfo }; +// copied from btMultiBodyLink.h +enum SensorType { + eSensorForceTorqueType = 1, +}; + struct b3JointSensorState { diff --git a/examples/pybullet/forcetorquesensor.py b/examples/pybullet/forcetorquesensor.py new file mode 100644 index 000000000..9e0f343a3 --- /dev/null +++ b/examples/pybullet/forcetorquesensor.py @@ -0,0 +1,26 @@ +import pybullet as p +p.connect(p.DIRECT) +hinge = p.loadURDF("hinge.urdf") +print("mass of linkA = 1kg, linkB = 1kg, total mass = 2kg") + +hingeJointIndex = 0 + +#by default, joint motors are enabled, maintaining zero velocity +p.setJointMotorControl2(hinge,hingeJointIndex,p.VELOCITY_CONTROL,0,0,0) + +p.setGravity(0,0,-10) +p.stepSimulation() +print("joint state without sensor") + +print(p.getJointState(0,0)) +p.enableJointForceTorqueSensor(hinge,hingeJointIndex) +p.stepSimulation() +print("joint state with force/torque sensor, gravity [0,0,-10]") +print(p.getJointState(0,0)) +p.setGravity(0,0,0) +p.stepSimulation() +print("joint state with force/torque sensor, no gravity") +print(p.getJointState(0,0)) + +p.disconnect() + diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 756894fb7..e024318ed 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3362,6 +3362,63 @@ static PyObject* pybullet_updateUserConstraint(PyObject* self, PyObject* args, P } */ +static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject* args, PyObject *keywds) +{ + int bodyUniqueId = -1; + int jointIndex = -1; + int enableSensor = 1; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + int numJoints = -1; + + static char *kwlist[] = {"bodyUniqueId", "jointIndex" ,"enableSensor", "physicsClientId" }; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &jointIndex, &enableSensor,&physicsClientId)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + + if (bodyUniqueId < 0) + { + PyErr_SetString(SpamError, "Error: invalid bodyUniqueId"); + return NULL; + } + numJoints = b3GetNumJoints(sm, bodyUniqueId); + if ((jointIndex < 0) || (jointIndex >= numJoints)) + { + PyErr_SetString(SpamError, "Error: invalid jointIndex."); + return NULL; + } + + { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3CreateSensorCommandInit(sm, bodyUniqueId); + b3CreateSensorEnable6DofJointForceTorqueSensor(commandHandle, jointIndex, enableSensor); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CLIENT_COMMAND_COMPLETED) + { + Py_INCREF(Py_None); + return Py_None; + } + } + + PyErr_SetString(SpamError, "Error creating sensor."); + return NULL; +} + + static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) { @@ -3455,15 +3512,11 @@ static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, P int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle); PyObject* ob = PyLong_FromLong(userConstraintUid); return ob; - } else - { - PyErr_SetString(SpamError, "createConstraint failed."); - return NULL; } - Py_INCREF(Py_None); - return Py_None; + PyErr_SetString(SpamError, "createConstraint failed."); + return NULL; } static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, PyObject *keywds) { @@ -4670,6 +4723,10 @@ static PyMethodDef SpamMethods[] = { "Remove a constraint using its unique id." }, + { "enableJointForceTorqueSensor", (PyCFunction)pybullet_enableJointForceTorqueSensor, METH_VARARGS | METH_KEYWORDS, + "Enable or disable a joint force/torque sensor measuring the joint reaction forces." + }, + {"saveWorld", (PyCFunction)pybullet_saveWorld, METH_VARARGS| METH_KEYWORDS, "Save a approximate Python file to reproduce the current state of the world: saveWorld" "(filename). (very preliminary and approximately)"}, @@ -4934,6 +4991,9 @@ initpybullet(void) PyModule_AddIntConstant(m, "JOINT_FIXED", eFixedType); // user read PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType); // user read + PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read + + PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE); PyModule_AddIntConstant(m, "VELOCITY_CONTROL", CONTROL_MODE_VELOCITY); // user read