Convert DOS (CRLF) source files to Unix (LF) line endings
Excluded `examples/pybullet/gym/pybullet_data/` which has many (3000+) CRLF data files (obj, mtl, urdf), and `docs/pybullet_quickstart_guide` which has generated .js and .htm files with CRLF line endings too.
This commit is contained in:
Rémi Verschelde
@@ -1,21 +1,21 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_INCLINED_PLANE_EXAMPLE_H
|
||||
#define ET_INCLINED_PLANE_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_InclinedPlaneCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_INCLINED_PLANE_EXAMPLE_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_INCLINED_PLANE_EXAMPLE_H
|
||||
#define ET_INCLINED_PLANE_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_InclinedPlaneCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_INCLINED_PLANE_EXAMPLE_H
|
||||
|
||||
@@ -1,21 +1,21 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_MULTI_PENDULUM_EXAMPLE_H
|
||||
#define ET_MULTI_PENDULUM_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_MultiPendulumCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_MULTI_PENDULUM_EXAMPLE_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_MULTI_PENDULUM_EXAMPLE_H
|
||||
#define ET_MULTI_PENDULUM_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_MultiPendulumCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_MULTI_PENDULUM_EXAMPLE_H
|
||||
|
||||
@@ -1,21 +1,21 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||
#define ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_NewtonsCradleCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||
#define ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_NewtonsCradleCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_NEWTONS_CRADLE_EXAMPLE_H
|
||||
|
||||
@@ -1,21 +1,21 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||
#define ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_NewtonsRopeCradleCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||
#define ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_NewtonsRopeCradleCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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]))
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -1 +1 @@
|
||||
from . import *
|
||||
from . import *
|
||||
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user