From ee30ca93c50fa9f43c7ac51c136815e775df51c2 Mon Sep 17 00:00:00 2001 From: Jeffrey Bingham Date: Sun, 24 Sep 2017 14:14:24 -0700 Subject: [PATCH] [sharedmemory] Fill-out calculateJacobian command. The server command processor actually didn't do anything with the local point that was passed along with the calculateJacobian command. Added in the necessary bit of math to return the corresponding jacobian. Also, fixed a typo in pybullet that was returning the same jacobian for translation and rotation. --- .../PhysicsServerCommandProcessor.cpp | 23 +++++++++++++++++++ examples/pybullet/pybullet.c | 11 ++++++++- src/BulletInverseDynamics/IDMath.cpp | 12 ++++++++++ src/BulletInverseDynamics/IDMath.hpp | 3 ++- 4 files changed, 47 insertions(+), 2 deletions(-) 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