[pybullet] Add calculateJacobian.

* Add the calculateJacobian method to the pybullet API.
* Adjust the shared memory interface to handle fixed/floating bases
  in the calculateJacobian method.
* Fix a few comments.
This commit is contained in:
Jeffrey Bingham
2017-09-23 19:58:59 -07:00
parent a506879d6c
commit 1727e47beb
7 changed files with 241 additions and 64 deletions

View File

@@ -304,7 +304,10 @@ B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHand
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian);
B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle,
int* dofCount,
double* linearJacobian,
double* angularJacobian);
///compute the joint positions to move the end effector to a desired target using inverse kinematics
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);