Merge pull request #784 from erwincoumans/master

experimental Inverse Kinematics for KUKA iiwa exposed in

examples/RenderingExamples/RaytracerSetup.cpp

@@ -260,11 +260,21 @@ void RaytracerPhysicsSetup::stepSimulation(float deltaTime)
 
 	float fov = 2.0 * atanf (tanFov);
 
+    
 	btVector3 cameraPosition(5,0,0);
 	btVector3 cameraTargetPosition(0,0,0);
 
+    
+    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;

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()
@@ -190,12 +191,17 @@ 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);
 

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,57 +166,47 @@ 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_currentJointPositions = q_current;
-			ikargs.m_numPositions = 7;
+//			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];
 			
-
+//todo: orientation IK target
-			ikargs.m_endEffectorTargetOrientation[0] = 0;
+//			ikargs.m_endEffectorTargetOrientation[0] = 0;
-			ikargs.m_endEffectorTargetOrientation[1] = 0;
+//			ikargs.m_endEffectorTargetOrientation[1] = 0;
-			ikargs.m_endEffectorTargetOrientation[2] = 0;
+//			ikargs.m_endEffectorTargetOrientation[2] = 0;
-			ikargs.m_endEffectorTargetOrientation[3] = 1;
+//			ikargs.m_endEffectorTargetOrientation[3] = 1;
 
 			if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
 			{
-			}
+                //copy the IK result to the desired state of the motor/actuator
-*/
-            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_targetPosition = ikresults.m_calculatedJointPositions[i];
                     t.m_maxTorqueValue = 1000;
                     t.m_kp= 1;
                     m_robotSim.setJointMotorControl(m_kukaIndex,i,t);
 
                 }
             }
+        }
         
         
 		m_robotSim.stepSimulation();
@@ -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())

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,
+		&numPos,
-		results.m_calculatedJointPositions);
+		0);
+    if (result && numPos)
+    {
+        results.m_calculatedJointPositions.resize(numPos);
+        result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
+                                                                 &results.m_bodyUniqueId,
+                                                                 &numPos,
+                                &results.m_calculatedJointPositions[0]);
         
+    }
 	return result;
 }
 

examples/RoboticsLearning/b3RobotSimAPI.h

@@ -90,16 +90,16 @@ enum b3InverseKinematicsFlags
 struct b3RobotSimInverseKinematicArgs
 {
 	int m_bodyUniqueId;
-	double* m_currentJointPositions;
+//	double* m_currentJointPositions;
-	int m_numPositions;
+//	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_currentJointPositions(0),
-		m_numPositions(0),
+//		m_numPositions(0),
 		m_flags(0)
 	{
 	}
@@ -108,8 +108,7 @@ struct b3RobotSimInverseKinematicArgs
 struct b3RobotSimInverseKinematicsResults
 {
 	int m_bodyUniqueId;
-	double* m_calculatedJointPositions;
+	b3AlignedObjectArray<double> m_calculatedJointPositions;
-	int m_numPositions;
 };
 
 class b3RobotSimAPI

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);
+    if (jacobian_size1)
+    {
+        b3Assert(jacobian_size1==nRow*nCol);
         MatrixRmn linearJacobian(nRow,nCol);
         for (int i = 0; i < nRow; ++i)
         {
             for (int j = 0; j < nCol; ++j)
             {
-            linearJacobian.Set(i,j,linear_jacobian[i*nCol+j]);
+                linearJacobian.Set(i,j,linear_jacobian1[i*nCol+j]);
             }
         }
         m_data->m_ikJacobian->SetJendTrans(linearJacobian);
+    }
     
     // Calculate the change in theta values
     switch (ikMethod) {

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);
+//todo
-	for (int i = 0; i < numJoints;i++)
+//	int numJoints = cl->getNumJoints(bodyIndex);
-	{
+//	for (int i = 0; i < numJoints;i++)
-		command->m_calculateInverseKinematicsArguments.m_jointPositionsQ[i] = jointPositionsQ[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;
 }

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,

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -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;
                         
-						if (treePtrPtr)
+						btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
-						{
-							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);
-							}
-						}
 
 						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)
+                        btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
-                        {
-                            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);
-                            }
-                        }
                         
                         if (tree)
                         {
@@ -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,9 +2560,76 @@ 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)
+							{
+                                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;
 							}
 						}

examples/SharedMemory/SharedMemoryCommands.h

@@ -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

examples/TinyRenderer/TinyRenderer.cpp

@@ -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;
 

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
-
+    // object names
-    // applyBaseForce
-    // applyLinkForce
 
     {NULL, NULL, 0, NULL} /* Sentinel */
 };