Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

pybullet vr_kuka_setup.py: add a gear joint, to keep the gripper centered,

Browse Source 
vr_kuka_control.py: control all joints, use analogue button to close gripper
remove some debug warnings/prints
pybullet, avoid crash in changeUserConstraint if not passing a [list]
allow some gym environments (pybullet_pendulum and locomotors) to re-use an existing physics client connection.
This commit is contained in:
erwincoumans
2017-11-12 10:36:30 -08:00
parent 99f9584a2c
commit 6be7e34dd6
10 changed files with 101 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
examples/pybullet/examples/vr_kuka_control.py
View File

@@ -10,14 +10,16 @@ kuka = 3
kuka_gripper = 7
POSITION = 1
ORIENTATION = 2
ANALOG=3
BUTTONS = 6
THRESHOLD = 1.3
THRESHOLD = .5
LOWER_LIMITS = [-.967, -2.0, -2.96, 0.19, -2.96, -2.09, -3.05]
UPPER_LIMITS = [.96, 2.0, 2.96, 2.29, 2.96, 2.09, 3.05]
JOINT_RANGE = [5.8, 4, 5.8, 4, 5.8, 4, 6]
REST_POSE = [0, 0, 0, math.pi / 2, 0, -math.pi * 0.66, 0]
JOINT_DAMP = [.1, .1, .1, .1, .1, .1, .1]
JOINT_DAMP = [0.1]*10
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.]
@@ -33,6 +35,8 @@ p.setRealTimeSimulation(1)
controllers = [e[0] for e in p.getVREvents()]
for j in range(p.getNumJoints(kuka_gripper)):
print(p.getJointInfo(kuka_gripper,j))
while True:
events = p.getVREvents()
@@ -48,46 +52,29 @@ while True:
# 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.POSITION_CONTROL, targetPosition=KUKA_GRIPPER_CLOZ_POS[i], force=50)
if e[BUTTONS][33] & p.VR_BUTTON_WAS_RELEASED:
for i in range(p.getNumJoints(kuka_gripper)):
p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=KUKA_GRIPPER_REST_POS[i], force=50)
#for i in range(p.getNumJoints(kuka_gripper)):
i=4
p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=e[ANALOG]*0.05, force=10)
i=6
p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=e[ANALOG]*0.05, force=10)
if sq_len < THRESHOLD * THRESHOLD:
eef_pos = e[POSITION]
joint_pos = p.calculateInverseKinematics(kuka, 6, eef_pos,
eef_orn = p.getQuaternionFromEuler([0,-math.pi,0])
joint_pos = p.calculateInverseKinematics(kuka, 6, eef_pos, eef_orn,
lowerLimits=LOWER_LIMITS, upperLimits=UPPER_LIMITS,
jointRanges=JOINT_RANGE, restPoses=REST_POSE, jointDamping=JOINT_DAMP)
# Only need links 1- 4, no need for joint 5-6 with pure position IK
for i in range(len(joint_pos) - 2):
for i in range(len(joint_pos)):
p.setJointMotorControl2(kuka, i, p.POSITION_CONTROL,
targetPosition=joint_pos[i], targetVelocity=0, positionGain=0.05,
targetPosition=joint_pos[i], targetVelocity=0, positionGain=0.15,
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
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=-1.57, targetVelocity=0,
positionGain=0.03, velocityGain=1.0, force=MAX_FORCE)
else:
# Set back to original rest pose

4
examples/pybullet/examples/vr_kuka_setup.py
View File

@@ -48,6 +48,10 @@ for jointIndex in range (p.getNumJoints(kuka_gripper)):
kuka_cid = p.createConstraint(kuka, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0])
pr2_cid2 = p.createConstraint(kuka_gripper,4,kuka_gripper,6,jointType=p.JOINT_GEAR,jointAxis =[1,1,1],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(pr2_cid2,gearRatio=-1, erp=0.5, relativePositionTarget=0, maxForce=100)
objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]