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"]
# 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", "../")
