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

Merge pull request #2322 from erwincoumans/master

Browse Source 
PyBullet.calculateInverseKinematics2 for IK with multiple end-effector locations
This commit is contained in:
erwincoumans
2019-07-10 19:58:17 -07:00
committed by GitHub
parent 67cab069c3 ee9575167d
commit d220101c5a
11 changed files with 549 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

111
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -46,7 +46,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
{ {
bool useAngularPart = (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false; bool useAngularPart = (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
Jacobian ikJacobian(useAngularPart, numQ); Jacobian ikJacobian(useAngularPart, numQ, 1);
ikJacobian.Reset(); ikJacobian.Reset();
@@ -185,6 +185,115 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
return true; return true;
} }
bool IKTrajectoryHelper::computeIK2(
const double* endEffectorTargetPositions,
const double* endEffectorCurrentPositions,
int numEndEffectors,
const double* q_current, int numQ,
double* q_new, int ikMethod, const double* linear_jacobians, const double dampIk[6])
{
bool useAngularPart = false;//for now (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
Jacobian ikJacobian(useAngularPart, numQ, numEndEffectors);
ikJacobian.Reset();
bool UseJacobianTargets1 = false;
if (UseJacobianTargets1)
{
ikJacobian.SetJtargetActive();
}
else
{
ikJacobian.SetJendActive();
}
VectorRn deltaC(numEndEffectors *3);
MatrixRmn completeJacobian(numEndEffectors*3, numQ);
for (int ne = 0; ne < numEndEffectors; ne++)
{
VectorR3 targets;
targets.Set(endEffectorTargetPositions[ne*3+0], endEffectorTargetPositions[ne * 3 + 1], endEffectorTargetPositions[ne * 3 + 2]);
// Set one end effector world position from Bullet
VectorRn deltaS(3);
for (int i = 0; i < 3; ++i)
{
deltaS.Set(i, dampIk[i] * (endEffectorTargetPositions[ne*3+i] - endEffectorCurrentPositions[ne*3+i]));
}
{
for (int i = 0; i < 3; ++i)
{
deltaC.Set(ne*3+i, deltaS[i]);
for (int j = 0; j < numQ; ++j)
{
completeJacobian.Set(ne * 3 + i, j, linear_jacobians[(ne*3+i * numQ) + j]);
}
}
}
}
ikJacobian.SetDeltaS(deltaC);
ikJacobian.SetJendTrans(completeJacobian);
// Calculate the change in theta values
switch (ikMethod)
{
case IK2_JACOB_TRANS:
ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
case IK2_VEL_DLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
break;
case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
break;
case IK2_DLS_SVD:
ikJacobian.CalcDeltaThetasDLSwithSVD();
break;
case IK2_PURE_PSEUDO:
ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
case IK2_VEL_SDLS:
case IK2_VEL_SDLS_WITH_ORIENTATION:
ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:
ikJacobian.ZeroDeltaThetas();
break;
}
// Use for velocity IK, update theta dot
//ikJacobian.UpdateThetaDot();
// Use for position IK, incrementally update theta
//ikJacobian.UpdateThetas();
// Apply the change in the theta values
//ikJacobian.UpdatedSClampValue(&targets);
for (int i = 0; i < numQ; i++)
{
// Use for velocity IK
q_new[i] = ikJacobian.dTheta[i] + q_current[i];
// Use for position IK
//q_new[i] = m_data->m_ikNodes[i]->GetTheta();
}
return true;
}
bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose) bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose)
{ {
m_data->m_nullSpaceVelocity.SetLength(numQ); m_data->m_nullSpaceVelocity.SetLength(numQ);

7
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -31,6 +31,13 @@ public:
const double* q_old, int numQ, int endEffectorIndex, const double* q_old, int numQ, int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]); double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]);
bool computeIK2(
const double* endEffectorTargetPositions,
const double* endEffectorCurrentPositions,
int numEndEffectors,
const double* q_current, int numQ,
double* q_new, int ikMethod, const double* linear_jacobians, const double dampIk[6]);
bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose); bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose);
bool setDampingCoeff(int numQ, const double* coeff); bool setDampingCoeff(int numQ, const double* coeff);
}; };

59
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -4905,28 +4905,51 @@ B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMem
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION; command->m_updateFlags |= IK_HAS_TARGET_POSITION;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex; command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0] = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_targetPositions[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0]; command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1]; command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2]; command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = 0; command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = 0; command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = 0; command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = 1; command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = 1;
} }
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetsPurePosition(b3SharedMemoryCommandHandle commandHandle, int numEndEffectorLinkIndices, const int* endEffectorIndices, const double* targetPositions)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION;
command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = numEndEffectorLinkIndices;
for (int i = 0; i < numEndEffectorLinkIndices; i++)
{
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[i] = endEffectorIndices[i];
command->m_calculateInverseKinematicsArguments.m_targetPositions[i * 3 + 0] = targetPositions[i * 3 + 0];
command->m_calculateInverseKinematicsArguments.m_targetPositions[i * 3 + 1] = targetPositions[i * 3 + 1];
command->m_calculateInverseKinematicsArguments.m_targetPositions[i * 3 + 2] = targetPositions[i * 3 + 2];
}
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = 1;
}
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]) B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4])
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION + IK_HAS_TARGET_ORIENTATION; command->m_updateFlags |= IK_HAS_TARGET_POSITION + IK_HAS_TARGET_ORIENTATION;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex; command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0] = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1;
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0]; command->m_calculateInverseKinematicsArguments.m_targetPositions[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1]; command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2]; command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1];
@@ -4940,11 +4963,12 @@ B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemor
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION + IK_HAS_NULL_SPACE_VELOCITY; command->m_updateFlags |= IK_HAS_TARGET_POSITION + IK_HAS_NULL_SPACE_VELOCITY;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex; command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0] = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1;
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0]; command->m_calculateInverseKinematicsArguments.m_targetPositions[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1]; command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2]; command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2];
for (int i = 0; i < numDof; ++i) for (int i = 0; i < numDof; ++i)
{ {
@@ -4961,11 +4985,12 @@ B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMe
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION + IK_HAS_TARGET_ORIENTATION + IK_HAS_NULL_SPACE_VELOCITY; command->m_updateFlags |= IK_HAS_TARGET_POSITION + IK_HAS_TARGET_ORIENTATION + IK_HAS_NULL_SPACE_VELOCITY;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex; command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0] = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1;
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0]; command->m_calculateInverseKinematicsArguments.m_targetPositions[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1]; command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2]; command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1];

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -424,6 +424,7 @@ extern "C"
///compute the joint positions to move the end effector to a desired target using inverse kinematics ///compute the joint positions to move the end effector to a desired target using inverse kinematics
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId); B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]); B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]);
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetsPurePosition(b3SharedMemoryCommandHandle commandHandle, int numEndEffectorLinkIndices, const int* endEffectorIndices, const double* targetPositions);
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/]); B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/]);
B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose); B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose); B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);

176
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -10227,34 +10227,10 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
ikHelperPtr = tmpHelper; ikHelperPtr = tmpHelper;
} }
int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex;
btAlignedObjectArray<double> startingPositions; btAlignedObjectArray<double> startingPositions;
startingPositions.reserve(bodyHandle->m_multiBody->getNumLinks()); startingPositions.reserve(bodyHandle->m_multiBody->getNumLinks());
btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0],
clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1],
clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]);
btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]);
btTransform targetBaseCoord;
if (clientCmd.m_updateFlags & IK_HAS_CURRENT_JOINT_POSITIONS)
{
targetBaseCoord.setOrigin(targetPosWorld);
targetBaseCoord.setRotation(targetOrnWorld);
}
else
{
btTransform targetWorld;
targetWorld.setOrigin(targetPosWorld);
targetWorld.setRotation(targetOrnWorld);
btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
targetBaseCoord = tr.inverse() * targetWorld;
}
{ {
int DofIndex = 0; int DofIndex = 0;
@@ -10290,6 +10266,9 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
} }
btScalar currentDiff = 1e30f; btScalar currentDiff = 1e30f;
b3AlignedObjectArray<double> endEffectorTargetWorldPositions;
b3AlignedObjectArray<double> endEffectorTargetWorldOrientations;
b3AlignedObjectArray<double> endEffectorCurrentWorldPositions;
b3AlignedObjectArray<double> jacobian_linear; b3AlignedObjectArray<double> jacobian_linear;
b3AlignedObjectArray<double> jacobian_angular; b3AlignedObjectArray<double> jacobian_angular;
btAlignedObjectArray<double> q_current; btAlignedObjectArray<double> q_current;
@@ -10302,13 +10281,64 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
btInverseDynamics::vecx nu(numDofs + baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); btInverseDynamics::vecx nu(numDofs + baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
endEffectorTargetWorldPositions.resize(0);
endEffectorTargetWorldPositions.reserve(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices * 3);
endEffectorTargetWorldOrientations.resize(0);
endEffectorTargetWorldOrientations.resize(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices * 4);
bool validEndEffectorLinkIndices = true;
for (int ne = 0; ne < clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices; ne++)
{
int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[ne];
validEndEffectorLinkIndices = validEndEffectorLinkIndices && (endEffectorLinkIndex < bodyHandle->m_multiBody->getNumLinks());
btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[ne*3+0],
clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[ne * 3 + 1],
clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[ne * 3 + 2]);
btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 0],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 1],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 2],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 3]);
btTransform targetBaseCoord;
if (clientCmd.m_updateFlags & IK_HAS_CURRENT_JOINT_POSITIONS)
{
targetBaseCoord.setOrigin(targetPosWorld);
targetBaseCoord.setRotation(targetOrnWorld);
}
else
{
btTransform targetWorld;
targetWorld.setOrigin(targetPosWorld);
targetWorld.setRotation(targetOrnWorld);
btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
targetBaseCoord = tr.inverse() * targetWorld;
}
btVector3DoubleData targetPosBaseCoord;
btQuaternionDoubleData targetOrnBaseCoord;
targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord);
targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord);
endEffectorTargetWorldPositions.push_back(targetPosBaseCoord.m_floats[0]);
endEffectorTargetWorldPositions.push_back(targetPosBaseCoord.m_floats[1]);
endEffectorTargetWorldPositions.push_back(targetPosBaseCoord.m_floats[2]);
endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[0]);
endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[1]);
endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[2]);
endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[3]);
}
for (int i = 0; i < numIterations && currentDiff > residualThreshold; i++) for (int i = 0; i < numIterations && currentDiff > residualThreshold; i++)
{ {
BT_PROFILE("InverseKinematics1Step"); BT_PROFILE("InverseKinematics1Step");
if (ikHelperPtr && (endEffectorLinkIndex < bodyHandle->m_multiBody->getNumLinks())) if (ikHelperPtr && validEndEffectorLinkIndices)
{ {
jacobian_linear.resize(3 * numDofs); jacobian_linear.resize(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices*3 * numDofs);
jacobian_angular.resize(3 * numDofs); jacobian_angular.resize(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices*3 * numDofs);
int jacSize = 0; int jacSize = 0;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
@@ -10329,7 +10359,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2) if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2)
{ {
// 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints. // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints.
double curPos = startingPositions[DofIndex]; double curPos = startingPositions[DofIndex];
q_current[DofIndex] = curPos; q_current[DofIndex] = curPos;
q[DofIndex + baseDofs] = curPos; q[DofIndex + baseDofs] = curPos;
@@ -10348,21 +10377,44 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
tree->calculateJacobians(q); tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs); btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs);
btInverseDynamics::mat3x jac_r(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. currentDiff = 0;
tree->getBodyJacobianTrans(endEffectorLinkIndex + 1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex + 1, &jac_r);
//calculatePositionKinematics is already done inside calculateInverseDynamics endEffectorCurrentWorldPositions.resize(0);
tree->getBodyOrigin(endEffectorLinkIndex + 1, &world_origin); endEffectorCurrentWorldPositions.reserve(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices * 3);
tree->getBodyTransform(endEffectorLinkIndex + 1, &world_rot);
for (int i = 0; i < 3; ++i) for (int ne = 0; ne < clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices; ne++)
{ {
for (int j = 0; j < numDofs; ++j)
int endEffectorLinkIndex2 = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[ne];
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
tree->getBodyJacobianTrans(endEffectorLinkIndex2 + 1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex2 + 1, &jac_r);
//calculatePositionKinematics is already done inside calculateInverseDynamics
tree->getBodyOrigin(endEffectorLinkIndex2 + 1, &world_origin);
tree->getBodyTransform(endEffectorLinkIndex2 + 1, &world_rot);
for (int i = 0; i < 3; ++i)
{ {
jacobian_linear[i * numDofs + j] = jac_t(i, (baseDofs + j)); for (int j = 0; j < numDofs; ++j)
jacobian_angular[i * numDofs + j] = jac_r(i, (baseDofs + j)); {
jacobian_linear[(ne*3+i) * numDofs + j] = jac_t(i, (baseDofs + j));
jacobian_angular[(ne * 3 + i) * numDofs + j] = jac_r(i, (baseDofs + j));
}
} }
endEffectorCurrentWorldPositions.push_back(world_origin[0]);
endEffectorCurrentWorldPositions.push_back(world_origin[1]);
endEffectorCurrentWorldPositions.push_back(world_origin[2]);
btInverseDynamics::vec3 targetPos(btVector3(endEffectorTargetWorldPositions[ne*3+0],
endEffectorTargetWorldPositions[ne * 3 + 1],
endEffectorTargetWorldPositions[ne * 3 + 2]));
//diff
currentDiff = btMax(currentDiff, (world_origin - targetPos).length());
} }
} }
} }
@@ -10451,13 +10503,8 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition); endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition);
endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation); endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation);
//diff
currentDiff = (endEffectorBaseCoord.getOrigin() - targetBaseCoord.getOrigin()).length();
btVector3DoubleData targetPosBaseCoord;
btQuaternionDoubleData targetOrnBaseCoord;
targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord);
targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord);
// Set joint damping coefficents. A small default // Set joint damping coefficents. A small default
// damping constant is added to prevent singularity // damping constant is added to prevent singularity
@@ -10477,25 +10524,48 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]); ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]);
double targetDampCoeff[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; double targetDampCoeff[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
bool performedIK = false;
if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices==1)
{ {
BT_PROFILE("computeIK"); BT_PROFILE("computeIK");
ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats, ikHelperPtr->computeIK(&endEffectorTargetWorldPositions[0],
&endEffectorTargetWorldOrientations[0],
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0], &q_current[0],
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0],
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize * 2, targetDampCoeff); &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize * 2, targetDampCoeff);
performedIK = true;
} }
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; else
for (int i = 0; i < numDofs; i++)
{ {
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices>1)
{
ikHelperPtr->computeIK2(&endEffectorTargetWorldPositions[0],
&endEffectorCurrentWorldPositions[0],
clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices,
//endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs,
&q_new[0], ikMethod, &jacobian_linear[0], targetDampCoeff);
performedIK = true;
}
} }
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs; if (performedIK)
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
for (int i = 0; i < numDofs; i++)
{ {
startingPositions[i] = q_new[i]; serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i = 0; i < numDofs; i++)
{
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
}
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
for (int i = 0; i < numDofs; i++)
{
startingPositions[i] = q_new[i];
}
} }
} }
} }

5
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -740,9 +740,10 @@ struct CalculateInverseKinematicsArgs
{ {
int m_bodyUniqueId; int m_bodyUniqueId;
// double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM]; // double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
double m_targetPosition[3]; double m_targetPositions[MAX_DEGREE_OF_FREEDOM*3];
int m_numEndEffectorLinkIndices;
double m_targetOrientation[4]; //orientation represented as quaternion, x,y,z,w double m_targetOrientation[4]; //orientation represented as quaternion, x,y,z,w
int m_endEffectorLinkIndex; int m_endEffectorLinkIndices[MAX_DEGREE_OF_FREEDOM];
double m_lowerLimit[MAX_DEGREE_OF_FREEDOM]; double m_lowerLimit[MAX_DEGREE_OF_FREEDOM];
double m_upperLimit[MAX_DEGREE_OF_FREEDOM]; double m_upperLimit[MAX_DEGREE_OF_FREEDOM];
double m_jointRange[MAX_DEGREE_OF_FREEDOM]; double m_jointRange[MAX_DEGREE_OF_FREEDOM];

4
examples/ThirdPartyLibs/BussIK/Jacobian.cpp
View File

@@ -84,10 +84,10 @@ Jacobian::Jacobian(Tree* tree)
Reset(); Reset();
} }
Jacobian::Jacobian(bool useAngularJacobian, int nDof) Jacobian::Jacobian(bool useAngularJacobian, int nDof, int numEndEffectors)
{ {
m_tree = 0; m_tree = 0;
m_nEffector = 1; m_nEffector = numEndEffectors;
if (useAngularJacobian) if (useAngularJacobian)
{ {

2
examples/ThirdPartyLibs/BussIK/Jacobian.h
View File

@@ -57,7 +57,7 @@ class Jacobian
{ {
public: public:
Jacobian(Tree*); Jacobian(Tree*);
Jacobian(bool useAngularJacobian, int nDof); Jacobian(bool useAngularJacobian, int nDof, int numEndEffectors);
void ComputeJacobian(VectorR3* targets); void ComputeJacobian(VectorR3* targets);
const MatrixRmn& ActiveJacobian() const { return *Jactive; } const MatrixRmn& ActiveJacobian() const { return *Jactive; }

4
examples/pybullet/examples/addPlanarReflection.py
View File

@@ -32,8 +32,8 @@ def getRayFromTo(mouseX, mouseY):
return rayFrom, rayTo return rayFrom, rayTo
cid = p.connect(p.SHARED_MEMORY_GUI) #cid = p.connect(p.SHARED_MEMORY_GUI)
#cid = p.connect(p.GUI) cid = p.connect(p.GUI)
if (cid < 0): if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.setPhysicsEngineParameter(numSolverIterations=10) p.setPhysicsEngineParameter(numSolverIterations=10)

2
examples/pybullet/examples/configureDebugVisualizer.py
View File

@@ -3,7 +3,7 @@ import math
import time import time
dt = 1./240. dt = 1./240.
p.connect(p.SHARED_MEMORY_GUI) p.connect(p.GUI)#SHARED_MEMORY_GUI)
p.loadURDF("r2d2.urdf",[0,0,1]) p.loadURDF("r2d2.urdf",[0,0,1])
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")
p.setGravity(0,0,-10) p.setGravity(0,0,-10)

255
examples/pybullet/pybullet.c
View File

@@ -51,7 +51,7 @@
#endif #endif
static PyObject* SpamError; static PyObject* SpamError;
#define B3_MAX_NUM_END_EFFECTORS 128
#define MAX_PHYSICS_CLIENTS 1024 #define MAX_PHYSICS_CLIENTS 1024
static b3PhysicsClientHandle sPhysicsClients1[MAX_PHYSICS_CLIENTS] = {0}; static b3PhysicsClientHandle sPhysicsClients1[MAX_PHYSICS_CLIENTS] = {0};
static int sPhysicsClientsGUI[MAX_PHYSICS_CLIENTS] = {0}; static int sPhysicsClientsGUI[MAX_PHYSICS_CLIENTS] = {0};
@@ -10009,6 +10009,253 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
return Py_None; return Py_None;
} }
///Inverse Kinematics binding
static PyObject* pybullet_calculateInverseKinematics2(PyObject* self,
PyObject* args, PyObject* keywds)
{
int bodyUniqueId;
int endEffectorLinkIndex=-1;
PyObject* targetPosObj = 0;
//PyObject* targetOrnObj = 0;
int solver = 0; // the default IK solver is DLS
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
PyObject* endEffectorLinkIndicesObj = 0;
PyObject* lowerLimitsObj = 0;
PyObject* upperLimitsObj = 0;
PyObject* jointRangesObj = 0;
PyObject* restPosesObj = 0;
PyObject* jointDampingObj = 0;
PyObject* currentPositionsObj = 0;
int maxNumIterations = -1;
double residualThreshold = -1;
static char* kwlist[] = { "bodyUniqueId", "endEffectorLinkIndices", "targetPositions", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "currentPositions", "maxNumIterations", "residualThreshold", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOO|OOOOOOiOidi", kwlist, &bodyUniqueId, &endEffectorLinkIndicesObj, &targetPosObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, &currentPositionsObj, &maxNumIterations, &residualThreshold, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int numEndEffectorPositions = extractVertices(targetPosObj, 0, B3_MAX_NUM_END_EFFECTORS);
int numIndices = extractIndices(endEffectorLinkIndicesObj, 0, B3_MAX_NUM_END_EFFECTORS);
double* positions = numEndEffectorPositions ? malloc(numEndEffectorPositions * 3 * sizeof(double)) : 0;
int* indices = numIndices ? malloc(numIndices * sizeof(int)) : 0;
numEndEffectorPositions = extractVertices(targetPosObj, positions, B3_MAX_NUM_VERTICES);
if (endEffectorLinkIndicesObj)
{
numIndices = extractIndices(endEffectorLinkIndicesObj, indices, B3_MAX_NUM_INDICES);
}
double pos[3] = { 0, 0, 0 };
double ori[4] = { 0, 0, 0, 1 };
int hasPos = numEndEffectorPositions > 0;
int hasOrn = 0;// pybullet_internalSetVector4d(targetOrnObj, ori);
int szLowerLimits = lowerLimitsObj ? PySequence_Size(lowerLimitsObj) : 0;
int szUpperLimits = upperLimitsObj ? PySequence_Size(upperLimitsObj) : 0;
int szJointRanges = jointRangesObj ? PySequence_Size(jointRangesObj) : 0;
int szRestPoses = restPosesObj ? PySequence_Size(restPosesObj) : 0;
int szJointDamping = jointDampingObj ? PySequence_Size(jointDampingObj) : 0;
int szCurrentPositions = currentPositionsObj ? PySequence_Size(currentPositionsObj) : 0;
int numJoints = b3GetNumJoints(sm, bodyUniqueId);
int dofCount = b3ComputeDofCount(sm, bodyUniqueId);
int hasNullSpace = 0;
int hasJointDamping = 0;
int hasCurrentPositions = 0;
double* lowerLimits = 0;
double* upperLimits = 0;
double* jointRanges = 0;
double* restPoses = 0;
double* jointDamping = 0;
double* currentPositions = 0;
if (dofCount && (szLowerLimits == dofCount) && (szUpperLimits == dofCount) &&
(szJointRanges == dofCount) && (szRestPoses == dofCount))
{
int szInBytes = sizeof(double) * dofCount;
int i;
lowerLimits = (double*)malloc(szInBytes);
upperLimits = (double*)malloc(szInBytes);
jointRanges = (double*)malloc(szInBytes);
restPoses = (double*)malloc(szInBytes);
for (i = 0; i < dofCount; i++)
{
lowerLimits[i] = pybullet_internalGetFloatFromSequence(lowerLimitsObj, i);
upperLimits[i] = pybullet_internalGetFloatFromSequence(upperLimitsObj, i);
jointRanges[i] = pybullet_internalGetFloatFromSequence(jointRangesObj, i);
restPoses[i] = pybullet_internalGetFloatFromSequence(restPosesObj, i);
}
hasNullSpace = 1;
}
if (szCurrentPositions > 0)
{
if (szCurrentPositions != dofCount)
{
PyErr_SetString(SpamError,
"calculateInverseKinematics the size of input current positions needs to be equal to the number of degrees of freedom.");
free(lowerLimits);
free(upperLimits);
free(jointRanges);
free(restPoses);
return NULL;
}
else
{
int szInBytes = sizeof(double) * szCurrentPositions;
int i;
currentPositions = (double*)malloc(szInBytes);
for (i = 0; i < szCurrentPositions; i++)
{
currentPositions[i] = pybullet_internalGetFloatFromSequence(currentPositionsObj, i);
}
hasCurrentPositions = 1;
}
}
if (szJointDamping > 0)
{
if (szJointDamping < dofCount)
{
printf("calculateInverseKinematics: the size of input joint damping values should be equal to the number of degrees of freedom, not using joint damping.");
}
else
{
int szInBytes = sizeof(double) * szJointDamping;
int i;
//if (szJointDamping != dofCount)
//{
// printf("calculateInverseKinematics: the size of input joint damping values should be equal to the number of degrees of freedom, ignoring the additonal values.");
//}
jointDamping = (double*)malloc(szInBytes);
for (i = 0; i < szJointDamping; i++)
{
jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i);
}
hasJointDamping = 1;
}
}
if (hasPos)
{
b3SharedMemoryStatusHandle statusHandle;
int numPos = 0;
int resultBodyIndex;
int result;
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId);
b3CalculateInverseKinematicsSelectSolver(command, solver);
if (hasCurrentPositions)
{
b3CalculateInverseKinematicsSetCurrentPositions(command, dofCount, currentPositions);
}
if (maxNumIterations > 0)
{
b3CalculateInverseKinematicsSetMaxNumIterations(command, maxNumIterations);
}
if (residualThreshold >= 0)
{
b3CalculateInverseKinematicsSetResidualThreshold(command, residualThreshold);
}
if (hasNullSpace)
{
if (hasOrn)
{
b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses);
}
else
{
b3CalculateInverseKinematicsPosWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses);
}
}
else
{
if (hasOrn)
{
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, endEffectorLinkIndex, pos, ori);
}
else
{
//b3CalculateInverseKinematicsAddTargetPurePosition(command, endEffectorLinkIndex, pos);
b3CalculateInverseKinematicsAddTargetsPurePosition(command, numEndEffectorPositions, indices, positions);
}
}
if (hasJointDamping)
{
b3CalculateInverseKinematicsSetJointDamping(command, dofCount, jointDamping);
}
free(currentPositions);
free(jointDamping);
free(lowerLimits);
free(upperLimits);
free(jointRanges);
free(restPoses);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
&resultBodyIndex,
&numPos,
0);
if (result && numPos)
{
int i;
PyObject* pylist;
double* ikOutPutJointPos = (double*)malloc(numPos * sizeof(double));
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
&resultBodyIndex,
&numPos,
ikOutPutJointPos);
pylist = PyTuple_New(numPos);
for (i = 0; i < numPos; i++)
{
PyTuple_SetItem(pylist, i,
PyFloat_FromDouble(ikOutPutJointPos[i]));
}
free(ikOutPutJointPos);
return pylist;
}
else
{
PyErr_SetString(SpamError,
"Error in calculateInverseKinematics");
return NULL;
}
}
else
{
PyErr_SetString(SpamError,
"calculateInverseKinematics couldn't extract position vector3");
return NULL;
}
}
Py_INCREF(Py_None);
return Py_None;
}
/// Given an object id, joint positions, joint velocities and joint /// Given an object id, joint positions, joint velocities and joint
/// accelerations, /// accelerations,
/// compute the joint forces using Inverse Dynamics /// compute the joint forces using Inverse Dynamics
@@ -10878,6 +11125,12 @@ static PyMethodDef SpamMethods[] = {
"current joint positions and target position" "current joint positions and target position"
" for the end effector," " for the end effector,"
"compute the inverse kinematics and return the new joint state"}, "compute the inverse kinematics and return the new joint state"},
{ "calculateInverseKinematics2", (PyCFunction)pybullet_calculateInverseKinematics2,
METH_VARARGS | METH_KEYWORDS,
"Inverse Kinematics bindings: Given an object id, "
"current joint positions and target positions"
" for the end effectors,"
"compute the inverse kinematics and return the new joint state" },
{"getVREvents", (PyCFunction)pybullet_getVREvents, METH_VARARGS | METH_KEYWORDS, {"getVREvents", (PyCFunction)pybullet_getVREvents, METH_VARARGS | METH_KEYWORDS,
"Get Virtual Reality events, for example to track VR controllers position/buttons"}, "Get Virtual Reality events, for example to track VR controllers position/buttons"},