From f6ea2a73796438734be1a0cb5f8124da8e976be8 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 1 Nov 2018 10:20:54 -0700 Subject: [PATCH 1/8] fix a potential data race condition. remove createObstacleCourse programmatic creation part until we can steam vertices/indices. --- .../SharedMemory/PhysicsServerExample.cpp | 3 ++- .../pybullet/examples/createObstacleCourse.py | 22 ------------------- 2 files changed, 2 insertions(+), 23 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 2dc7145be..36ec1f18b 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1764,8 +1764,9 @@ void PhysicsServerExample::initPhysics() } } + m_args[0].m_cs->lock(); m_args[0].m_cs->setSharedParam(1, eGUIHelperIdle); - + m_args[0].m_cs->unlock(); m_args[0].m_cs2->lock(); { diff --git a/examples/pybullet/examples/createObstacleCourse.py b/examples/pybullet/examples/createObstacleCourse.py index 1194a213a..ef145c72c 100644 --- a/examples/pybullet/examples/createObstacleCourse.py +++ b/examples/pybullet/examples/createObstacleCourse.py @@ -21,28 +21,6 @@ colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) #convex mesh from obj stoneId = p.createCollisionShape(p.GEOM_MESH,fileName="stone.obj") -#concave mesh from obj -stoneId = p.createCollisionShape(p.GEOM_MESH,fileName="stone.obj", flags=p.GEOM_FORCE_CONCAVE_TRIMESH) - - -verts=[[-0.246350, -0.246483, -0.000624], - [ -0.151407, -0.176325, 0.172867], - [ -0.246350, 0.249205, -0.000624], - [ -0.151407, 0.129477, 0.172867], - [ 0.249338, -0.246483, -0.000624], - [ 0.154395, -0.176325, 0.172867], - [ 0.249338, 0.249205, -0.000624], - [ 0.154395, 0.129477, 0.172867]] -#convex mesh from vertices -stoneConvexId = p.createCollisionShape(p.GEOM_MESH,vertices=verts) - -indices=[0,3,2,3,6,2,7,4,6,5,0,4,6,0,2,3,5,7,0,1,3,3,7,6,7,5,4,5,1,0,6,4,0,3,1,5] - -#concave mesh from vertices+indices -stoneConcaveId = p.createCollisionShape(p.GEOM_MESH,vertices=verts, indices=indices) - -stoneId = stoneConvexId -#stoneId = stoneConcaveId boxHalfLength = 0.5 From 8b53e47fe8531710eb1ae0533035ef4e51f855d4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 2 Nov 2018 11:19:46 -0700 Subject: [PATCH 2/8] add simpler ARS implementation, thanks to Alexis Jacq and Hadelin de Ponteves (will add save/restore of policy and rendering movies through command-line arguments soon) --- .../pybullet/gym/pybullet_envs/ARS/ars.py | 142 ++++++++++++++++++ 1 file changed, 142 insertions(+) create mode 100644 examples/pybullet/gym/pybullet_envs/ARS/ars.py diff --git a/examples/pybullet/gym/pybullet_envs/ARS/ars.py b/examples/pybullet/gym/pybullet_envs/ARS/ars.py new file mode 100644 index 000000000..b4c0c3127 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/ARS/ars.py @@ -0,0 +1,142 @@ +# AI 2018 + +# Importing the libraries +import os +import numpy as np +import gym +from gym import wrappers +import pybullet_envs + +# Setting the Hyper Parameters + +class Hp(): + + def __init__(self): + self.nb_steps = 1000 + self.episode_length = 1000 + self.learning_rate = 0.02 + self.nb_directions = 16 + self.nb_best_directions = 16 + assert self.nb_best_directions <= self.nb_directions + self.noise = 0.03 + self.seed = 1 + self.env_name = 'HalfCheetahBulletEnv-v0' + +# Normalizing the states + +class Normalizer(): + + def __init__(self, nb_inputs): + self.n = np.zeros(nb_inputs) + self.mean = np.zeros(nb_inputs) + self.mean_diff = np.zeros(nb_inputs) + self.var = np.zeros(nb_inputs) + + def observe(self, x): + self.n += 1. + last_mean = self.mean.copy() + self.mean += (x - self.mean) / self.n + self.mean_diff += (x - last_mean) * (x - self.mean) + self.var = (self.mean_diff / self.n).clip(min = 1e-2) + + def normalize(self, inputs): + obs_mean = self.mean + obs_std = np.sqrt(self.var) + return (inputs - obs_mean) / obs_std + +# Building the AI + +class Policy(): + + def __init__(self, input_size, output_size): + self.theta = np.zeros((output_size, input_size)) + print("self.theta=",self.theta) + def evaluate(self, input, delta = None, direction = None): + if direction is None: + return np.clip(self.theta.dot(input), -1.0, 1.0) + elif direction == "positive": + return np.clip((self.theta + hp.noise*delta).dot(input), -1.0, 1.0) + else: + return np.clip((self.theta - hp.noise*delta).dot(input), -1.0, 1.0) + + def sample_deltas(self): + return [np.random.randn(*self.theta.shape) for _ in range(hp.nb_directions)] + + def update(self, rollouts, sigma_r): + step = np.zeros(self.theta.shape) + for r_pos, r_neg, d in rollouts: + step += (r_pos - r_neg) * d + self.theta += hp.learning_rate / (hp.nb_best_directions * sigma_r) * step + +# Exploring the policy on one specific direction and over one episode + +def explore(env, normalizer, policy, direction = None, delta = None): + state = env.reset() + done = False + num_plays = 0. + sum_rewards = 0 + while not done and num_plays < hp.episode_length: + normalizer.observe(state) + state = normalizer.normalize(state) + action = policy.evaluate(state, delta, direction) + state, reward, done, _ = env.step(action) + reward = max(min(reward, 1), -1) + sum_rewards += reward + num_plays += 1 + return sum_rewards + +# Training the AI + +def train(env, policy, normalizer, hp): + + for step in range(hp.nb_steps): + + # Initializing the perturbations deltas and the positive/negative rewards + deltas = policy.sample_deltas() + positive_rewards = [0] * hp.nb_directions + negative_rewards = [0] * hp.nb_directions + + # Getting the positive rewards in the positive directions + for k in range(hp.nb_directions): + positive_rewards[k] = explore(env, normalizer, policy, direction = "positive", delta = deltas[k]) + + # Getting the negative rewards in the negative/opposite directions + for k in range(hp.nb_directions): + negative_rewards[k] = explore(env, normalizer, policy, direction = "negative", delta = deltas[k]) + + # Gathering all the positive/negative rewards to compute the standard deviation of these rewards + all_rewards = np.array(positive_rewards + negative_rewards) + sigma_r = all_rewards.std() + + # Sorting the rollouts by the max(r_pos, r_neg) and selecting the best directions + scores = {k:max(r_pos, r_neg) for k,(r_pos,r_neg) in enumerate(zip(positive_rewards, negative_rewards))} + order = sorted(scores.keys(), key = lambda x:scores[x])[:hp.nb_best_directions] + rollouts = [(positive_rewards[k], negative_rewards[k], deltas[k]) for k in order] + + # Updating our policy + policy.update(rollouts, sigma_r) + + # Printing the final reward of the policy after the update + reward_evaluation = explore(env, normalizer, policy) + print('Step:', step, 'Reward:', reward_evaluation) + +# Running the main code + +def mkdir(base, name): + path = os.path.join(base, name) + if not os.path.exists(path): + os.makedirs(path) + return path +work_dir = mkdir('exp', 'brs') +monitor_dir = mkdir(work_dir, 'monitor') + +hp = Hp() +np.random.seed(hp.seed) +env = gym.make(hp.env_name) +# env.render(mode = "human") +#env = wrappers.Monitor(env, monitor_dir, force = True) +nb_inputs = env.observation_space.shape[0] +nb_outputs = env.action_space.shape[0] +policy = Policy(nb_inputs, nb_outputs) +normalizer = Normalizer(nb_inputs) +train(env, policy, normalizer, hp) From ac18c95ea1df312c9af7538bf290b712c5e17ebf Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 5 Nov 2018 10:04:19 -0800 Subject: [PATCH 3/8] fix some race conditions --- examples/SharedMemory/PhysicsServerExample.cpp | 15 +++++++-------- .../TaskScheduler/btThreadSupportPosix.cpp | 10 ---------- 2 files changed, 7 insertions(+), 18 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 36ec1f18b..e864508b4 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -27,7 +27,6 @@ bool gEnableRendering = true; bool gActivedVRRealTimeSimulation = false; bool gEnableSyncPhysicsRendering = true; -bool gEnableUpdateDebugDrawLines = true; static int gCamVisualizerWidth = 228; static int gCamVisualizerHeight = 192; @@ -177,6 +176,7 @@ struct MotionArgs { MotionArgs() : m_debugDrawFlags(0), + m_enableUpdateDebugDrawLines(true), m_physicsServerPtr(0) { for (int i = 0; i < MAX_VR_CONTROLLERS; i++) @@ -201,7 +201,7 @@ struct MotionArgs b3CriticalSection* m_csGUI; int m_debugDrawFlags; - + bool m_enableUpdateDebugDrawLines; btAlignedObjectArray m_mouseCommands; b3VRControllerEvent m_vrControllerEvents[MAX_VR_CONTROLLERS]; @@ -424,13 +424,13 @@ void MotionThreadFunc(void* userPtr, void* lsMemory) args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents, numSendVrControllers, keyEvents, args->m_sendKeyEvents.size(), mouseEvents, args->m_sendMouseEvents.size()); } { - if (gEnableUpdateDebugDrawLines) + args->m_csGUI->lock(); + if (args->m_enableUpdateDebugDrawLines) { - args->m_csGUI->lock(); args->m_physicsServerPtr->physicsDebugDraw(args->m_debugDrawFlags); - gEnableUpdateDebugDrawLines = false; - args->m_csGUI->unlock(); + args->m_enableUpdateDebugDrawLines = false; } + args->m_csGUI->unlock(); } deltaTimeInSeconds = 0; } @@ -1763,7 +1763,6 @@ void PhysicsServerExample::initPhysics() #endif } } - m_args[0].m_cs->lock(); m_args[0].m_cs->setSharedParam(1, eGUIHelperIdle); m_args[0].m_cs->unlock(); @@ -2845,7 +2844,7 @@ void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags) //draw stuff and flush? this->m_multiThreadedHelper->m_debugDraw->drawDebugDrawerLines(); m_args[0].m_debugDrawFlags = debugDrawFlags; - gEnableUpdateDebugDrawLines = true; + m_args[0].m_enableUpdateDebugDrawLines = true; m_args[0].m_csGUI->unlock(); } } diff --git a/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp b/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp index 25bfda0bf..02f4ed163 100644 --- a/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp +++ b/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp @@ -198,12 +198,10 @@ static void* threadFunction(void* argument) status->m_status = 3; status->m_cs->unlock(); checkPThreadFunction(sem_post(status->m_mainSemaphore)); - printf("Thread with taskId %i exiting\n", status->m_taskId); break; } } - printf("Thread TERMINATED\n"); return 0; } @@ -272,7 +270,6 @@ void btThreadSupportPosix::waitForAllTasks() void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructionInfo) { m_numThreads = btGetNumHardwareThreads() - 1; // main thread exists already - printf("%s creating %i threads.\n", __FUNCTION__, m_numThreads); m_activeThreadStatus.resize(m_numThreads); m_startedThreadsMask = 0; @@ -281,7 +278,6 @@ void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructi for (int i = 0; i < m_numThreads; i++) { - printf("starting thread %d\n", i); btThreadStatus& threadStatus = m_activeThreadStatus[i]; threadStatus.startSemaphore = createSem("threadLocal"); threadStatus.m_userPtr = 0; @@ -294,7 +290,6 @@ void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructi threadStatus.threadUsed = 0; checkPThreadFunction(pthread_create(&threadStatus.thread, NULL, &threadFunction, (void*)&threadStatus)); - printf("started thread %d \n", i); } } @@ -304,20 +299,15 @@ void btThreadSupportPosix::stopThreads() for (size_t t = 0; t < size_t(m_activeThreadStatus.size()); ++t) { btThreadStatus& threadStatus = m_activeThreadStatus[t]; - printf("%s: Thread %i used: %ld\n", __FUNCTION__, int(t), threadStatus.threadUsed); threadStatus.m_userPtr = 0; checkPThreadFunction(sem_post(threadStatus.startSemaphore)); checkPThreadFunction(sem_wait(m_mainSemaphore)); - printf("destroy semaphore\n"); destroySem(threadStatus.startSemaphore); - printf("semaphore destroyed\n"); checkPThreadFunction(pthread_join(threadStatus.thread, 0)); } - printf("destroy main semaphore\n"); destroySem(m_mainSemaphore); - printf("main semaphore destroyed\n"); m_activeThreadStatus.clear(); } From 882252f8c0ddbdb8beb55c25971497c7942b33b3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 5 Nov 2018 10:50:03 -0800 Subject: [PATCH 4/8] move global from btMultiBody into dynamicsWorld.getSolverInfo --- .../invdyn_bullet_comparison.cpp | 2 +- .../MultiBody/InvertedPendulumPDControl.cpp | 9 +++--- .../MultiBody/MultiBodyConstraintFeedback.cpp | 9 +++--- examples/MultiBody/TestJointTorqueSetup.cpp | 9 +++--- .../PhysicsServerCommandProcessor.cpp | 10 +++--- .../ConstraintSolver/btContactSolverInfo.h | 4 +++ .../Featherstone/btMultiBody.cpp | 17 +++++----- src/BulletDynamics/Featherstone/btMultiBody.h | 21 +++++++------ .../Featherstone/btMultiBodyDynamicsWorld.cpp | 31 ++++++++++++++----- 9 files changed, 64 insertions(+), 48 deletions(-) diff --git a/Extras/InverseDynamics/invdyn_bullet_comparison.cpp b/Extras/InverseDynamics/invdyn_bullet_comparison.cpp index ffa2a8e07..02adce603 100644 --- a/Extras/InverseDynamics/invdyn_bullet_comparison.cpp +++ b/Extras/InverseDynamics/invdyn_bullet_comparison.cpp @@ -176,7 +176,7 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g btAlignedObjectArray world_to_local; btAlignedObjectArray local_origin; btmb->forwardKinematics(world_to_local, local_origin); - btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass); + btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false); // read generalized accelerations back from btMultiBody // the mapping from scratch variables to accelerations is taken from the implementation diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index ff0c8d2b3..d842bb43b 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -48,9 +48,6 @@ InvertedPendulumPDControl::~InvertedPendulumPDControl() { } -///this is a temporary global, until we determine if we need the option or not -extern bool gJointFeedbackInWorldSpace; -extern bool gJointFeedbackInJointFrame; btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans, bool fixedBase) { @@ -315,8 +312,10 @@ void InvertedPendulumPDControl::initPhysics() } int upAxis = 1; - gJointFeedbackInWorldSpace = true; - gJointFeedbackInJointFrame = true; + + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true; + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true; + m_guiHelper->setUpAxis(upAxis); diff --git a/examples/MultiBody/MultiBodyConstraintFeedback.cpp b/examples/MultiBody/MultiBodyConstraintFeedback.cpp index 4e2bf60fd..b8b421fc1 100644 --- a/examples/MultiBody/MultiBodyConstraintFeedback.cpp +++ b/examples/MultiBody/MultiBodyConstraintFeedback.cpp @@ -44,14 +44,9 @@ MultiBodyConstraintFeedbackSetup::~MultiBodyConstraintFeedbackSetup() { } -///this is a temporary global, until we determine if we need the option or not -extern bool gJointFeedbackInWorldSpace; -extern bool gJointFeedbackInJointFrame; void MultiBodyConstraintFeedbackSetup::initPhysics() { int upAxis = 2; - gJointFeedbackInWorldSpace = true; - gJointFeedbackInJointFrame = true; m_guiHelper->setUpAxis(upAxis); btVector4 colors[4] = @@ -69,6 +64,10 @@ void MultiBodyConstraintFeedbackSetup::initPhysics() //btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits); + + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true; + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true; + //create a static ground object if (1) { diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp index 27122ba3c..7963713d2 100644 --- a/examples/MultiBody/TestJointTorqueSetup.cpp +++ b/examples/MultiBody/TestJointTorqueSetup.cpp @@ -44,15 +44,10 @@ TestJointTorqueSetup::~TestJointTorqueSetup() { } -///this is a temporary global, until we determine if we need the option or not -extern bool gJointFeedbackInWorldSpace; -extern bool gJointFeedbackInJointFrame; void TestJointTorqueSetup::initPhysics() { int upAxis = 1; - gJointFeedbackInWorldSpace = true; - gJointFeedbackInJointFrame = true; m_guiHelper->setUpAxis(upAxis); @@ -71,6 +66,10 @@ void TestJointTorqueSetup::initPhysics() //btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits); + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true; + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true; + + //create a static ground object if (1) { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 537f3e555..2a48843ed 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -96,8 +96,6 @@ #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #endif -extern bool gJointFeedbackInWorldSpace; -extern bool gJointFeedbackInJointFrame; int gInternalSimFlags = 0; bool gResetSimulation = 0; @@ -7697,11 +7695,11 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2]; serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags; serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode = 0; - if (gJointFeedbackInWorldSpace) + if (m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace) { serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_WORLD_SPACE; } - if (gJointFeedbackInJointFrame) + if (m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame) { serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_JOINT_FRAME; } @@ -7747,8 +7745,8 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE) { - gJointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_WORLD_SPACE) != 0; - gJointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_JOINT_FRAME) != 0; + m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_WORLD_SPACE) != 0; + m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_JOINT_FRAME) != 0; } if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME) diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 439cc4428..89f8db8b1 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -62,6 +62,8 @@ struct btContactSolverInfoData btScalar m_singleAxisRollingFrictionThreshold; btScalar m_leastSquaresResidualThreshold; btScalar m_restitutionVelocityThreshold; + bool m_jointFeedbackInWorldSpace; + bool m_jointFeedbackInJointFrame; }; struct btContactSolverInfo : public btContactSolverInfoData @@ -94,6 +96,8 @@ struct btContactSolverInfo : public btContactSolverInfoData m_singleAxisRollingFrictionThreshold = 1e30f; ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows. m_leastSquaresResidualThreshold = 0.f; m_restitutionVelocityThreshold = 0.2f; //if the relative velocity is below this threshold, there is zero restitution + m_jointFeedbackInWorldSpace = false; + m_jointFeedbackInJointFrame = false; } }; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index a890da46f..172472988 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -30,9 +30,6 @@ //#include "Bullet3Common/b3Logging.h" // #define INCLUDE_GYRO_TERM -///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals -bool gJointFeedbackInWorldSpace = false; -bool gJointFeedbackInJointFrame = false; namespace { @@ -718,10 +715,12 @@ inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1) //ren // void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m, - bool isConstraintPass) + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m, + bool isConstraintPass, + bool jointFeedbackInWorldSpace, + bool jointFeedbackInJointFrame) { // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) // and the base linear & angular accelerations. @@ -1124,7 +1123,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec; btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec; - if (gJointFeedbackInJointFrame) + if (jointFeedbackInJointFrame) { //shift the reaction forces to the joint frame //linear (force) component is the same @@ -1132,7 +1131,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector); } - if (gJointFeedbackInWorldSpace) + if (jointFeedbackInWorldSpace) { if (isConstraintPass) { diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 145ed4677..8a693f539 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -338,17 +338,20 @@ public: btAlignedObjectArray & scratch_r, btAlignedObjectArray & scratch_v, btAlignedObjectArray & scratch_m, - bool isConstraintPass = false); + bool isConstraintPass, + bool jointFeedbackInWorldSpace, + bool jointFeedbackInJointFrame + ); ///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead - void stepVelocitiesMultiDof(btScalar dt, - btAlignedObjectArray & scratch_r, - btAlignedObjectArray & scratch_v, - btAlignedObjectArray & scratch_m, - bool isConstraintPass = false) - { - computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass); - } + //void stepVelocitiesMultiDof(btScalar dt, + // btAlignedObjectArray & scratch_r, + // btAlignedObjectArray & scratch_v, + // btAlignedObjectArray & scratch_m, + // bool isConstraintPass = false) + //{ + // computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false); + //} // calcAccelerationDeltasMultiDof // input: force vector (in same format as jacobian, i.e.: diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index a452d61b7..1557987bc 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -491,11 +491,14 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) m_scratch_v.resize(bod->getNumLinks() + 1); m_scratch_m.resize(bod->getNumLinks() + 1); bool doNotUpdatePos = false; - + bool isConstraintPass = false; { if (!bod->isUsingRK4Integration()) { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, + m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); } else { @@ -593,7 +596,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btScalar h = solverInfo.m_timeStep; #define output &m_scratch_r[bod->getNumDofs()] //calc qdd0 from: q0 & qd0 - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd0, 0, numDofs); //calc q1 = q0 + h/2 * qd0 pResetQx(); @@ -603,7 +608,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd1 from: q1 & qd1 pCopyToVelocityVector(bod, scratch_qd1); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd1, 0, numDofs); //calc q2 = q0 + h/2 * qd1 pResetQx(); @@ -613,7 +620,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd2 from: q2 & qd2 pCopyToVelocityVector(bod, scratch_qd2); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd2, 0, numDofs); //calc q3 = q0 + h * qd2 pResetQx(); @@ -623,7 +632,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd3 from: q3 & qd3 pCopyToVelocityVector(bod, scratch_qd3); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd3, 0, numDofs); // @@ -660,7 +671,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { for (int link = 0; link < bod->getNumLinks(); ++link) bod->getLink(link).updateCacheMultiDof(); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); } } } @@ -708,7 +721,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) if (!bod->isUsingRK4Integration()) { bool isConstraintPass = true; - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); } } } From 5d20b2d3468a0a1872315db66a65e460d1e25d2a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 6 Nov 2018 12:33:46 -0800 Subject: [PATCH 5/8] bump up pybullet version --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index ce61fb7d2..e1ccdf5b6 100644 --- a/setup.py +++ b/setup.py @@ -582,7 +582,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name = 'pybullet', - version='2.3.4', + version='2.3.6', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From 49b098854efb9b839ea816d822197b55323e033d Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 7 Nov 2018 09:29:19 -0800 Subject: [PATCH 6/8] PyBullet: avoid calling syncBodies for each DOF in pdControl. Allow PD_CONTROL in setJointMotorControlArray. --- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 5 +++++ .../plugins/pdControlPlugin/pdControlPlugin.cpp | 6 +++++- examples/pybullet/pybullet.c | 14 ++++++-------- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2a48843ed..2e0fb0b94 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5581,6 +5581,11 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct args.m_ints[1] = bodyUniqueId; //find the joint motors and apply the desired velocity and maximum force/torque { + args.m_numInts = 0; + args.m_numFloats = 0; + //syncBodies is expensive/slow, use it only once + m_data->m_pluginManager.executePluginCommand(m_data->m_pdControlPlugin, &args); + int velIndex = 6; //skip the 3 linear + 3 angular degree of freedom velocity entries of the base int posIndex = 7; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base for (int link = 0; link < mb->getNumLinks(); link++) diff --git a/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp b/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp index 942b87f3f..5fa29e137 100644 --- a/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp +++ b/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp @@ -89,7 +89,11 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c { MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer; - obj->m_api.syncBodies(); + if (arguments->m_numInts == 0) + { + obj->m_api.syncBodies(); + return -1; + } int numObj = obj->m_api.getNumBodies(); //printf("numObj = %d\n", numObj); diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index c025ee9cd..375b41a0f 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2039,7 +2039,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) (controlMode != CONTROL_MODE_TORQUE) && (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) { - PyErr_SetString(SpamError, "Illegral control mode."); + PyErr_SetString(SpamError, "Illegal control mode."); return NULL; } @@ -2140,9 +2140,10 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar if ((controlMode != CONTROL_MODE_VELOCITY) && (controlMode != CONTROL_MODE_TORQUE) && - (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)&& + (controlMode != CONTROL_MODE_PD)) { - PyErr_SetString(SpamError, "Illegral control mode."); + PyErr_SetString(SpamError, "Illegal control mode."); return NULL; } @@ -2340,7 +2341,7 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar break; } - case CONTROL_MODE_POSITION_VELOCITY_PD: + default: { b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition); @@ -2351,9 +2352,6 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force); break; } - default: - { - } }; } @@ -2440,7 +2438,7 @@ static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD) && (controlMode != CONTROL_MODE_PD)) { - PyErr_SetString(SpamError, "Illegral control mode."); + PyErr_SetString(SpamError, "Illegal control mode."); return NULL; } From 036d75bc041eafbc169c27ec4e9ab3cd97127c56 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 8 Nov 2018 11:33:45 -0800 Subject: [PATCH 7/8] fix issue that cause hanging/stallig setup.py due to monkey-patch, see also https://github.com/bulletphysics/bullet3/issues/1956 --- setup.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/setup.py b/setup.py index 0704d952b..a38907320 100644 --- a/setup.py +++ b/setup.py @@ -23,7 +23,8 @@ def parallelCCompile(self, sources, output_dir=None, macros=None, include_dirs=N except KeyError: return self._compile(obj, src, ext, cc_args, extra_postargs, pp_opts) # convert to list, imap is evaluated on-demand - list(multiprocessing.pool.ThreadPool(N).imap(_single_compile,objects)) + pool = multiprocessing.pool.ThreadPool(N) + list(pool.imap(_single_compile,objects)) return objects import distutils.ccompiler distutils.ccompiler.CCompiler.compile=parallelCCompile @@ -583,7 +584,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name = 'pybullet', - version='2.3.6', + version='2.3.7', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From 642c6a71d215bbd16c3c5a153837a4145541ff69 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 8 Nov 2018 14:41:22 -0800 Subject: [PATCH 8/8] fix 2 potential data race conditions. --- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 6 +++++ .../CollisionShapes/btShapeHull.cpp | 26 +++++++++---------- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 4302ca347..9930ac994 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -1,6 +1,9 @@ + #include #include "LinearMath/btTransform.h" +#include "LinearMath/btThreads.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" + #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" @@ -28,10 +31,13 @@ static btVector4 colors[4] = static btVector4 selectColor2() { + static btSpinMutex sMutex; + sMutex.lock(); static int curColor = 0; btVector4 color = colors[curColor]; curColor++; curColor &= 3; + sMutex.unlock(); return color; } diff --git a/src/BulletCollision/CollisionShapes/btShapeHull.cpp b/src/BulletCollision/CollisionShapes/btShapeHull.cpp index 8f44e31f1..a2c490faf 100644 --- a/src/BulletCollision/CollisionShapes/btShapeHull.cpp +++ b/src/BulletCollision/CollisionShapes/btShapeHull.cpp @@ -37,21 +37,8 @@ btShapeHull::~btShapeHull() bool btShapeHull::buildHull(btScalar /*margin*/, int highres) { + int numSampleDirections = highres ? NUM_UNITSPHERE_POINTS_HIGHRES : NUM_UNITSPHERE_POINTS; - { - int numPDA = m_shape->getNumPreferredPenetrationDirections(); - if (numPDA) - { - for (int i = 0; i < numPDA; i++) - { - btVector3 norm; - m_shape->getPreferredPenetrationDirection(i, norm); - getUnitSpherePoints(highres)[numSampleDirections] = norm; - numSampleDirections++; - } - } - } - btVector3 supportPoints[NUM_UNITSPHERE_POINTS_HIGHRES + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; int i; for (i = 0; i < numSampleDirections; i++) @@ -59,6 +46,17 @@ bool btShapeHull::buildHull(btScalar /*margin*/, int highres) supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints(highres)[i]); } + int numPDA = m_shape->getNumPreferredPenetrationDirections(); + if (numPDA) + { + for (int s = 0; s < numPDA; s++) + { + btVector3 norm; + m_shape->getPreferredPenetrationDirection(s, norm); + supportPoints[i++] = m_shape->localGetSupportingVertex(norm); + numSampleDirections++; + } + } HullDesc hd; hd.mFlags = QF_TRIANGLES; hd.mVcount = static_cast(numSampleDirections);