jointCopyExceptions = ["limit", "safety_controller"] for armJointName in ["arm_shx", "arm_ely", "arm_elx", "arm_uwy", "arm_mwx", "arm_lwy"]: mit.copyJointProperties(urdf, "r_" + armJointName, "l_" + armJointName, jointCopyExceptions) mit.copyJointProperties(urdf, "r_arm_shz", "l_arm_shz", jointCopyExceptions + ["origin"]) for jointName in ["arm_shx", "arm_ely", "arm_elx", "arm_uwy"]: # , 'arm_lwy']: mit.invertJointAxis(urdf, "l_" + jointName) mit.setJointOriginRPY(urdf, "l_arm_shz", [0, 0, math.pi]) mit.setJointOriginRPY(urdf, "l_arm_uwy", [0, math.pi, 0]) mit.setJointOriginRPY(urdf, "l_arm_lwy", [0, math.pi, 0]) urdf.write(full_mesh_urdf_path, pretty_print=True) # Create minimal contact skeleton mit.removeAllCollisions(urdf) minimal_contact_urdf = copy.deepcopy(urdf) contact_pts = { "r_foot": { "heel": ["-0.0876 0.0626 -0.07645", "-0.0876 -0.066 -0.07645"], "toe": ["0.1728 0.0626 -0.07645", "0.1728 -0.066 -0.07645"], "midfoot_front": ["0.086 0.0626 -0.07645", "0.086 -0.066 -0.07645"], "midfoot_rear": ["-0.0008 0.0626 -0.07645", "-0.0008 -0.066 -0.07645"], }, "l_foot": { "heel": ["-0.0876 0.066 -0.07645", "-0.0876 -0.0626 -0.07645"], "toe": ["0.1728 0.066 -0.07645", "0.1728 -0.0626 -0.07645"], "midfoot_front": ["0.086 0.066 -0.07645", "0.086 -0.0626 -0.07645"], "midfoot_rear": ["-0.008 0.066 -0.07645", "-0.008 -0.0626 -0.07645"],
# Load urdf urdf = etree.parse(tmp.name) # Use .obj meshes mit.useObjMeshes(urdf) # Use convex hull meshes for collisions mit.useConvexHullMeshes(urdf) # Generate full urdf urdf.write(urdf_path, pretty_print=True) # Generate no-collision urdf no_collision_urdf = copy.deepcopy(urdf) mit.removeAllCollisions(no_collision_urdf) no_collision_urdf.write(no_collision_urdf_path, pretty_print=True) # Generate no-joint urdf for joint in urdf.findall("//joint"): joint.set("type", "fixed") urdf.write(no_joint_urdf_path, pretty_print=True) # Generate no-joint, no-collision urdf mit.removeAllCollisions(urdf) urdf.write(no_joint_no_collision_urdf_path, pretty_print=True) # Copy over convex-hull hand shutil.copy("robotiq_hand_convex_hull.xacro", "../")
mit.setJointLimits(urdf, 'l_leg_hpx', -0.23, 0.52) mit.setJointLimits(urdf, 'r_leg_hpx', -0.52, 0.23) mit.setJointLimits(urdf, 'l_leg_hpz', -0.2, 0.82) mit.setJointLimits(urdf, 'r_leg_hpz', -0.82, 0.2) # arms mit.setJointLimits(urdf, 'l_arm_uwy', -2.98, 2.98) mit.setJointLimits(urdf, 'r_arm_uwy', -2.98, 2.98) mit.setJointLimits(urdf, 'l_arm_mwx', -1.76, 1.76) mit.setJointLimits(urdf, 'r_arm_mwx', -1.76, 1.76) mit.setJointLimits(urdf, 'l_arm_lwy', -2.82, 2.82) mit.setJointLimits(urdf, 'r_arm_lwy', -2.82, 2.82) # neck mit.setJointLimits(urdf, 'neck_ay', -0.605, 1.16) # Create minimal contact skeleton mit.removeAllCollisions(urdf) minimal_contact_urdf = copy.deepcopy(urdf) contact_pts = { 'r_foot': { 'heel': ["-0.0876 0.0626 -0.07645", "-0.0876 -0.066 -0.07645"], 'toe': ["0.1728 0.0626 -0.07645", "0.1728 -0.066 -0.07645"], 'midfoot_front': ["0.086 0.0626 -0.07645", "0.086 -0.066 -0.07645"], 'midfoot_rear': ["-0.0008 0.0626 -0.07645", "-0.0008 -0.066 -0.07645"] }, 'l_foot': { 'heel': ["-0.0876 0.066 -0.07645", "-0.0876 -0.0626 -0.07645"], 'toe': ["0.1728 0.066 -0.07645", "0.1728 -0.0626 -0.07645"], 'midfoot_front': ["0.086 0.066 -0.07645", "0.086 -0.0626 -0.07645"], 'midfoot_rear': ["-0.008 0.066 -0.07645", "-0.008 -0.0626 -0.07645"]