Add profile markers for BenchmarkDemo raytest.

Implement pybullet enableJointForceTorqueSensor, add forcetorquesensor.py example.

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

examples/SharedMemory/SharedMemoryPublic.h

@@ -189,6 +189,11 @@ struct b3BodyInfo
 };
 
 
+// copied from btMultiBodyLink.h
+enum SensorType {
+	eSensorForceTorqueType = 1,
+};
+
 
 struct b3JointSensorState
 {

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()
+

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) {
@@ -4668,6 +4721,10 @@ static PyMethodDef SpamMethods[] = {
 
 	{"removeConstraint", (PyCFunction)pybullet_removeUserConstraint, METH_VARARGS | METH_KEYWORDS,
      "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,
@@ -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