Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-10-16 13:55:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Erwin Coumans
2022-03-05 13:41:28 -08:00
parent 28b951c128 9f3c4123a2
commit dd93f73fc4
6 changed files with 120 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files

171
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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)
{
addUserData(link->m_userData, bodyUniqueId, *linkIndex);
linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex);
}
for (int i = 0; i < urdfModel->m_links.size(); ++i)
{
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

21
examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py
View File

@@ -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

36
examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_push_randomizer.py
View File

@@ -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)

2
examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py
View File

@@ -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)

5
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@@ -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;

10
src/BulletSoftBody/btSoftBody.h
View File

@@ -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;