[pybullet] implement addUserDebugParameter / readUserDebugParameter

fix inertial frame for door.urdf
allow picking of links of multi-bodies with a fixed base

data/door.urdf

@@ -72,7 +72,7 @@
   </link>
    <link name="childA">
     <inertial>
-      <origin rpy="0 0 0" xyz="0 0 -0.36"/>
+      <origin rpy="0 0 0" xyz="0.4 0 0.4"/>
       <mass value="1.0"/>
       <inertia ixx="0.048966669" ixy="0" ixz="0" iyy="0.046466667" iyz="0" izz="0.0041666669"/>
     </inertial>

examples/CommonInterfaces/CommonGUIHelperInterface.h

@@ -72,6 +72,9 @@ struct GUIHelperInterface
 	
 	virtual int		addUserDebugText3D( const char* txt, const double posisionXYZ[3], const double	textColorRGB[3], double size, double lifeTime){return -1;};
 	virtual int		addUserDebugLine(const double	debugLineFromXYZ[3], const double	debugLineToXYZ[3], const double	debugLineColorRGB[3], double lineWidth, double lifeTime ){return -1;};
+	virtual int		addUserDebugParameter(const char* txt, double	rangeMin, double	rangeMax, double startValue){return -1;};
+	virtual int		readUserDebugParameter(int itemUniqueId, double* value) { return 0;}
+
 	virtual void	removeUserDebugItem( int debugItemUniqueId){};
 	virtual void	removeAllUserDebugItems( ){};
 

examples/ExampleBrowser/OpenGLGuiHelper.h

@@ -63,6 +63,11 @@ struct OpenGLGuiHelper : public GUIHelperInterface
 	{
 		return -1;
 	}
+	virtual int		addUserDebugParameter(const char* txt, double	rangeMin, double	rangeMax, double startValue)
+	{
+		return -1;
+	}
+
 	virtual void	removeUserDebugItem( int debugItemUniqueId)
 	{
 	}

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1338,6 +1338,55 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle p
 	return (b3SharedMemoryCommandHandle) command;
 }
 
+b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+	b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    command->m_type =CMD_USER_DEBUG_DRAW;
+	command->m_updateFlags = USER_DEBUG_ADD_PARAMETER;
+	int len = strlen(txt);
+    if (len<MAX_FILENAME_LENGTH)
+    {
+		strcpy(command->m_userDebugDrawArgs.m_text,txt);
+    } else
+    {
+        command->m_userDebugDrawArgs.m_text[0] = 0;
+    }
+	command->m_userDebugDrawArgs.m_rangeMin = rangeMin;
+	command->m_userDebugDrawArgs.m_rangeMax = rangeMax;
+	command->m_userDebugDrawArgs.m_startValue = startValue;
+	return (b3SharedMemoryCommandHandle)command;
+}
+
+b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId)
+{
+		PhysicsClient* cl = (PhysicsClient* ) physClient;
+	b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    command->m_type =CMD_USER_DEBUG_DRAW;
+	command->m_updateFlags = USER_DEBUG_READ_PARAMETER;
+	command->m_userDebugDrawArgs.m_itemUniqueId = debugItemUniqueId;
+	return (b3SharedMemoryCommandHandle) command;
+}
+
+int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue)
+{
+	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED);
+	if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED))
+	{
+		*paramValue = status->m_userDebugDrawArgs.m_parameterValue;
+		return 1;
+	}
+	return 0;
+}
+
+
 b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId)
 {
 	PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -1347,7 +1396,7 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle phys
     b3Assert(command);
     command->m_type =CMD_USER_DEBUG_DRAW;
 	command->m_updateFlags = USER_DEBUG_REMOVE_ONE_ITEM;
-	command->m_userDebugDrawArgs.m_removeItemUniqueId = debugItemUniqueId;
+	command->m_userDebugDrawArgs.m_itemUniqueId = debugItemUniqueId;
 	return (b3SharedMemoryCommandHandle) command;
 
 }

examples/SharedMemory/PhysicsClientC_API.h

@@ -93,6 +93,10 @@ void    b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l
 /// Add/remove user-specific debug lines and debug text messages
 b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime);
 b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime);
+b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue);
+b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId);
+int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue);
+
 b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId);
 b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient);
 

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -792,6 +792,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 				b3Warning("Save .bullet failed");
 				break;
 			}
+			case CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED:
 			case CMD_USER_DEBUG_DRAW_COMPLETED:
 			{
 				break;

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -4042,7 +4042,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 						SharedMemoryStatus& serverCmd = serverStatusOut;
 						serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED;
 						hasStatus = true;
+						if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER)
+						{
+							int uid = m_data->m_guiHelper->addUserDebugParameter(
+								clientCmd.m_userDebugDrawArgs.m_text,
+								clientCmd.m_userDebugDrawArgs.m_rangeMin,
+								clientCmd.m_userDebugDrawArgs.m_rangeMax,
+								clientCmd.m_userDebugDrawArgs.m_startValue
+							);
+							serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
+							serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
+						}
+						if (clientCmd.m_updateFlags &USER_DEBUG_READ_PARAMETER)
+						{
 						
+							int ok = m_data->m_guiHelper->readUserDebugParameter(
+								clientCmd.m_userDebugDrawArgs.m_itemUniqueId,
+								&serverCmd.m_userDebugDrawArgs.m_parameterValue);
+							if (ok)
+							{
+								serverCmd.m_type = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED;
+							} 
+						}
 						if ((clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) || (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR))
 						{
 							int bodyUniqueId = clientCmd.m_userDebugDrawArgs.m_objectUniqueId;
@@ -4131,7 +4152,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 						}
 						if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM)
 						{
-							m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_removeItemUniqueId);
+							m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_itemUniqueId);
 							serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
 
 						}
@@ -4229,7 +4250,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
 		} else
 		{
 			btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
-			if (multiCol && multiCol->m_multiBody && multiCol->m_multiBody->getBaseMass()>0.)
+			if (multiCol && multiCol->m_multiBody)
 			{
 
 				m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep();
@@ -4483,6 +4504,8 @@ void PhysicsServerCommandProcessor::resetSimulation()
 		m_data->m_visualConverter.resetAll();
 	}
 
+	removePickingConstraint();
+
 	deleteDynamicsWorld();
 	createEmptyDynamicsWorld();
 

examples/SharedMemory/PhysicsServerExample.cpp

@@ -176,6 +176,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
 	eGUIHelperAutogenerateGraphicsObjects,
 	eGUIUserDebugAddText,
 	eGUIUserDebugAddLine,
+	eGUIUserDebugAddParameter,
 	eGUIUserDebugRemoveItem,
 	eGUIUserDebugRemoveAllItems,
 };
@@ -481,6 +482,15 @@ struct UserDebugDrawLine
 	int m_itemUniqueId;
 };
 
+struct UserDebugParameter
+{
+	char m_text[1024];
+	double m_rangeMin;
+	double m_rangeMax;
+	double m_value;
+	int m_itemUniqueId;
+};
+
 struct UserDebugText
 {
 	char m_text[1024];
@@ -807,8 +817,6 @@ public:
 	}
 
 
-
-
 	btAlignedObjectArray<UserDebugText> m_userDebugText;
 	
 	UserDebugText m_tmpText;
@@ -835,6 +843,38 @@ public:
 		return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId;
 	}
 
+	btAlignedObjectArray<UserDebugParameter*> m_userDebugParams;
+	UserDebugParameter m_tmpParam;
+
+	virtual int		readUserDebugParameter(int itemUniqueId, double* value) 
+	{ 
+		for (int i=0;i<m_userDebugParams.size();i++)
+		{
+			if (m_userDebugParams[i]->m_itemUniqueId == itemUniqueId)
+			{
+				*value = m_userDebugParams[i]->m_value;
+				return 1;
+			}
+		}
+		return 0;
+	}
+
+	virtual int		addUserDebugParameter(const char* txt, double	rangeMin, double	rangeMax, double startValue)
+	{
+		strcpy(m_tmpParam.m_text,txt);
+		m_tmpParam.m_rangeMin = rangeMin;
+		m_tmpParam.m_rangeMax = rangeMax;
+		m_tmpParam.m_value = startValue;
+		m_tmpParam.m_itemUniqueId = m_uidGenerator++;
+
+		m_cs->lock();
+		m_cs->setSharedParam(1, eGUIUserDebugAddParameter);
+		workerThreadWait();
+
+		return (*m_userDebugParams[m_userDebugParams.size()-1]).m_itemUniqueId;
+	}
+
+
 	btAlignedObjectArray<UserDebugDrawLine> m_userDebugLines;
 	UserDebugDrawLine m_tmpLine;
 
@@ -1407,6 +1447,24 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
 		m_multiThreadedHelper->mainThreadRelease();
 		break;
 	}
+	case eGUIUserDebugAddParameter:
+	{
+		UserDebugParameter* param = new UserDebugParameter(m_multiThreadedHelper->m_tmpParam);
+		m_multiThreadedHelper->m_userDebugParams.push_back(param);
+
+		{
+        SliderParams slider(param->m_text,&param->m_value);
+        slider.m_minVal=param->m_rangeMin;
+        slider.m_maxVal=param->m_rangeMax;
+		
+		if (m_multiThreadedHelper->m_childGuiHelper->getParameterInterface())
+	        m_multiThreadedHelper->m_childGuiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+		}
+
+		//also add actual menu
+		m_multiThreadedHelper->mainThreadRelease();
+		break;
+	}
 	case eGUIUserDebugAddLine:
 	{
 		m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine);

examples/SharedMemory/SharedMemoryCommands.h

@@ -571,6 +571,8 @@ enum EnumUserDebugDrawFlags
 	USER_DEBUG_REMOVE_ALL=8,	
 	USER_DEBUG_SET_CUSTOM_OBJECT_COLOR = 16,
 	USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR = 32,
+	USER_DEBUG_ADD_PARAMETER=64,
+	USER_DEBUG_READ_PARAMETER=128,
 
 };
 
@@ -582,13 +584,17 @@ struct UserDebugDrawArgs
 	double	m_lineWidth;
 	
 	double m_lifeTime;
-	int m_removeItemUniqueId;
+	int m_itemUniqueId;
 
 	char m_text[MAX_FILENAME_LENGTH];
 	double m_textPositionXYZ[3];
 	double m_textColorRGB[3];
 	double m_textSize;
 
+	double m_rangeMin;
+	double m_rangeMax;
+	double m_startValue;
+
 	double m_objectDebugColorRGB[3];
 	int m_objectUniqueId;
 	int m_linkIndex;
@@ -599,6 +605,7 @@ struct UserDebugDrawArgs
 struct UserDebugDrawResultArgs
 {
 	int m_debugItemUniqueId;
+	double m_parameterValue;
 };
 
 struct SendVREvents

examples/SharedMemory/SharedMemoryPublic.h

@@ -109,6 +109,7 @@ enum EnumSharedMemoryServerStatus
         CMD_LOAD_TEXTURE_COMPLETED,
         CMD_LOAD_TEXTURE_FAILED,
 		CMD_USER_DEBUG_DRAW_COMPLETED,
+		CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED,
 		CMD_USER_DEBUG_DRAW_FAILED,
 		CMD_USER_CONSTRAINT_COMPLETED,
 		CMD_USER_CONSTRAINT_FAILED,

examples/pybullet/constraint.py

@@ -9,7 +9,7 @@ cubeId = p.loadURDF("cube_small.urdf",0,0,1)
 p.setGravity(0,0,-10)
 p.setRealTimeSimulation(1)
 cid = p.createConstraint(cubeId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1])
-
+prev=[0,0,1]
 a=-math.pi
 while 1:
 	a=a+0.01
@@ -19,6 +19,6 @@ while 1:
 	p.setGravity(0,0,-10)
 	pivot=[a,0,1]
 	orn = p.getQuaternionFromEuler([a,0,0])
-	p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn)
+	p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn, maxForce=50)
 
 p.removeConstraint(cid)

examples/pybullet/premake4.lua

@@ -20,6 +20,7 @@ project ("pybullet")
                 }
 
 	if os.is("MacOSX") then
+--		targetextension {"so"}
 		links{"Cocoa.framework","Python"}
 	end
 
@@ -38,6 +39,7 @@ if not _OPTIONS["no-enet"] then
 		includedirs {"../../examples/ThirdPartyLibs/enet/include"}
 	
 		if os.is("Windows") then 
+--			targetextension {"dylib"}
 			defines { "WIN32" }
 			links {"Ws2_32","Winmm"}
 		end

examples/pybullet/pybullet.c

@@ -2126,6 +2126,103 @@ b3PhysicsClientHandle sm = 0;
 }
 
 
+static PyObject* pybullet_readUserDebugParameter(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	int itemUniqueId;
+	int physicsClientId = 0;
+	b3PhysicsClientHandle sm = 0;
+
+	static char *kwlist[] = { "itemUniqueId", "physicsClientId", NULL };
+
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&itemUniqueId, &physicsClientId))
+	{
+		return NULL;
+	}
+
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	commandHandle = b3InitUserDebugReadParameter(sm,itemUniqueId);
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED) 
+	{
+		double paramValue=0.f;
+		int ok = b3GetStatusDebugParameterValue(statusHandle,&paramValue);
+		if (ok)
+		{
+			PyObject* item = PyFloat_FromDouble(paramValue);
+			return item;
+		}
+	}
+
+	PyErr_SetString(SpamError, "Failed to read parameter.");
+	return NULL;
+
+}
+
+	
+
+static PyObject* pybullet_addUserDebugParameter(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+  
+  b3SharedMemoryCommandHandle commandHandle;
+  b3SharedMemoryStatusHandle statusHandle;
+  int statusType;
+  int res = 0;
+
+  char* text;
+  double posXYZ[3];
+  double colorRGB[3]={1,1,1};
+
+  
+  PyObject* textPositionObj=0;
+  double rangeMin = 0.f;
+  double rangeMax = 1.f;
+  double startValue = 0.f;
+  int physicsClientId = 0;
+  b3PhysicsClientHandle sm=0;
+  static char *kwlist[] = { "paramName", "rangeMin", "rangeMax","startValue", "physicsClientId", NULL };
+
+
+  if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|dddi", kwlist, &text,  &rangeMin,  &rangeMax,&startValue, &physicsClientId))
+  {
+	  return NULL;
+  }
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+  
+   commandHandle = b3InitUserDebugAddParameter(sm,text,rangeMin,rangeMax, startValue);
+
+  statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+  statusType = b3GetStatusType(statusHandle);
+  
+	if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) 
+	{
+		int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
+		PyObject* item = PyInt_FromLong(debugItemUniqueId);
+		return item;
+	}
+
+
+  PyErr_SetString(SpamError, "Error in addUserDebugParameter.");
+  return NULL;
+}
+
+
 static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds) 
 {
   
@@ -4543,6 +4640,13 @@ static PyMethodDef SpamMethods[] = {
 	 "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."
 	  },
 
+	  { "addUserDebugParameter", (PyCFunction)pybullet_addUserDebugParameter, METH_VARARGS | METH_KEYWORDS,
+	 "Add a user debug parameter, such as a slider, that can be controlled using a GUI."
+	  },
+	 { "readUserDebugParameter", (PyCFunction)pybullet_readUserDebugParameter, METH_VARARGS | METH_KEYWORDS,
+	 "Read the current value of a user debug parameter, given the user debug item unique id."
+	  },
+
 	 { "removeUserDebugItem", (PyCFunction)pybullet_removeUserDebugItem, METH_VARARGS | METH_KEYWORDS,
 	 "remove a user debug draw item, giving its unique id"
 	  },

test/InverseDynamics/test_invdyn_jacobian.cpp

@@ -76,7 +76,7 @@ void calculateDotJacUError(const MultiBodyTreeCreator& creator, const int nloops
         EXPECT_EQ(0, tree2->calculatePositionAndVelocityKinematics(q, u));
         EXPECT_EQ(0, tree2->calculateJacobians(q, u));
 
-        for (size_t idx = 0; idx < nbodies; idx++) {
+        for (int idx = 0; idx < nbodies; idx++) {
             vec3 tmp1, tmp2;
             vec3 diff;
             EXPECT_EQ(0, tree1->getBodyLinearAcceleration(idx, &tmp1));
@@ -134,7 +134,7 @@ void calculateJacobianError(const MultiBodyTreeCreator& creator, const int nloop
         EXPECT_EQ(0, tree2->calculatePositionKinematics(q));
         EXPECT_EQ(0, tree2->calculateJacobians(q));
 
-        for (size_t idx = 0; idx < nbodies; idx++) {
+        for (int idx = 0; idx < nbodies; idx++) {
             mat3x ref_jac_r(3, ndofs);
             mat3x ref_jac_t(3, ndofs);
             ref_jac_r.setZero();
@@ -211,7 +211,7 @@ void calculateVelocityJacobianError(const MultiBodyTreeCreator& creator, const i
         EXPECT_EQ(0, tree2->calculatePositionKinematics(q));
         EXPECT_EQ(0, tree2->calculateJacobians(q));
 
-        for (size_t idx = 0; idx < nbodies; idx++) {
+        for (int idx = 0; idx < nbodies; idx++) {
             vec3 vel1;
             vec3 omg1;
             vec3 vel2;