AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-15 14:10:11 +00:00
Code Issues Releases Wiki Activity

fixed numpy dependency and gripper

Browse Source
This commit is contained in:
Julian 2017-03-14 14:43:08 -07:00
parent b686d1274c
commit bb9601bf64
1 changed files with 18 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
examples/pybullet/vr_kuka_control.py
View File

@ -2,7 +2,7 @@
# Require p.setInternalSimFlags(0) in kuka_setup # Require p.setInternalSimFlags(0) in kuka_setup
import pybullet as p import pybullet as p
import math import math
import numpy as np # import numpy as np
p.connect(p.SHARED_MEMORY) 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] JOINT_DAMP = [.1, .1, .1, .1, .1, .1, .1]
REST_JOINT_POS = [-0., -0., 0., 1.570793, 0., -1.036725, 0.000001] REST_JOINT_POS = [-0., -0., 0., 1.570793, 0., -1.036725, 0.000001]
MAX_FORCE = 500 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): def euc_dist(posA, posB):
dist = 0. dist = 0.
@ -37,40 +39,23 @@ while True:
for e in (events): for e in (events):
# Only use one controller # Only use one controller
###########################################
# This is important: make sure there's only one VR Controller!
if e[0] == controllers[0]: if e[0] == controllers[0]:
break break
sq_len = euc_dist(p.getLinkState(kuka, 6)[0], e[POSITION]) sq_len = euc_dist(p.getLinkState(kuka, 6)[0], e[POSITION])
# A simplistic version of gripper control # A simplistic version of gripper control
#@TO-DO: Add slider for the gripper
if e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED: if e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED:
# avg = 0. # avg = 0.
for i in range(p.getNumJoints(kuka_gripper)): 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_CLOZ_POS[i], 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=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: if e[BUTTONS][33] & p.VR_BUTTON_WAS_RELEASED:
for i in range(p.getNumJoints(kuka_gripper)): 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: if sq_len < THRESHOLD * THRESHOLD:
eef_pos = e[POSITION] eef_pos = e[POSITION]
@ -90,12 +75,18 @@ while True:
_, _, z = p.getEulerFromQuaternion(targetOrn) _, _, z = p.getEulerFromQuaternion(targetOrn)
# End effector needs protection, done by using triangular tricks # 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, if LOWER_LIMITS[6] < z < UPPER_LIMITS[6]:
velocityGain=1.0, force=MAX_FORCE) 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, 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) positionGain=0.03, velocityGain=1.0, force=MAX_FORCE)
else: else: