Merge branch 'master' of https://github.com/erwincoumans/bullet3

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -2128,7 +2128,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHan
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
 	b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
 	command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
-	command->m_changeDynamicsInfoArgs.m_linearDamping = angularDamping;
+	command->m_changeDynamicsInfoArgs.m_angularDamping = angularDamping;
 	command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING;
 	return 0;
 }
@@ -2292,10 +2292,47 @@ B3_SHARED_API	int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle c
 }
 
 
+B3_SHARED_API	b3SharedMemoryCommandHandle b3InitGetUserConstraintStateCommand(b3PhysicsClientHandle physClient, int constraintUniqueId)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	b3Assert(command);
+
+	command->m_type = CMD_USER_CONSTRAINT;
+	command->m_updateFlags = USER_CONSTRAINT_REQUEST_STATE;
+	command->m_userConstraintArguments.m_userConstraintUniqueId = constraintUniqueId;
+	return (b3SharedMemoryCommandHandle)command;
+}
+
+B3_SHARED_API	int b3GetStatusUserConstraintState(b3SharedMemoryStatusHandle statusHandle, struct b3UserConstraintState* constraintState)
+{
+	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	if (status)
+	{
+		btAssert(status->m_type == CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED);
+		if (status && status->m_type == CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED)
+		{
+			int i = 0;
+			constraintState->m_numDofs = status->m_userConstraintStateResultArgs.m_numDofs;
+			for (i = 0; i < constraintState->m_numDofs; i++)
+			{
+				constraintState->m_appliedConstraintForces[i] = status->m_userConstraintStateResultArgs.m_appliedConstraintForces[i];
+			}
+			for (; i < 6; i++)
+			{
+				constraintState->m_appliedConstraintForces[i] = 0;
+			}
+			return 1;
+		}
+	}
+	return 0;
+}
 
 B3_SHARED_API	b3SharedMemoryCommandHandle  b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
 {
-	 PhysicsClient* cl = (PhysicsClient* ) physClient;
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
     b3Assert(cl);
     b3Assert(cl->canSubmitCommand());
     struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
@@ -2341,6 +2378,7 @@ B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle s
 }
 
 
+
 B3_SHARED_API	b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
                                        double rayFromWorldY, double rayFromWorldZ,
                                        double rayToWorldX, double rayToWorldY, double rayToWorldZ)

examples/SharedMemory/PhysicsClientC_API.h

@@ -143,6 +143,11 @@ B3_SHARED_API	int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle c
 B3_SHARED_API	b3SharedMemoryCommandHandle  b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
 
 B3_SHARED_API	int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
+
+B3_SHARED_API	b3SharedMemoryCommandHandle b3InitGetUserConstraintStateCommand(b3PhysicsClientHandle physClient, int constraintUniqueId);
+B3_SHARED_API	int b3GetStatusUserConstraintState(b3SharedMemoryStatusHandle statusHandle, struct b3UserConstraintState* constraintState);
+
+
 B3_SHARED_API	int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
 /// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
 B3_SHARED_API	int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex);

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -601,6 +601,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                 
                 break;
             }
+			case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED:
+			{
+				break;
+			}
 			case CMD_USER_CONSTRAINT_INFO_COMPLETED:
 			{
 				B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED");

examples/SharedMemory/PhysicsDirect.cpp

@@ -783,7 +783,10 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 		}
 		break;
 	}
-
+	case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED:
+	{
+		break;
+	}
 	case CMD_SYNC_BODY_INFO_COMPLETED:
 	case CMD_MJCF_LOADING_COMPLETED:
 	case CMD_SDF_LOADING_COMPLETED:

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -5630,13 +5630,22 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction;
 					double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution;
 					btAssert(bodyUniqueId >= 0);
-					btAssert(linkIndex >= -1);
 						
 					InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 
 					if (body && body->m_multiBody)
 					{
 						btMultiBody* mb = body->m_multiBody;
+
+						if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
+						{
+							mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping);
+						}
+						if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
+						{
+							mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
+						}
+
 						if (linkIndex == -1)
 						{
 							if (mb->getBaseCollider())
@@ -5645,14 +5654,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								{
 									mb->getBaseCollider()->setRestitution(restitution);
 								}
-								if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
+								
-								{
-									mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping);
-								}
-								if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
-								{
-									mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
-								}
 
 								if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
 								{
@@ -7173,6 +7175,29 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					SharedMemoryStatus& serverCmd =serverStatusOut;
                     serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
                     hasStatus = true;
+					if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_STATE)
+					{
+						int constraintUid = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
+						InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(constraintUid);
+						if (userConstraintPtr)
+						{
+							serverCmd.m_userConstraintStateResultArgs.m_numDofs = 0;
+							for (int i = 0; i < 6; i++)
+							{
+								serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = 0;
+							}
+							if (userConstraintPtr->m_mbConstraint)
+							{
+								serverCmd.m_userConstraintStateResultArgs.m_numDofs = userConstraintPtr->m_mbConstraint->getNumRows();
+								for (int i = 0; i < userConstraintPtr->m_mbConstraint->getNumRows(); i++)
+								{
+									serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = userConstraintPtr->m_mbConstraint->getAppliedImpulse(i) / m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
+								}
+								serverCmd.m_type = CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED;
+							}
+						}
+
+					};
 					if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_INFO)
 					{
 						int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
@@ -7180,6 +7205,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 						if (userConstraintPtr)
 						{
 							serverCmd.m_userConstraintResultArgs = userConstraintPtr->m_userConstraintData;
+							
 							serverCmd.m_type = CMD_USER_CONSTRAINT_INFO_COMPLETED;
 						}
 					}

examples/SharedMemory/SharedMemoryCommands.h

@@ -676,7 +676,7 @@ enum EnumUserConstraintFlags
 	USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256,
 	USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET=512,
 	USER_CONSTRAINT_CHANGE_ERP=1024,
-
+	USER_CONSTRAINT_REQUEST_STATE=2048,
 };
 
 enum EnumBodyChangeFlags
@@ -1039,6 +1039,7 @@ struct SharedMemoryStatus
 		struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
 		struct UserDebugDrawResultArgs m_userDebugDrawArgs;
 		struct b3UserConstraint m_userConstraintResultArgs;
+		struct b3UserConstraintState m_userConstraintStateResultArgs;
 		struct SendVREvents m_sendVREvents;
 		struct SendKeyboardEvents m_sendKeyboardEvents;
 		struct SendRaycastHits m_raycastHits;

examples/SharedMemory/SharedMemoryPublic.h

@@ -5,7 +5,8 @@
 ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
 ///my convention is year/month/day/rev
 
-#define SHARED_MEMORY_MAGIC_NUMBER 201710050
+#define SHARED_MEMORY_MAGIC_NUMBER 201710180
+//#define SHARED_MEMORY_MAGIC_NUMBER 201710050
 //#define SHARED_MEMORY_MAGIC_NUMBER 201708270
 //#define SHARED_MEMORY_MAGIC_NUMBER 201707140
 //#define SHARED_MEMORY_MAGIC_NUMBER 201706015
@@ -143,6 +144,7 @@ enum EnumSharedMemoryServerStatus
 		CMD_USER_DEBUG_DRAW_FAILED,
 		CMD_USER_CONSTRAINT_COMPLETED,
 		CMD_USER_CONSTRAINT_INFO_COMPLETED,
+		CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED,
         CMD_REMOVE_USER_CONSTRAINT_COMPLETED,
         CMD_CHANGE_USER_CONSTRAINT_COMPLETED,
 		CMD_REMOVE_USER_CONSTRAINT_FAILED,
@@ -253,7 +255,6 @@ struct b3UserConstraint
 	int m_gearAuxLink;
 	double m_relativePositionTarget;
 	double m_erp;
-
 };
 
 struct b3BodyInfo
@@ -331,6 +332,12 @@ struct b3OpenGLVisualizerCameraInfo
 	float m_target[3];
 };
 
+struct b3UserConstraintState
+{
+	double m_appliedConstraintForces[6];
+	int m_numDofs;
+};
+
 enum b3VREventType
 {
 	VR_CONTROLLER_MOVE_EVENT=1,

examples/pybullet/gym/pybullet_envs/agents/configs.py

@@ -36,7 +36,7 @@ def default():
   eval_episodes = 25
   use_gpu = False
   # Network
-  network = networks.ForwardGaussianPolicy
+  network = networks.feed_forward_gaussian
   weight_summaries = dict(
       all=r'.*',
       policy=r'.*/policy/.*',

examples/pybullet/gym/pybullet_envs/agents/train_ppo.py

@@ -101,8 +101,6 @@ def train(config, env_processes):
   """
   tf.reset_default_graph()
   with config.unlocked:
-    config.network = functools.partial(
-        utility.define_network, config.network, config)
     config.policy_optimizer = getattr(tf.train, config.policy_optimizer)
     config.value_optimizer = getattr(tf.train, config.value_optimizer)
   if config.update_every % config.num_agents:

examples/pybullet/gym/pybullet_envs/agents/visualize_ppo.py

@@ -98,8 +98,6 @@ def visualize(
   """
   config = utility.load_config(logdir)
   with config.unlocked:
-    config.network = functools.partial(
-        utility.define_network, config.network, config)
     config.policy_optimizer = getattr(tf.train, config.policy_optimizer)
     config.value_optimizer = getattr(tf.train, config.value_optimizer)
   with tf.device('/cpu:0'):

examples/pybullet/pybullet.c

@@ -2470,7 +2470,7 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb
 		b3PhysicsClientHandle sm = 0;
 
 		int physicsClientId = 0;
-		static char* kwlist[] = {"constraintUniqueId", "physicsClientId", NULL};
+		static char* kwlist[] = { "constraintUniqueId", "physicsClientId", NULL };
 		if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &constraintUniqueId, &physicsClientId))
 		{
 			return NULL;
@@ -2539,10 +2539,61 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb
 		}
 	}
 
+
 	PyErr_SetString(SpamError, "Couldn't get user constraint info");
 	return NULL;
 }
 
+static PyObject* pybullet_getConstraintState(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	{
+		int constraintUniqueId = -1;
+		b3PhysicsClientHandle sm = 0;
+
+		int physicsClientId = 0;
+		static char* kwlist[] = { "constraintUniqueId", "physicsClientId", NULL };
+		if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &constraintUniqueId, &physicsClientId))
+		{
+			return NULL;
+		}
+		sm = getPhysicsClient(physicsClientId);
+		if (sm == 0)
+		{
+			PyErr_SetString(SpamError, "Not connected to physics server.");
+			return NULL;
+		}
+
+		{
+			b3SharedMemoryCommandHandle cmd_handle;
+			b3SharedMemoryStatusHandle statusHandle;
+			int statusType;
+			if (b3CanSubmitCommand(sm))
+			{
+				struct b3UserConstraintState constraintState;
+				cmd_handle = b3InitGetUserConstraintStateCommand(sm, constraintUniqueId);
+				statusHandle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
+				statusType = b3GetStatusType(statusHandle);
+
+				if (b3GetStatusUserConstraintState(statusHandle, &constraintState))
+				{
+					if (constraintState.m_numDofs)
+					{
+						PyObject* appliedConstraintForces = PyTuple_New(constraintState.m_numDofs);
+						int i = 0;
+						for (i = 0; i < constraintState.m_numDofs; i++)
+						{
+							PyTuple_SetItem(appliedConstraintForces, i, PyFloat_FromDouble(constraintState.m_appliedConstraintForces[i]));
+						}
+						return appliedConstraintForces;
+					}
+				}
+			}
+		}
+	}
+	PyErr_SetString(SpamError, "Couldn't getConstraintState.");
+	return NULL;
+}
+
 static PyObject* pybullet_getConstraintUniqueId(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	int physicsClientId = 0;
@@ -7699,6 +7750,9 @@ static PyMethodDef SpamMethods[] = {
 	{"getConstraintInfo", (PyCFunction)pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS,
 	 "Get the user-created constraint info, given a constraint unique id."},
 
+	 { "getConstraintState", (PyCFunction)pybullet_getConstraintState, METH_VARARGS | METH_KEYWORDS,
+		"Get the user-created constraint state (applied forces), given a constraint unique id." },
+
 	{"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)."},
 

src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp

@@ -156,7 +156,7 @@ void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray
         btVector3 constraintNormalAng(0,0,0);
         btScalar posError = 0.0;
         if (i < 3) {
-            constraintNormalLin[i] = -1;
+            constraintNormalLin[i] = 1;
             posError = (pivotAworld-pivotBworld).dot(constraintNormalLin);
             fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
                                     constraintNormalLin, pivotAworld, pivotBworld,