diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 47dff31b3..970967b40 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index dea5e306a..5a4c4cda0 100644 --- a/examples/pybullet/pybullet.c +++ b/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"}, diff --git a/src/BulletInverseDynamics/IDMath.cpp b/src/BulletInverseDynamics/IDMath.cpp index ecd62f76d..99fe20e49 100644 --- a/src/BulletInverseDynamics/IDMath.cpp +++ b/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++) { diff --git a/src/BulletInverseDynamics/IDMath.hpp b/src/BulletInverseDynamics/IDMath.hpp index 63699712a..b355474d4 100644 --- a/src/BulletInverseDynamics/IDMath.hpp +++ b/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