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

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
Erwin Coumans 2017-09-24 22:50:55 -07:00
commit 705a59ea73
5 changed files with 48 additions and 3 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)

2
examples/SharedMemory/b3PluginManager.cpp
View File

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

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