Merge pull request #1393 from YunfeiBai/master

Expose IK solver options: DLS and SDLS.

data/pole.urdf

@@ -0,0 +1,140 @@
+<?xml version="1.0"?>
+<robot name="physics">
+
+    <link name="slideBar">
+    <visual>
+      <geometry>
+        <box size="30 0.05 0.05"/>
+      </geometry>
+      <origin xyz="0 0 0"/>
+      <material name="green">
+        <color rgba="0 0.8 .8 1"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <link name="pole">
+    <visual>
+      <geometry>
+        <box size="0.05 0.05 1.0"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <inertial>
+    <origin xyz="0 0 0"/>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+     <collision>
+      <geometry>
+        <box size="0.05 0.05 1.0"/>	
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+  </link>
+
+  <joint name="cart_to_pole" type="prismatic">
+    <axis xyz="1 0 0"/>
+    <origin xyz="0.0 0.0 0.5"/>
+    <parent link="slideBar"/>
+    <child link="pole"/>
+    <limit effort="1000.0" lower="-5" upper="5" velocity="0.5"/>
+  </joint>
+  
+   <link name="pole2">
+    <visual>
+      <geometry>
+        <box size="0.05 0.05 1.0"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0.5"/>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <inertial>
+    <origin xyz="0 0 0.5"/>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+     <collision>
+      <geometry>
+        <box size="0.05 0.05 1.0"/>	
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0.5"/>
+    </collision>
+  </link>
+
+  <joint name="pole_to_pole2" type="continuous">
+    <axis xyz="1 0 0"/>
+    <origin xyz="0.0 0.0 0.5"/>
+    <parent link="pole"/>
+    <child link="pole2"/>
+  </joint>
+  
+  <link name="pole3">
+    <visual>
+      <geometry>
+        <box size="0.05 0.05 1.0"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0.5"/>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <inertial>
+    <origin xyz="0 0 0.5"/>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+     <collision>
+      <geometry>
+        <box size="0.05 0.05 1.0"/>	
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0.5"/>
+    </collision>
+  </link>
+
+  <joint name="pole2_to_pole3" type="continuous">
+    <axis xyz="0 1 0"/>
+    <origin xyz="0.0 0.0 1"/>
+    <parent link="pole2"/>
+    <child link="pole3"/>
+  </joint>
+  
+   <link name="endeffector">
+    <visual>
+      <geometry>
+        <box size="0.06 0.06 .06"/>	
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <material name="red">
+        <color rgba="1 0 0 1"/>
+      </material>
+    </visual>
+    <inertial>
+    <origin xyz="0 0 0"/>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+     <collision>
+      <geometry>
+        <box size="0.06 0.06 .06"/>	
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+  </link>
+
+  <joint name="pole2_to_endeffector" type="fixed">
+    <origin xyz="0.0 0.0 1"/>
+    <parent link="pole3"/>
+    <child link="endeffector"/>
+  </joint>
+ 
+</robot>

examples/SharedMemory/IKTrajectoryHelper.cpp

@@ -47,7 +47,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
                const double* q_current, int numQ,int endEffectorIndex,
                double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
 {
-	bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? 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);
 
@@ -136,8 +137,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
             ikJacobian.CalcDeltaThetasTranspose();		// Jacobian transpose method
             break;
 		case IK2_DLS:
-        case IK2_VEL_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);
@@ -154,6 +155,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
             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:

examples/SharedMemory/IKTrajectoryHelper.h

@@ -12,6 +12,8 @@ enum IK2_Method
 	IK2_VEL_DLS_WITH_ORIENTATION,
     IK2_VEL_DLS_WITH_NULLSPACE,
     IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE,
+	IK2_VEL_SDLS,
+	IK2_VEL_SDLS_WITH_ORIENTATION,
 };
 
 

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -3721,6 +3721,14 @@ B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCom
     }
 }
 
+B3_SHARED_API void b3CalculateInverseKinematicsSelectSolver(b3SharedMemoryCommandHandle commandHandle, int solver)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
+	command->m_updateFlags |= solver;
+}
+
 B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
 	int* bodyUniqueId,
 	int* dofCount,

examples/SharedMemory/PhysicsClientC_API.h

@@ -338,6 +338,7 @@ B3_SHARED_API	void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(
 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 b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff);
+B3_SHARED_API	void b3CalculateInverseKinematicsSelectSolver(b3SharedMemoryCommandHandle commandHandle, int solver);
 B3_SHARED_API	int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
 	int* bodyUniqueId,
 	int* dofCount,

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -7731,19 +7731,35 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 									int ikMethod = 0;
 									if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY))
 									{
+										//Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
 										ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
 									}
 									else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)
+									{
+										if (clientCmd.m_updateFlags & IK_SDLS)
+										{
+											ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION;
+										}
+										else
 										{
 											ikMethod = IK2_VEL_DLS_WITH_ORIENTATION;
 										}
+									}
 									else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
 									{
+										//Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
 										ikMethod = IK2_VEL_DLS_WITH_NULLSPACE;
 									}
 									else
 									{
-										ikMethod = IK2_VEL_DLS;
+										if (clientCmd.m_updateFlags & IK_SDLS)
+										{
+											ikMethod = IK2_VEL_SDLS;
+										}
+										else
+										{
+											ikMethod = IK2_VEL_DLS;;
+										}
 									}
                                 
 									if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)

examples/SharedMemory/SharedMemoryCommands.h

@@ -633,15 +633,6 @@ struct CalculateMassMatrixResultArgs
 	int m_dofCount;
 };
 
-enum EnumCalculateInverseKinematicsFlags
-{
-    IK_HAS_TARGET_POSITION=1,
-	IK_HAS_TARGET_ORIENTATION=2,
-    IK_HAS_NULL_SPACE_VELOCITY=4,
-    IK_HAS_JOINT_DAMPING=8,
-    //IK_HAS_CURRENT_JOINT_POSITIONS=16,//not used yet
-};
-
 struct CalculateInverseKinematicsArgs
 {
 	int m_bodyUniqueId;

examples/SharedMemory/SharedMemoryPublic.h

@@ -572,6 +572,18 @@ enum EnumRenderer
     //ER_FIRE_RAYS=(1<<18),
 };
 
+///flags to pick the IK solver and other options
+enum EnumCalculateInverseKinematicsFlags
+{
+	IK_DLS=0,
+	IK_SDLS=1, //TODO: can add other IK solvers
+	IK_HAS_TARGET_POSITION=16,
+	IK_HAS_TARGET_ORIENTATION=32,
+	IK_HAS_NULL_SPACE_VELOCITY=64,
+	IK_HAS_JOINT_DAMPING=128,
+	//IK_HAS_CURRENT_JOINT_POSITIONS=256,//not used yet
+};
+
 enum b3ConfigureDebugVisualizerEnum
 {
     COV_ENABLE_GUI=1,

examples/ThirdPartyLibs/BussIK/Jacobian.cpp

@@ -462,7 +462,8 @@ void Jacobian::CalcDeltaThetasSDLS()
 	// Calculate response vector dTheta that is the SDLS solution.
 	//	Delta target values are the dS values
 	int nRows = J.GetNumRows();
-	int numEndEffectors = m_tree->GetNumEffector();		// Equals the number of rows of J divided by three
+	// TODO: Modify it to work with multiple end effectors.
+	int numEndEffectors = 1;
 	int nCols = J.GetNumColumns();
 	dTheta.SetZero();
 

examples/pybullet/examples/inverse_kinematics.py

@@ -39,8 +39,9 @@ useNullSpace = 0
 useOrientation = 1
 #If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control.
 #This can be used to test the IK result accuracy.
-useSimulation = 0
+useSimulation = 1
 useRealTimeSimulation = 1
+ikSolver = 0
 p.setRealTimeSimulation(useRealTimeSimulation)
 #trailDuration is duration (in seconds) after debug lines will be removed automatically
 #use 0 for no-removal
@@ -68,9 +69,9 @@ while 1:
 				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,jointDamping=jd)
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd,solver=ikSolver)
 			else:
-				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos)
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,solver=ikSolver)
 	
 		if (useSimulation):
 			for i in range (numJoints):

examples/pybullet/examples/inverse_kinematics_pole.py

@@ -0,0 +1,59 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+
+clid = p.connect(p.SHARED_MEMORY)
+if (clid<0):
+	p.connect(p.GUI)
+p.loadURDF("plane.urdf",[0,0,-1.3])
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+sawyerId = p.loadURDF("pole.urdf",[0,0,0])
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
+p.resetBasePositionAndOrientation(sawyerId,[0,0,0],[0,0,0,1])
+
+
+sawyerEndEffectorIndex = 3
+numJoints = p.getNumJoints(sawyerId)
+#joint damping coefficents
+jd=[0.01,0.01,0.01,0.01]
+
+p.setGravity(0,0,0)
+t=0.
+prevPose=[0,0,0]
+prevPose1=[0,0,0]
+hasPrevPose = 0
+
+ikSolver = 0
+useRealTimeSimulation = 0
+p.setRealTimeSimulation(useRealTimeSimulation)
+#trailDuration is duration (in seconds) after debug lines will be removed automatically
+#use 0 for no-removal
+trailDuration = 1
+	
+while 1:
+	if (useRealTimeSimulation):
+		dt = datetime.now()
+		t = (dt.second/60.)*2.*math.pi
+	else:
+		t=t+0.01
+		time.sleep(0.01)
+	
+	for i in range (1):
+		pos = [2.*math.cos(t),2.*math.cos(t),0.+2.*math.sin(t)]
+		jointPoses = p.calculateInverseKinematics(sawyerId,sawyerEndEffectorIndex,pos,jointDamping=jd,solver=ikSolver)
+
+		#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
+		for i in range (numJoints):
+			jointInfo = p.getJointInfo(sawyerId, i)
+			qIndex = jointInfo[3]
+			if qIndex > -1:
+				p.resetJointState(sawyerId,i,jointPoses[qIndex-7])
+
+	ls = p.getLinkState(sawyerId,sawyerEndEffectorIndex)
+	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

@@ -7050,6 +7050,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 	PyObject* targetPosObj = 0;
 	PyObject* targetOrnObj = 0;
 	
+	int solver = 0; // the default IK solver is DLS
 	int physicsClientId = 0;
 	b3PhysicsClientHandle sm = 0;
 	PyObject* lowerLimitsObj = 0;
@@ -7058,8 +7059,8 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 	PyObject* restPosesObj = 0;
 	PyObject* jointDampingObj = 0;
 
-	static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId",  NULL};
+	static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "physicsClientId",  NULL};
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOi", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOii", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, &physicsClientId))
 	{
 		//backward compatibility bodyIndex -> bodyUniqueId. don't update keywords, people need to migrate to bodyUniqueId version
 		static char* kwlist2[] = {"bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId",  NULL};
@@ -7157,6 +7158,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 			int result;
 
 			b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId);
+			b3CalculateInverseKinematicsSelectSolver(command, solver);
 
 			if (hasNullSpace)
 			{
@@ -8188,6 +8190,13 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER);
 	PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL);
 	
+	PyModule_AddIntConstant(m, "IK_DLS", IK_DLS);
+	PyModule_AddIntConstant(m, "IK_SDLS", IK_SDLS);
+	PyModule_AddIntConstant(m, "IK_HAS_TARGET_POSITION", IK_HAS_TARGET_POSITION);
+	PyModule_AddIntConstant(m, "IK_HAS_TARGET_ORIENTATION", IK_HAS_TARGET_ORIENTATION);
+	PyModule_AddIntConstant(m, "IK_HAS_NULL_SPACE_VELOCITY", IK_HAS_NULL_SPACE_VELOCITY);
+	PyModule_AddIntConstant(m, "IK_HAS_JOINT_DAMPING", IK_HAS_JOINT_DAMPING);
+
 	PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE);
 	PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);
 	PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT);