AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +00:00
Code Issues Releases Wiki Activity

[sharedmemory] Fill-out calculateJacobian command.

Browse Source 
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.
This commit is contained in:
Jeffrey Bingham 2017-09-24 14:14:24 -07:00
parent 8eb69552d1
commit ee30ca93c5
4 changed files with 47 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

11
examples/pybullet/pybullet.c
View File

@ -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"},

12
src/BulletInverseDynamics/IDMath.cpp
View File

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

3
src/BulletInverseDynamics/IDMath.hpp
View File

@ -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