Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Fix the translation jacobian.

Browse Source
This commit is contained in:
Jeffrey Bingham
2017-09-27 22:23:22 -07:00
parent 6da931d0bd
commit d8b80bce40
3 changed files with 35 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -6667,6 +6667,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
// Only calculate if the localPosition is non-zero.
if (btInverseDynamics::maxAbs(localPosition) > 0.0) {
// Write the localPosition into world coordinates.
btInverseDynamics::mat33 world_rotation_body;
tree->getBodyTransform(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &world_rotation_body);
localPosition = world_rotation_body * localPosition;
// Correct the translational jacobian.
btInverseDynamics::mat33 skewCrossProduct;
btInverseDynamics::skew(localPosition, &skewCrossProduct);
btInverseDynamics::mat3x jac_l(3, numDofs + baseDofs);