Merge pull request #1029 from YunfeiBai/master

Minor modifications related to gripper.
This commit is contained in:
erwincoumans 2017-03-20 11:03:31 -07:00 committed by GitHub
commit 3532762cdc
2 changed files with 13 additions and 11 deletions

View File

@ -215,6 +215,10 @@
</joint> </joint>
<link name='finger_right'> <link name='finger_right'>
<contact>
<lateral_friction>1.0</lateral_friction>
<spinning_friction>1.5</spinning_friction>
</contact>
<pose frame=''>0.062 0 0.145 0 0 1.5708</pose> <pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
<inertial> <inertial>
<mass>0.2</mass> <mass>0.2</mass>
@ -255,6 +259,10 @@
</joint> </joint>
<link name='finger_left'> <link name='finger_left'>
<contact>
<lateral_friction>1.0</lateral_friction>
<spinning_friction>1.5</spinning_friction>
</contact>
<pose frame=''>-0.062 0 0.145 0 0 4.71239</pose> <pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
<inertial> <inertial>
<mass>0.2</mass> <mass>0.2</mass>

View File

@ -15,14 +15,8 @@ BUTTONS=6
gripper_max_joint = 0.550569 gripper_max_joint = 0.550569
while True: while True:
events = p.getVREvents() events = p.getVREvents()
for e in (events):
for e in (events): if e[CONTROLLER_ID] == 3: # To make sure we only get the value for one of the remotes
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.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, 0, controlMode=p.POSITION_CONTROL, p.setJointMotorControl2(pr2_gripper, 2, controlMode=p.POSITION_CONTROL,targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,force=1.1)
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)