55 lines
1.7 KiB
Python
55 lines
1.7 KiB
Python
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.urdf")
|
|
|
|
ed0 = ed.UrdfEditor()
|
|
ed0.initializeFromBulletBody(husky, p1._client)
|
|
ed1 = ed.UrdfEditor()
|
|
ed1.initializeFromBulletBody(kuka, p0._client)
|
|
#ed1.saveUrdf("combined.urdf")
|
|
|
|
parentLinkIndex = 0
|
|
|
|
jointPivotXYZInParent = [0, 0, 0]
|
|
jointPivotRPYInParent = [0, 0, 0]
|
|
|
|
jointPivotXYZInChild = [0, 0, 0]
|
|
jointPivotRPYInChild = [0, 0, 0]
|
|
|
|
newjoint = ed0.joinUrdf(ed1, parentLinkIndex, jointPivotXYZInParent, jointPivotRPYInParent,
|
|
jointPivotXYZInChild, jointPivotRPYInChild, p0._client, p1._client)
|
|
newjoint.joint_type = p0.JOINT_FIXED
|
|
|
|
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)
|
|
pgui.configureDebugVisualizer(pgui.COV_ENABLE_RENDERING, 0)
|
|
|
|
orn = [0, 0, 0, 1]
|
|
ed0.createMultiBody([0, 0, 0], orn, pgui._client)
|
|
pgui.setRealTimeSimulation(1)
|
|
|
|
pgui.configureDebugVisualizer(pgui.COV_ENABLE_RENDERING, 1)
|
|
|
|
while (pgui.isConnected()):
|
|
pgui.getCameraImage(320, 200, renderer=pgui.ER_BULLET_HARDWARE_OPENGL)
|
|
time.sleep(1. / 240.)
|