Merge remote-tracking branch 'upstream/master'

Extras/InverseDynamics/invdyn_bullet_comparison.cpp

@@ -176,7 +176,7 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
 	btAlignedObjectArray<btQuaternion> world_to_local;
 	btAlignedObjectArray<btVector3> 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

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -1,6 +1,9 @@
+
 #include <stdio.h>
 #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;
 }
 

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

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

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

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;
+		m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = (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_jointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_JOINT_FRAME) != 0;
 	}
 
 	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME)

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<MyMouseCommand> 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_enableUpdateDebugDrawLines = false;
-						args->m_csGUI->unlock();
 					}
+					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();
 	}
 }

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

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

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)

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

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',

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<unsigned int>(numSampleDirections);

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

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<btScalar> &scratch_r,
+    btAlignedObjectArray<btScalar> &scratch_r,
-																	   btAlignedObjectArray<btVector3> &scratch_v,
+    btAlignedObjectArray<btVector3> &scratch_v,
-																	   btAlignedObjectArray<btMatrix3x3> &scratch_m,
+    btAlignedObjectArray<btMatrix3x3> &scratch_m,
-																	   bool isConstraintPass)
+	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)
 				{

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -338,17 +338,20 @@ public:
 															  btAlignedObjectArray<btScalar> & scratch_r,
 															  btAlignedObjectArray<btVector3> & scratch_v,
 															  btAlignedObjectArray<btMatrix3x3> & scratch_m,
-															  bool isConstraintPass = false);
+															  bool isConstraintPass,
+                                                              bool jointFeedbackInWorldSpace,
+                                                              bool jointFeedbackInJointFrame
+                                                              );
 
 	///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead
-	void stepVelocitiesMultiDof(btScalar dt,
+	//void stepVelocitiesMultiDof(btScalar dt,
-								btAlignedObjectArray<btScalar> & scratch_r,
+	//							btAlignedObjectArray<btScalar> & scratch_r,
-								btAlignedObjectArray<btVector3> & scratch_v,
+	//							btAlignedObjectArray<btVector3> & scratch_v,
-								btAlignedObjectArray<btMatrix3x3> & scratch_m,
+	//							btAlignedObjectArray<btMatrix3x3> & scratch_m,
-								bool isConstraintPass = false)
+	//							bool isConstraintPass = false)
-	{
+	//{
-		computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass);
+	//	computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false);
-	}
+	//}
 
 	// calcAccelerationDeltasMultiDof
 	// input: force vector (in same format as jacobian, i.e.:

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);
 					}
 				}
 			}

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();
 }