Merge remote-tracking branch 'bp/master'

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -6639,9 +6639,32 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                 tree->calculateJacobians(q);
                                 btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs);
                                 btInverseDynamics::mat3x jac_r(3, numDofs + baseDofs);
+                                
                                 // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
                                 tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_t);
                                 tree->getBodyJacobianRot(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_r);
+                                // Update the translational jacobian based on the desired local point.
+                                // v_pt = v_frame + w x pt
+                                // v_pt = J_t * qd + (J_r * qd) x pt
+                                // v_pt = J_t * qd - pt x (J_r * qd)
+                                // v_pt = J_t * qd - pt_x * J_r * qd)
+                                // v_pt = (J_t - pt_x * J_r) * qd
+                                // J_t_new = J_t - pt_x * J_r
+                                btInverseDynamics::vec3 localPosition;
+                                for (int i = 0; i < 3; ++i) {
+                                    localPosition(i) = clientCmd.m_calculateJacobianArguments.m_localPosition[i];
+                                }
+                                // Only calculate if the localPosition is non-zero.
+                                if (btInverseDynamics::maxAbs(localPosition) > 0.0) {
+                                    btInverseDynamics::mat33 skewCrossProduct;
+                                    btInverseDynamics::skew(localPosition, &skewCrossProduct);
+                                    btInverseDynamics::mat3x jac_l(3, numDofs + baseDofs);
+                                    btInverseDynamics::mul(skewCrossProduct, jac_r, &jac_l);
+                                    btInverseDynamics::mat3x jac_t_new(3, numDofs + baseDofs);
+                                    btInverseDynamics::sub(jac_t, jac_l, &jac_t_new);
+                                    jac_t = jac_t_new;
+                                }
+                                // Fill in the result into the shared memory.
                                 for (int i = 0; i < 3; ++i)
                                 {
                                     for (int j = 0; j < (numDofs + baseDofs); ++j)

examples/SharedMemory/b3PluginManager.cpp

@@ -48,8 +48,8 @@ struct b3Plugin
 		m_initFunc(0),
 		m_exitFunc(0),
 		m_executeCommandFunc(0),
-		m_postTickFunc(0),
 		m_preTickFunc(0),
+		m_postTickFunc(0),
 		m_userPointer(0)
 	{
 	}

examples/pybullet/pybullet.c

@@ -7184,7 +7184,7 @@ static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyOb
 									for (c = 0; c < dofCount; ++c) {
 										int element = r * dofCount + c;
 										PyTuple_SetItem(pyrow, c,
-													PyFloat_FromDouble(linearJacobian[element]));
+													PyFloat_FromDouble(angularJacobian[element]));
 									}
 									PyTuple_SetItem(pymat, r, pyrow);
 								}
@@ -7225,31 +7225,40 @@ static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyOb
 static PyMethodDef SpamMethods[] = {
 
 	{"connect", (PyCFunction)pybullet_connectPhysicsServer, METH_VARARGS | METH_KEYWORDS,
+	"connect(method, key=SHARED_MEMORY_KEY, options='')\n"
+	"connect(method, hostname='localhost', port=1234, options='')\n"
 	 "Connect to an existing physics server (using shared memory by default)."},
 
 	{"disconnect", (PyCFunction)pybullet_disconnectPhysicsServer, METH_VARARGS | METH_KEYWORDS,
+	"disconnect(physicsClientId=0)\n"
 	 "Disconnect from the physics server."},
 
 	{"resetSimulation", (PyCFunction)pybullet_resetSimulation, METH_VARARGS | METH_KEYWORDS,
+	"resetSimulation(physicsClientId=0)\n"
 	 "Reset the simulation: remove all objects and start from an empty world."},
 
 	{"stepSimulation", (PyCFunction)pybullet_stepSimulation, METH_VARARGS | METH_KEYWORDS,
+	"stepSimulation(physicsClientId=0)\n"
 	 "Step the simulation using forward dynamics."},
 
 	{"setGravity", (PyCFunction)pybullet_setGravity, METH_VARARGS | METH_KEYWORDS,
+	"setGravity(gravX, gravY, gravZ, physicsClientId=0)\n"
 	 "Set the gravity acceleration (x,y,z)."},
 
 	{"setTimeStep", (PyCFunction)pybullet_setTimeStep, METH_VARARGS | METH_KEYWORDS,
+	"setTimeStep(timestep, physicsClientId=0)\n"
 	 "Set the amount of time to proceed at each call to stepSimulation. (unit "
 	 "is seconds, typically range is 0.01 or 0.001)"},
 
 	{"setDefaultContactERP", (PyCFunction)pybullet_setDefaultContactERP, METH_VARARGS | METH_KEYWORDS,
+	 "setDefaultContactERP(defaultContactERP, physicsClientId=0)\n"
 	 "Set the amount of contact penetration Error Recovery Paramater "
 	 "(ERP) in each time step. \
 		This is an tuning parameter to control resting contact stability. "
 	 "This value depends on the time step."},
 
 	{"setRealTimeSimulation", (PyCFunction)pybullet_setRealTimeSimulation, METH_VARARGS | METH_KEYWORDS,
+	"setRealTimeSimulation(enableRealTimeSimulation, physicsClientId=0)\n"
 	 "Enable or disable real time simulation (using the real time clock,"
 	 " RTC) in the physics server. Expects one integer argument, 0 or 1"},
 

src/BulletInverseDynamics/IDMath.cpp

@@ -33,6 +33,18 @@ void setZero(mat33 &m) {
 	m(2, 2) = 0;
 }
 
+void skew(vec3& v, mat33* result) {
+	(*result)(0, 0) = 0.0;
+	(*result)(0, 1) = -v(2);
+	(*result)(0, 2) = v(1);
+	(*result)(1, 0) = v(2);
+	(*result)(1, 1) = 0.0;
+	(*result)(1, 2) = -v(0);
+	(*result)(2, 0) = -v(1);
+	(*result)(2, 1) = v(0);
+	(*result)(2, 2) = 0.0;
+}
+
 idScalar maxAbs(const vecx &v) {
 	idScalar result = 0.0;
 	for (int i = 0; i < v.size(); i++) {

src/BulletInverseDynamics/IDMath.hpp

@@ -12,7 +12,8 @@ void setZero(vec3& v);
 void setZero(vecx& v);
 /// set all elements to zero
 void setZero(mat33& m);
-
+/// create a skew symmetric matrix from a vector (useful for cross product abstraction, e.g. v x a = V * a)
+void skew(vec3& v, mat33* result);
 /// return maximum absolute value
 idScalar maxAbs(const vecx& v);
 #ifndef ID_LINEAR_MATH_USE_EIGEN