From b67bccc575c19e37b678a3b6390eb5c09153efc6 Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Thu, 19 Oct 2017 14:30:37 -0700
Subject: [PATCH] Add inverse kinematics example for pole, which has a
 prismatic joint.

---
 .../examples/inverse_kinematics_pole.py       | 59 +++++++++++++++++++
 1 file changed, 59 insertions(+)
 create mode 100755 examples/pybullet/examples/inverse_kinematics_pole.py

diff --git a/examples/pybullet/examples/inverse_kinematics_pole.py b/examples/pybullet/examples/inverse_kinematics_pole.py
new file mode 100755
index 000000000..f910e7033
--- /dev/null
+++ b/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