diff options
author | Erwin Coumans <erwin.coumans@gmail.com> | 2022-03-05 13:41:28 -0800 |
---|---|---|
committer | Erwin Coumans <erwin.coumans@gmail.com> | 2022-03-05 13:41:28 -0800 |
commit | dd93f73fc4c594e90adee28437335ef32f189302 (patch) | |
tree | a8eb35c46bf3d716780c627186b6b2ad443d1af9 | |
parent | 28b951c128b53e1dcf26271dd47b88776148a940 (diff) | |
parent | 9f3c4123a2177cddf437487e089a434c6b09fe25 (diff) | |
download | bullet3-dd93f73fc4c594e90adee28437335ef32f189302.tar.gz |
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; |