pybullet.calculateInverseKinematics: expose null space method with and without orientation

add inverse_kinematics.py and hello_pybullet.py pybullet examples
add m_worldLinkFramePosition/Orientation fields to b3LinkState, and in pybullet.getLinkState (URDF link frame in Cartesian/world coordinates)

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -3,6 +3,7 @@
 #include "Bullet3Common/b3Scalar.h"
 #include "Bullet3Common/b3Vector3.h"
 #include "Bullet3Common/b3Matrix3x3.h"
+#include "Bullet3Common/b3Transform.h"
 
 #include <string.h>
 #include "SharedMemoryCommands.h"
@@ -534,6 +535,10 @@ void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
   b3Assert(bodyIndex>=0);
   if (bodyIndex>=0)
   {
+	  b3Transform wlf,com,inertial;
+	
+	  
+
     for (int i = 0; i < 3; ++i) 
     {
       state->m_worldPosition[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + i];
@@ -544,6 +549,20 @@ void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
       state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + 3 + i];
       state->m_localInertialOrientation[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + 3 + i];
     }
+	com.setOrigin(b3MakeVector3(state->m_worldPosition[0],state->m_worldPosition[1],state->m_worldPosition[2]));
+	com.setRotation(b3Quaternion(state->m_worldOrientation[0],state->m_worldOrientation[1],state->m_worldOrientation[2],state->m_worldOrientation[3]));
+	inertial.setOrigin(b3MakeVector3(state->m_localInertialPosition[0],state->m_localInertialPosition[1],state->m_localInertialPosition[2]));
+	inertial.setRotation(b3Quaternion(state->m_localInertialOrientation[0],state->m_localInertialOrientation[1],state->m_localInertialOrientation[2],state->m_localInertialOrientation[3]));
+	wlf = com*inertial.inverse();
+	for (int i = 0; i < 3; ++i) 
+    {
+		state->m_worldLinkFramePosition[i] = wlf.getOrigin()[i];
+	}
+	b3Quaternion wlfOrn = wlf.getRotation();
+	for (int i = 0; i < 4; ++i) 
+    {
+		state->m_worldLinkFrameOrientation[i] = wlfOrn[i];
+	}
   }
 }
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -325,11 +325,16 @@ struct b3VisualShapeInformation
 ///use URDF link frame = link COM frame * inertiaFrame.inverse()
 struct b3LinkState
 {
+	//m_worldPosition and m_worldOrientation of the Center Of Mass (COM)
     double m_worldPosition[3];
     double m_worldOrientation[4];
 
     double m_localInertialPosition[3];
     double m_localInertialOrientation[4];
+
+	///world position and orientation of the (URDF) link frame
+	double m_worldLinkFramePosition[3];
+    double m_worldLinkFrameOrientation[4];
 };
 
 //todo: discuss and decide about control mode and combinations

examples/pybullet/hello_pybullet.py

@@ -0,0 +1,23 @@
+import pybullet as p
+from time import sleep
+
+physicsClient = p.connect(p.GUI)
+
+p.setGravity(0,0,-10)
+planeId = p.loadURDF("plane.urdf")
+cubeStartPos = [0,0,1]
+cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
+boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
+cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
+
+useRealTimeSimulation = 0
+
+if (useRealTimeSimulation):
+	p.setRealTimeSimulation(1)
+
+while 1:
+	if (useRealTimeSimulation):
+		p.setGravity(0,0,-10)
+		sleep(0.01) # Time in seconds.
+	else:
+		p.stepSimulation()

examples/pybullet/inverse_kinematics.py

@@ -0,0 +1,85 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+
+
+p.connect(p.GUI)
+p.loadURDF("plane.urdf",[0,0,-0.3])
+kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
+p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
+kukaEndEffectorIndex = 6
+numJoints = p.getNumJoints(kukaId)
+if (numJoints!=7):
+	exit()
+	
+
+#lower limits for null space
+ll=[-.967,-2	,-2.96,0.19,-2.96,-2.09,-3.05]
+#upper limits for null space
+ul=[.967,2	,2.96,2.29,2.96,2.09,3.05]
+#joint ranges for null space
+jr=[5.8,4,5.8,4,5.8,4,6]
+#restposes for null space
+rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0]
+
+for i in range (numJoints):
+	p.resetJointState(kukaId,i,rp[i])
+
+p.setGravity(0,0,0)
+t=0.
+prevPose=[0,0,0]
+prevPose1=[0,0,0]
+hasPrevPose = 0
+useNullSpace = 0
+
+useOrientation = 1
+useSimulation = 1
+useRealTimeSimulation = 1
+p.setRealTimeSimulation(useRealTimeSimulation)
+#trailDuration is duration (in seconds) after debug lines will be removed automatically
+#use 0 for no-removal
+trailDuration = 15
+
+	
+while 1:
+	if (useRealTimeSimulation):
+		dt = datetime.now()
+		t = (dt.second/60.)*2.*math.pi
+	else:
+		t=t+0.1
+	
+	if (useSimulation and useRealTimeSimulation==0):
+		p.stepSimulation()
+	
+	for i in range (25):
+		pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)]
+		#end effector points down, not up (in case useOrientation==1)
+		orn = p.getQuaternionFromEuler([0,-math.pi,0])
+		
+		if (useNullSpace==1):
+			if (useOrientation==1):
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,ll,ul,jr,rp)
+			else:
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp)
+		else:
+			if (useOrientation==1):
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn)
+			else:
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos)
+	
+		if (useSimulation):
+			for i in range (numJoints):
+				p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=300,positionGain=0.3,velocityGain=1)
+		else:
+			#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
+			for i in range (numJoints):
+				p.resetJointState(kukaId,i,jointPoses[i])
+
+	ls = p.getLinkState(kukaId,kukaEndEffectorIndex)	
+	if (hasPrevPose):
+		p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration)
+		p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration)
+	prevPose=pos
+	prevPose1=ls[4]
+	hasPrevPose = 1		

examples/pybullet/pybullet.c

@@ -2020,6 +2020,8 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args,PyObject*
   PyObject* pyLinkStateWorldOrientation;
   PyObject* pyLinkStateLocalInertialPosition;
   PyObject* pyLinkStateLocalInertialOrientation;
+  PyObject* pyLinkStateWorldLinkFramePosition;
+  PyObject* pyLinkStateWorldLinkFrameOrientation;
 
   struct b3LinkState linkState;
 
@@ -2094,11 +2096,26 @@ b3PhysicsClientHandle sm = 0;
                         PyFloat_FromDouble(linkState.m_localInertialOrientation[i]));
       }
 
-      pyLinkState = PyTuple_New(4);
+	pyLinkStateWorldLinkFramePosition = PyTuple_New(3);
+      for (i = 0; i < 3; ++i) {
+        PyTuple_SetItem(pyLinkStateWorldLinkFramePosition , i,
+                        PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i]));
+      }
+
+	pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4);
+      for (i = 0; i < 4; ++i) {
+        PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i,
+                        PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i]));
+      }
+
+
+      pyLinkState = PyTuple_New(6);
       PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition);
       PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation);
       PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition);
       PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation);
+      PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition );
+      PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation);
 
       return pyLinkState;
     }
@@ -4048,13 +4065,18 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
         int bodyIndex;
 		int endEffectorLinkIndex;
 		
-        PyObject* targetPosObj;
-		PyObject* targetOrnObj;
+        PyObject* targetPosObj=0;
+		PyObject* targetOrnObj=0;
 
 		int physicsClientId = 0;
 		b3PhysicsClientHandle sm = 0;
-	static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","physicsClientId", NULL };
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOO|i", kwlist,  &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&physicsClientId))
+		PyObject* lowerLimitsObj = 0;
+		PyObject* upperLimitsObj = 0;
+		PyObject* jointRangesObj = 0;
+		PyObject* restPosesObj = 0;
+
+	static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOi", kwlist,  &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -4067,8 +4089,49 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 	{
 		double pos[3];
 		double ori[4]={0,1.0,0,0};
-            
-		if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4d(targetOrnObj,ori))
+        int hasPos =pybullet_internalSetVectord(targetPosObj,pos);
+		int hasOrn = 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 numJoints = b3GetNumJoints(sm, bodyIndex);
+
+
+		int hasNullSpace = 0;
+		double* lowerLimits = 0;
+		double* upperLimits = 0;
+		double* jointRanges = 0;
+		double* restPoses = 0;
+		
+		if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) &&
+          (szJointRanges == numJoints) && (szRestPoses == numJoints)) 
+		{
+			int szInBytes = sizeof(double) * numJoints;
+			int i;
+			PyObject* pylist = 0;
+			lowerLimits = (double*)malloc(szInBytes);
+			upperLimits = (double*)malloc(szInBytes);
+			jointRanges = (double*)malloc(szInBytes);
+			restPoses = (double*)malloc(szInBytes);
+
+			for (i = 0; i < numJoints; 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 (hasPos)
 		{
 			b3SharedMemoryStatusHandle statusHandle;
 			int numPos=0;
@@ -4076,7 +4139,27 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 			int result;
 
 			b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex);
-			b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,6,pos,ori);
+
+			if (hasNullSpace)
+			{
+				if (hasOrn)
+				{
+					b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses);
+				} else
+				{
+					b3CalculateInverseKinematicsPosWithNullSpaceVel(command,numJoints, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses);
+				}
+			} else
+			{
+				if (hasOrn)
+				{
+					b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,endEffectorLinkIndex,pos,ori);
+				} else
+				{
+					b3CalculateInverseKinematicsAddTargetPurePosition(command,endEffectorLinkIndex,pos);
+				}
+			}
+
 			statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
                 
 			result = b3GetStatusInverseKinematicsJointPositions(statusHandle,