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

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
bla
2019-06-06 21:54:47 +00:00
parent 6ab09fe06e d7c0c9c017
commit 3e32cf94e7
59 changed files with 135459 additions and 11686 deletions
Download Patch File Download Diff File Expand all files Collapse all files

206
examples/pybullet/CMakeLists.txt
View File

@@ -1,165 +1,21 @@
INCLUDE_DIRECTORIES(
${BULLET_PHYSICS_SOURCE_DIR}/src
${BULLET_PHYSICS_SOURCE_DIR}/examples
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src
${PYTHON_INCLUDE_DIRS}
)
INCLUDE_DIRECTORIES(
${BULLET_PHYSICS_SOURCE_DIR}/src
${BULLET_PHYSICS_SOURCE_DIR}/examples
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src
${PYTHON_INCLUDE_DIRS}
)
IF(BUILD_PYBULLET_NUMPY)
INCLUDE_DIRECTORIES(
${PYTHON_NUMPY_INCLUDE_DIR}
)
INCLUDE_DIRECTORIES(${PYTHON_NUMPY_INCLUDE_DIR})
ENDIF()
ADD_DEFINITIONS(-DSTATIC_LINK_SPD_PLUGIN)
SET(pybullet_SRCS
pybullet.c
../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp
../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h
../../examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp
../../examples/SharedMemory/plugins/stablePDPlugin/Shape.h
../../examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp
../../examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.h
../../examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp
../../examples/SharedMemory/plugins/stablePDPlugin/RBDModel.h
../../examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp
../../examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h
../../examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp
../../examples/SharedMemory/plugins/stablePDPlugin/KinTree.h
../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp
../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h
../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
../../examples/SharedMemory/IKTrajectoryHelper.cpp
../../examples/SharedMemory/IKTrajectoryHelper.h
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.h
../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h
../../examples/OpenGLWindow/SimpleCamera.cpp
../../examples/OpenGLWindow/SimpleCamera.h
../../examples/TinyRenderer/geometry.cpp
../../examples/TinyRenderer/model.cpp
../../examples/TinyRenderer/tgaimage.cpp
../../examples/TinyRenderer/our_gl.cpp
../../examples/TinyRenderer/TinyRenderer.cpp
../../examples/SharedMemory/InProcessMemory.cpp
../../examples/SharedMemory/PhysicsClient.cpp
../../examples/SharedMemory/PhysicsClient.h
../../examples/SharedMemory/PhysicsServer.cpp
../../examples/SharedMemory/PhysicsServer.h
../../examples/SharedMemory/PhysicsServerExample.cpp
../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp
../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.h
../../examples/SharedMemory/PhysicsDirect.cpp
../../examples/SharedMemory/PhysicsDirect.h
../../examples/SharedMemory/PhysicsDirectC_API.cpp
../../examples/SharedMemory/PhysicsDirectC_API.h
../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
../../examples/SharedMemory/PhysicsServerCommandProcessor.h
../../examples/SharedMemory/b3PluginManager.cpp
../../examples/SharedMemory/b3PluginManager.h
../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
../../examples/SharedMemory/PhysicsClientSharedMemory.h
../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp
../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h
../../examples/SharedMemory/PhysicsClientC_API.cpp
../../examples/SharedMemory/PhysicsClientC_API.h
../../examples/SharedMemory/Win32SharedMemory.cpp
../../examples/SharedMemory/Win32SharedMemory.h
../../examples/SharedMemory/PosixSharedMemory.cpp
../../examples/SharedMemory/PosixSharedMemory.h
../../examples/Utils/b3ResourcePath.cpp
../../examples/Utils/b3ResourcePath.h
../../examples/Utils/RobotLoggingUtil.cpp
../../examples/Utils/RobotLoggingUtil.h
../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
../../examples/ThirdPartyLibs/stb_image/stb_image_write.cpp
../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
../../examples/MultiThreading/b3PosixThreadSupport.cpp
../../examples/MultiThreading/b3Win32ThreadSupport.cpp
../../examples/MultiThreading/b3ThreadSupportInterface.cpp
SET(
pybullet_SRCS
pybullet.c
)
IF(BUILD_PYBULLET_CLSOCKET)
ADD_DEFINITIONS(-DBT_ENABLE_CLSOCKET)
ENDIF(BUILD_PYBULLET_CLSOCKET)
IF(WIN32)
LINK_LIBRARIES(
${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
)
IF(BUILD_PYBULLET_ENET)
ADD_DEFINITIONS(-DWIN32 -DBT_ENABLE_ENET)
ENDIF(BUILD_PYBULLET_ENET)
IF(BUILD_PYBULLET_CLSOCKET)
ADD_DEFINITIONS(-DWIN32)
ENDIF(BUILD_PYBULLET_CLSOCKET)
ELSE(WIN32)
IF(BUILD_PYBULLET_ENET)
ADD_DEFINITIONS(-DHAS_SOCKLEN_T -DBT_ENABLE_ENET)
ENDIF(BUILD_PYBULLET_ENET)
IF(BUILD_PYBULLET_CLSOCKET)
ADD_DEFINITIONS(${OSDEF})
ENDIF(BUILD_PYBULLET_CLSOCKET)
ENDIF(WIN32)
IF(BUILD_PYBULLET_ENET)
set(pybullet_SRCS ${pybullet_SRCS}
../../examples/SharedMemory/PhysicsClientUDP.cpp
../../examples/SharedMemory/PhysicsClientUDP_C_API.cpp
../../examples/SharedMemory/PhysicsClientUDP.h
../../examples/SharedMemory/PhysicsClientUDP_C_API.h
../../examples/ThirdPartyLibs/enet/win32.c
../../examples/ThirdPartyLibs/enet/unix.c
../../examples/ThirdPartyLibs/enet/callbacks.c
../../examples/ThirdPartyLibs/enet/compress.c
../../examples/ThirdPartyLibs/enet/host.c
../../examples/ThirdPartyLibs/enet/list.c
../../examples/ThirdPartyLibs/enet/packet.c
../../examples/ThirdPartyLibs/enet/peer.c
../../examples/ThirdPartyLibs/enet/protocol.c
)
ENDIF(BUILD_PYBULLET_ENET)
IF(BUILD_PYBULLET_CLSOCKET)
set(pybullet_SRCS ${pybullet_SRCS}
../../examples/SharedMemory/PhysicsClientTCP.cpp
../../examples/SharedMemory/PhysicsClientTCP.h
../../examples/SharedMemory/PhysicsClientTCP_C_API.cpp
../../examples/SharedMemory/PhysicsClientTCP_C_API.h
../../examples/ThirdPartyLibs/clsocket/src/SimpleSocket.cpp
../../examples/ThirdPartyLibs/clsocket/src/ActiveSocket.cpp
../../examples/ThirdPartyLibs/clsocket/src/PassiveSocket.cpp
)
ENDIF()
ADD_LIBRARY(pybullet SHARED ${pybullet_SRCS})
@@ -170,35 +26,41 @@ SET_TARGET_PROPERTIES(pybullet PROPERTIES VERSION ${BULLET_VERSION})
SET_TARGET_PROPERTIES(pybullet PROPERTIES SOVERSION ${BULLET_VERSION})
SET_TARGET_PROPERTIES(pybullet PROPERTIES DEBUG_POSTFIX "_d")
IF(WIN32)
IF(BUILD_PYBULLET_ENET OR BUILD_PYBULLET_CLSOCKET)
TARGET_LINK_LIBRARIES(pybullet ws2_32 )
ENDIF(BUILD_PYBULLET_ENET OR BUILD_PYBULLET_CLSOCKET)
IF(BUILD_PYBULLET_ENET OR BUILD_PYBULLET_CLSOCKET)
TARGET_LINK_LIBRARIES(pybullet ws2_32)
ENDIF(BUILD_PYBULLET_ENET OR BUILD_PYBULLET_CLSOCKET)
SET_TARGET_PROPERTIES(pybullet PROPERTIES SUFFIX ".pyd" )
SET_TARGET_PROPERTIES(pybullet PROPERTIES SUFFIX ".pyd")
ENDIF(WIN32)
IF (APPLE)
SET_TARGET_PROPERTIES(pybullet PROPERTIES SUFFIX ".so" )
IF(APPLE)
SET_TARGET_PROPERTIES(pybullet PROPERTIES SUFFIX ".so")
ENDIF()
TARGET_LINK_LIBRARIES(
pybullet
bullet_c_api
)
TARGET_LINK_LIBRARIES(pybullet BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK Bullet3Common)
IF (WIN32)
IF(WIN32)
TARGET_LINK_LIBRARIES(pybullet ${PYTHON_LIBRARIES})
ELSEIF (APPLE)
SET_TARGET_PROPERTIES(pybullet PROPERTIES LINK_FLAGS "-undefined dynamic_lookup")
ENDIF ()
ELSEIF(APPLE)
SET_TARGET_PROPERTIES(
pybullet
PROPERTIES LINK_FLAGS "-undefined dynamic_lookup"
)
ENDIF()
# else Linux: dont link
IF(WIN32)
SET(PYTHON_SITE_PACKAGES Lib/site-packages CACHE PATH "Python install path")
ELSE()
SET(PYTHON_SITE_PACKAGES lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/site-packages CACHE PATH "Python install path")
SET(
PYTHON_SITE_PACKAGES
lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/site-packages
CACHE PATH "Python install path"
)
ENDIF()
INSTALL(TARGETS pybullet DESTINATION ${PYTHON_SITE_PACKAGES})

256
examples/pybullet/examples/createObstacleCourse.py
View File

@@ -1,128 +1,128 @@
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)
#a few different ways to create a mesh:
#convex mesh from obj
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 = 10
if (i % 2):
targetVelocity = -10
p.setJointMotorControl2(boxId,
joint,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=100)
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)
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)
#a few different ways to create a mesh:
#convex mesh from obj
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 = 10
if (i % 2):
targetVelocity = -10
p.setJointMotorControl2(boxId,
joint,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=100)
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)

52
examples/pybullet/examples/loadingBench.py
View File

@@ -1,26 +1,26 @@
import pybullet as p
import time
p.connect(p.GUI)
p.resetSimulation()
timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json")
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
print("load plane.urdf")
p.loadURDF("plane.urdf")
print("load r2d2.urdf")
p.loadURDF("r2d2.urdf", 0, 0, 1)
print("load kitchen/1.sdf")
p.loadSDF("kitchens/1.sdf")
print("load 100 times plate.urdf")
for i in range(100):
p.loadURDF("dinnerware/plate.urdf", 0, i, 1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(timinglog)
print("stopped state logging")
p.getCameraImage(320, 200)
while (1):
p.stepSimulation()
import pybullet as p
import time
p.connect(p.GUI)
p.resetSimulation()
timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json")
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
print("load plane.urdf")
p.loadURDF("plane.urdf")
print("load r2d2.urdf")
p.loadURDF("r2d2.urdf", 0, 0, 1)
print("load kitchen/1.sdf")
p.loadSDF("kitchens/1.sdf")
print("load 100 times plate.urdf")
for i in range(100):
p.loadURDF("dinnerware/plate.urdf", 0, i, 1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(timinglog)
print("stopped state logging")
p.getCameraImage(320, 200)
while (1):
p.stepSimulation()

300
examples/pybullet/examples/rendertest.py
View File

@@ -1,150 +1,150 @@
#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled
#otherwise use testrender.py (slower but compatible without numpy)
#you can also use GUI mode, for faster OpenGL rendering (instead of TinyRender CPU)
import os
import sys
import time
import itertools
import subprocess
import numpy as np
import pybullet
from multiprocessing import Process
camTargetPos = [0, 0, 0]
cameraUp = [0, 0, 1]
cameraPos = [1, 1, 1]
pitch = -10.0
roll = 0
upAxisIndex = 2
camDistance = 4
pixelWidth = 84 # 320
pixelHeight = 84 # 200
nearPlane = 0.01
farPlane = 100
fov = 60
import matplotlib.pyplot as plt
class BulletSim():
def __init__(self, connection_mode, *argv):
self.connection_mode = connection_mode
self.argv = argv
def __enter__(self):
print("connecting")
optionstring = '--width={} --height={}'.format(pixelWidth, pixelHeight)
optionstring += ' --window_backend=2 --render_device=0'
print(self.connection_mode, optionstring, *self.argv)
cid = pybullet.connect(self.connection_mode, options=optionstring, *self.argv)
if cid < 0:
raise ValueError
print("connected")
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
pybullet.resetSimulation()
pybullet.loadURDF("plane.urdf", [0, 0, -1])
pybullet.loadURDF("r2d2.urdf")
pybullet.loadURDF("duck_vhacd.urdf")
pybullet.setGravity(0, 0, -10)
def __exit__(self, *_, **__):
pybullet.disconnect()
def test(num_runs=300, shadow=1, log=True, plot=False):
if log:
logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")
if plot:
plt.ion()
img = np.random.rand(200, 320)
#img = [tandard_normal((50,100))
image = plt.imshow(img, interpolation='none', animated=True, label="blah")
ax = plt.gca()
times = np.zeros(num_runs)
yaw_gen = itertools.cycle(range(0, 360, 10))
for i, yaw in zip(range(num_runs), yaw_gen):
pybullet.stepSimulation()
start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
roll, upAxisIndex)
aspect = pixelWidth / pixelHeight
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
img_arr = pybullet.getCameraImage(pixelWidth,
pixelHeight,
viewMatrix,
projectionMatrix,
shadow=shadow,
lightDirection=[1, 1, 1],
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
#renderer=pybullet.ER_TINY_RENDERER)
stop = time.time()
duration = (stop - start)
if (duration):
fps = 1. / duration
#print("fps=",fps)
else:
fps = 0
#print("fps=",fps)
#print("duraction=",duration)
#print("fps=",fps)
times[i] = fps
if plot:
rgb = img_arr[2]
image.set_data(rgb) #np_img_arr)
ax.plot([0])
#plt.draw()
#plt.show()
plt.pause(0.01)
mean_time = float(np.mean(times))
print("mean: {0} for {1} runs".format(mean_time, num_runs))
print("")
if log:
pybullet.stopStateLogging(logId)
return mean_time
if __name__ == "__main__":
res = []
with BulletSim(pybullet.DIRECT):
print("\nTesting DIRECT")
mean_time = test(log=False, plot=True)
res.append(("tiny", mean_time))
with BulletSim(pybullet.DIRECT):
plugin_fn = os.path.join(
pybullet.__file__.split("bullet3")[0],
"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
plugin = pybullet.loadPlugin(plugin_fn, "_tinyRendererPlugin")
if plugin < 0:
print("\nPlugin Failed to load!\n")
sys.exit()
print("\nTesting DIRECT+OpenGL")
mean_time = test(log=True)
res.append(("plugin", mean_time))
with BulletSim(pybullet.GUI):
print("\nTesting GUI")
mean_time = test(log=False)
res.append(("egl", mean_time))
print()
print("rendertest.py")
print("back nenv fps fps_tot")
for r in res:
print(r[0], "\t", 1, round(r[1]), "\t", round(r[1]))
#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled
#otherwise use testrender.py (slower but compatible without numpy)
#you can also use GUI mode, for faster OpenGL rendering (instead of TinyRender CPU)
import os
import sys
import time
import itertools
import subprocess
import numpy as np
import pybullet
from multiprocessing import Process
camTargetPos = [0, 0, 0]
cameraUp = [0, 0, 1]
cameraPos = [1, 1, 1]
pitch = -10.0
roll = 0
upAxisIndex = 2
camDistance = 4
pixelWidth = 84 # 320
pixelHeight = 84 # 200
nearPlane = 0.01
farPlane = 100
fov = 60
import matplotlib.pyplot as plt
class BulletSim():
def __init__(self, connection_mode, *argv):
self.connection_mode = connection_mode
self.argv = argv
def __enter__(self):
print("connecting")
optionstring = '--width={} --height={}'.format(pixelWidth, pixelHeight)
optionstring += ' --window_backend=2 --render_device=0'
print(self.connection_mode, optionstring, *self.argv)
cid = pybullet.connect(self.connection_mode, options=optionstring, *self.argv)
if cid < 0:
raise ValueError
print("connected")
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
pybullet.resetSimulation()
pybullet.loadURDF("plane.urdf", [0, 0, -1])
pybullet.loadURDF("r2d2.urdf")
pybullet.loadURDF("duck_vhacd.urdf")
pybullet.setGravity(0, 0, -10)
def __exit__(self, *_, **__):
pybullet.disconnect()
def test(num_runs=300, shadow=1, log=True, plot=False):
if log:
logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")
if plot:
plt.ion()
img = np.random.rand(200, 320)
#img = [tandard_normal((50,100))
image = plt.imshow(img, interpolation='none', animated=True, label="blah")
ax = plt.gca()
times = np.zeros(num_runs)
yaw_gen = itertools.cycle(range(0, 360, 10))
for i, yaw in zip(range(num_runs), yaw_gen):
pybullet.stepSimulation()
start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
roll, upAxisIndex)
aspect = pixelWidth / pixelHeight
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
img_arr = pybullet.getCameraImage(pixelWidth,
pixelHeight,
viewMatrix,
projectionMatrix,
shadow=shadow,
lightDirection=[1, 1, 1],
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
#renderer=pybullet.ER_TINY_RENDERER)
stop = time.time()
duration = (stop - start)
if (duration):
fps = 1. / duration
#print("fps=",fps)
else:
fps = 0
#print("fps=",fps)
#print("duraction=",duration)
#print("fps=",fps)
times[i] = fps
if plot:
rgb = img_arr[2]
image.set_data(rgb) #np_img_arr)
ax.plot([0])
#plt.draw()
#plt.show()
plt.pause(0.01)
mean_time = float(np.mean(times))
print("mean: {0} for {1} runs".format(mean_time, num_runs))
print("")
if log:
pybullet.stopStateLogging(logId)
return mean_time
if __name__ == "__main__":
res = []
with BulletSim(pybullet.DIRECT):
print("\nTesting DIRECT")
mean_time = test(log=False, plot=True)
res.append(("tiny", mean_time))
with BulletSim(pybullet.DIRECT):
plugin_fn = os.path.join(
pybullet.__file__.split("bullet3")[0],
"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
plugin = pybullet.loadPlugin(plugin_fn, "_tinyRendererPlugin")
if plugin < 0:
print("\nPlugin Failed to load!\n")
sys.exit()
print("\nTesting DIRECT+OpenGL")
mean_time = test(log=True)
res.append(("plugin", mean_time))
with BulletSim(pybullet.GUI):
print("\nTesting GUI")
mean_time = test(log=False)
res.append(("egl", mean_time))
print()
print("rendertest.py")
print("back nenv fps fps_tot")
for r in res:
print(r[0], "\t", 1, round(r[1]), "\t", round(r[1]))

220
examples/pybullet/gym/pybullet_data/laikago/laikago.py
View File

@@ -1,110 +1,110 @@
import pybullet as p
import time
p.connect(p.GUI)
plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
flags=urdfFlags,
useFixedBase=False)
#enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
#2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
joints = []
with open("data1.txt", "r") as filestream:
for line in filestream:
print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints = currentline[2:14]
#print("joints=",joints)
for j in range(12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,
jointIds[j],
p.POSITION_CONTROL,
jointDirections[j] * targetPos + jointOffsets[j],
force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped, -1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1. / 500.)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
js = p.getJointState(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
paramIds.append(
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
(js[0] - jointOffsets[j]) / jointDirections[j]))
p.setRealTimeSimulation(1)
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,
jointIds[i],
p.POSITION_CONTROL,
jointDirections[i] * targetPos + jointOffsets[i],
force=maxForce)
import pybullet as p
import time
p.connect(p.GUI)
plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
flags=urdfFlags,
useFixedBase=False)
#enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
#2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
joints = []
with open("data1.txt", "r") as filestream:
for line in filestream:
print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints = currentline[2:14]
#print("joints=",joints)
for j in range(12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,
jointIds[j],
p.POSITION_CONTROL,
jointDirections[j] * targetPos + jointOffsets[j],
force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped, -1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1. / 500.)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
js = p.getJointState(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
paramIds.append(
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
(js[0] - jointOffsets[j]) / jointDirections[j]))
p.setRealTimeSimulation(1)
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,
jointIds[i],
p.POSITION_CONTROL,
jointDirections[i] * targetPos + jointOffsets[i],
force=maxForce)

10259
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_abad.obj Normal file
View File

File diff suppressed because it is too large Load Diff

37168
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_body.obj Normal file
View File

File diff suppressed because it is too large Load Diff

32070
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_lower_link.obj Normal file
View File

File diff suppressed because it is too large Load Diff

45910
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_upper_link.obj Normal file
View File

File diff suppressed because it is too large Load Diff

480
examples/pybullet/gym/pybullet_data/mini_cheetah/mini_cheetah.urdf Normal file
View File

@@ -0,0 +1,480 @@
<?xml version="1.0" ?>
<robot name="mini_cheetah" xmlns:xacro="http://ros.org/wiki/xacro">
<link name="body">
<inertial>
<mass value="3.3"/>
<origin xyz="0.0 0.0 0.0"/>
<inertia ixx="0.011253" ixy="0" ixz="0" iyy="0.362030" iyz="0" izz="0.042673"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_body.obj"/>
</geometry>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_body.obj"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
</link>
<!--!!!!!!!!!!!! Front Right Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="torso_to_abduct_fr_j" type="continuous">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="0.19 -0.049 0.0"/>
<parent link="body"/>
<child link="abduct_fr"/>
</joint>
<link name="abduct_fr">
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="3.141592 0.0 1.5708" xyz="-0.055 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="3.141592 0 1.5708" xyz="-0.055 0 0"/>
</collision>
</link>
<joint name="abduct_fr_to_thigh_fr_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.062 0.00"/>
<parent link="abduct_fr"/>
<child link="thigh_fr"/>
</joint>
<link name="thigh_fr">
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
</collision>
</link>
<joint name="thigh_fr_to_knee_fr_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="thigh_fr"/>
<child link="shank_fr"/>
</joint>
<link name="shank_fr">
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
</collision>
</link>
<link name="toe_fr">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.3"/>
<lateral_friction value="3.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
<material name="darkgray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
</collision>
<inertial>
<mass value="0.15"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="toe_fr_joint" type="fixed">
<parent link="shank_fr"/>
<child link="toe_fr"/>
<origin xyz="0 0 -0.18"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<!--!!!!!!!!!!!! Front Left Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="torso_to_abduct_fl_j" type="continuous">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="0.19 0.049 0.0"/>
<parent link="body"/>
<child link="abduct_fl"/>
</joint>
<link name="abduct_fl">
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="0. 0. -1.5708" xyz="-0.055 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="0 0 -1.5708" xyz="-0.055 0 0"/>
</collision>
</link>
<joint name="abduct_fl_to_thigh_fl_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.062 0.00"/>
<parent link="abduct_fl"/>
<child link="thigh_fl"/>
</joint>
<link name="thigh_fl">
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
</collision>
</link>
<joint name="thigh_fl_to_knee_fl_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="thigh_fl"/>
<child link="shank_fl"/>
</joint>
<link name="shank_fl">
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
</collision>
</link>
<link name="toe_fl">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.3"/>
<lateral_friction value="3.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
<material name="darkgray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
</collision>
<inertial>
<mass value="0.15"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="toe_fl_joint" type="fixed">
<parent link="shank_fl"/>
<child link="toe_fl"/>
<origin xyz="0 0 -0.18"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<!--!!!!!!!!!!!! Hind Right Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="torso_to_abduct_hr_j" type="continuous">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="-0.19 -0.049 0.0"/>
<parent link="body"/>
<child link="abduct_hr"/>
</joint>
<link name="abduct_hr">
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="0.0 0.0 1.5708" xyz="0.055 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="0 0 1.5708" xyz="0.055 0 0"/>
</collision>
</link>
<joint name="abduct_hr_to_thigh_hr_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.062 0.00"/>
<parent link="abduct_hr"/>
<child link="thigh_hr"/>
</joint>
<link name="thigh_hr">
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
</collision>
</link>
<joint name="thigh_hr_to_knee_hr_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="thigh_hr"/>
<child link="shank_hr"/>
</joint>
<link name="shank_hr">
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
</collision>
</link>
<link name="toe_hr">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.3"/>
<lateral_friction value="3.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
<material name="darkgray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
</collision>
<inertial>
<mass value="0.15"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="toe_hr_joint" type="fixed">
<parent link="shank_hr"/>
<child link="toe_hr"/>
<origin xyz="0 0 -0.18"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<!--!!!!!!!!!!!! Hind Left Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="torso_to_abduct_hl_j" type="continuous">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="-0.19 0.049 0.0"/>
<parent link="body"/>
<child link="abduct_hl"/>
</joint>
<link name="abduct_hl">
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="3.141592 0.0 -1.5708" xyz="0.055 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="3.141592 0 -1.5708" xyz="0.055 0 0"/>
</collision>
</link>
<joint name="abduct_hl_to_thigh_hl_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.062 0.00"/>
<parent link="abduct_hl"/>
<child link="thigh_hl"/>
</joint>
<link name="thigh_hl">
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
</collision>
</link>
<joint name="thigh_hl_to_knee_hl_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="thigh_hl"/>
<child link="shank_hl"/>
</joint>
<link name="shank_hl">
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
</collision>
</link>
<link name="toe_hl">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.3"/>
<lateral_friction value="3.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
<material name="darkgray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
</collision>
<inertial>
<mass value="0.15"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="toe_hl_joint" type="fixed">
<parent link="shank_hl"/>
<child link="toe_hl"/>
<origin xyz="0 0 -0.18"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
</robot>

BIN
examples/pybullet/gym/pybullet_data/quadruped/microtaur/channel.stl
View File

Binary file not shown.

29
examples/pybullet/gym/pybullet_data/quadruped/microtaur/channel.urdf
View File

@@ -1,29 +0,0 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.13"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.019 -0.019 -0.1502"/>
<geometry>
<mesh filename="channel.stl" scale="1.0 1.0 1.0"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.038 0.038 0.305"/>
</geometry>
</collision>
</link>
</robot>

BIN
examples/pybullet/gym/pybullet_data/quadruped/microtaur/d435i.stl
View File

Binary file not shown.

29
examples/pybullet/gym/pybullet_data/quadruped/microtaur/d435i.urdf
View File

@@ -1,29 +0,0 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.081"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0125"/>
<geometry>
<mesh filename="d435i.stl" scale="1.0 1.0 1.0"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.09 0.025 0.025"/>
</geometry>
</collision>
</link>
</robot>

1663
examples/pybullet/gym/pybullet_data/quadruped/microtaur/microtaur.urdf
View File

File diff suppressed because it is too large Load Diff

BIN
examples/pybullet/gym/pybullet_data/quadruped/microtaur/plate.stl
View File

Binary file not shown.

29
examples/pybullet/gym/pybullet_data/quadruped/microtaur/plate.urdf
View File

@@ -1,29 +0,0 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.13"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.037 0.0045 0"/>
<geometry>
<mesh filename="plate.stl" scale="1.0 1.0 1.0"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.074 0.009 0.138"/>
</geometry>
</collision>
</link>
</robot>

22
examples/pybullet/gym/pybullet_data/quadruped/microtaur/quadru.py
View File

@@ -1,22 +0,0 @@
import pybullet as p
cin = p.connect(p.SHARED_MEMORY)
if (cin < 0):
cin = p.connect(p.GUI)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("quadruped/microtaur/microtaur.urdf", 0.858173,-0.698485,0.227967,-0.002864,0.000163,0.951778,0.306776)]
ob = objects[0]
jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.568555, 0.000000, -2.177277, 1.570089, 0.000000, -2.184705, 1.570229, 0.000000, -2.182261, 1.570008, 0.000000, -2.184197, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, -1.569978, 0.000000, 2.184092, -1.569669, 0.000000, 2.186906, -1.570584, 0.000000, 2.181503, -1.568404, 0.000000, 2.178427 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
cid0 = p.createConstraint(1,35,1,32,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
p.changeConstraint(cid0,maxForce=1000.000000)
cid1 = p.createConstraint(1,7,1,10,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
p.changeConstraint(cid1,maxForce=1000.000000)
cid2 = p.createConstraint(1,41,1,38,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
p.changeConstraint(cid2,maxForce=1000.000000)
cid3 = p.createConstraint(1,13,1,16,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
p.changeConstraint(cid3,maxForce=1000.000000)
p.setGravity(0.000000,0.000000,-10.000000)
p.stepSimulation()
p.disconnect()

BIN
examples/pybullet/gym/pybullet_data/quadruped/microtaur/xavier.stl
View File

Binary file not shown.

29
examples/pybullet/gym/pybullet_data/quadruped/microtaur/xavier.urdf
View File

@@ -1,29 +0,0 @@
<?xml version="0.0" ?>
<robot name="xavier">
<link name="baseLink">
<contact>
<lateral_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.64"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.045 -0.045 -0.018"/>
<geometry>
<mesh filename="xavier.stl" scale="1.0 1.0 1.0"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.092 0.105 0.045"/>
</geometry>
</collision>
</link>
</robot>

BIN
examples/pybullet/gym/pybullet_data/quadruped/microtaur/xm430w210.stl
View File

Binary file not shown.

29
examples/pybullet/gym/pybullet_data/quadruped/microtaur/xm430w210.urdf
View File

@@ -1,29 +0,0 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.082"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0.011 0"/>
<geometry>
<mesh filename="xm430w210.stl" scale="1.0 1.0 1.0"/>
</geometry>
<material name="black">
<color rgba=".3 .3 .3 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.024 0.047 0.034"/>
</geometry>
</collision>
</link>
</robot>

2
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/__init__.py
View File

@@ -1 +1 @@
from . import *
from . import *

228
examples/pybullet/gym/pybullet_envs/examples/laikago.py
View File

@@ -1,114 +1,114 @@
import pybullet as p
import pybullet_data as pd
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
flags=urdfFlags,
useFixedBase=False)
#enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
#2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
joints = []
with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
for line in filestream:
print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints = currentline[2:14]
#print("joints=",joints)
for j in range(12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,
jointIds[j],
p.POSITION_CONTROL,
jointDirections[j] * targetPos + jointOffsets[j],
force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped, -1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1. / 500.)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
js = p.getJointState(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
paramIds.append(
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
(js[0] - jointOffsets[j]) / jointDirections[j]))
p.setRealTimeSimulation(1)
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,
jointIds[i],
p.POSITION_CONTROL,
jointDirections[i] * targetPos + jointOffsets[i],
force=maxForce)
import pybullet as p
import pybullet_data as pd
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
flags=urdfFlags,
useFixedBase=False)
#enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
#2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
joints = []
with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
for line in filestream:
print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints = currentline[2:14]
#print("joints=",joints)
for j in range(12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,
jointIds[j],
p.POSITION_CONTROL,
jointDirections[j] * targetPos + jointOffsets[j],
force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped, -1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1. / 500.)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
js = p.getJointState(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
paramIds.append(
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
(js[0] - jointOffsets[j]) / jointDirections[j]))
p.setRealTimeSimulation(1)
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,
jointIds[i],
p.POSITION_CONTROL,
jointDirections[i] * targetPos + jointOffsets[i],
force=maxForce)

467
examples/pybullet/gym/pybullet_envs/examples/microtaur.py
View File

@@ -1,467 +0,0 @@
import pybullet as p
import pybullet_data as pd
import time
import math
def drawInertiaBox(parentUid, parentLinkIndex, color):
return
dyn = p.getDynamicsInfo(parentUid, parentLinkIndex)
mass = dyn[0]
frictionCoeff = dyn[1]
inertia = dyn[2]
if (mass > 0):
Ixx = inertia[0]
Iyy = inertia[1]
Izz = inertia[2]
boxScaleX = 0.5 * math.sqrt(6 * (Izz + Iyy - Ixx) / mass)
boxScaleY = 0.5 * math.sqrt(6 * (Izz + Ixx - Iyy) / mass)
boxScaleZ = 0.5 * math.sqrt(6 * (Ixx + Iyy - Izz) / mass)
halfExtents = [boxScaleX, boxScaleY, boxScaleZ]
pts = [[halfExtents[0], halfExtents[1], halfExtents[2]],
[-halfExtents[0], halfExtents[1], halfExtents[2]],
[halfExtents[0], -halfExtents[1], halfExtents[2]],
[-halfExtents[0], -halfExtents[1], halfExtents[2]],
[halfExtents[0], halfExtents[1], -halfExtents[2]],
[-halfExtents[0], halfExtents[1], -halfExtents[2]],
[halfExtents[0], -halfExtents[1], -halfExtents[2]],
[-halfExtents[0], -halfExtents[1], -halfExtents[2]]]
p.addUserDebugLine(pts[0],
pts[1],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[1],
pts[3],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[3],
pts[2],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[2],
pts[0],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[0],
pts[4],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[1],
pts[5],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[2],
pts[6],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[3],
pts[7],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 0],
pts[4 + 1],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 1],
pts[4 + 3],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 3],
pts[4 + 2],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 2],
pts[4 + 0],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
toeConstraint = True
useMaximalCoordinates = False
useRealTime = 1
#the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance
fixedTimeStep = 1. / 100
numSolverIterations = 50
if (useMaximalCoordinates):
fixedTimeStep = 1. / 500
numSolverIterations = 200
speed = 10
amplitude = 0.8
jump_amp = 0.5
maxForce = 3.5
kneeFrictionForce = 0
kp = 1
kd = .5
maxKneeForce = 1000
physId = p.connect(p.SHARED_MEMORY)
if (physId < 0):
p.connect(p.GUI)
#p.resetSimulation()
p.setAdditionalSearchPath(pd.getDataPath())
angle = 0 # pick in range 0..0.2 radians
orn = p.getQuaternionFromEuler([0, angle, 0])
p.loadURDF("plane.urdf", [0, 0, 0], orn)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
"genericlogdata.bin",
maxLogDof=16,
logFlags=p.STATE_LOG_JOINT_TORQUES)
p.setTimeOut(4000000)
p.setGravity(0, 0, 0)
p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0, 0, 0.4])
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/microtaur/microtaur.urdf", [1, -1, .3],
orn,
useFixedBase=False,
useMaximalCoordinates=useMaximalCoordinates,
flags=p.URDF_USE_IMPLICIT_CYLINDER)
nJoints = p.getNumJoints(quadruped)
jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(quadruped, i)
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
knee_front_rightL_joint = jointNameToId['knee_front_rightL_joint']
hip_front_rightR_joint = jointNameToId['hip_front_rightR_joint']
knee_front_rightR_joint = jointNameToId['knee_front_rightR_joint']
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint']
hip_front_leftR_joint = jointNameToId['hip_front_leftR_joint']
knee_front_leftR_joint = jointNameToId['knee_front_leftR_joint']
motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
knee_front_leftL_joint = jointNameToId['knee_front_leftL_joint']
motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint']
knee_back_rightR_joint = jointNameToId['knee_back_rightR_joint']
motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
knee_back_rightL_joint = jointNameToId['knee_back_rightL_joint']
motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint']
knee_back_leftR_joint = jointNameToId['knee_back_leftR_joint']
motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
knee_back_leftL_joint = jointNameToId['knee_back_leftL_joint']
#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
motordir = [-1, -1, -1, -1, 1, 1, 1, 1]
halfpi = 1.57079632679
twopi = 4 * halfpi
kneeangle = -2.1834
dyn = p.getDynamicsInfo(quadruped, -1)
mass = dyn[0]
friction = dyn[1]
localInertiaDiagonal = dyn[2]
print("localInertiaDiagonal", localInertiaDiagonal)
#this is a no-op, just to show the API
p.changeDynamics(quadruped, -1, localInertiaDiagonal=localInertiaDiagonal)
#for i in range (nJoints):
# p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001])
drawInertiaBox(quadruped, -1, [1, 0, 0])
#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
for i in range(nJoints):
drawInertiaBox(quadruped, i, [0, 1, 0])
if (useMaximalCoordinates):
steps = 400
for aa in range(steps):
p.setJointMotorControl2(quadruped, motor_front_leftL_joint, p.POSITION_CONTROL,
motordir[0] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_front_leftR_joint, p.POSITION_CONTROL,
motordir[1] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_back_leftL_joint, p.POSITION_CONTROL,
motordir[2] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_back_leftR_joint, p.POSITION_CONTROL,
motordir[3] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_front_rightL_joint, p.POSITION_CONTROL,
motordir[4] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_front_rightR_joint, p.POSITION_CONTROL,
motordir[5] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_back_rightL_joint, p.POSITION_CONTROL,
motordir[6] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_back_rightR_joint, p.POSITION_CONTROL,
motordir[7] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_leftL_joint, p.POSITION_CONTROL,
motordir[0] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_leftR_joint, p.POSITION_CONTROL,
motordir[1] * kneeangle * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_leftL_joint, p.POSITION_CONTROL,
motordir[2] * kneeangle * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_leftR_joint, p.POSITION_CONTROL,
motordir[3] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_rightL_joint, p.POSITION_CONTROL,
motordir[4] * (kneeangle) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_rightR_joint, p.POSITION_CONTROL,
motordir[5] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_rightL_joint, p.POSITION_CONTROL,
motordir[6] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_rightR_joint, p.POSITION_CONTROL,
motordir[7] * kneeangle * float(aa) / steps)
p.stepSimulation()
#time.sleep(fixedTimeStep)
else:
p.resetJointState(quadruped, motor_front_leftL_joint, motordir[0] * halfpi)
p.resetJointState(quadruped, knee_front_leftL_joint, motordir[0] * kneeangle)
p.resetJointState(quadruped, motor_front_leftR_joint, motordir[1] * halfpi)
p.resetJointState(quadruped, knee_front_leftR_joint, motordir[1] * kneeangle)
p.resetJointState(quadruped, motor_back_leftL_joint, motordir[2] * halfpi)
p.resetJointState(quadruped, knee_back_leftL_joint, motordir[2] * kneeangle)
p.resetJointState(quadruped, motor_back_leftR_joint, motordir[3] * halfpi)
p.resetJointState(quadruped, knee_back_leftR_joint, motordir[3] * kneeangle)
p.resetJointState(quadruped, motor_front_rightL_joint, motordir[4] * halfpi)
p.resetJointState(quadruped, knee_front_rightL_joint, motordir[4] * kneeangle)
p.resetJointState(quadruped, motor_front_rightR_joint, motordir[5] * halfpi)
p.resetJointState(quadruped, knee_front_rightR_joint, motordir[5] * kneeangle)
p.resetJointState(quadruped, motor_back_rightL_joint, motordir[6] * halfpi)
p.resetJointState(quadruped, knee_back_rightL_joint, motordir[6] * kneeangle)
p.resetJointState(quadruped, motor_back_rightR_joint, motordir[7] * halfpi)
p.resetJointState(quadruped, knee_back_rightR_joint, motordir[7] * kneeangle)
#p.getNumJoints(1)
if (toeConstraint):
cid = p.createConstraint(quadruped, knee_front_leftR_joint, quadruped, knee_front_leftL_joint,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped, knee_front_rightR_joint, quadruped, knee_front_rightL_joint,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped, knee_back_leftR_joint, quadruped, knee_back_leftL_joint,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped, knee_back_rightR_joint, quadruped, knee_back_rightL_joint,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
if (1):
p.setJointMotorControl(quadruped, knee_front_leftL_joint, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_front_leftR_joint, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_front_rightL_joint, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_front_rightR_joint, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftL_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftR_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftL_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftR_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_rightL_joint, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_rightR_joint, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setGravity(0, 0, -10)
legnumbering = [
motor_front_leftL_joint, motor_front_leftR_joint, motor_back_leftL_joint,
motor_back_leftR_joint, motor_front_rightL_joint, motor_front_rightR_joint,
motor_back_rightL_joint, motor_back_rightR_joint
]
for i in range(8):
print(legnumbering[i])
#use the Minitaur leg numbering
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[0],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[0] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[1],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[1] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[2],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[2] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[3],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[3] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[4],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[4] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[5],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[5] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[6],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[6] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[7],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[7] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
#stand still
p.setRealTimeSimulation(useRealTime)
t = 0.0
t_end = t + 5
ref_time = time.time()
while (t < t_end):
p.setGravity(0, 0, -10)
if (useRealTime):
t = time.time() - ref_time
else:
t = t + fixedTimeStep
if (useRealTime == 0):
p.stepSimulation()
time.sleep(fixedTimeStep)
print("quadruped Id = ")
print(quadruped)
p.saveWorld("quadru.py")
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "quadrupedLog.bin", [quadruped])
#jump
t = 0.0
t_end = t + 100
i = 0
ref_time = time.time()
while (1):
if (useRealTime):
t = time.time() - ref_time
else:
t = t + fixedTimeStep
if (True):
target = math.sin(t * speed) * jump_amp + 1.57
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[0],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[0] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[1],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[1] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[2],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[2] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[3],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[3] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[4],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[4] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[5],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[5] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[6],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[6] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[7],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[7] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
if (useRealTime == 0):
p.stepSimulation()
time.sleep(fixedTimeStep)

23
examples/pybullet/gym/pybullet_envs/examples/mini_cheetah_test.py Normal file
View File

@@ -0,0 +1,23 @@
import pybullet as p
import pybullet_data as pd
import time
p.connect(p.GUI)
p.setGravity(0,0,-9.8)
p.setAdditionalSearchPath(pd.getDataPath())
floor = p.loadURDF("plane.urdf")
startPos = [0,0,0.5]
robot = p.loadURDF("mini_cheetah/mini_cheetah.urdf",startPos)
numJoints = p.getNumJoints(robot)
p.changeVisualShape(robot,-1,rgbaColor=[1,1,1,1])
for j in range (numJoints):
p.changeVisualShape(robot,j,rgbaColor=[1,1,1,1])
force=200
pos=0
p.setJointMotorControl2(robot,j,p.POSITION_CONTROL,pos,force=force)
dt = 1./240.
p.setTimeStep(dt)
while (1):
p.stepSimulation()
time.sleep(dt)