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

Merge pull request #784 from erwincoumans/master

Browse Source 
experimental Inverse Kinematics for KUKA iiwa exposed in
This commit is contained in:
erwincoumans
2016-09-14 02:52:09 +01:00
committed by GitHub
parent e5a8eb2425 f9679bd594
commit 79d24db60f
12 changed files with 313 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
examples/RenderingExamples/RaytracerSetup.cpp
View File

@@ -260,12 +260,22 @@ void RaytracerPhysicsSetup::stepSimulation(float deltaTime)
float fov = 2.0 * atanf (tanFov);
btVector3 cameraPosition(5,0,0);
btVector3 cameraTargetPosition(0,0,0);
btVector3 rayFrom = cameraPosition;
btVector3 rayForward = cameraTargetPosition-cameraPosition;
rayForward.normalize();
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
{
m_app->m_renderer->getActiveCamera()->getCameraPosition(cameraPosition);
m_app->m_renderer->getActiveCamera()->getCameraTargetPosition(cameraTargetPosition);
}
btVector3 rayFrom = cameraPosition;
btVector3 rayForward = cameraTargetPosition-cameraPosition;
rayForward.normalize();
float farPlane = 600.f;
rayForward*= farPlane;

8
examples/RenderingExamples/TinyRendererSetup.cpp
View File

@@ -53,6 +53,7 @@ struct TinyRendererSetupInternalData
m_animateRenderer(0)
{
m_depthBuffer.resize(m_width*m_height);
// m_segmentationMaskBuffer.resize(m_width*m_height);
}
void updateTransforms()
@@ -190,14 +191,19 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
&m_internalData->m_segmentationMaskBuffer,
m_internalData->m_renderObjects.size());
meshData.m_gfxShape->m_scaling[0] = scaling[0];
meshData.m_gfxShape->m_scaling[1] = scaling[1];
meshData.m_gfxShape->m_scaling[2] = scaling[2];
const int* indices = &meshData.m_gfxShape->m_indices->at(0);
ob->registerMeshShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
meshData.m_gfxShape->m_numvertices,
indices,
meshData.m_gfxShape->m_numIndices,color, meshData.m_textureImage,meshData.m_textureWidth,meshData.m_textureHeight);
ob->m_localScaling.setValue(scaling[0],scaling[1],scaling[2]);
m_internalData->m_renderObjects.push_back(ob);
m_internalData->m_renderObjects.push_back(ob);

69
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -153,13 +153,10 @@ public:
if (numJoints==7)
{
double q_current[7]={0,0,0,0,0,0,0};
double qdot_current[7]={0,0,0,0,0,0,0};
double qddot_current[7]={0,0,0,0,0,0,0};
double local_position[3]={0,0,0};
double jacobian_linear[21];
double jacobian_angular[21];
double world_position[3]={0,0,0};
b3JointStates jointStates;
if (m_robotSim.getJointStates(m_kukaIndex,jointStates))
{
//skip the base positions (7 values)
@@ -169,55 +166,45 @@ public:
q_current[i] = jointStates.m_actualStateQ[i+7];
}
}
// compute body position
m_robotSim.getLinkState(0, 6, world_position);
m_worldPos.setValue(world_position[0], world_position[1], world_position[2]);
// compute body Jacobian
m_robotSim.getBodyJacobian(0, 6, local_position, q_current, qdot_current, qddot_current, jacobian_linear, jacobian_angular);
// m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo);
/*
b3Vector3DoubleData dataOut;
m_targetPos.serializeDouble(dataOut);
b3Vector3DoubleData worldPosDataOut;
m_worldPos.serializeDouble(worldPosDataOut);
b3RobotSimInverseKinematicArgs ikargs;
b3RobotSimInverseKinematicsResults ikresults;
ikargs.m_bodyUniqueId = m_kukaIndex;
ikargs.m_currentJointPositions = q_current;
ikargs.m_numPositions = 7;
ikargs.m_endEffectorTargetPosition[0] = dataOut.m_floats[0];
ikargs.m_endEffectorTargetPosition[1] = dataOut.m_floats[1];
ikargs.m_endEffectorTargetPosition[2] = dataOut.m_floats[2];
// ikargs.m_currentJointPositions = q_current;
// ikargs.m_numPositions = 7;
ikargs.m_endEffectorTargetPosition[0] = dataOut.m_floats[0];
ikargs.m_endEffectorTargetPosition[1] = dataOut.m_floats[1];
ikargs.m_endEffectorTargetPosition[2] = dataOut.m_floats[2];
ikargs.m_endEffectorTargetOrientation[0] = 0;
ikargs.m_endEffectorTargetOrientation[1] = 0;
ikargs.m_endEffectorTargetOrientation[2] = 0;
ikargs.m_endEffectorTargetOrientation[3] = 1;
//todo: orientation IK target
// ikargs.m_endEffectorTargetOrientation[0] = 0;
// ikargs.m_endEffectorTargetOrientation[1] = 0;
// ikargs.m_endEffectorTargetOrientation[2] = 0;
// ikargs.m_endEffectorTargetOrientation[3] = 1;
if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
{
}
*/
double q_new[7];
int ikMethod=IK2_DLS;
b3Vector3DoubleData dataOut;
m_targetPos.serializeDouble(dataOut);
b3Vector3DoubleData worldPosDataOut;
m_worldPos.serializeDouble(worldPosDataOut);
m_ikHelper.computeIK(dataOut.m_floats,worldPosDataOut.m_floats,q_current, numJoints, q_new, ikMethod,jacobian_linear,(sizeof(jacobian_linear)/sizeof(*jacobian_linear)));
printf("q_new = %f,%f,%f,%f,%f,%f,%f\n", q_new[0],q_new[1],q_new[2],
q_new[3],q_new[4],q_new[5],q_new[6]);
//set the
for (int i=0;i<numJoints;i++)
{
b3JointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
t.m_targetPosition = q_new[i];
t.m_maxTorqueValue = 1000;
t.m_kp= 1;
m_robotSim.setJointMotorControl(m_kukaIndex,i,t);
//copy the IK result to the desired state of the motor/actuator
for (int i=0;i<numJoints;i++)
{
b3JointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
t.m_targetPosition = ikresults.m_calculatedJointPositions[i];
t.m_maxTorqueValue = 1000;
t.m_kp= 1;
m_robotSim.setJointMotorControl(m_kukaIndex,i,t);
}
}
}
@@ -261,7 +248,7 @@ public:
virtual void resetCamera()
{
float dist = 3;
float pitch = -75;
float pitch = 0;
float yaw = 30;
float targetPos[3]={-0.2,0.8,0.3};
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())

16
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -482,16 +482,26 @@ bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKin
{
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId,
args.m_currentJointPositions,args.m_endEffectorTargetPosition);
args.m_endEffectorTargetPosition);
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
int numPos=0;
bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
&results.m_bodyUniqueId,
&results.m_numPositions,
results.m_calculatedJointPositions);
&numPos,
0);
if (result && numPos)
{
results.m_calculatedJointPositions.resize(numPos);
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
&results.m_bodyUniqueId,
&numPos,
&results.m_calculatedJointPositions[0]);
}
return result;
}

13
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -90,16 +90,16 @@ enum b3InverseKinematicsFlags
struct b3RobotSimInverseKinematicArgs
{
int m_bodyUniqueId;
double* m_currentJointPositions;
int m_numPositions;
// double* m_currentJointPositions;
// int m_numPositions;
double m_endEffectorTargetPosition[3];
double m_endEffectorTargetOrientation[3];
// double m_endEffectorTargetOrientation[4];
int m_flags;
b3RobotSimInverseKinematicArgs()
:m_bodyUniqueId(-1),
m_currentJointPositions(0),
m_numPositions(0),
// m_currentJointPositions(0),
// m_numPositions(0),
m_flags(0)
{
}
@@ -108,8 +108,7 @@ struct b3RobotSimInverseKinematicArgs
struct b3RobotSimInverseKinematicsResults
{
int m_bodyUniqueId;
double* m_calculatedJointPositions;
int m_numPositions;
b3AlignedObjectArray<double> m_calculatedJointPositions;
};
class b3RobotSimAPI

17
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -99,7 +99,7 @@ void IKTrajectoryHelper::createKukaIIWA()
bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double endEffectorWorldPosition[3],
const double* q_current, int numQ,
double* q_new, int ikMethod, const double* linear_jacobian, int jacobian_size)
double* q_new, int ikMethod, const double* linear_jacobian1, int jacobian_size1)
{
if (numQ != 7)
@@ -134,16 +134,19 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
// Set Jacobian from Bullet body Jacobian
int nRow = m_data->m_ikJacobian->GetNumRows();
int nCol = m_data->m_ikJacobian->GetNumCols();
b3Assert(jacobian_size==nRow*nCol);
MatrixRmn linearJacobian(nRow,nCol);
for (int i = 0; i < nRow; ++i)
if (jacobian_size1)
{
for (int j = 0; j < nCol; ++j)
b3Assert(jacobian_size1==nRow*nCol);
MatrixRmn linearJacobian(nRow,nCol);
for (int i = 0; i < nRow; ++i)
{
linearJacobian.Set(i,j,linear_jacobian[i*nCol+j]);
for (int j = 0; j < nCol; ++j)
{
linearJacobian.Set(i,j,linear_jacobian1[i*nCol+j]);
}
}
m_data->m_ikJacobian->SetJendTrans(linearJacobian);
}
m_data->m_ikJacobian->SetJendTrans(linearJacobian);
// Calculate the change in theta values
switch (ikMethod) {

15
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1320,7 +1320,7 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ
///compute the joint positions to move the end effector to a desired target using inverse kinematics
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
const double* jointPositionsQ, const double targetPosition[3])
const double targetPosition[3])
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
@@ -1331,11 +1331,12 @@ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsCli
command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS;
command->m_updateFlags = 0;
command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex;
int numJoints = cl->getNumJoints(bodyIndex);
for (int i = 0; i < numJoints;i++)
{
command->m_calculateInverseKinematicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
}
//todo
// int numJoints = cl->getNumJoints(bodyIndex);
// for (int i = 0; i < numJoints;i++)
// {
// command->m_calculateInverseKinematicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
// }
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
@@ -1372,5 +1373,5 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
}
}
return false;
return true;
}

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -122,7 +122,7 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ
///compute the joint positions to move the end effector to a desired target using inverse kinematics
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
const double* jointPositionsQ, const double targetPosition[3]);
const double targetPosition[3]);
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,

158
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -495,6 +495,34 @@ struct PhysicsServerCommandProcessorInternalData
}
btInverseDynamics::MultiBodyTree* findOrCreateTree(btMultiBody* multiBody)
{
btInverseDynamics::MultiBodyTree* tree = 0;
btInverseDynamics::MultiBodyTree** treePtrPtr =
m_inverseDynamicsBodies.find(multiBody);
if (treePtrPtr)
{
tree = *treePtrPtr;
}
else
{
btInverseDynamics::btMultiBodyTreeCreator id_creator;
if (-1 == id_creator.createFromBtMultiBody(multiBody, false))
{
}
else
{
tree = btInverseDynamics::CreateMultiBodyTree(id_creator);
m_inverseDynamicsBodies.insert(multiBody, tree);
}
}
return tree;
}
};
@@ -850,6 +878,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
return loadOk;
}
bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes)
{
@@ -2280,29 +2311,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
btInverseDynamics::MultiBodyTree** treePtrPtr =
m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody);
btInverseDynamics::MultiBodyTree* tree = 0;
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
if (treePtrPtr)
{
tree = *treePtrPtr;
}
else
{
btInverseDynamics::btMultiBodyTreeCreator id_creator;
if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false))
{
b3Error("error creating tree\n");
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
}
else
{
tree = btInverseDynamics::CreateMultiBodyTree(id_creator);
m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree);
}
}
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
if (tree)
{
@@ -2350,29 +2361,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
btInverseDynamics::MultiBodyTree** treePtrPtr =
m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody);
btInverseDynamics::MultiBodyTree* tree = 0;
serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
if (treePtrPtr)
{
tree = *treePtrPtr;
}
else
{
btInverseDynamics::btMultiBodyTreeCreator id_creator;
if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false))
{
b3Error("error creating tree\n");
serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
}
else
{
tree = btInverseDynamics::CreateMultiBodyTree(id_creator);
m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree);
}
}
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
if (tree)
{
@@ -2423,9 +2414,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_APPLY_EXTERNAL_FORCE:
{
if (m_data->m_verboseOutput)
{
b3Printf("CMD_APPLY_EXTERNAL_FORCE clientCmd = %d\n", clientCmd.m_sequenceNumber);
}
{
b3Printf("CMD_APPLY_EXTERNAL_FORCE clientCmd = %d\n", clientCmd.m_sequenceNumber);
}
for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i)
{
InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]);
@@ -2561,7 +2552,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
if (tmpHelper->createFromMultiBody(bodyHandle->m_multiBody))
{
m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, ikHelperPtr);
m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
ikHelperPtr = tmpHelper;
} else
{
@@ -2569,10 +2560,77 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
if (ikHelperPtr)
{
//todo: make this generic. Right now, only support/tested KUKA iiwa
int numJoints = 7;
int endEffectorLinkIndex = 6;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
if (ikHelperPtr && bodyHandle->m_multiBody->getNumLinks()==numJoints)
{
b3AlignedObjectArray<double> jacobian_linear;
jacobian_linear.resize(3*7);
int jacSize = 0;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
double q_current[7];
if (tree)
{
jacSize = jacobian_linear.size();
// Set jacobian value
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs);
for (int i = 0; i < num_dofs; i++)
{
q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i);
qdot[i + baseDofs] = 0;
nu[i+baseDofs] = 0;
}
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, num_dofs);
tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < num_dofs; ++j)
{
jacobian_linear[i*num_dofs+j] = jac_t(i,j);
}
}
}
}
double q_new[7];
int ikMethod=IK2_DLS;
btVector3DoubleData endEffectorWorldPosition;
btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin();
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition,
endEffectorWorldPosition.m_floats,
q_current,
numJoints, q_new, ikMethod, &jacobian_linear[0],jacSize);
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numJoints;i++)
{
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
}
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numJoints;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
}
}
hasStatus = true;

10
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -391,12 +391,18 @@ struct CalculateJacobianResultArgs
double m_angularJacobian[3*MAX_DEGREE_OF_FREEDOM];
};
enum EnumCalculateInverseKinematicsFlags
{
IK_HAS_TARGET_ORIENTATION=1,
IK_HAS_CURRENT_JOINT_POSITIONS=2,
};
struct CalculateInverseKinematicsArgs
{
int m_bodyUniqueId;
double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
// double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
double m_targetPosition[3];
double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w
// double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w
};
struct CalculateInverseKinematicsResultArgs

2
examples/TinyRenderer/TinyRenderer.cpp
View File

@@ -268,7 +268,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
renderData.m_viewportMatrix = viewport(0,0,width, height);
b3AlignedObjectArray<float>& zbuffer = renderData.m_depthBuffer;
int* segmentationMaskBufferPtr = renderData.m_segmentationMaskBufferPtr?&renderData.m_segmentationMaskBufferPtr->at(0):0;
int* segmentationMaskBufferPtr = (renderData.m_segmentationMaskBufferPtr && renderData.m_segmentationMaskBufferPtr->size())?&renderData.m_segmentationMaskBufferPtr->at(0):0;
TGAImage& frame = renderData.m_rgbColorBuffer;

112
examples/pybullet/pybullet.c
View File

@@ -1000,6 +1000,24 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) {
return 0;
}
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
int i, len;
PyObject* seq;
seq = PySequence_Fast(obVec, "expected a sequence");
len = PySequence_Size(obVec);
if (len == 3) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
return 0;
}
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
int size = PySequence_Size(args);
int objectUniqueIdA = -1;
@@ -1626,6 +1644,85 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self,
return Py_None;
}
///Experimental Inverse Kinematics binding ,7-dof KUKA IIWA only
static PyObject* pybullet_calculateInverseKinematicsKuka(PyObject* self,
PyObject* args) {
int size;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
size = PySequence_Size(args);
if (size == 2)
{
int bodyIndex;
PyObject* targetPosObj;
if (PyArg_ParseTuple(args, "iO", &bodyIndex, &targetPosObj))
{
double pos[3];
if (pybullet_internalSetVectord(targetPosObj,pos))
{
b3SharedMemoryStatusHandle statusHandle;
int numPos=0;
int resultBodyIndex;
int result;
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex,
pos);
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;
}
}
} else
{
PyErr_SetString(SpamError,
"calculateInverseKinematics expects 2 arguments, body index, "
"and target position for end effector");
return NULL;
}
Py_INCREF(Py_None);
return Py_None;
}
/// Given an object id, joint positions, joint velocities and joint
/// accelerations,
/// compute the joint forces using Inverse Dynamics
@@ -1844,16 +1941,19 @@ static PyMethodDef SpamMethods[] = {
METH_VARARGS,
"Given an object id, joint positions, joint velocities and joint "
"accelerations, compute the joint forces using Inverse Dynamics"},
{"calculateInverseKinematicsKuka", pybullet_calculateInverseKinematicsKuka,
METH_VARARGS,
"Experimental, KUKA IIWA only: Given an object id, "
"current joint positions and target position"
" for the end effector,"
"compute the inverse kinematics and return the new joint state"},
// todo(erwincoumans)
// saveSnapshot
// loadSnapshot
////todo(erwincoumans)
// collision info
// raycast info
// applyBaseForce
// applyLinkForce
// object names
{NULL, NULL, 0, NULL} /* Sentinel */
};