Merge remote-tracking branch 'bp/master' into master

6 files changed, 120 insertions, 125 deletions

diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index a676764bf..aade5904c 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -3433,34 +3433,46 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
 			}
 		}
 
-		// Because the link order between UrdfModel and MultiBody may be different,
-		// create a mapping from link name to link index in order to apply the user
-		// data to the correct link in the MultiBody.
-		btHashMap<btHashString, int> linkNameToIndexMap;
-		if (bodyHandle->m_multiBody)
-		{
-			btMultiBody* mb = bodyHandle->m_multiBody;
-			linkNameToIndexMap.insert(mb->getBaseName(), -1);
-			for (int linkIndex = 0; linkIndex < mb->getNumLinks(); ++linkIndex)
-			{
-				linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex);
-			}
-		}
-
+		// Add user data specified in URDF to the added body.
 		const UrdfModel* urdfModel = u2b.getUrdfModel();
 		if (urdfModel)
 		{
 			addUserData(urdfModel->m_userData, bodyUniqueId);
-			for (int i = 0; i < urdfModel->m_links.size(); ++i)
+			if (bodyHandle->m_multiBody)
 			{
-				const UrdfLink* link = *urdfModel->m_links.getAtIndex(i);
-				int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str());
-				if (linkIndex)
+				btMultiBody* mb = bodyHandle->m_multiBody;
+				// Because the link order between UrdfModel and MultiBody may be different,
+				// create a mapping from link name to link index in order to apply the user
+				// data to the correct link in the MultiBody.
+				btHashMap<btHashString, int> linkNameToIndexMap;
+				linkNameToIndexMap.insert(mb->getBaseName(), -1);
+				for (int linkIndex = 0; linkIndex < mb->getNumLinks(); ++linkIndex)
+				{
+					linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex);
+				}
+				for (int i = 0; i < urdfModel->m_links.size(); ++i)
 				{
-					addUserData(link->m_userData, bodyUniqueId, *linkIndex);
+					const UrdfLink* link = *urdfModel->m_links.getAtIndex(i);
+					int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str());
+					if (linkIndex)
+					{
+						addUserData(link->m_userData, bodyUniqueId, *linkIndex);
+						for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex)
+						{
+							addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, *linkIndex, visualShapeIndex);
+						}
+					}
+				}
+			}
+			else if (bodyHandle->m_rigidBody)
+			{
+				for (int i = 0; i < urdfModel->m_links.size(); ++i)
+				{
+					const UrdfLink* link = *urdfModel->m_links.getAtIndex(i);
+					addUserData(link->m_userData, bodyUniqueId, -1);
 					for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex)
 					{
-						addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, *linkIndex, visualShapeIndex);
+						addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, -1, visualShapeIndex);
 					}
 				}
 			}
@@ -8073,26 +8085,21 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
 	{
 		return false;
 	}
+	int numSoftbodyContact = 0;
+	for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
+	{
+		numSoftbodyContact += deformWorld->getSoftBodyArray()[i]->m_faceRigidContacts.size();
+	}
+	int num_contact_points = m_data->m_cachedContactPoints.size();
+	m_data->m_cachedContactPoints.reserve(num_contact_points + numSoftbodyContact);
 
 	for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
 	{
 		btSoftBody* psb = deformWorld->getSoftBodyArray()[i];
-		btAlignedObjectArray<b3ContactPointData> distinctContactPoints;
-		btAlignedObjectArray<btSoftBody::Node*> nodesInContact;
 		for (int c = 0; c < psb->m_faceRigidContacts.size(); c++)
 		{
 			const btSoftBody::DeformableFaceRigidContact* contact = &psb->m_faceRigidContacts[c];
-			// calculate normal and tangent impulse
-			btVector3 impulse = contact->m_cti.m_impulse;
-			btVector3 impulseNormal = impulse.dot(contact->m_cti.m_normal) * contact->m_cti.m_normal;
-			btVector3 impulseTangent = impulse - impulseNormal;
-			// get node in contact
-			int contactNodeIdx = contact->m_bary.maxAxis();
-			btSoftBody::Node* node = contact->m_face->m_n[contactNodeIdx];
-			// check if node is already in the list
-			int idx = nodesInContact.findLinearSearch2(node);
-
-			//apply the filter, if the user provides it
+			//convert rigidbody contact
 			int linkIndexA = -1;
 			int linkIndexB = -1;
 			int objectIndexA = psb->getUserIndex2();
@@ -8109,6 +8116,8 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
 				linkIndexB = mblB->m_link;
 				objectIndexB = mblB->m_multiBody->getUserIndex2();
 			}
+
+			//apply the filter, if the user provides it
 			bool swap = false;
 			if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
 			{
@@ -8154,87 +8163,37 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
 			{
 				continue;
 			}
-
-			if (idx < 0)
+			b3ContactPointData pt;
+			pt.m_bodyUniqueIdA = objectIndexA;
+			pt.m_bodyUniqueIdB = objectIndexB;
+			pt.m_contactDistance = contact->m_cti.m_offset;
+			pt.m_contactFlags = 0;
+			pt.m_linkIndexA = linkIndexA;
+			pt.m_linkIndexB = linkIndexB;
+			for (int j = 0; j < 3; j++)
 			{
-				// add new node and contact point
-				nodesInContact.push_back(node);
-				b3ContactPointData pt;
-				pt.m_bodyUniqueIdA = objectIndexA;
-				pt.m_bodyUniqueIdB = objectIndexB;
-				pt.m_contactDistance = -contact->m_cti.m_offset;
-				pt.m_contactFlags = 0;
-				pt.m_linkIndexA = linkIndexA;
-				pt.m_linkIndexB = linkIndexB;
-				for (int j = 0; j < 3; j++)
+				if (swap)
 				{
-					if (swap)
-					{
-						pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j];
-						pt.m_positionOnAInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912
-						// node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node)
-						pt.m_positionOnBInWS[j] = node->m_x[j];
-					}
-					else
-					{
-						pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j];
-						// node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node)
-						pt.m_positionOnAInWS[j] = node->m_x[j];
-						pt.m_positionOnBInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912
-					}
+					pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j];
+					pt.m_positionOnAInWS[j] = contact->m_cti.m_normal[j];
+					pt.m_positionOnBInWS[j] = -contact->m_cti.m_normal[j];
 				}
-				pt.m_normalForce = (impulseNormal / m_data->m_physicsDeltaTime).norm();
-				pt.m_linearFrictionForce1 = (impulseTangent.dot(contact->t1) * contact->t1 / m_data->m_physicsDeltaTime).norm();
-				pt.m_linearFrictionForce2 = (impulseTangent.dot(contact->t2) * contact->t2 / m_data->m_physicsDeltaTime).norm();
-				for (int j = 0; j < 3; j++)
+				else
 				{
-					pt.m_linearFrictionDirection1[j] = contact->t1[j];
-					pt.m_linearFrictionDirection2[j] = contact->t2[j];
+					pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j];
+					pt.m_positionOnAInWS[j] = -contact->m_cti.m_normal[j];
+					pt.m_positionOnBInWS[j] = contact->m_cti.m_normal[j];
 				}
-				distinctContactPoints.push_back(pt);
 			}
-			else
+			pt.m_normalForce = 1;
+			pt.m_linearFrictionForce1 = 0;
+			pt.m_linearFrictionForce2 = 0;
+			for (int j = 0; j < 3; j++)
 			{
-				// add values to existing contact point
-				b3ContactPointData* pt = &distinctContactPoints[idx];
-				// current normal force of node
-				btVector3 normalForce = btVector3(btScalar(pt->m_contactNormalOnBInWS[0]),
-												  btScalar(pt->m_contactNormalOnBInWS[1]),
-												  btScalar(pt->m_contactNormalOnBInWS[2])) * pt->m_normalForce;
-				// add normal force of additional node contact
-				btScalar swapFactor = swap ? -1.0 : 1.0;
-				normalForce += swapFactor * contact->m_cti.m_normal * (impulseNormal / m_data->m_physicsDeltaTime).norm();
-				// get magnitude of normal force
-				pt->m_normalForce = normalForce.norm();
-				// get direction of normal force
-				if (!normalForce.fuzzyZero())
-				{
-					// normalize for unit vectors if above numerical threshold
-					normalForce.normalize();
-					for (int j = 0; j < 3; j++)
-					{
-						pt->m_contactNormalOnBInWS[j] = normalForce[j];
-					}
-				}
-
-				// add magnitudes of tangential forces in existing directions
-				btVector3 linearFrictionDirection1 = btVector3(btScalar(pt->m_linearFrictionDirection1[0]),
-															   btScalar(pt->m_linearFrictionDirection1[1]),
-															   btScalar(pt->m_linearFrictionDirection1[2]));
-				btVector3 linearFrictionDirection2 = btVector3(btScalar(pt->m_linearFrictionDirection2[0]),
-															   btScalar(pt->m_linearFrictionDirection2[1]),
-															   btScalar(pt->m_linearFrictionDirection2[2]));
-				pt->m_linearFrictionForce1 = (impulseTangent.dot(linearFrictionDirection1) * linearFrictionDirection1 / m_data->m_physicsDeltaTime).norm();
-				pt->m_linearFrictionForce2 = (impulseTangent.dot(linearFrictionDirection2) * linearFrictionDirection2 / m_data->m_physicsDeltaTime).norm();
+				pt.m_linearFrictionDirection1[j] = 0;
+				pt.m_linearFrictionDirection2[j] = 0;
 			}
-		}
-
-		int num_contact_points = m_data->m_cachedContactPoints.size() + distinctContactPoints.size();
-		m_data->m_cachedContactPoints.reserve(num_contact_points);
-		// add points to contact points cache
-		for (int p = 0; p < distinctContactPoints.size(); p++)
-		{
-			m_data->m_cachedContactPoints.push_back(distinctContactPoints[p]);
+			m_data->m_cachedContactPoints.push_back(pt);
 		}
 	}
 #endif
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py
index 8ab49b103..e07c8d505 100644
--- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py
@@ -25,11 +25,28 @@ class EnvRandomizerBase(object):
     pass
 
   def randomize_step(self, env):
-    """Randomize simulation steps.
+    """Randomize environment steps.
 
-    Will be called at every timestep. May add random forces/torques to Minitaur.
+    Will be called at every environment step.
+
+    It is NOT recommended to use this for force / torque disturbance because
+    pybullet applyExternalForce/Torque only persist for single simulation step
+    not the entire env step which can contain multiple simulation steps.
+
+    Args:
+      env: The Minitaur gym environment to be randomized.
+    """
+    pass
+
+  def randomize_sub_step(self, env, sub_step_index, num_sub_steps):
+    """Randomize simulation sub steps.
+
+    Will be called at every simulation step. This is the correct place to add
+    random forces/torques.
 
     Args:
       env: The Minitaur gym environment to be randomized.
+      sub_step_index: Index of sub step, from 0 to N-1. N is the action repeat.
+      num_sub_steps: Number of sub steps, equals to action repeat.
     """
     pass
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_push_randomizer.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_push_randomizer.py
index 8d9aa8e64..aa4adb7d4 100644
--- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_push_randomizer.py
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_push_randomizer.py
@@ -10,7 +10,9 @@ parentdir = os.path.dirname(os.path.dirname(currentdir))
 parentdir = os.path.dirname(os.path.dirname(parentdir))
 os.sys.path.insert(0, parentdir)
 
+import functools
 import math
+import gin
 import numpy as np
 from pybullet_envs.minitaur.envs import env_randomizer_base
 
@@ -23,6 +25,7 @@ _VERTICAL_FORCE_UPPER_BOUND = 300
 _VERTICAL_FORCE_LOWER_BOUND = 500
 
 
+@gin.configurable
 class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase):
   """Applies a random impulse to the base of Minitaur."""
 
@@ -54,6 +57,7 @@ class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase):
                                     [_HORIZONTAL_FORCE_LOWER_BOUND, _HORIZONTAL_FORCE_UPPER_BOUND])
     self._vertical_force_bound = (vertical_force_bound if vertical_force_bound else
                                   [_VERTICAL_FORCE_LOWER_BOUND, _VERTICAL_FORCE_UPPER_BOUND])
+    self._perturbation_parameter_dict = None
 
   def randomize_env(self, env):
     """Randomizes the simulation environment.
@@ -64,9 +68,10 @@ class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase):
     pass
 
   def randomize_step(self, env):
-    """Randomizes simulation steps.
+    """Randomizes env steps.
 
-    Will be called at every timestep. May add random forces/torques to Minitaur.
+    Will be called at every env step. Called to generate randomized  force and
+    torque to apply. Application of forces are done in randomize_sub_step.
 
     Args:
       env: The Minitaur gym environment to be randomized.
@@ -85,8 +90,25 @@ class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase):
     if (env.env_step_counter % self._perturbation_interval_steps <
         self._perturbation_duration_steps) and (env.env_step_counter >=
                                                 self._perturbation_start_step):
-      env.pybullet_client.applyExternalForce(objectUniqueId=env.minitaur.quadruped,
-                                             linkIndex=self._applied_link_id,
-                                             forceObj=self._applied_force,
-                                             posObj=[0.0, 0.0, 0.0],
-                                             flags=env.pybullet_client.LINK_FRAME)
+      # Parameter of pybullet_client.applyExternalForce()
+      self._perturbation_parameter_dict = dict(objectUniqueId=env.minitaur.quadruped,
+                                               linkIndex=self._applied_link_id,
+                                               forceObj=self._applied_force,
+                                               posObj=[0.0, 0.0, 0.0],
+                                               flags=env.pybullet_client.LINK_FRAME)
+    else:
+      self._perturbation_parameter_dict = None
+
+  def randomize_sub_step(self, env, sub_step_index, num_sub_steps):
+    """Randomize simulation steps per sub steps (simulation step).
+
+    Will be called at every simulation step. This is the correct place to add
+    random forces/torques to Minitaur.
+
+    Args:
+      env: The Minitaur gym environment to be randomized.
+      sub_step_index: Index of sub step, from 0 to N-1. N is the action repeat.
+      num_sub_steps: Number of sub steps, equals to action repeat.
+    """
+    if self._perturbation_parameter_dict is not None:
+      env.pybullet_client.applyExternalForce(**self._perturbation_parameter_dict)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py
index 0c30e5b8e..b98d3b005 100644
--- a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py
@@ -457,6 +457,8 @@ class LocomotionGymEnv(gym.Env):
     for obj in self._dynamic_objects():
       obj.pre_control_step(autonomous_object.AUTONOMOUS_ACTION)
     for _ in range(self._num_action_repeat):
+      for env_randomizer in self._env_randomizers:
+        env_randomizer.randomize_sub_step(self, i, self._num_action_repeat)
       self._robot.apply_action(action)
       for obj in self._dynamic_objects():
         obj.update(self.get_time_since_reset(), self._observation_dict)
diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp
index 9c8e72f50..09398d79a 100644
--- a/src/BulletSoftBody/btDeformableContactConstraint.cpp
+++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp
@@ -268,7 +268,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
 	{
 		dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
 	}
-	// dn is the normal component of velocity difference. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+	// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
 	btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0)));
 	if (!infoGlobal.m_splitImpulse)
 	{
@@ -487,9 +487,6 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
 	btVector3 dv = impulse * contact->m_c2;
 	btSoftBody::Face* face = contact->m_face;
 
-	// save applied impulse
-	contact->m_cti.m_impulse = impulse;
-
 	btVector3& v0 = face->m_n[0]->m_v;
 	btVector3& v1 = face->m_n[1]->m_v;
 	btVector3& v2 = face->m_n[2]->m_v;
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index cee3ae0f2..d5b01faf3 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/src/BulletSoftBody/btSoftBody.h
@@ -223,12 +223,10 @@ public:
 	/* sCti is Softbody contact info	*/
 	struct sCti
 	{
-		const btCollisionObject* m_colObj; /* Rigid body			        */
-		btVector3 m_normal;                /* Outward normal		        */
-		mutable btVector3 m_impulse;	   /* Applied impulse        	    */
-		btScalar m_offset;                 /* Offset from origin	        */
+		const btCollisionObject* m_colObj; /* Rigid body			*/
+		btVector3 m_normal;                /* Outward normal		*/
+		btScalar m_offset;                 /* Offset from origin	*/
 		btVector3 m_bary;                  /* Barycentric weights for faces */
-		sCti() : m_impulse(0, 0, 0) {}
 	};
 
 	/* sMedium		*/
@@ -894,7 +892,7 @@ public:
 				   int node1) const;
 	bool checkLink(const Node* node0,
 				   const Node* node1) const;
-	/* Check for existing face												*/
+	/* Check for existring face												*/
 	bool checkFace(int node0,
 				   int node1,
 				   int node2) const;