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/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/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..2e0fb0b94 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; @@ -5583,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++) @@ -7697,11 +7700,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 +7750,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/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 2dc7145be..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,9 +1763,9 @@ void PhysicsServerExample::initPhysics() #endif } } - + 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(); { @@ -2844,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/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/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 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) 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; } diff --git a/setup.py b/setup.py index f8f6976ef..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.4', + 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', 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); 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); } } } 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(); }