58 lines
1.6 KiB
Python
58 lines
1.6 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.)
|