add 'createObstacleCourse.py' example, helps reproducing
Parkour paper: https://arxiv.org/abs/1707.02286
This commit is contained in:
10
data/stone.mtl
Normal file
10
data/stone.mtl
Normal file
@@ -0,0 +1,10 @@
|
|||||||
|
# Blender MTL File: 'None'
|
||||||
|
# Material Count: 1
|
||||||
|
|
||||||
|
newmtl None
|
||||||
|
Ns 0
|
||||||
|
Ka 0.000000 0.000000 0.000000
|
||||||
|
Kd 0.8 0.8 0.8
|
||||||
|
Ks 0.8 0.8 0.8
|
||||||
|
d 1
|
||||||
|
illum 2
|
||||||
32
data/stone.obj
Normal file
32
data/stone.obj
Normal file
@@ -0,0 +1,32 @@
|
|||||||
|
# Blender v2.78 (sub 0) OBJ File: ''
|
||||||
|
# www.blender.org
|
||||||
|
mtllib stone.mtl
|
||||||
|
o Cube
|
||||||
|
v -0.246350 -0.246483 -0.000624
|
||||||
|
v -0.151407 -0.176325 0.172867
|
||||||
|
v -0.246350 0.249205 -0.000624
|
||||||
|
v -0.151407 0.129477 0.172867
|
||||||
|
v 0.249338 -0.246483 -0.000624
|
||||||
|
v 0.154395 -0.176325 0.172867
|
||||||
|
v 0.249338 0.249205 -0.000624
|
||||||
|
v 0.154395 0.129477 0.172867
|
||||||
|
vn -0.8772 0.0000 0.4801
|
||||||
|
vn 0.0000 0.8230 0.5680
|
||||||
|
vn 0.8772 0.0000 0.4801
|
||||||
|
vn 0.0000 -0.9271 0.3749
|
||||||
|
vn 0.0000 0.0000 -1.0000
|
||||||
|
vn 0.0000 0.0000 1.0000
|
||||||
|
usemtl None
|
||||||
|
s off
|
||||||
|
f 1//1 4//1 3//1
|
||||||
|
f 4//2 7//2 3//2
|
||||||
|
f 8//3 5//3 7//3
|
||||||
|
f 6//4 1//4 5//4
|
||||||
|
f 7//5 1//5 3//5
|
||||||
|
f 4//6 6//6 8//6
|
||||||
|
f 1//1 2//1 4//1
|
||||||
|
f 4//2 8//2 7//2
|
||||||
|
f 8//3 6//3 5//3
|
||||||
|
f 6//4 2//4 1//4
|
||||||
|
f 7//5 5//5 1//5
|
||||||
|
f 4//6 2//6 6//6
|
||||||
93
examples/pybullet/examples/createObstacleCourse.py
Normal file
93
examples/pybullet/examples/createObstacleCourse.py
Normal file
@@ -0,0 +1,93 @@
|
|||||||
|
import pybullet as p
|
||||||
|
import time
|
||||||
|
import math
|
||||||
|
|
||||||
|
p.connect(p.GUI)
|
||||||
|
#don't create a ground plane, to allow for gaps etc
|
||||||
|
p.resetSimulation()
|
||||||
|
#p.createCollisionShape(p.GEOM_PLANE)
|
||||||
|
#p.createMultiBody(0,0)
|
||||||
|
#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
|
||||||
|
p.resetDebugVisualizerCamera(15,-346,-16,[-15,0,1]);
|
||||||
|
|
||||||
|
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
||||||
|
|
||||||
|
sphereRadius = 0.05
|
||||||
|
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
|
||||||
|
stoneId = p.createCollisionShape(p.GEOM_MESH,fileName="stone.obj")
|
||||||
|
|
||||||
|
boxHalfLength = 0.5
|
||||||
|
boxHalfWidth = 2.5
|
||||||
|
boxHalfHeight = 0.1
|
||||||
|
segmentLength = 5
|
||||||
|
|
||||||
|
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight])
|
||||||
|
|
||||||
|
mass = 1
|
||||||
|
visualShapeId = -1
|
||||||
|
|
||||||
|
segmentStart = 0
|
||||||
|
|
||||||
|
for i in range (segmentLength):
|
||||||
|
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
|
||||||
|
segmentStart=segmentStart-1
|
||||||
|
|
||||||
|
for i in range (segmentLength):
|
||||||
|
height = 0
|
||||||
|
if (i%2):
|
||||||
|
height=1
|
||||||
|
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1+height])
|
||||||
|
segmentStart=segmentStart-1
|
||||||
|
|
||||||
|
baseOrientation = p.getQuaternionFromEuler([math.pi/2.,0,math.pi/2.])
|
||||||
|
|
||||||
|
for i in range (segmentLength):
|
||||||
|
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
|
||||||
|
segmentStart=segmentStart-1
|
||||||
|
if (i%2):
|
||||||
|
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,i%3,-0.1+height+boxHalfWidth],baseOrientation=baseOrientation)
|
||||||
|
|
||||||
|
for i in range (segmentLength):
|
||||||
|
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
|
||||||
|
width=4
|
||||||
|
for j in range (width):
|
||||||
|
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = stoneId,basePosition = [segmentStart,0.5*(i%2)+j-width/2.,0])
|
||||||
|
segmentStart=segmentStart-1
|
||||||
|
|
||||||
|
|
||||||
|
link_Masses=[1]
|
||||||
|
linkCollisionShapeIndices=[colBoxId]
|
||||||
|
linkVisualShapeIndices=[-1]
|
||||||
|
linkPositions=[[0,0,0]]
|
||||||
|
linkOrientations=[[0,0,0,1]]
|
||||||
|
linkInertialFramePositions=[[0,0,0]]
|
||||||
|
linkInertialFrameOrientations=[[0,0,0,1]]
|
||||||
|
indices=[0]
|
||||||
|
jointTypes=[p.JOINT_REVOLUTE]
|
||||||
|
axis=[[1,0,0]]
|
||||||
|
|
||||||
|
baseOrientation = [0,0,0,1]
|
||||||
|
for i in range (segmentLength):
|
||||||
|
boxId = p.createMultiBody(0,colSphereId,-1,[segmentStart,0,-0.1],baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis)
|
||||||
|
p.changeDynamics(boxId,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
|
||||||
|
print(p.getNumJoints(boxId))
|
||||||
|
for joint in range (p.getNumJoints(boxId)):
|
||||||
|
targetVelocity = 1
|
||||||
|
if (i%2):
|
||||||
|
targetVelocity =-1
|
||||||
|
p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=10)
|
||||||
|
segmentStart=segmentStart-1.1
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
|
||||||
|
while (1):
|
||||||
|
camData = p.getDebugVisualizerCamera()
|
||||||
|
viewMat = camData[2]
|
||||||
|
projMat = camData[3]
|
||||||
|
p.getCameraImage(256,256,viewMatrix=viewMat, projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL)
|
||||||
|
keys = p.getKeyboardEvents()
|
||||||
|
p.stepSimulation()
|
||||||
|
#print(keys)
|
||||||
|
time.sleep(0.01)
|
||||||
|
|
||||||
Reference in New Issue
Block a user