From 8145203222a1d7603dcd7defaaab238fb342e0e3 Mon Sep 17 00:00:00 2001
From: Julian <yg1811@hotmail.com>
Date: Mon, 13 Mar 2017 22:04:16 -0700
Subject: [PATCH] add 1dof to kuka end effector

---
 examples/pybullet/.DS_Store          | Bin 0 -> 8196 bytes
 examples/pybullet/vr_kuka_control.py |  25 +++++++++++++++++++++----
 2 files changed, 21 insertions(+), 4 deletions(-)
 create mode 100644 examples/pybullet/.DS_Store

diff --git a/examples/pybullet/.DS_Store b/examples/pybullet/.DS_Store
new file mode 100644
index 0000000000000000000000000000000000000000..239940476bbd0d679a427e6eaba1074db1ac5a30
GIT binary patch
literal 8196
zcmeHM!EO^V5FLj~4Q*0V(TWNrv`0>rK-{=Qr9B{lgv0@H3Y&C+s@rVXZrXCog@51|
z_!@qLJ0Ae=jaQ9#*P%Hf3U=k4tut>tex9*Asfd`p>ET0>9T91xQ})*J@HBqT-e|3I
z<UXVUUL=v{awMlxpobP12n+-U0t118z`)<g0PbwI_JVugej1*Efxy6j$pFuX2%WND
z%9)jVbl{;S0OSgWwc#CgfMTLj_DealQlYSEs|Qm;O<7_X3&-aX)gk+(oLOn%Vk}%t
zIkPD%6r-~P7pc0KzLnt_7zhkB8DQ<cAy4In#ByAz-*-{PUr6yR%|^pC8$yq%^#uLF
zPi=S;>B>kl8Eeg-!^RQb3)z*4e5xV8){tBB4#*t%1PH!;lqud%(P@t(9y{hv%}!fB
zVBWDDLYuQ^)p^b5n*OSz-|*7>sA+FPJCPJWnGCop8Ny5cPOm-wwc?tEZT0F5`nj%c
z0G{b*A}8>>!21Z}K1<(fmNtQ$qQ~&$9Jm6$4S=4)r^IJL$oUs%x9<Md0@s(qI@cJ(
z?y=V<Bfzz}q1?Pj1aVdLDuzdk_pXX12V=~5in+;jj_zec_tC|!gTE_hY>`73wbeik
zFyk4#NZ~d2!^@<`g=?qhu|q7s`||B2qr$~^uwJf}>u26Q21-4sMlbWtw4EJX^Tgn&
zhQY2^FdfaT7#WgdjT6kqT=B+!1-I?NEfpio(Tb&eUQ5jMLOoi}Dl)lZ^%mBXAyzY3
zv8Pa>BdKq7%u5fNnfy|tc7*tQgWWjyq7g?GbO$>fBW@CTttVCL=T!Ia!r8x7jj#n#
z!hAKZcK1@7vBEQs8RFYJa^oU<UgQ8aCSE*u%*Efv+H#yO^Am^h`UoQ@STEP@@j^Lv
zt@IjJ=6mkLlVumbruYM$Z_&NN4EgRbd{Z&=JE0Nh%Cd{-Dq@M#MB~~;BB`%&yJ3Cb
zVf}xw`1}8Byu#pmU?4DX6$55v|JD8;w94ALq0d@7LVt_SM!lJp3c*9maY!x4Aus=7
ui1!Gp#Pmx!vl1g{fBcI8TopR8{p%sz|614S&F0>`;r{pheey5e|GxmBpN}K}

literal 0
HcmV?d00001

diff --git a/examples/pybullet/vr_kuka_control.py b/examples/pybullet/vr_kuka_control.py
index d35e8ef45..0117dcb4c 100644
--- a/examples/pybullet/vr_kuka_control.py
+++ b/examples/pybullet/vr_kuka_control.py
@@ -7,6 +7,7 @@ p.connect(p.SHARED_MEMORY)
 kuka = 3
 kuka_gripper = 7
 POSITION = 1
+ORIENTATION = 2
 BUTTONS = 6
 
 THRESHOLD = 1.3
@@ -71,14 +72,30 @@ while True:
 				p.setJointMotorControl2(kuka_gripper, i, p.VELOCITY_CONTROL, targetVelocity=-5, force=50)
 
 		if sq_len < THRESHOLD * THRESHOLD:
+			eef_pos = e[POSITION]
 
-			joint_pos = p.calculateInverseKinematics(kuka, 6, e[POSITION], (0, 1, 0, 0), 
+			joint_pos = p.calculateInverseKinematics(kuka, 6, eef_pos, 
 				lowerLimits=LOWER_LIMITS, upperLimits=UPPER_LIMITS, 
 				jointRanges=JOINT_RANGE, restPoses=REST_POSE, jointDamping=JOINT_DAMP)
-			for i in range(len(joint_pos)):
+
+			# Only need links 1- 5, no need for joint 6 with pure position IK
+			for i in range(len(joint_pos) - 1):
 				p.setJointMotorControl2(kuka, i, p.POSITION_CONTROL, 
-					targetPosition=joint_pos[i], targetVelocity=0, 
-					positionGain=0.6, velocityGain=1.0, force=MAX_FORCE)
+					targetPosition=joint_pos[i], targetVelocity=0, positionGain=0.05, 
+					velocityGain=1.0, force=MAX_FORCE)
+	
+			# Rotate the end effector
+			targetOrn = e[ORIENTATION]
+
+			_, _, z = p.getEulerFromQuaternion(targetOrn)
+			# End effector needs protection, done by using triangular tricks
+			p.setJointMotorControl2(kuka, 6, p.POSITION_CONTROL, 
+				targetPosition=np.arcsin(np.sin(z)), targetVelocity=0, positionGain=0.6, 
+				velocityGain=1.0, force=MAX_FORCE)
+
+			p.setJointMotorControl2(kuka, 5, p.POSITION_CONTROL, 
+				targetPosition=-math.pi, targetVelocity=0, 
+				positionGain=0.03, velocityGain=1.0, force=MAX_FORCE)
 
 		else:
 			# Set back to original rest pose