From 6f2a7220a56495878b05f7886447e234942ba88b Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 22 Feb 2017 13:52:49 -0800 Subject: [PATCH] Modify controller constraint in the pybullet vr gripper setup. --- examples/pybullet/vr_kuka_pr2_move.py | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/examples/pybullet/vr_kuka_pr2_move.py b/examples/pybullet/vr_kuka_pr2_move.py index e5a44140c..86db524a3 100644 --- a/examples/pybullet/vr_kuka_pr2_move.py +++ b/examples/pybullet/vr_kuka_pr2_move.py @@ -9,14 +9,19 @@ pr2_cid = 1 CONTROLLER_ID = 0 POSITION=1 ORIENTATION=2 +ANALOG=3 BUTTONS=6 while True: events = p.getVREvents() - for e in (events): - if (e[BUTTONS][33]&p.VR_BUTTON_IS_DOWN): - p.changeConstraint(pr2_cid,e[POSITION],e[ORIENTATION], maxForce=50) - #todo - #p.setJointMotorControl2(pr2_gripper,0) + for e in (events): + if e[CONTROLLER_ID] == 3: # To make sure we only get the value for one of the remotes + p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500) + p.setJointMotorControl2(pr2_gripper, 0, controlMode=p.POSITION_CONTROL, + targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint, + force=1.0) + p.setJointMotorControl2(pr2_gripper, 2, controlMode=p.POSITION_CONTROL, + targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint, + force=1.1) \ No newline at end of file