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.)