From 8145203222a1d7603dcd7defaaab238fb342e0e3 Mon Sep 17 00:00:00 2001 From: Julian Date: Mon, 13 Mar 2017 22:04:16 -0700 Subject: [PATCH 1/6] 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 zB>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`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`;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 From fabe09fe535145edb927fbf4aee49f4d70fa2037 Mon Sep 17 00:00:00 2001 From: Julian Date: Mon, 13 Mar 2017 22:04:28 -0700 Subject: [PATCH 2/6] add 1 dof to kuka end effector --- examples/pybullet/.DS_Store | Bin 8196 -> 0 bytes 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 examples/pybullet/.DS_Store diff --git a/examples/pybullet/.DS_Store b/examples/pybullet/.DS_Store deleted file mode 100644 index 239940476bbd0d679a427e6eaba1074db1ac5a30..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 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 zB>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`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`;r{pheey5e|GxmBpN}K} From 7cc31e7c9802040a1fe5359448fc7d12a2b6e133 Mon Sep 17 00:00:00 2001 From: Julian Date: Mon, 13 Mar 2017 22:08:17 -0700 Subject: [PATCH 3/6] fixed minor issues, added 1dof on end effector rotation. gripper is raw --- examples/pybullet/vr_kuka_control.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/examples/pybullet/vr_kuka_control.py b/examples/pybullet/vr_kuka_control.py index 0117dcb4c..ba52b5c18 100644 --- a/examples/pybullet/vr_kuka_control.py +++ b/examples/pybullet/vr_kuka_control.py @@ -35,7 +35,7 @@ while True: for e in (events): # Only use one controller - if e[0] == min(controllers): + if e[0] == controllers[0]: break sq_len = euc_dist(p.getLinkState(kuka, 6)[0], e[POSITION]) @@ -66,7 +66,6 @@ while True: # positionGain=1, velocityGain=0.5, force=50) # avg = p.getJointState(kuka_gripper, i)[0] - if e[BUTTONS][33] & p.VR_BUTTON_WAS_RELEASED: for i in range(p.getNumJoints(kuka_gripper)): p.setJointMotorControl2(kuka_gripper, i, p.VELOCITY_CONTROL, targetVelocity=-5, force=50) @@ -78,8 +77,8 @@ while True: lowerLimits=LOWER_LIMITS, upperLimits=UPPER_LIMITS, jointRanges=JOINT_RANGE, restPoses=REST_POSE, jointDamping=JOINT_DAMP) - # Only need links 1- 5, no need for joint 6 with pure position IK - for i in range(len(joint_pos) - 1): + # Only need links 1- 4, no need for joint 5-6 with pure position IK + for i in range(len(joint_pos) - 2): p.setJointMotorControl2(kuka, i, p.POSITION_CONTROL, targetPosition=joint_pos[i], targetVelocity=0, positionGain=0.05, velocityGain=1.0, force=MAX_FORCE) @@ -90,7 +89,7 @@ while True: _, _, 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, + targetPosition=np.arcsin(np.sin(z)), targetVelocity=0, positionGain=0.5, velocityGain=1.0, force=MAX_FORCE) p.setJointMotorControl2(kuka, 5, p.POSITION_CONTROL, From b686d1274c49e809fddb34904057b61d0218396e Mon Sep 17 00:00:00 2001 From: Julian Date: Mon, 13 Mar 2017 22:09:08 -0700 Subject: [PATCH 4/6] fixed minor issues, added 1dof on end effector rotation. gripper is raw --- examples/pybullet/vr_kuka_control.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/pybullet/vr_kuka_control.py b/examples/pybullet/vr_kuka_control.py index ba52b5c18..dccbf5453 100644 --- a/examples/pybullet/vr_kuka_control.py +++ b/examples/pybullet/vr_kuka_control.py @@ -2,6 +2,8 @@ # Require p.setInternalSimFlags(0) in kuka_setup import pybullet as p import math +import numpy as np + p.connect(p.SHARED_MEMORY) kuka = 3 From bb9601bf643184a86d23547525be36adf9681696 Mon Sep 17 00:00:00 2001 From: Julian Date: Tue, 14 Mar 2017 14:43:08 -0700 Subject: [PATCH 5/6] fixed numpy dependency and gripper --- examples/pybullet/vr_kuka_control.py | 45 +++++++++++----------------- 1 file changed, 18 insertions(+), 27 deletions(-) diff --git a/examples/pybullet/vr_kuka_control.py b/examples/pybullet/vr_kuka_control.py index dccbf5453..08f71d336 100644 --- a/examples/pybullet/vr_kuka_control.py +++ b/examples/pybullet/vr_kuka_control.py @@ -2,7 +2,7 @@ # Require p.setInternalSimFlags(0) in kuka_setup import pybullet as p import math -import numpy as np +# import numpy as np p.connect(p.SHARED_MEMORY) @@ -20,6 +20,8 @@ REST_POSE = [0, 0, 0, math.pi / 2, 0, -math.pi * 0.66, 0] JOINT_DAMP = [.1, .1, .1, .1, .1, .1, .1] REST_JOINT_POS = [-0., -0., 0., 1.570793, 0., -1.036725, 0.000001] MAX_FORCE = 500 +KUKA_GRIPPER_REST_POS = [0., -0.011130, -0.206421, 0.205143, -0.009999, 0., -0.010055, 0.] +KUKA_GRIPPER_CLOZ_POS = [0.0, -0.047564246423083795, 0.6855956234759611, -0.7479294372303137, 0.05054599996976922, 0.0, 0.049838105678835724, 0.0] def euc_dist(posA, posB): dist = 0. @@ -37,40 +39,23 @@ while True: for e in (events): # Only use one controller + ########################################### + # This is important: make sure there's only one VR Controller! if e[0] == controllers[0]: break sq_len = euc_dist(p.getLinkState(kuka, 6)[0], e[POSITION]) # A simplistic version of gripper control + #@TO-DO: Add slider for the gripper if e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED: # avg = 0. for i in range(p.getNumJoints(kuka_gripper)): - p.setJointMotorControl2(kuka_gripper, i, p.VELOCITY_CONTROL, targetVelocity=5, force=50) - # posTarget = 0.1 + (1 - min(0.75, e[3])) * 1.5 * math.pi * 0.29; - # maxPosTarget = 0.55 - # correction = 0. - # jointPosition = p.getJointState(kuka_gripper, i)[0] - # if avg: - # correction = jointPosition - avg - # if jointPosition < 0: - # p.resetJointState(kuka_gripper, i, 0) - # if jointPosition > maxPosTarget: - # p.resetJointState(kuka_gripper, i, maxPosTarget) - # if avg: + p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=KUKA_GRIPPER_CLOZ_POS[i], force=50) - # p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, - # targetPosition=avg, targetVelocity=0., - # positionGain=1, velocityGain=0.5, force=50) - # else: - # p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, - # targetPosition=posTarget, targetVelocity=0., - # positionGain=1, velocityGain=0.5, force=50) - # avg = p.getJointState(kuka_gripper, i)[0] - if e[BUTTONS][33] & p.VR_BUTTON_WAS_RELEASED: for i in range(p.getNumJoints(kuka_gripper)): - p.setJointMotorControl2(kuka_gripper, i, p.VELOCITY_CONTROL, targetVelocity=-5, force=50) + p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=KUKA_GRIPPER_REST_POS[i], force=50) if sq_len < THRESHOLD * THRESHOLD: eef_pos = e[POSITION] @@ -90,12 +75,18 @@ while True: _, _, 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.5, - velocityGain=1.0, force=MAX_FORCE) + + if LOWER_LIMITS[6] < z < UPPER_LIMITS[6]: + p.setJointMotorControl2(kuka, 6, p.POSITION_CONTROL, + targetPosition=z, targetVelocity=0, positionGain=0.03, velocityGain=1.0, force=MAX_FORCE) + + else: + p.setJointMotorControl2(kuka, 6, p.POSITION_CONTROL, + targetPosition=joint_pos[6], targetVelocity=0, positionGain=0.05, + velocityGain=1.0, force=MAX_FORCE) p.setJointMotorControl2(kuka, 5, p.POSITION_CONTROL, - targetPosition=-math.pi, targetVelocity=0, + targetPosition=-1.57, targetVelocity=0, positionGain=0.03, velocityGain=1.0, force=MAX_FORCE) else: From f90986d6f01a32db047dd438b454d08962abedca Mon Sep 17 00:00:00 2001 From: Jie Tan Date: Tue, 14 Mar 2017 15:04:43 -0700 Subject: [PATCH 6/6] add cartpole urdf --- data/cartpole.urdf | 74 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100644 data/cartpole.urdf diff --git a/data/cartpole.urdf b/data/cartpole.urdf new file mode 100644 index 000000000..c22ca920c --- /dev/null +++ b/data/cartpole.urdf @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +