Merge pull request #1091 from YunfeiBai/master

Add APIs to reset object mass, lateral friction coefficient, and to get user constraint id.

examples/SharedMemory/PhysicsClient.h

@@ -37,6 +37,8 @@ public:
     virtual int getNumUserConstraints() const = 0;
     
     virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const = 0;
+	
+	virtual int getUserConstraintId(int serialIndex) const = 0;
     
     virtual void setSharedMemoryKey(int key) = 0;
 

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1179,6 +1179,13 @@ int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniq
     return 0;
 }
 
+/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
+int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+	return cl->getUserConstraintId(serialIndex);
+}
+
 /// return the body unique id, given the index in range [0 , b3GetNumBodies() )
 int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex)
 {
@@ -1208,7 +1215,41 @@ int	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd
 	return cl->getJointInfo(bodyIndex, jointIndex, *info);
 }
 
+b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	b3Assert(command);
+	command->m_type = CMD_RESET_DYNAMIC_INFO;
+	command->m_updateFlags = 0;
+	
+	return (b3SharedMemoryCommandHandle) command;
+}
 
+int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_RESET_DYNAMIC_INFO);
+	b3Assert(mass > 0);
+	command->m_resetDynamicInfoArgs.m_bodyUniqueId = bodyUniqueId;
+	command->m_resetDynamicInfoArgs.m_linkIndex = linkIndex;
+	command->m_resetDynamicInfoArgs.m_mass = mass;
+	command->m_updateFlags |= RESET_DYNAMIC_INFO_SET_MASS;
+	return 0;
+}
+
+int b3ResetDynamicInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_RESET_DYNAMIC_INFO);
+	command->m_resetDynamicInfoArgs.m_bodyUniqueId = bodyUniqueId;
+	command->m_resetDynamicInfoArgs.m_linkIndex = linkIndex;
+	command->m_resetDynamicInfoArgs.m_lateralFriction = lateralFriction;
+	command->m_updateFlags |= RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION;
+	return 0;
+}
 
 b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
 {

examples/SharedMemory/PhysicsClientC_API.h

@@ -74,6 +74,13 @@ int	b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
 
 ///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
 int	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
+
+///given a body unique id and link index, return the dynamic information. See b3DynamicInfo in SharedMemoryPublic.h
+int b3GetDynamicInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, struct b3DynamicInfo* info);
+	
+b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient);
+int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
+int b3ResetDynamicInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
     
 b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
 
@@ -90,6 +97,8 @@ b3SharedMemoryCommandHandle  b3InitRemoveUserConstraintCommand(b3PhysicsClientHa
 
 int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
 int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
+/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
+int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex);
     
 ///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
 ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -163,6 +163,15 @@ int PhysicsClientSharedMemory::getUserConstraintInfo(int constraintUniqueId, str
     return 0;
 }
 
+int PhysicsClientSharedMemory::getUserConstraintId(int serialIndex) const
+{
+	if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints()))
+	{
+		return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1();
+	}
+	return -1;
+}
+
 PhysicsClientSharedMemory::PhysicsClientSharedMemory()
 
 {

examples/SharedMemory/PhysicsClientSharedMemory.h

@@ -47,6 +47,8 @@ public:
     virtual int getNumUserConstraints() const;
     
     virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
+	
+	virtual int getUserConstraintId(int serialIndex) const;
     
     virtual void setSharedMemoryKey(int key);
 

examples/SharedMemory/PhysicsDirect.cpp

@@ -925,7 +925,14 @@ int PhysicsDirect::getUserConstraintInfo(int constraintUniqueId, struct b3UserCo
     return 0;
 }
 
-
+int PhysicsDirect::getUserConstraintId(int serialIndex) const
+{
+	if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints()))
+	{
+		return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1();
+	}
+	return -1;
+}
 
 int PhysicsDirect::getBodyUniqueId(int serialIndex) const
 {

examples/SharedMemory/PhysicsDirect.h

@@ -66,6 +66,8 @@ public:
     virtual int getNumUserConstraints() const;
     
     virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
+	
+	virtual int getUserConstraintId(int serialIndex) const;
     
 	///todo: move this out of the
     virtual void setSharedMemoryKey(int key);

examples/SharedMemory/PhysicsLoopBack.cpp

@@ -108,6 +108,11 @@ int PhysicsLoopBack::getUserConstraintInfo(int constraintUniqueId, struct b3User
     return m_data->m_physicsClient->getUserConstraintInfo( constraintUniqueId, info);
 }
 
+int PhysicsLoopBack::getUserConstraintId(int serialIndex) const
+{
+	return m_data->m_physicsClient->getUserConstraintId(serialIndex);
+}
+
 ///todo: move this out of the interface
 void PhysicsLoopBack::setSharedMemoryKey(int key)
 {

examples/SharedMemory/PhysicsLoopBack.h

@@ -51,6 +51,8 @@ public:
     virtual int getNumUserConstraints() const;
     
     virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const;
+	
+	virtual int getUserConstraintId(int serialIndex) const;
     
 	///todo: move this out of the
     virtual void setSharedMemoryKey(int key);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -3901,6 +3901,63 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 
 					break;
 				};
+				case CMD_RESET_DYNAMIC_INFO:
+				{
+					BT_PROFILE("CMD_RESET_DYNAMIC_INFO");
+					
+					if (clientCmd.m_updateFlags & RESET_DYNAMIC_INFO_SET_MASS)
+					{
+						int bodyUniqueId = clientCmd.m_resetDynamicInfoArgs.m_bodyUniqueId;
+						int linkIndex = clientCmd.m_resetDynamicInfoArgs.m_linkIndex;
+						double mass = clientCmd.m_resetDynamicInfoArgs.m_mass;
+						btAssert(bodyUniqueId >= 0);
+						btAssert(linkIndex >= -1);
+						
+						InteralBodyData* body = m_data->getHandle(bodyUniqueId);
+						if (body && body->m_multiBody)
+						{
+							btMultiBody* mb = body->m_multiBody;
+							if (linkIndex == -1)
+							{
+								mb->setBaseMass(mass);
+							}
+							else
+							{
+								mb->getLink(linkIndex).m_mass = mass;
+							}
+						}
+					}
+					
+					if (clientCmd.m_updateFlags & RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION)
+					{
+						int bodyUniqueId = clientCmd.m_resetDynamicInfoArgs.m_bodyUniqueId;
+						int linkIndex = clientCmd.m_resetDynamicInfoArgs.m_linkIndex;
+						double lateralFriction = clientCmd.m_resetDynamicInfoArgs.m_lateralFriction;
+						btAssert(bodyUniqueId >= 0);
+						btAssert(linkIndex >= -1);
+						
+						InteralBodyData* body = m_data->getHandle(bodyUniqueId);
+						if (body && body->m_multiBody)
+						{
+							btMultiBody* mb = body->m_multiBody;
+							if (linkIndex == -1)
+							{
+								mb->getBaseCollider()->setFriction(lateralFriction);
+							}
+							else
+							{
+								mb->getLinkCollider(linkIndex)->setFriction(lateralFriction);
+							}
+						}
+					}
+
+					
+					SharedMemoryStatus& serverCmd =serverStatusOut;
+					serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
+					hasStatus = true;
+					
+					break;
+				};
 				case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
 				{
 					BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");

examples/SharedMemory/SharedMemoryCommands.h

@@ -106,6 +106,22 @@ struct BulletDataStreamArgs
 	char m_bodyName[MAX_FILENAME_LENGTH];
 };
 
+enum EnumResetDynamicInfoFlags
+{
+	RESET_DYNAMIC_INFO_SET_MASS=1,
+	RESET_DYNAMIC_INFO_SET_COM=2,
+	RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION=4,
+};
+
+struct ResetDynamicInfoArgs
+{
+	int m_bodyUniqueId;
+	int m_linkIndex;
+	double m_mass;
+	double m_COM[3];
+	double m_lateralFriction;
+};
+
 struct SetJointFeedbackArgs
 {
 	int m_bodyUniqueId;
@@ -702,6 +718,7 @@ struct SharedMemoryCommand
 		struct MjcfArgs	m_mjcfArguments;
 		struct FileArgs m_fileArguments;
 		struct SdfRequestInfoArgs m_sdfRequestInfoArgs;
+		struct ResetDynamicInfoArgs m_resetDynamicInfoArgs;
 		struct InitPoseArgs m_initPoseArgs;
 		struct SendPhysicsSimulationParameters m_physSimParamArgs;
 		struct BulletDataStreamArgs	m_dataStreamArguments;

examples/SharedMemory/SharedMemoryPublic.h

@@ -55,6 +55,7 @@ enum EnumSharedMemoryClientCommand
     CMD_CONFIGURE_OPENGL_VISUALIZER,
 	CMD_REQUEST_KEYBOARD_EVENTS_DATA,
 	CMD_REQUEST_OPENGL_VISUALIZER_CAMERA,
+	CMD_RESET_DYNAMIC_INFO,
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     
@@ -215,6 +216,11 @@ struct b3BodyInfo
 	const char* m_bodyName; // for btRigidBody, it does not have a base, but can still have a body name from urdf
 };
 
+struct b3DynamicInfo
+{
+	double m_mass;
+	double m_localInertialPosition[3];
+};
 
 // copied from btMultiBodyLink.h
 enum SensorType {

examples/pybullet/examples/constraint.py

@@ -9,6 +9,8 @@ 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])
+print cid
+print p.getConstraintUniqueId(0)
 prev=[0,0,1]
 a=-math.pi
 while 1:
@@ -21,4 +23,4 @@ while 1:
 	orn = p.getQuaternionFromEuler([a,0,0])
 	p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn, maxForce=50)
 
-p.removeConstraint(cid)
+p.removeConstraint(cid)

examples/pybullet/examples/reset_dynamic_info.py

@@ -0,0 +1,20 @@
+import pybullet as p
+import time
+import math
+
+p.connect(p.GUI)
+
+p.loadURDF(fileName="plane.urdf",baseOrientation=[0.25882,0,0,0.96593])
+p.loadURDF(fileName="cube.urdf",baseOrientation=[0.25882,0,0,0.96593],basePosition=[0,0,2])
+p.loadURDF(fileName="cube.urdf",baseOrientation=[0,0,0,1],basePosition=[0,0,4])
+p.resetDynamicInfo(bodyUniqueId=2,linkIndex=-1,mass=0.1)
+#p.resetDynamicInfo(bodyUniqueId=2,linkIndex=-1,mass=100.0)
+p.setGravity(0,0,-10)
+p.setRealTimeSimulation(0)
+t=0
+while 1:
+	t=t+1
+	if t > 400:
+		p.resetDynamicInfo(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01)
+	time.sleep(.01)
+	p.stepSimulation()

examples/pybullet/pybullet.c

@@ -604,6 +604,49 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key
 	return pylist;
 }
 
+static PyObject* pybullet_resetDynamicInfo(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	int bodyUniqueId = -1;
+	int linkIndex = -2;
+	double mass = -1;
+	double lateralFriction = -1;
+	b3PhysicsClientHandle sm = 0;
+	
+	int physicsClientId = 0;
+	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "physicsClientId", NULL};
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &physicsClientId))
+	{
+		return NULL;
+	}
+	
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+	
+	{
+		b3SharedMemoryCommandHandle command = b3InitResetDynamicInfo(sm);
+		b3SharedMemoryStatusHandle statusHandle;
+		
+		if (mass >= 0)
+		{
+			b3ResetDynamicInfoSetMass(command, bodyUniqueId, linkIndex, mass);
+		}
+		
+		if (lateralFriction >= 0)
+		{
+			b3ResetDynamicInfoSetLateralFriction(command, bodyUniqueId, linkIndex, lateralFriction);
+		}
+		
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	}
+	
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
 static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	double fixedTimeStep = -1;
@@ -1809,6 +1852,36 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb
 	return NULL;
 }
 
+static PyObject* pybullet_getConstraintUniqueId(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	int physicsClientId = 0;
+	int serialIndex = -1;
+	b3PhysicsClientHandle sm = 0;
+	
+	static char* kwlist[] = {"serialIndex", "physicsClientId", NULL};
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &serialIndex, &physicsClientId))
+	{
+		return NULL;
+	}
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+	
+	{
+		int userConstraintId = -1;
+		userConstraintId = b3GetUserConstraintId(sm, serialIndex);
+		
+#if PY_MAJOR_VERSION >= 3
+		return PyLong_FromLong(userConstraintId);
+#else
+		return PyInt_FromLong(userConstraintId);
+#endif
+	}
+}
+
 static PyObject* pybullet_getNumConstraints(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	int numConstraints = 0;
@@ -5393,7 +5466,7 @@ static PyMethodDef SpamMethods[] = {
 	{"getConstraintInfo", (PyCFunction)pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS,
 	 "Get the user-created constraint info, given a constraint unique id."},
 
-	{"getConstraintUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS | METH_KEYWORDS,
+	{"getConstraintUniqueId", (PyCFunction)pybullet_getConstraintUniqueId, METH_VARARGS | METH_KEYWORDS,
 	 "Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."},
 
 	{"getBasePositionAndOrientation", (PyCFunction)pybullet_getBasePositionAndOrientation,
@@ -5435,6 +5508,9 @@ static PyMethodDef SpamMethods[] = {
 	{"resetJointState", (PyCFunction)pybullet_resetJointState, METH_VARARGS | METH_KEYWORDS,
 	 "Reset the state (position, velocity etc) for a joint on a body "
 	 "instantaneously, not through physics simulation."},
+	
+	{"resetDynamicInfo", (PyCFunction)pybullet_resetDynamicInfo, METH_VARARGS | METH_KEYWORDS,
+	 "Reset dynamic information such as mass, lateral friction coefficient."},
 
 	{"setJointMotorControl", (PyCFunction)pybullet_setJointMotorControl, METH_VARARGS,
 	 "This (obsolete) method cannot select non-zero physicsClientId, use setJointMotorControl2 instead."

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -140,6 +140,11 @@ public:
 		return m_baseCollider;
 	}
 
+	btMultiBodyLinkCollider* getLinkCollider(int index)
+	{
+		return m_colliders[index];
+	}
+
     //
     // get parent
     // input: link num from 0 to num_links-1