diff --git a/README.md b/README.md index a6cbbff4..dab2f14e 100755 --- a/README.md +++ b/README.md @@ -37,10 +37,10 @@ make install-third-party-external ``` ### Running Stompy experiments -1. Download our model from +1. Download our URDF model from ```bash wget https://media.kscale.dev/stompy.tar.gz && tar -xzvf stompy.tar.gz -python sim/scripts/create_fixed_urdf.py +python sim/scripts/create_fixed_torso.py export MODEL_DIR=stompy ``` diff --git a/pyproject.toml b/pyproject.toml index f3aa14d5..d863eeb5 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -35,6 +35,7 @@ namespace_packages = false module = [ "isaacgym.*", "humanoid.*", + "IsaacGymEnvs.*", ] ignore_missing_imports = true diff --git a/sim/env.py b/sim/env.py index 9a383e3d..d509a566 100755 --- a/sim/env.py +++ b/sim/env.py @@ -22,3 +22,12 @@ def stompy_urdf_path(legs_only: bool = False) -> Path: raise FileNotFoundError(f"URDF file not found: {stompy_path}") return stompy_path.resolve() + + +def stompy_mjcf_path() -> Path: + stompy_path = model_dir() / "robot.xml" + + if not stompy_path.exists(): + raise FileNotFoundError(f"MJCF file not found: {stompy_path}") + + return stompy_path.resolve() diff --git a/sim/scripts/create_fixed_torso.py b/sim/scripts/create_fixed_torso.py new file mode 100644 index 00000000..d751f70e --- /dev/null +++ b/sim/scripts/create_fixed_torso.py @@ -0,0 +1,36 @@ +# mypy: disable-error-code="valid-newtype" +"""This script updates the URDF file to fix the joints of the robot.""" + +import xml.etree.ElementTree as ET +from sim.stompy.joints import StompyFixed + +STOMPY_URDF = "stompy/robot.urdf" + + +def update_urdf(): + tree = ET.parse(STOMPY_URDF) + root = tree.getroot() + stompy = StompyFixed() + + revolute_joints = set(stompy.default_standing().keys()) + joint_limits = stompy.default_limits() + + for joint in root.findall("joint"): + joint_name = joint.get("name") + if joint_name not in revolute_joints: + joint.set("type", "fixed") + else: + limit = joint.find("limit") + if limit is not None: + limits = joint_limits.get(joint_name, {}) + lower = str(limits.get("lower", 0.0)) + upper = str(limits.get("upper", 0.0)) + limit.set("lower", lower) + limit.set("upper", upper) + + # Save the modified URDF to a new file + tree.write("stompy/robot_fixed.urdf") + + +if __name__ == "__main__": + update_urdf() diff --git a/sim/scripts/create_fixed_urdf.py b/sim/scripts/create_fixed_urdf.py deleted file mode 100644 index 74da8a27..00000000 --- a/sim/scripts/create_fixed_urdf.py +++ /dev/null @@ -1,96 +0,0 @@ -# mypy: disable-error-code="valid-newtype" -"""This script updates the URDF file to fix the joints of the robot.""" - -import xml.etree.ElementTree as ET -from sim.stompy.joints import StompyFixed - -STOMPY_URDF = "stompy/robot.urdf" -STOMPY_MJCF = "stompy/robot.xml" - - -def update_urdf(): - tree = ET.parse() - root = tree.getroot() - stompy = StompyFixed() - - revolute_joints = set(stompy.default_standing().keys()) - joint_limits = stompy.default_limits() - - for joint in root.findall("joint"): - joint_name = joint.get("name") - if joint_name not in revolute_joints: - print(joint) - joint.set("type", "fixed") - else: - limit = joint.find("limit") - if limit is not None: - limits = joint_limits.get(joint_name, {}) - lower = str(limits.get("lower", 0.0)) - upper = str(limits.get("upper", 0.0)) - limit.set("lower", lower) - limit.set("upper", upper) - - # Save the modified URDF to a new file - tree.write("stompy/robot_fixed.urdf") - - -def update_mjcf(): - tree = ET.parse(STOMPY_MJCF) - root = tree.getroot() - # Create light element - light = ET.Element("light", { - "cutoff": "100", - "diffuse": "1 1 1", - "dir": "-0 0 -1.3", - "directional": "true", - "exponent": "1", - "pos": "0 0 1.3", - "specular": ".1 .1 .1" - }) - - # Create floor geometry element - floor = ET.Element("geom", { - "conaffinity": "1", - "condim": "3", - "name": "floor", - "pos": "0 0 -1.5", - "rgba": "0.8 0.9 0.8 1", - "size": "40 40 40", - "type": "plane", - "material": "MatPlane" - }) - # Access the element - worldbody = root.find("worldbody") - # Insert new elements at the beginning of the worldbody - worldbody.insert(0, floor) - worldbody.insert(0, light) - - new_root_body = ET.Element("body", name="root", pos="-1.4 0 -0.3", quat="1 0 0 1") - - # List to store items to be moved to the new root body - items_to_move = [] - # Gather all children (geoms and bodies) that need to be moved under the new root body - for element in worldbody: - items_to_move.append(element) - # Move gathered elements to the new root body - for item in items_to_move: - worldbody.remove(item) - new_root_body.append(item) - # Add the new root body to the worldbody - worldbody.append(new_root_body) - worldbody2 = worldbody - # # Create a new root worldbody and add the new root body to it - # new_worldbody = ET.Element("worldbody", name="new_root") - index = list(root).index(worldbody) - - # # Remove the old and insert the new one at the same index - root.remove(worldbody) - root.insert(index, worldbody2) - modified_xml = ET.tostring(root, encoding="unicode") - - with open("stompy/robot_fixed.xml", "w") as f: - f.write(modified_xml) - - -if __name__ == "__main__": - # update_urdf() diff --git a/sim/scripts/create_mjcf.py b/sim/scripts/create_mjcf.py new file mode 100644 index 00000000..60f6109f --- /dev/null +++ b/sim/scripts/create_mjcf.py @@ -0,0 +1,77 @@ +# mypy: disable-error-code="valid-newtype" +"""This script updates the MJCF version for proper rendering. + +The mcf format can be created by loading URDF file to the viewer and saving as xml. +""" +import xml.etree.ElementTree as ET + +STOMPY_MJCF = "stompy/robot.xml" + + +def update_mjcf(): + tree = ET.parse(STOMPY_MJCF) + root = tree.getroot() + # Create light element + light = ET.Element( + "light", + { + "cutoff": "100", + "diffuse": "1 1 1", + "dir": "-0 0 -1.3", + "directional": "true", + "exponent": "1", + "pos": "0 0 1.3", + "specular": ".1 .1 .1", + }, + ) + + # Create floor geometry element + floor = ET.Element( + "geom", + { + "conaffinity": "1", + "condim": "3", + "name": "floor", + "pos": "0 0 -1.5", + "rgba": "0.8 0.9 0.8 1", + "size": "40 40 40", + "type": "plane", + "material": "MatPlane", + }, + ) + # Access the element + worldbody = root.find("worldbody") + # Insert new elements at the beginning of the worldbody + worldbody.insert(0, floor) + worldbody.insert(0, light) + + new_root_body = ET.Element("body", name="root", pos="-1.4 0 -0.3", quat="1 0 0 1") + + # List to store items to be moved to the new root body + items_to_move = [] + # Gather all children (geoms and bodies) that need to be moved under the new root body + for element in worldbody: + items_to_move.append(element) + # Move gathered elements to the new root body + for item in items_to_move: + worldbody.remove(item) + new_root_body.append(item) + # Add the new root body to the worldbody + worldbody.append(new_root_body) + worldbody2 = worldbody + # # Create a new root worldbody and add the new root body to it + # new_worldbody = ET.Element("worldbody", name="new_root") + index = list(root).index(worldbody) + + # # Remove the old and insert the new one at the same index + root.remove(worldbody) + root.insert(index, worldbody2) + modified_xml = ET.tostring(root, encoding="unicode") + + with open(STOMPY_MJCF, "w") as f: + f.write(modified_xml) + + +if __name__ == "__main__": + # update_urdf() + update_mjcf() diff --git a/sim/scripts/simulate_mjcf.py b/sim/scripts/simulate_mjcf.py new file mode 100644 index 00000000..7480ade6 --- /dev/null +++ b/sim/scripts/simulate_mjcf.py @@ -0,0 +1,27 @@ +# mypy: disable-error-code="valid-newtype" +"""Defines a simple demo script for simulating a MJCF to observe the physics. + +Run with mjpython: + mjpython mjpython sim/scripts/simulate_mjcf.py +""" +import time +import mujoco +import mujoco.viewer + +from sim.env import stompy_mjcf_path + + +model = mujoco.MjModel.from_xml_path(stompy_mjcf_path()) +data = mujoco.MjData(model) + + +with mujoco.viewer.launch_passive(model, data) as viewer: + start = time.time() + while viewer.is_running(): + step_start = time.time() + mujoco.mj_step(model, data) + viewer.sync() + + time_until_next_step = model.opt.timestep - (time.time() - step_start) + if time_until_next_step > 0: + time.sleep(time_until_next_step) diff --git a/sim/scripts/drop_urdf.py b/sim/scripts/simulate_urdf.py similarity index 100% rename from sim/scripts/drop_urdf.py rename to sim/scripts/simulate_urdf.py diff --git a/third_party/IsaacGymEnvs b/third_party/IsaacGymEnvs index f9818228..1aaffb2a 160000 --- a/third_party/IsaacGymEnvs +++ b/third_party/IsaacGymEnvs @@ -1 +1 @@ -Subproject commit f9818228ce92f46c94ec4090d8bf8e7e46077d58 +Subproject commit 1aaffb2a43ab97cdea13ff98b9b7ad2f653794f0