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)]

31
examples/pybullet/gym/pybullet_envs/env_bases.py
View File

@@ -18,6 +18,7 @@ class MJCFBaseBulletEnv(gym.Env):
def __init__(self, robot, render=False):
self.scene = None
self.physicsClientId=-1
self.ownsPhysicsClient = 0
self.camera = Camera()
self.isRender = render
self.robot = robot
@@ -39,17 +40,24 @@ class MJCFBaseBulletEnv(gym.Env):
def _reset(self):
if (self.physicsClientId<0):
self.physicsClientId = p.connect(p.SHARED_MEMORY)
if (self.physicsClientId<0):
if (self.isRender):
self.physicsClientId = p.connect(p.GUI)
else:
self.physicsClientId = p.connect(p.DIRECT)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
conInfo = p.getConnectionInfo()
if (conInfo['isConnected']):
self.ownsPhysicsClient = False
self.physicsClientId = 0
else:
self.ownsPhysicsClient = True
self.physicsClientId = p.connect(p.SHARED_MEMORY)
if (self.physicsClientId<0):
if (self.isRender):
self.physicsClientId = p.connect(p.GUI)
else:
self.physicsClientId = p.connect(p.DIRECT)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
if self.scene is None:
self.scene = self.create_single_player_scene()
if not self.scene.multiplayer:
if not self.scene.multiplayer and self.ownsPhysicsClient:
self.scene.episode_restart()
self.robot.scene = self.scene
@@ -93,9 +101,10 @@ class MJCFBaseBulletEnv(gym.Env):
return rgb_array
def _close(self):
if (self.physicsClientId>=0):
p.disconnect(self.physicsClientId)
self.physicsClientId = -1
if (self.ownsPhysicsClient):
if (self.physicsClientId>=0):
p.disconnect(self.physicsClientId)
self.physicsClientId = -1
def HUD(self, state, a, done):
pass

13
examples/pybullet/gym/pybullet_envs/robot_bases.py
View File

@@ -17,6 +17,7 @@ class MJCFBasedRobot:
def __init__(self, model_xml, robot_name, action_dim, obs_dim):
self.parts = None
self.objects = []
self.jdict = None
self.ordered_joints = None
self.robot_body = None
@@ -85,13 +86,15 @@ class MJCFBasedRobot:
def reset(self):
self.ordered_joints = []
for ob in self.objects:
p.removeBody(ob)
if self.self_collision:
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml), flags=p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS))
self.objects = p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml), flags=p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self.objects )
else:
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml)))
self.objects = p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml))
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self.objects)
self.robot_specific_reset()

2
examples/pybullet/gym/pybullet_envs/scene_abstract.py
View File

@@ -62,7 +62,7 @@ class World:
self.clean_everything()
def clean_everything(self):
p.resetSimulation()
#p.resetSimulation()
p.setGravity(0, 0, -self.gravity)
p.setDefaultContactERP(0.9)
p.setPhysicsEngineParameter(fixedTimeStep=self.timestep*self.frame_skip, numSolverIterations=5, numSubSteps=self.frame_skip)

21
examples/pybullet/pybullet.c
View File

@@ -200,17 +200,20 @@ static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4])
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
len = PySequence_Size(obVec);
if (len == 4)
if (seq)
{
for (i = 0; i < len; i++)
len = PySequence_Size(obVec);
if (len == 4)
{
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
for (i = 0; i < len; i++)
{
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
return 0;
}
@@ -5155,10 +5158,16 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P
if (pybullet_internalSetVectord(jointChildPivotObj, jointChildPivot))
{
b3InitChangeUserConstraintSetPivotInB(commandHandle, jointChildPivot);
} else
{
return NULL;
}
if (pybullet_internalSetVector4d(jointChildFrameOrnObj, jointChildFrameOrn))
{
b3InitChangeUserConstraintSetFrameInB(commandHandle, jointChildFrameOrn);
} else
{
return NULL;
}
if (relativePositionTarget<1e10)