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

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
Erwin Coumans
2017-03-15 11:05:16 -07:00
parent 6e2850e08a 6f171f90dc
commit a613911c84
2 changed files with 111 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
data/cartpole.urdf Normal file
View File

@@ -0,0 +1,74 @@
<?xml version="1.0"?>
<robot name="physics">
<link name="slideBar">
<visual>
<geometry>
<box size="30 0.05 0.05"/>
</geometry>
<origin xyz="0 0 0"/>
<material name="green">
<color rgba="0 0.8 .8 1"/>
</material>
</visual>
<inertial>
<mass value="0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="cart">
<visual>
<geometry>
<box size="0.5 0.5 0.2"/>
</geometry>
<origin xyz="0 0 0"/>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.5 0.5 0.2"/>
</geometry>
<origin xyz="0 0 0"/>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="slider_to_cart" type="prismatic">
<axis xyz="1 0 0"/>
<origin xyz="0.0 0.0 0.0"/>
<parent link="slideBar"/>
<child link="cart"/>
<limit effort="1000.0" lower="-15" upper="15" velocity="5"/>
</joint>
<link name="pole">
<visual>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0.5"/>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="cart_to_pole" type="continuous">
<axis xyz="0 1 0"/>
<origin xyz="0.0 0.0 0"/>
<parent link="cart"/>
<child link="pole"/>
</joint>
</robot>

65
examples/pybullet/vr_kuka_control.py
View File

@@ -2,11 +2,14 @@
# Require p.setInternalSimFlags(0) in kuka_setup
import pybullet as p
import math
# import numpy as np
p.connect(p.SHARED_MEMORY)
kuka = 3
kuka_gripper = 7
POSITION = 1
ORIENTATION = 2
BUTTONS = 6
THRESHOLD = 1.3
@@ -17,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.
@@ -34,51 +39,55 @@ while True:
for e in (events):
# Only use one controller
if e[0] == min(controllers):
###########################################
# 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=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]
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.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]
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- 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.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
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