diff --git a/examples/RenderingExamples/RaytracerSetup.cpp b/examples/RenderingExamples/RaytracerSetup.cpp index 3e328a811..2996e8bec 100644 --- a/examples/RenderingExamples/RaytracerSetup.cpp +++ b/examples/RenderingExamples/RaytracerSetup.cpp @@ -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; diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index 9114d0889..a38b13abd 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -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() @@ -189,15 +190,20 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui) m_internalData->m_depthBuffer, &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); diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 3e0de985d..b8c2d4eec 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -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;im_renderer && m_app->m_renderer->getActiveCamera()) diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 7a6667c62..81a9872a3 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -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; } diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 54ebec698..fe640a0a9 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -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 m_calculatedJointPositions; }; class b3RobotSimAPI diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 735cf389f..7c8087a4d 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -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) { diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d6d869a4d..b9279938a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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; } \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 08a179680..ba843d5f9 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 22505bd69..0d94d0b8e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -495,7 +495,35 @@ 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; + } + }; void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiHelper) @@ -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; - - 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); - } - } + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + + 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; + + if (ikHelperPtr && bodyHandle->m_multiBody->getNumLinks()==numJoints) { - - serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED; + b3AlignedObjectArray 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& 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; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 221f310b9..95a200271 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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 */ };