def _load_with_tool(self, tool):
p0 = bc.BulletClient(connection_mode=p.DIRECT)
p1 = bc.BulletClient(connection_mode=p.DIRECT)
kuka = p0.loadURDF(get_resource_path('kuka_iiwa_insertion','robot', 'urdf', 'iiwa14.urdf'))
if tool is "square":
tool = p1.loadURDF(get_resource_path('kuka_iiwa_insertion','models', 'square10x10', 'tool9x9.urdf'))
elif tool is "triangular":
tool = p1.loadURDF(get_resource_path('kuka_iiwa_insertion','models', 'triangle10x10', 'TriangleTool9x9.urdf'))
elif tool is "zylindric":
tool = p1.loadURDF(get_resource_path('kuka_iiwa_insertion','models', 'zylinder10x10', 'ZylinderTool9x9.urdf'))
else:
raise NotImplementedError("This tool has not been implemented")
ed0 = ed.UrdfEditor()
ed0.initializeFromBulletBody(kuka, p0._client)
ed1 = ed.UrdfEditor()
ed1.initializeFromBulletBody(tool, p1._client)
jointPivotXYZInParent = [0, 0, 0.026]
jointPivotRPYInParent = [0, 0, 0]
jointPivotXYZInChild = [0, 0, 0]
jointPivotRPYInChild = [0, 0, 0]
newjoint = ed0.joinUrdf(ed1, self.ee_index, jointPivotXYZInParent, jointPivotRPYInParent,
jointPivotXYZInChild, jointPivotRPYInChild, p0._client, p1._client)
newjoint.joint_type = p.JOINT_FIXED
self.kuka_uid = ed0.createMultiBody([0, 0, 0], [0,0,0,1], self.client)
def convert_mjcf_to_urdf(input_mjcf, output_path):
"""Convert MuJoCo mjcf to URDF format and save.
Parameters
----------
input_mjcf : str
input path of mjcf file.
output_path : str
output directory path of urdf.
"""
client = bulllet_client.BulletClient()
objs = client.loadMJCF(
input_mjcf, flags=client.URDF_USE_IMPLICIT_CYLINDER)
# create output directory
mjcf2urdf.makedirs(output_path)
for obj in objs:
humanoid = objs[obj]
ue = urdfEditor.UrdfEditor()
ue.initializeFromBulletBody(humanoid, client._client)
robot_name = str(client.getBodyInfo(obj)[1], 'utf-8')
part_name = str(client.getBodyInfo(obj)[0], 'utf-8')
save_visuals = False
outpath = osp.join(
output_path, "{}_{}.urdf".format(robot_name, part_name))
ue.saveUrdf(outpath, save_visuals)
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]
cubeStartPos2 = [0,0,1.9]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
window = p0.loadURDF("models/window/window.urdf", cubeStartPos2,p.getQuaternionFromEuler([0,0,1.571]),useFixedBase = 0)
robot2 = p2.loadURDF("models/actuatorBot/actuatorFixedTop1.urdf",cubeStartPos4,
cubeStartOrientation,useFixedBase = 0)
robot3 = p1.loadURDF("models/actuatorBot/actuatorFixedTop.urdf",cubeStartPos5,
cubeStartOrientation,useFixedBase = 0)
editor = ed.UrdfEditor()
editor1 = ed.UrdfEditor()
editor2 = ed.UrdfEditor()
editor.initializeFromBulletBody(robot3, p1._client)
editor2.initializeFromBulletBody(robot2, p2._client)
editor1.initializeFromBulletBody(window, p0._client)
parentLinkIndex = 0
jointPivotXYZInParent = [0, 0, 0]
jointPivotRPYInParent = [0, 0, 0]
jointPivotXYZInChild = [0.25,0.0,0.005+startZ+1.45]
jointPivotRPYInChild = [0, 0, 0]
newJ = editor1.joinUrdf(editor,parentLinkIndex, jointPivotXYZInParent, jointPivotRPYInParent,
for i in range(p.getNumJoints(mb, physicsClientId=org)):
p.setJointMotorControl2(mb,
i,
p.VELOCITY_CONTROL,
force=0,
physicsClientId=org)
#print("numJoints:",p.getNumJoints(mb,physicsClientId=org))
#print("base name:",p.getBodyInfo(mb,physicsClientId=org))
#for i in range(p.getNumJoints(mb,physicsClientId=org)):
# print("jointInfo(",i,"):",p.getJointInfo(mb,i,physicsClientId=org))
# print("linkState(",i,"):",p.getLinkState(mb,i,physicsClientId=org))
parser = urdfEditor.UrdfEditor()
parser.initializeFromBulletBody(mb, physicsClientId=org)
parser.saveUrdf("test.urdf")
if (1):
print("\ncreatingMultiBody...................n")
obUid = parser.createMultiBody(physicsClientId=gui)
parser2 = urdfEditor.UrdfEditor()
print("\n###########################################\n")
parser2.initializeFromBulletBody(obUid, physicsClientId=gui)
parser2.saveUrdf("test2.urdf")
for i in range(p.getNumJoints(obUid, physicsClientId=gui)):
from pybullet_utils import bullet_client as bc
import pybullet_data as pd
import pybullet_utils.urdfEditor as ed
import argparse
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--mjcf',
help='MuJoCo xml file to be converted to URDF',
default='mjcf/humanoid.xml')
args = parser.parse_args()
p = bc.BulletClient()
p.setAdditionalSearchPath(pd.getDataPath())
objs = p.loadMJCF(args.mjcf, flags=p.URDF_USE_IMPLICIT_CYLINDER)
for o in objs:
#print("o=",o, p.getBodyInfo(o), p.getNumJoints(o))
humanoid = objs[o]
ed0 = ed.UrdfEditor()
ed0.initializeFromBulletBody(humanoid, p._client)
robotName = str(p.getBodyInfo(o)[1], 'utf-8')
partName = str(p.getBodyInfo(o)[0], 'utf-8')
print("robotName=", robotName)
print("partName=", partName)
saveVisuals = False
ed0.saveUrdf(robotName + "_" + partName + ".urdf", saveVisuals)
import meshcat_utils_pb
vis = meshcat.Visualizer(zmq_url='tcp://127.0.0.1:6000')
# Or don"t use this line to preserve state of visualization upon
# running script.
vis.delete()
p0 = bc.BulletClient(connection_mode=1) #pybullet.GUI)#DIRECT)
p0.configureDebugVisualizer(p0.COV_ENABLE_RENDERING, 0)
p0.setAdditionalSearchPath(pd.getDataPath())
flags = p0.URDF_MAINTAIN_LINK_ORDER
laikago = p0.loadURDF("plane_implicit.urdf", flags=flags)
start_pos = [0, 0, 0.6]
laikago = p0.loadURDF("laikago/laikago_toes_zup.urdf", start_pos, flags=flags)
p0.configureDebugVisualizer(p0.COV_ENABLE_RENDERING, 1)
med0 = ued.UrdfEditor()
med0.initializeFromBulletBody(laikago, p0._client)
texture_path = os.path.join(pd.getDataPath(), 'laikago/laikago_tex.jpg')
b2vis = meshcat_utils_pb.convert_visuals_pb(vis, med0.urdfLinks,
med0.urdfJoints, p0, texture_path)
p0.setGravity(0, 0, -10)
while p0.isConnected():
p0.stepSimulation()
meshcat_utils_pb.sync_visual_transforms_pb(b2vis, laikago, p0, vis)
time.sleep(1. / 240.)
