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

Merge pull request #1678 from erwincoumans/master

Browse Source 
PyBullet urdfEditor improvements
This commit is contained in:
erwincoumans
2018-05-09 11:12:21 -07:00
committed by GitHub
parent 57b67faddb a0e72b2ae3
commit 928acbfe62
7 changed files with 248 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2156,9 +2156,9 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
UrdfModel model;// = m_data->m_urdfParser.getModel();
UrdfLink link;
if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]>=0)
if (m_createBodyArgs.m_linkVisualShapeUniqueIds[urdfIndex]>=0)
{
const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[urdfIndex]);
if (visHandle)
{
for (int i=0;i<visHandle->m_visualShapes.size();i++)

50
examples/pybullet/gym/pybullet_utils/bullet_client.py Normal file
View File

@@ -0,0 +1,50 @@
"""A wrapper for pybullet to manage different clients."""
from __future__ import absolute_import
from __future__ import division
import functools
import inspect
import pybullet
class BulletClient(object):
"""A wrapper for pybullet to manage different clients."""
def __init__(self, connection_mode=None):
"""Creates a Bullet client and connects to a simulation.
Args:
connection_mode:
`None` connects to an existing simulation or, if fails, creates a
new headless simulation,
`pybullet.GUI` creates a new simulation with a GUI,
`pybullet.DIRECT` creates a headless simulation,
`pybullet.SHARED_MEMORY` connects to an existing simulation.
"""
self._shapes = {}
if connection_mode is None:
self._client = pybullet.connect(pybullet.SHARED_MEMORY)
if self._client >= 0:
return
else:
connection_mode = pybullet.DIRECT
self._client = pybullet.connect(connection_mode)
def __del__(self):
"""Clean up connection if not already done."""
try:
pybullet.disconnect(physicsClientId=self._client)
except pybullet.error:
pass
def __getattr__(self, name):
"""Inject the client id into Bullet functions."""
attribute = getattr(pybullet, name)
if inspect.isbuiltin(attribute):
if name not in [
"invertTransform", "multiplyTransforms", "getMatrixFromQuaternion",
"getEulerFromQuaternion", "computeViewMatrixFromYawPitchRoll",
"computeProjectionMatrixFOV", "getQuaternionFromEuler",
]: # A temporary hack for now.
attribute = functools.partial(attribute, physicsClientId=self._client)
return attribute

0
examples/pybullet/gym/pybullet_utils/examples/__init__.py Normal file
View File

102
examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py Normal file
View File

@@ -0,0 +1,102 @@
import pybullet_utils.bullet_client as bc
import pybullet_utils.urdfEditor as ed
import pybullet
import pybullet_data
import time
p0 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p0.setAdditionalSearchPath(pybullet_data.getDataPath())
p1 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p1.setAdditionalSearchPath(pybullet_data.getDataPath())
#can also connect using different modes, GUI, SHARED_MEMORY, TCP, UDP, SHARED_MEMORY_SERVER, GUI_SERVER
husky = p1.loadURDF("husky/husky.urdf", flags=p0.URDF_USE_IMPLICIT_CYLINDER)
kuka = p0.loadURDF("kuka_iiwa/model_free_base.urdf", flags=p0.URDF_USE_IMPLICIT_CYLINDER)
ed0 = ed.UrdfEditor()
ed0.initializeFromBulletBody(husky, p1._client)
ed1 = ed.UrdfEditor()
ed1.initializeFromBulletBody(kuka, p0._client)
#ed1.saveUrdf("combined.urdf")
parentLinkIndex = 0
childLinkIndex = len(ed0.urdfLinks)
insertJointIndex = len(ed0.urdfJoints)
#combine all links, and add a joint
for link in ed1.urdfLinks:
ed0.linkNameToIndex[link.link_name]=len(ed0.urdfLinks)
ed0.urdfLinks.append(link)
for joint in ed1.urdfJoints:
ed0.urdfJoints.append(joint)
#add a new joint between a particular
jointPivotXYZInParent = [0.1,0,0.1]
jointPivotRPYInParent = [0,0,0]
jointPivotXYZInChild = [0,0,0]
jointPivotRPYInChild = [0,0,0]
jointPivotQuatInChild = p0.getQuaternionFromEuler(jointPivotRPYInChild)
invJointPivotXYZInChild, invJointPivotQuatInChild = p0.invertTransform(jointPivotXYZInChild,jointPivotQuatInChild)
#apply this invJointPivot***InChild to all inertial, visual and collision element in the child link
#inertial
pos, orn = p0.multiplyTransforms(ed0.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz,p0.getQuaternionFromEuler(ed0.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild)
ed0.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz = pos
ed0.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy = p0.getEulerFromQuaternion(orn)
#all visual
for v in ed0.urdfLinks[childLinkIndex].urdf_visual_shapes:
pos, orn = p0.multiplyTransforms(v.origin_xyz,p0.getQuaternionFromEuler(v.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild)
v.origin_xyz = pos
v.origin_rpy = p0.getEulerFromQuaternion(orn)
#all collision
for c in ed0.urdfLinks[childLinkIndex].urdf_collision_shapes:
pos, orn = p0.multiplyTransforms(c.origin_xyz,p0.getQuaternionFromEuler(c.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild)
c.origin_xyz = pos
c.origin_rpy = p0.getEulerFromQuaternion(orn)
childLink = ed0.urdfLinks[childLinkIndex]
parentLink = ed0.urdfLinks[parentLinkIndex]
joint = ed.UrdfJoint()
joint.link = childLink
joint.joint_name = "joint_dummy1"
joint.joint_type = p0.JOINT_REVOLUTE
joint.joint_lower_limit = 0
joint.joint_upper_limit = -1
joint.parent_name = parentLink.link_name
joint.child_name = childLink.link_name
joint.joint_origin_xyz = jointPivotXYZInParent
joint.joint_origin_rpy = jointPivotRPYInParent
joint.joint_axis_xyz = [0,0,1]
#the following commented line would crash PyBullet, it messes up the joint indexing/ordering
#ed0.urdfJoints.append(joint)
#so make sure to insert the joint in the right place, to links/joints match
ed0.urdfJoints.insert(insertJointIndex,joint)
ed0.saveUrdf("combined.urdf")
print(p0._client)
print(p1._client)
print("p0.getNumBodies()=",p0.getNumBodies())
print("p1.getNumBodies()=",p1.getNumBodies())
pgui = bc.BulletClient(connection_mode=pybullet.GUI)
ed0.createMultiBody([0,0,0],pgui._client)
pgui.setRealTimeSimulation(1)
while (pgui.isConnected()):
pgui.getCameraImage(320,200)
time.sleep(1./240.)

13
examples/pybullet/gym/pybullet_utils/examples/combined.py Normal file
View File

@@ -0,0 +1,13 @@
import pybullet as p
p.connect(p.GUI)
p.loadURDF("combined.urdf", useFixedBase=True)
#for j in range (p.getNumJoints(0)):
# p.setJointMotorControl2(0,j,p.VELOCITY_CONTROL,targetVelocity=0.1)
p.setRealTimeSimulation(1)
while (p.isConnected()):
p.getCameraImage(320,200)
import time
time.sleep(1./240.)

20
examples/pybullet/gym/pybullet_utils/examples/multipleScenes.py Normal file
View File

@@ -0,0 +1,20 @@
import pybullet_utils.bullet_client as bc
import pybullet
import pybullet_data
p0 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p0.setAdditionalSearchPath(pybullet_data.getDataPath())
p1 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p1.setAdditionalSearchPath(pybullet_data.getDataPath())
#can also connect using different modes, GUI, SHARED_MEMORY, TCP, UDP, SHARED_MEMORY_SERVER, GUI_SERVER
#pgui = bc.BulletClient(connection_mode=pybullet.GUI)
p0.loadURDF("r2d2.urdf")
p1.loadSDF("stadium.sdf")
print(p0._client)
print(p1._client)
print("p0.getNumBodies()=",p0.getNumBodies())
print("p1.getNumBodies()=",p1.getNumBodies())

94
examples/pybullet/gym/pybullet_utils/urdfEditor.py
View File

@@ -216,7 +216,9 @@ class UrdfEditor(object):
prec=precision)
file.write(str)
if urdfVisual.geom_type == p.GEOM_MESH:
str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfVisual.geom_meshfilename,\
str = '\t\t\t\t<mesh filename=\"{}\" scale=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(
urdfVisual.geom_meshfilename,urdfVisual.geom_meshscale[0],urdfVisual.geom_meshscale[1],urdfVisual.geom_meshscale[2],\
prec=precision)
file.write(str)
if urdfVisual.geom_type == p.GEOM_CYLINDER:
@@ -253,8 +255,8 @@ class UrdfEditor(object):
prec=precision)
file.write(str)
if urdfCollision.geom_type == p.GEOM_MESH:
str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfCollision.geom_meshfilename,\
prec=precision)
str = '\t\t\t\t<mesh filename=\"{}\"/> scale=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"\n'.format(urdfCollision.geom_meshfilename,\
urdfCollision.geom_meshscale[0],urdfCollision.geom_meshscale[1],urdfCollision.geom_meshscale[2],prec=precision)
file.write(str)
if urdfCollision.geom_type == p.GEOM_CYLINDER:
str = '\t\t\t\t<cylinder length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
@@ -308,6 +310,11 @@ class UrdfEditor(object):
file.write("\t\t<dynamics damping=\"1.0\" friction=\"0.0001\"/>\n")
str = '\t\t<origin xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_xyz[0],\
urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision)
str = '\t\t<origin rpy=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\" xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_rpy[0],\
urdfJoint.joint_origin_rpy[1],urdfJoint.joint_origin_rpy[2],\
urdfJoint.joint_origin_xyz[0],\
urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision)
file.write(str)
str = '\t\t<axis xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_axis_xyz[0],\
urdfJoint.joint_axis_xyz[1],urdfJoint.joint_axis_xyz[2], prec=precision)
@@ -337,6 +344,10 @@ class UrdfEditor(object):
if (len(self.urdfLinks)==0):
return -1
#for i in range (len(self.urdfLinks)):
# print("link", i, "=",self.urdfLinks[i].link_name)
base = self.urdfLinks[0]
#v.tmp_collision_shape_ids=[]
@@ -364,6 +375,7 @@ class UrdfEditor(object):
baseOrientationsArray.append(orn)
if (len(baseShapeTypeArray)):
#print("fileNameArray=",fileNameArray)
baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
radii=baseRadiusArray,
halfExtents=baseHalfExtentsArray,
@@ -376,28 +388,34 @@ class UrdfEditor(object):
urdfVisuals = base.urdf_visual_shapes
baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals],
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals],
radii=[v.geom_radius for v in urdfVisuals],
lengths=[v.geom_length[0] for v in urdfVisuals],
fileNames=[v.geom_meshfilename for v in urdfVisuals],
meshScales=[v.geom_meshscale for v in urdfVisuals],
rgbaColors=[v.material_rgba for v in urdfVisuals],
visualFramePositions=[v.origin_xyz for v in urdfVisuals],
visualFrameOrientations=[v.origin_rpy for v in urdfVisuals],
physicsClientId=physicsClientId)
# urdfVisual = base.urdf_visual_shapes[0]
# baseVisualShapeIndex = p.createVisualShape(shapeType=urdfVisual.geom_type,
# halfExtents=[ext * 0.5 for ext in urdfVisual.geom_extents],
# radius=urdfVisual.geom_radius,
# length=urdfVisual.geom_length[0],
# fileName=urdfVisual.geom_meshfilename,
# meshScale=urdfVisual.geom_meshscale,
# rgbaColor=urdfVisual.material_rgba,
# visualFramePosition=urdfVisual.origin_xyz,
# visualFrameOrientation=urdfVisual.origin_rpy,
# physicsClientId=physicsClientId)
shapeTypes=[v.geom_type for v in urdfVisuals]
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals]
radii=[v.geom_radius for v in urdfVisuals]
lengths=[v.geom_length[0] for v in urdfVisuals]
fileNames=[v.geom_meshfilename for v in urdfVisuals]
meshScales=[v.geom_meshscale for v in urdfVisuals]
rgbaColors=[v.material_rgba for v in urdfVisuals]
visualFramePositions=[v.origin_xyz for v in urdfVisuals]
visualFrameOrientations=[v.origin_rpy for v in urdfVisuals]
baseVisualShapeIndex = -1
if (len(shapeTypes)):
#print("len(shapeTypes)=",len(shapeTypes))
#print("len(halfExtents)=",len(halfExtents))
#print("len(radii)=",len(radii))
#print("len(lengths)=",len(lengths))
#print("len(fileNames)=",len(fileNames))
#print("len(meshScales)=",len(meshScales))
#print("len(rgbaColors)=",len(rgbaColors))
#print("len(visualFramePositions)=",len(visualFramePositions))
#print("len(visualFrameOrientations)=",len(visualFrameOrientations))
print("fileNames=",fileNames)
baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes,
halfExtents=halfExtents,radii=radii,lengths=lengths,fileNames=fileNames,
meshScales=meshScales,rgbaColors=rgbaColors,visualFramePositions=visualFramePositions,
visualFrameOrientations=visualFrameOrientations,physicsClientId=physicsClientId)
linkMasses=[]
linkCollisionShapeIndices=[]
@@ -454,15 +472,25 @@ class UrdfEditor(object):
physicsClientId=physicsClientId)
urdfVisuals = link.urdf_visual_shapes
linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals],
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals],
radii=[v.geom_radius for v in urdfVisuals],
lengths=[v.geom_length[0] for v in urdfVisuals],
fileNames=[v.geom_meshfilename for v in urdfVisuals],
meshScales=[v.geom_meshscale for v in urdfVisuals],
rgbaColors=[v.material_rgba for v in urdfVisuals],
visualFramePositions=[v.origin_xyz for v in urdfVisuals],
visualFrameOrientations=[v.origin_rpy for v in urdfVisuals],
linkVisualShapeIndex = -1
shapeTypes=[v.geom_type for v in urdfVisuals]
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals]
radii=[v.geom_radius for v in urdfVisuals]
lengths=[v.geom_length[0] for v in urdfVisuals]
fileNames=[v.geom_meshfilename for v in urdfVisuals]
meshScales=[v.geom_meshscale for v in urdfVisuals]
rgbaColors=[v.material_rgba for v in urdfVisuals]
visualFramePositions=[v.origin_xyz for v in urdfVisuals]
visualFrameOrientations=[v.origin_rpy for v in urdfVisuals]
if (len(shapeTypes)):
print("fileNames=",fileNames)
linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes,
halfExtents=halfExtents,radii=radii,lengths=lengths,
fileNames=fileNames,meshScales=meshScales,rgbaColors=rgbaColors,
visualFramePositions=visualFramePositions,
visualFrameOrientations=visualFrameOrientations,
physicsClientId=physicsClientId)
linkMasses.append(linkMass)