diff --git a/.gitignore b/.gitignore
index 9adf7c6f..b71fa871 100755
--- a/.gitignore
+++ b/.gitignore
@@ -2,7 +2,6 @@
# Robot Parts
*.stl
*.dae
-*.urdf
# Local files
.env
*.DS_Store
diff --git a/sim/mjcf.py b/sim/mjcf.py
new file mode 100644
index 00000000..9cf5fa4d
--- /dev/null
+++ b/sim/mjcf.py
@@ -0,0 +1,523 @@
+# mypy: disable-error-code="operator,union-attr"
+"""Defines common types and functions for exporting MJCF files.
+
+API reference:
+https://github.com/google-deepmind/mujoco/blob/main/src/xml/xml_native_writer.cc#L780
+
+Todo:
+ 1. Clean up the inertia config
+ 2. Add visual and imu support
+"""
+
+import glob
+import os
+import shutil
+import xml.etree.ElementTree as ET
+from dataclasses import dataclass, field
+from pathlib import Path
+from typing import List, Literal, Optional, Tuple, Union
+
+import mujoco
+
+
+@dataclass
+class Compiler:
+ coordinate: Optional[Literal["local", "global"]] = None
+ angle: Literal["radian", "degree"] = "radian"
+ meshdir: Optional[str] = None
+ eulerseq: Optional[Literal["xyz", "zxy", "zyx", "yxz", "yzx", "xzy"]] = None
+ autolimits: Optional[bool] = None
+
+ def to_xml(self, compiler: Optional[ET.Element] = None) -> ET.Element:
+ if compiler is None:
+ compiler = ET.Element("compiler")
+ if self.coordinate is not None:
+ compiler.set("coordinate", self.coordinate)
+ compiler.set("angle", self.angle)
+ if self.meshdir is not None:
+ compiler.set("meshdir", self.meshdir)
+ if self.eulerseq is not None:
+ compiler.set("eulerseq", self.eulerseq)
+ if self.autolimits is not None:
+ compiler.set("autolimits", str(self.autolimits).lower())
+ return compiler
+
+
+@dataclass
+class Mesh:
+ name: str
+ file: str
+ scale: Optional[Tuple[float, float, float]] = None
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ mesh = ET.Element("mesh") if root is None else ET.SubElement(root, "mesh")
+ mesh.set("name", self.name)
+ mesh.set("file", self.file)
+ if self.scale is not None:
+ mesh.set("scale", " ".join(map(str, self.scale)))
+ return mesh
+
+
+@dataclass
+class Joint:
+ name: Optional[str] = None
+ type: Optional[Literal["hinge", "slide", "ball", "free"]] = None
+ pos: Optional[Tuple[float, float, float]] = None
+ axis: Optional[Tuple[float, float, float]] = None
+ limited: Optional[bool] = None
+ range: Optional[Tuple[float, float]] = None
+ damping: Optional[float] = None
+ stiffness: Optional[float] = None
+ armature: Optional[float] = None
+ frictionloss: Optional[float] = None
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ joint = ET.Element("joint") if root is None else ET.SubElement(root, "joint")
+ if self.name is not None:
+ joint.set("name", self.name)
+ if self.type is not None:
+ joint.set("type", self.type)
+ if self.pos is not None:
+ joint.set("pos", " ".join(map(str, self.pos)))
+ if self.axis is not None:
+ joint.set("axis", " ".join(map(str, self.axis)))
+ if self.range is not None:
+ joint.set("range", " ".join(map(str, self.range)))
+ if self.limited is not None:
+ joint.set("limited", str(self.limited).lower())
+ if self.damping is not None:
+ joint.set("damping", str(self.damping))
+ if self.stiffness is not None:
+ joint.set("stiffness", str(self.stiffness))
+ if self.armature is not None:
+ joint.set("armature", str(self.armature))
+ if self.frictionloss is not None:
+ joint.set("frictionloss", str(self.frictionloss))
+ return joint
+
+
+@dataclass
+class Geom:
+ name: Optional[str] = None
+ type: Optional[Literal["plane", "sphere", "cylinder", "box", "capsule", "ellipsoid", "mesh"]] = None
+ plane: Optional[str] = None
+ rgba: Optional[Tuple[float, float, float, float]] = None
+ pos: Optional[Tuple[float, float, float]] = None
+ quat: Optional[Tuple[float, float, float, float]] = None
+ matplane: Optional[str] = None
+ material: Optional[str] = None
+ condim: Optional[int] = None
+ contype: Optional[int] = None
+ conaffinity: Optional[int] = None
+ size: Optional[Tuple[float, float, float]] = None
+ friction: Optional[Tuple[float, float, float]] = None
+ solref: Optional[Tuple[float, float]] = None
+ density: Optional[float] = None
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ geom = ET.Element("geom") if root is None else ET.SubElement(root, "geom")
+ if self.name is not None:
+ geom.set("name", self.name)
+ if self.type is not None:
+ geom.set("type", self.type)
+ if self.rgba is not None:
+ geom.set("rgba", " ".join(map(str, self.rgba)))
+ if self.pos is not None:
+ geom.set("pos", " ".join(map(str, self.pos)))
+ if self.quat is not None:
+ geom.set("quat", " ".join(map(str, self.quat)))
+ if self.matplane is not None:
+ geom.set("matplane", self.matplane)
+ if self.material is not None:
+ geom.set("material", self.material)
+ if self.condim is not None:
+ geom.set("condim", str(self.condim))
+ if self.contype is not None:
+ geom.set("contype", str(self.contype))
+ if self.conaffinity is not None:
+ geom.set("conaffinity", str(self.conaffinity))
+ if self.plane is not None:
+ geom.set("plane", self.plane)
+ if self.size is not None:
+ geom.set("size", " ".join(map(str, self.size)))
+ if self.friction is not None:
+ geom.set("friction", " ".join(map(str, self.friction)))
+ if self.solref is not None:
+ geom.set("solref", " ".join(map(str, self.solref)))
+ if self.density is not None:
+ geom.set("density", str(self.density))
+ return geom
+
+
+@dataclass
+class Body:
+ name: str
+ pos: Optional[Tuple[float, float, float]] = field(default=None)
+ quat: Optional[Tuple[float, float, float, float]] = field(default=None)
+ geom: Optional[Geom] = field(default=None)
+ joint: Optional[Joint] = field(default=None)
+ # TODO - fix inertia, until then rely on Mujoco's engine
+ # inertial: Inertial = None
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ body = ET.Element("body") if root is None else ET.SubElement(root, "body")
+ body.set("name", self.name)
+ if self.pos is not None:
+ body.set("pos", " ".join(map(str, self.pos)))
+ if self.quat is not None:
+ body.set("quat", " ".join(f"{x:.8g}" for x in self.quat))
+ if self.joint is not None:
+ self.joint.to_xml(body)
+ if self.geom is not None:
+ self.geom.to_xml(body)
+ return body
+
+
+@dataclass
+class Flag:
+ frictionloss: Optional[str] = None
+ # removed at 3.1.4
+ # sensornoise: str | None = None
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ flag = ET.Element("flag") if root is None else ET.SubElement(root, "flag")
+ if self.frictionloss is not None:
+ flag.set("frictionloss", self.frictionloss)
+ return flag
+
+
+@dataclass
+class Option:
+ timestep: Optional[float] = None
+ viscosity: Optional[float] = None
+ iterations: Optional[int] = None
+ solver: Optional[Literal["PGS", "CG", "Newton"]] = None
+ gravity: Optional[Tuple[float, float, float]] = None
+ flag: Optional[Flag] = None
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ if root is None:
+ option = ET.Element("option")
+ else:
+ option = ET.SubElement(root, "option")
+ if self.iterations is not None:
+ option.set("iterations", str(self.iterations))
+ if self.timestep is not None:
+ option.set("timestep", str(self.timestep))
+ if self.viscosity is not None:
+ option.set("viscosity", str(self.viscosity))
+ if self.solver is not None:
+ option.set("solver", self.solver)
+ if self.gravity is not None:
+ option.set("gravity", " ".join(map(str, self.gravity)))
+ if self.flag is not None:
+ self.flag.to_xml(option)
+ return option
+
+
+@dataclass
+class Motor:
+ name: Optional[str] = None
+ joint: Optional[str] = None
+ ctrlrange: Optional[Tuple[float, float]] = None
+ ctrllimited: Optional[bool] = None
+ gear: Optional[float] = None
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ if root is None:
+ motor = ET.Element("motor")
+ else:
+ motor = ET.SubElement(root, "motor")
+ if self.name is not None:
+ motor.set("name", self.name)
+ if self.joint is not None:
+ motor.set("joint", self.joint)
+ if self.ctrllimited is not None:
+ motor.set("ctrllimited", str(self.ctrllimited).lower())
+ if self.ctrlrange is not None:
+ motor.set("ctrlrange", " ".join(map(str, self.ctrlrange)))
+ if self.gear is not None:
+ motor.set("gear", str(self.gear))
+ return motor
+
+
+@dataclass
+class Light:
+ directional: bool = True
+ diffuse: Optional[Tuple[float, float, float]] = None
+ specular: Optional[Tuple[float, float, float]] = None
+ pos: Optional[Tuple[float, float, float]] = None
+ dir: Optional[Tuple[float, float, float]] = None
+ castshadow: Optional[bool] = None
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ if root is None:
+ light = ET.Element("light")
+ else:
+ light = ET.SubElement(root, "light")
+ if self.directional is not None:
+ light.set("directional", str(self.directional).lower())
+ if self.diffuse is not None:
+ light.set("diffuse", " ".join(map(str, self.diffuse)))
+ if self.specular is not None:
+ light.set("specular", " ".join(map(str, self.specular)))
+ if self.pos is not None:
+ light.set("pos", " ".join(map(str, self.pos)))
+ if self.dir is not None:
+ light.set("dir", " ".join(map(str, self.dir)))
+ if self.castshadow is not None:
+ light.set("castshadow", str(self.castshadow).lower())
+ return light
+
+
+@dataclass
+class Equality:
+ solref: Tuple[float, float]
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ equality = ET.Element("equality") if root is None else ET.SubElement(root, "equality")
+ equality.set("solref", " ".join(map(str, self.solref)))
+ return equality
+
+
+@dataclass
+class Site:
+ name: Optional[str] = None
+ size: Optional[float] = None
+ pos: Optional[Tuple[float, float, float]] = None
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ site = ET.Element("site") if root is None else ET.SubElement(root, "site")
+ if self.name is not None:
+ site.set("name", self.name)
+ if self.size is not None:
+ site.set("size", str(self.size))
+ if self.pos is not None:
+ site.set("pos", " ".join(map(str, self.pos)))
+ return site
+
+
+@dataclass
+class Default:
+ joint: Optional[Joint] = None
+ geom: Optional[Geom] = None
+ class_: Optional[str] = None
+ motor: Optional[Motor] = None
+ equality: Optional[Equality] = None
+ visual_geom: Optional[ET.Element] = None
+
+ def to_xml(self, default: Optional[ET.Element] = None) -> ET.Element:
+ default = ET.Element("default") if default is None else ET.SubElement(default, "default")
+ if self.joint is not None:
+ self.joint.to_xml(default)
+ if self.geom is not None:
+ self.geom.to_xml(default)
+ if self.class_ is not None:
+ default.set("class", self.class_)
+ if self.motor is not None:
+ self.motor.to_xml(default)
+ if self.equality is not None:
+ self.equality.to_xml(default)
+ if self.visual_geom is not None:
+ default.append(self.visual_geom)
+ return default
+
+
+@dataclass
+class Actuator:
+ motors: List[Motor]
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ actuator = ET.Element("actuator") if root is None else ET.SubElement(root, "actuator")
+ for motor in self.motors:
+ motor.to_xml(actuator)
+ return actuator
+
+
+@dataclass
+class Actuatorpos:
+ name: Optional[str] = None
+ actuator: Optional[str] = None
+ user: Optional[str] = None
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ actuatorpos = ET.Element("actuatorpos") if root is None else ET.SubElement(root, "actuatorpos")
+ if self.name is not None:
+ actuatorpos.set("name", self.name)
+ if self.actuator is not None:
+ actuatorpos.set("actuator", self.actuator)
+ if self.user is not None:
+ actuatorpos.set("user", self.user)
+ return actuatorpos
+
+
+@dataclass
+class Actuatorvel:
+ name: Optional[str] = None
+ actuator: Optional[str] = None
+ user: Optional[str] = None
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ actuatorvel = ET.Element("actuatorvel") if root is None else ET.SubElement(root, "actuatorvel")
+ if self.name is not None:
+ actuatorvel.set("name", self.name)
+ if self.actuator is not None:
+ actuatorvel.set("actuator", self.actuator)
+ if self.user is not None:
+ actuatorvel.set("user", self.user)
+ return actuatorvel
+
+
+@dataclass
+class Actuatorfrc:
+ name: Optional[str] = None
+ actuator: Optional[str] = None
+ user: Optional[str] = None
+ noise: Optional[float] = None
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ actuatorfrc = ET.Element("actuatorfrc") if root is None else ET.SubElement(root, "actuatorfrc")
+ if self.name is not None:
+ actuatorfrc.set("name", self.name)
+ if self.actuator is not None:
+ actuatorfrc.set("actuator", self.actuator)
+ if self.user is not None:
+ actuatorfrc.set("user", self.user)
+ if self.noise is not None:
+ actuatorfrc.set("noise", str(self.noise))
+ return actuatorfrc
+
+
+@dataclass
+class Sensor:
+ actuatorpos: Optional[List[Actuatorpos]] = None
+ actuatorvel: Optional[List[Actuatorvel]] = None
+ actuatorfrc: Optional[List[Actuatorfrc]] = None
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ sensor = ET.Element("sensor") if root is None else ET.SubElement(root, "sensor")
+ if self.actuatorpos is not None:
+ for actuatorpos in self.actuatorpos:
+ actuatorpos.to_xml(sensor)
+ if self.actuatorvel is not None:
+ for actuatorvel in self.actuatorvel:
+ actuatorvel.to_xml(sensor)
+ if self.actuatorfrc is not None:
+ for actuatorfrc in self.actuatorfrc:
+ actuatorfrc.to_xml(sensor)
+ return sensor
+
+
+@dataclass
+class Key:
+ name: Optional[str] = None
+ qpos: Optional[str] = None
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ key = ET.Element("key") if root is None else ET.SubElement(root, "key")
+ if self.name is not None:
+ key.set("name", self.name)
+ if self.qpos is not None:
+ key.set("qpos", self.qpos)
+ return key
+
+
+@dataclass
+class Keyframe:
+ keys: List[Key]
+
+ def to_xml(self, root: Optional[ET.Element] = None) -> ET.Element:
+ keyframe = ET.Element("keyframe") if root is None else ET.SubElement(root, "keyframe")
+ for key in self.keys:
+ key.to_xml(keyframe)
+ return keyframe
+
+
+def _copy_stl_files(source_directory: Union[str, Path], destination_directory: Union[str, Path]) -> None:
+ # Ensure the destination directory exists, create if not
+ os.makedirs(destination_directory, exist_ok=True)
+
+ # Use glob to find all .stl files in the source directory
+ pattern = os.path.join(source_directory, "*.stl")
+ for file_path in glob.glob(pattern):
+ destination_path = os.path.join(destination_directory, os.path.basename(file_path))
+ shutil.copy(file_path, destination_path)
+ print(f"Copied {file_path} to {destination_path}")
+
+
+def _remove_stl_files(source_directory: Union[str, Path]) -> None:
+ for filename in os.listdir(source_directory):
+ if filename.endswith(".stl"):
+ file_path = os.path.join(source_directory, filename)
+ os.remove(file_path)
+
+
+class Robot:
+ """A class to adapt the world in a Mujoco XML file."""
+
+ def __init__(
+ self,
+ robot_name: str,
+ output_dir: Union[str, Path],
+ compiler: Optional[Compiler] = None,
+ ) -> None:
+ """Initialize the robot.
+
+ Args:
+ robot_name (str): The name of the robot.
+ output_dir (Union[str, Path]): The output directory.
+ compiler (Compiler, optional): The compiler settings.
+ """
+ self.robot_name = robot_name
+ self.output_dir = output_dir
+ self.compiler = compiler
+ self._set_clean_up()
+ self.tree = ET.parse(self.output_dir / f"{self.robot_name}.xml")
+
+ def _set_clean_up(self) -> None:
+ # HACK
+ # mujoco has a hard time reading meshes
+ _copy_stl_files(self.output_dir / "meshes", self.output_dir)
+
+ urdf_tree = ET.parse(self.output_dir / f"{self.robot_name}.xml")
+ root = urdf_tree.getroot()
+
+ tree = ET.ElementTree(root)
+ tree.write(self.output_dir / f"{self.robot_name}.urdf", encoding="utf-8", xml_declaration=True)
+ model = mujoco.MjModel.from_xml_path((self.output_dir / f"{self.robot_name}.urdf").as_posix())
+ mujoco.mj_saveLastXML((self.output_dir / f"{self.robot_name}.xml").as_posix(), model)
+ # remove all the files
+ _remove_stl_files(self.output_dir)
+
+ def adapt_world(self) -> None:
+ root = self.tree.getroot()
+
+ # Turn off internal collisions
+ for element in root:
+ if element.tag == "geom":
+ element.attrib["contype"] = str(1)
+ element.attrib["conaffinity"] = str(0)
+
+ compiler = root.find("compiler")
+ if self.compiler is not None:
+ compiler = self.compiler.to_xml(compiler)
+
+ worldbody = root.find("worldbody")
+ new_root_body = Body(name="root", pos=(0, 0, 0), quat=(1, 0, 0, 0)).to_xml()
+
+ # 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)
+ self.tree = ET.ElementTree(root)
+
+ def save(self, path: Union[str, Path]) -> None:
+ self.tree.write(path, encoding="utf-8", xml_declaration=True)
diff --git a/sim/scripts/create_fixed_torso.py b/sim/scripts/create_fixed_torso.py
index 264554b1..88408dca 100755
--- a/sim/scripts/create_fixed_torso.py
+++ b/sim/scripts/create_fixed_torso.py
@@ -3,6 +3,7 @@
import xml.etree.ElementTree as ET
+from sim.scripts.create_mjcf import create_mjcf
from sim.stompy_legs.joints import Stompy
STOMPY_URDF = "sim/stompy_legs/robot.urdf"
@@ -15,6 +16,10 @@ def update_urdf() -> None:
print(stompy.default_standing())
revolute_joints = set(stompy.default_standing().keys())
joint_limits = stompy.default_limits()
+ effort = stompy.effort()
+ velocity = stompy.velocity()
+ friction = stompy.friction()
+
for joint in root.findall("joint"):
joint_name = joint.get("name")
if joint_name not in revolute_joints:
@@ -27,9 +32,25 @@ def update_urdf() -> None:
upper = str(limits.get("upper", 0.0))
limit.set("lower", lower)
limit.set("upper", upper)
+
+ for key, value in effort.items():
+ if key in joint_name:
+ limit.set("effort", str(value))
+
+ for key, value in velocity.items():
+ if key in joint_name:
+ limit.set("velocity", str(value))
+
+ dynamics = joint.find("dynamics")
+ if dynamics is not None:
+ for key, value in friction.items():
+ if key in joint_name:
+ dynamics.set("friction", str(value))
+
# Save the modified URDF to a new file
tree.write("sim/stompy_legs/robot_fixed.urdf")
if __name__ == "__main__":
update_urdf()
+ create_mjcf(STOMPY_URDF)
diff --git a/sim/scripts/create_mjcf.py b/sim/scripts/create_mjcf.py
index ba2d30e6..26c15dd9 100644
--- a/sim/scripts/create_mjcf.py
+++ b/sim/scripts/create_mjcf.py
@@ -2,43 +2,54 @@
"""Defines common types and functions for exporting MJCF files.
Run:
- python sim/scripts/create_mjcf.py
+ python sim/scripts/create_mjcf.py /path/to/stompy.xml
Todo:
0. Add IMU to the right position
1. Armature damping setup for different parts of body
2. Test control range limits?
3. Add inertia in the first part of the body
+
"""
+import argparse
import logging
import xml.dom.minidom
import xml.etree.ElementTree as ET
from pathlib import Path
from typing import List, Union
-from kol.formats import mjcf
-
-from sim.env import model_dir, stompy_mjcf_path
-from sim.stompy.joints import MjcfStompy, StompyFixed
+from sim import mjcf
+from sim.stompy.joints import Stompy
logger = logging.getLogger(__name__)
-STOMPY_HEIGHT = 1.0
+# Links that will have collision with the floor
+COLLISION_LINKS = [
+ "right_foot_1_rubber_grip_3_simple",
+ "left_foot_1_rubber_grip_1_simple",
+]
+
+ROOT_HEIGHT = 0.72
+
+stompy = Stompy()
def _pretty_print_xml(xml_string: str) -> str:
"""Formats the provided XML string into a pretty-printed version."""
parsed_xml = xml.dom.minidom.parseString(xml_string)
- return parsed_xml.toprettyxml(indent=" ")
+ pretty_xml = parsed_xml.toprettyxml(indent=" ")
+
+ # Split the pretty-printed XML into lines and filter out empty lines
+ lines = pretty_xml.split("\n")
+ non_empty_lines = [line for line in lines if line.strip() != ""]
+ return "\n".join(non_empty_lines)
class Sim2SimRobot(mjcf.Robot):
"""A class to adapt the world in a Mujoco XML file."""
- def adapt_world(
- self, add_floor: bool = True, add_reference_position: bool = False, remove_frc_range: bool = False
- ) -> None:
+ def adapt_world(self, add_floor: bool = True, remove_frc_range: bool = False) -> None:
root: ET.Element = self.tree.getroot()
if add_floor:
@@ -72,29 +83,6 @@ def adapt_world(
compiler = self.compiler.to_xml(compiler)
worldbody = root.find("worldbody")
- # 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)
-
- new_root_body = mjcf.Body(name="root", pos=(0, 0, STOMPY_HEIGHT), quat=(1, 0, 0, 0)).to_xml()
- # Add joints to all the movement of the base
- new_root_body.extend(
- [
- mjcf.Joint(name="root_x", type="slide", axis=(1, 0, 0), limited=False).to_xml(),
- mjcf.Joint(name="root_y", type="slide", axis=(0, 1, 0), limited=False).to_xml(),
- mjcf.Joint(name="root_z", type="slide", axis=(0, 0, 1), limited=False).to_xml(),
- mjcf.Joint(name="root_ball", type="ball", limited=False).to_xml(),
- ]
- )
-
- # Add imu site to the body - relative position to the body
- # check at what stage we use this
- new_root_body.append(mjcf.Site(name="imu", size=0.01, pos=(0, 0, 0)).to_xml())
-
- # Add the new root body to the worldbody
- worldbody.append(new_root_body)
worldbody.insert(
0,
mjcf.Light(
@@ -122,9 +110,8 @@ def adapt_world(
pos=(0.001, 0, 0),
quat=(1, 0, 0, 0),
material="matplane",
- condim=3,
- conaffinity=1,
- contype=0,
+ condim=1,
+ conaffinity=15,
).to_xml(),
)
@@ -132,14 +119,23 @@ def adapt_world(
sensor_pos: List[mjcf.Actuatorpos] = []
sensor_vel: List[mjcf.Actuatorvel] = []
sensor_frc: List[mjcf.Actuatorfrc] = []
- for joint, limits in StompyFixed.default_limits().items():
- if joint in StompyFixed.default_standing().keys():
+ # Create motors and sensors for the joints
+ joints = list(root.findall("joint"))
+ for joint, _ in stompy.default_limits().items():
+ if joint in stompy.default_standing().keys():
+ joint_name = joint
+ limit = 200.0 # Ensure limit is a float
+ keys = stompy.effort().keys()
+ for key in keys:
+ if key in joint_name:
+ limit = stompy.effort()[key]
+
motors.append(
mjcf.Motor(
name=joint,
joint=joint,
gear=1,
- ctrlrange=(-200, 200),
+ ctrlrange=(-limit, limit),
ctrllimited=True,
)
)
@@ -147,13 +143,13 @@ def adapt_world(
sensor_vel.append(mjcf.Actuatorvel(name=joint + "_v", actuator=joint, user="13"))
sensor_frc.append(mjcf.Actuatorfrc(name=joint + "_f", actuator=joint, user="13", noise=0.001))
- root = self.add_joint_limits(root, fixed=False)
+ root = self.update_joints(root)
# Add motors and sensors
root.append(mjcf.Actuator(motors).to_xml())
root.append(mjcf.Sensor(sensor_pos, sensor_vel, sensor_frc).to_xml())
- # Add imus
+ # TODO: Add additional sensors when necessary
sensors = root.find("sensor")
sensors.extend(
[
@@ -170,42 +166,53 @@ def adapt_world(
1,
mjcf.Option(
timestep=0.001,
- viscosity=1e-6,
iterations=50,
solver="PGS",
gravity=(0, 0, -9.81),
- flag=mjcf.Flag(frictionloss="enable"),
).to_xml(),
)
visual_geom = ET.Element("default", {"class": "visualgeom"})
- geom_attributes = {"material": "visualgeom", "condim": "1", "contype": "1", "conaffinity": "0"}
+ geom_attributes = {"material": "visualgeom", "condim": "1", "contype": "0", "conaffinity": "0"}
ET.SubElement(visual_geom, "geom", geom_attributes)
root.insert(
1,
mjcf.Default(
- joint=mjcf.Joint(armature=0.01, damping=0.1, limited=True, frictionloss=0.01),
+ joint=mjcf.Joint(armature=0.01, stiffness=0, damping=0.01, limited=True, frictionloss=0.01),
motor=mjcf.Motor(ctrllimited=True),
equality=mjcf.Equality(solref=(0.001, 2)),
geom=mjcf.Geom(
solref=(0.001, 2),
friction=(0.9, 0.2, 0.2),
- condim=3,
+ condim=4,
contype=1,
- conaffinity=0,
+ conaffinity=15,
),
visual_geom=visual_geom,
).to_xml(),
)
- if add_reference_position:
- root = self.add_reference_position(root)
-
- # Move gathered elements to the new root body
- for item in items_to_move:
- worldbody.remove(item)
- new_root_body.append(item)
+ # Locate actual root body inside of worldbody
+ root_body = worldbody.find(".//body")
+ # Make position and orientation of the root body
+ root_body.set("pos", "0 0 0")
+ root_body.set("quat", "1 0 0 0")
+ root_body.insert(0, ET.Element("joint", name="floating_base_joint", type="free"))
+
+ # Add cameras and imu
+ root_body.insert(1, ET.Element("camera", name="front", pos="0 -3 1", xyaxes="1 0 0 0 1 2", mode="trackcom"))
+ root_body.insert(
+ 2,
+ ET.Element(
+ "camera",
+ name="side",
+ pos="-2.893 -1.330 0.757",
+ xyaxes="0.405 -0.914 0.000 0.419 0.186 0.889",
+ mode="trackcom",
+ ),
+ )
+ root_body.insert(3, ET.Element("site", name="imu", size="0.01", pos="0 0 0"))
# add visual geom logic
for body in root.findall(".//body"):
@@ -214,17 +221,19 @@ def adapt_world(
geom.set("class", "visualgeom")
# Create a new geom element
new_geom = ET.Element("geom")
- new_geom.set("type", geom.get("type"))
- new_geom.set("rgba", geom.get("rgba"))
- new_geom.set("mesh", geom.get("mesh"))
+ new_geom.set("type", geom.get("type") or "") # Ensure type is not None
+ new_geom.set("rgba", geom.get("rgba") or "") # Ensure rgba is not None
+ new_geom.set("mesh", geom.get("mesh") or "") # Ensure mesh is not None
if geom.get("pos"):
- new_geom.set("pos", geom.get("pos"))
+ new_geom.set("pos", geom.get("pos") or "")
if geom.get("quat"):
- new_geom.set("quat", geom.get("quat"))
- new_geom.set("contype", "0")
- new_geom.set("conaffinity", "0")
- new_geom.set("group", "1")
- new_geom.set("density", "0")
+ new_geom.set("quat", geom.get("quat") or "")
+ # Exclude collision meshes
+ if geom.get("mesh") not in COLLISION_LINKS:
+ new_geom.set("contype", "0")
+ new_geom.set("conaffinity", "0")
+ new_geom.set("group", "1")
+ new_geom.set("density", "0")
# Append the new geom to the body
index = list(body).index(geom)
@@ -237,19 +246,26 @@ def adapt_world(
if "actuatorfrcrange" in join.attrib:
join.attrib.pop("actuatorfrcrange")
- def add_reference_position(self, root: ET.Element) -> None:
- # Find all 'joint' elements
- joints = root.findall(".//joint")
-
- default_standing = MjcfStompy.default_standing()
- for joint in joints:
- if joint.get("name") in default_standing.keys():
- joint.set("ref", str(default_standing[joint.get("name")]))
-
- return root
-
- def add_joint_limits(self, root: ET.Element, fixed: bool = False) -> None:
- joint_limits = MjcfStompy.default_limits()
+ # Adding keyframe
+ default_standing = stompy.default_standing()
+ qpos = [0, 0, ROOT_HEIGHT, 1, 0, 0, 0] + list(default_standing.values())
+ default_key = mjcf.Key(name="default", qpos=" ".join(map(str, qpos)))
+ keyframe = mjcf.Keyframe(keys=[default_key])
+ root.append(keyframe.to_xml())
+
+ # Swap left and right leg
+ parent_body = root.find(".//body[@name='root']")
+ if parent_body is not None:
+ left = parent_body.find(".//body[@name='link_leg_assembly_left_1_rmd_x12_150_mock_1_inner_x12_150_1']")
+ right = parent_body.find(".//body[@name='link_leg_assembly_right_1_rmd_x12_150_mock_1_inner_x12_150_1']")
+ if left is not None and right is not None:
+ left_index = list(parent_body).index(left)
+ right_index = list(parent_body).index(right)
+ # Swap the bodies
+ parent_body[left_index], parent_body[right_index] = parent_body[right_index], parent_body[left_index]
+
+ def update_joints(self, root: ET.Element) -> ET.Element:
+ joint_limits = stompy.default_limits()
for joint in root.findall(".//joint"):
joint_name = joint.get("name")
@@ -259,6 +275,21 @@ def add_joint_limits(self, root: ET.Element, fixed: bool = False) -> None:
upper = str(limits.get("upper", 0.0))
joint.set("range", f"{lower} {upper}")
+ damping = 0.01
+ keys = stompy.damping().keys()
+ for key in keys:
+ if key in joint_name:
+ damping = stompy.damping()[key]
+ joint.set("damping", str(damping))
+
+ stiffness = 0.0
+ keys = stompy.stiffness().keys()
+ for key in keys:
+ if key in joint_name:
+ stiffness = stompy.stiffness()[key]
+
+ joint.set("stiffness", str(stiffness))
+
return root
def save(self, path: Union[str, Path]) -> None:
@@ -270,13 +301,23 @@ def save(self, path: Union[str, Path]) -> None:
f.write(formatted_xml)
-if __name__ == "__main__":
- robot_name = "robot"
+def create_mjcf(filepath: str) -> None:
+ """Create a MJCF file for the Stompy robot."""
+ path = Path(filepath)
+ robot_name = path.stem
+ path = path.parent
robot = Sim2SimRobot(
robot_name,
- model_dir(),
- mjcf.Compiler(angle="radian", meshdir="meshes", autolimits=True),
- remove_inertia=True,
+ path,
+ mjcf.Compiler(angle="radian", meshdir="meshes", autolimits=True, eulerseq="zyx"),
)
- robot.adapt_world(add_reference_position=True)
- robot.save(stompy_mjcf_path(legs_only=False))
+ robot.adapt_world()
+ robot.save(path / f"{robot_name}_fixed.xml")
+
+
+if __name__ == "__main__":
+ parser = argparse.ArgumentParser(description="Create a MJCF file for the Stompy robot.")
+ parser.add_argument("filepath", type=str, help="The path to load and save the MJCF file.")
+ args = parser.parse_args()
+ # Robot name is whatever string comes right before ".urdf" extension
+ create_mjcf(args.filepath)
diff --git a/sim/stompy/joints.py b/sim/stompy/joints.py
index 9f92fc98..6756f89e 100755
--- a/sim/stompy/joints.py
+++ b/sim/stompy/joints.py
@@ -9,8 +9,6 @@
from abc import ABC
from typing import Dict, List, Tuple, Union
-import numpy as np
-
class Node(ABC):
@classmethod
@@ -54,42 +52,6 @@ def __str__(self) -> str:
return f"[{self.__class__.__name__}]{parts_str}"
-class Torso(Node):
- roll = "torso roll"
-
-
-class LeftHand(Node):
- wrist_roll = "wrist roll"
- wrist_pitch = "wrist pitch"
- wrist_yaw = "wrist yaw"
- left_finger = "left hand left finger"
- right_finger = "left hand right finger"
-
-
-class LeftArm(Node):
- shoulder_yaw = "shoulder yaw"
- shoulder_pitch = "shoulder pitch"
- shoulder_roll = "shoulder roll"
- elbow_pitch = "elbow pitch"
- hand = LeftHand()
-
-
-class RightHand(Node):
- wrist_roll = "right wrist roll"
- wrist_pitch = "right wrist pitch"
- wrist_yaw = "right wrist yaw"
- left_finger = "right hand left finger"
- right_finger = "right hand right finger"
-
-
-class RightArm(Node):
- shoulder_yaw = "right shoulder yaw"
- shoulder_pitch = "right shoulder pitch"
- shoulder_roll = "right shoulder roll"
- elbow_pitch = "right elbow pitch"
- hand = RightHand()
-
-
class LeftLeg(Node):
hip_roll = "left hip roll"
hip_yaw = "left hip yaw"
@@ -114,586 +76,140 @@ class Legs(Node):
class Stompy(Node):
- torso = Torso()
- left_arm = LeftArm()
- right_arm = RightArm()
legs = Legs()
@classmethod
def default_positions(cls) -> Dict[str, float]:
- return {
- Stompy.torso.roll: 0.0,
- Stompy.left_arm.shoulder_yaw: np.deg2rad(60),
- Stompy.left_arm.shoulder_pitch: np.deg2rad(60),
- Stompy.right_arm.shoulder_yaw: np.deg2rad(-60),
- }
+ return {}
@classmethod
def default_standing(cls) -> Dict[str, float]:
return {
- Stompy.torso.roll: 2.58,
- # arms
- Stompy.left_arm.shoulder_pitch: -0.534,
- Stompy.left_arm.shoulder_yaw: 2.54,
- Stompy.left_arm.shoulder_roll: -0.0314,
- Stompy.right_arm.shoulder_pitch: 2.45,
- Stompy.right_arm.shoulder_yaw: 3.77,
- Stompy.right_arm.shoulder_roll: -0.0314,
- Stompy.left_arm.elbow_pitch: 2.35,
- Stompy.right_arm.elbow_pitch: 2.65,
- # hands
- Stompy.left_arm.hand.left_finger: 0.0,
- Stompy.left_arm.hand.right_finger: 0.0,
- Stompy.right_arm.hand.left_finger: 0.0,
- Stompy.right_arm.hand.right_finger: 0.0,
- Stompy.left_arm.hand.wrist_roll: 1.79,
- Stompy.left_arm.hand.wrist_pitch: 1.35,
- Stompy.left_arm.hand.wrist_yaw: 1.07,
- Stompy.right_arm.hand.wrist_roll: -2.13,
- Stompy.right_arm.hand.wrist_pitch: 1.79,
- Stompy.right_arm.hand.wrist_yaw: -0.251,
# legs
- Stompy.legs.left.hip_pitch: -1.6,
- Stompy.legs.left.hip_roll: 1.41,
- Stompy.legs.left.hip_yaw: -2.12,
- Stompy.legs.left.knee_pitch: 2.01,
- Stompy.legs.left.ankle_pitch: 0.238,
- Stompy.legs.left.ankle_roll: 1.85,
- Stompy.legs.right.hip_pitch: 1.76,
- Stompy.legs.right.hip_roll: -1.54,
- Stompy.legs.right.hip_yaw: 0.967,
- Stompy.legs.right.knee_pitch: 2.07,
- Stompy.legs.right.ankle_pitch: 0.377,
- Stompy.legs.right.ankle_roll: 1.92,
- }
-
- @classmethod
- def default_sitting(cls) -> Dict[str, float]:
- return {
- Stompy.torso.roll: 0.0,
- # arms
- Stompy.left_arm.shoulder_pitch: -0.126,
- Stompy.left_arm.shoulder_yaw: 2.12,
- Stompy.left_arm.shoulder_roll: 1.89,
- Stompy.right_arm.shoulder_pitch: -1.13,
- Stompy.right_arm.shoulder_yaw: 2.1,
- Stompy.right_arm.shoulder_roll: -1.23,
- Stompy.left_arm.elbow_pitch: 3.0,
- Stompy.right_arm.elbow_pitch: 3.0,
- # hands
- Stompy.left_arm.hand.left_finger: 0.0,
- Stompy.left_arm.hand.right_finger: 0.0,
- Stompy.right_arm.hand.left_finger: 0.0,
- Stompy.right_arm.hand.right_finger: 0.0,
- Stompy.left_arm.hand.wrist_roll: -0.346,
- Stompy.left_arm.hand.wrist_pitch: -0.251,
- Stompy.left_arm.hand.wrist_yaw: 0.377,
- Stompy.right_arm.hand.wrist_roll: -3.14,
- Stompy.right_arm.hand.wrist_pitch: -0.437,
- Stompy.right_arm.hand.wrist_yaw: 0.911,
- # legs
- Stompy.legs.left.hip_pitch: -1.55,
- Stompy.legs.left.hip_roll: 1.46,
- Stompy.legs.left.hip_yaw: 1.45,
- Stompy.legs.left.knee_pitch: 2.17,
- Stompy.legs.left.ankle_pitch: 0.238,
- Stompy.legs.left.ankle_roll: 1.79,
- Stompy.legs.right.hip_pitch: -1.55,
- Stompy.legs.right.hip_roll: -1.67,
- Stompy.legs.right.hip_yaw: 1.04,
- Stompy.legs.right.knee_pitch: 2.01,
- Stompy.legs.right.ankle_pitch: 0.44,
- Stompy.legs.right.ankle_roll: 1.79,
+ Stompy.legs.left.hip_pitch: 1.61,
+ Stompy.legs.left.hip_yaw: 1,
+ Stompy.legs.left.hip_roll: 0,
+ Stompy.legs.left.knee_pitch: 2.05,
+ Stompy.legs.left.ankle_pitch: 0.33,
+ Stompy.legs.left.ankle_roll: 1.73,
+ Stompy.legs.right.hip_pitch: 0,
+ Stompy.legs.right.hip_yaw: -2.15,
+ Stompy.legs.right.hip_roll: -1.6,
+ Stompy.legs.right.knee_pitch: 2.16,
+ Stompy.legs.right.ankle_pitch: 0.5,
+ Stompy.legs.right.ankle_roll: 1.72,
}
@classmethod
def default_limits(cls) -> Dict[str, Dict[str, float]]:
return {
- Stompy.torso.roll: {
- "lower": -4.36332,
- "upper": 4.36332,
- },
- Stompy.left_arm.shoulder_pitch: {
- "lower": -0.0,
- "upper": 0.2,
- },
- Stompy.left_arm.shoulder_yaw: {
- "lower": 0.97738438,
- "upper": 5.3058009,
- },
- Stompy.left_arm.shoulder_roll: {
- "lower": -4.71239,
- "upper": 4.71239,
- },
- Stompy.right_arm.shoulder_pitch: {
- "lower": -4.71239,
- "upper": 4.71239,
- },
- Stompy.right_arm.shoulder_yaw: {
- "lower": 0.97738438,
- "upper": 5.3058009,
- },
- Stompy.right_arm.shoulder_roll: {
- "lower": -4.71239,
- "upper": 4.71239,
- },
- Stompy.left_arm.hand.wrist_roll: {
- "lower": -4.71239,
- "upper": 4.71239,
- },
- Stompy.left_arm.hand.wrist_pitch: {
- "lower": -3.66519,
- "upper": -1.39626,
- },
- Stompy.left_arm.hand.wrist_yaw: {
- "lower": 0,
- "upper": 1.5708,
- },
- Stompy.right_arm.hand.wrist_roll: {
- "lower": -4.71239,
- "upper": 4.71239,
- },
- Stompy.right_arm.hand.wrist_pitch: {
- "lower": -1.5708,
- "upper": 0.523599,
- },
- Stompy.right_arm.hand.wrist_yaw: {
- "lower": -1.5708,
- "upper": 0,
- },
Stompy.legs.left.hip_pitch: {
- "lower": -4.712389,
- "upper": 4.712389,
- },
- Stompy.legs.left.hip_roll: {
- "lower": -3.14159,
- "upper": 0,
+ "lower": 0.5,
+ "upper": 2.69,
},
Stompy.legs.left.hip_yaw: {
- "lower": -1.0472,
- "upper": 2.0944,
+ "lower": 0.5,
+ "upper": 1.19,
+ },
+ Stompy.legs.left.hip_roll: {
+ "lower": -0.5,
+ "upper": 0.5,
},
Stompy.legs.left.knee_pitch: {
- "lower": -4.18879,
- "upper": 0,
+ "lower": 0.5,
+ "upper": 2.5,
},
Stompy.legs.left.ankle_pitch: {
- "lower": -1.5708,
- "upper": 2.18166,
+ "lower": -0.8,
+ "upper": 0.6,
},
Stompy.legs.left.ankle_roll: {
- "lower": -2.26893,
- "upper": -1.22173,
+ "lower": 1,
+ "upper": 2.3,
},
Stompy.legs.right.hip_pitch: {
- "lower": -4.712389,
- "upper": 4.712389,
- },
- Stompy.legs.right.hip_roll: {
- "lower": 0,
- "upper": 3.14159,
+ "lower": -1,
+ "upper": 1,
},
Stompy.legs.right.hip_yaw: {
- "lower": -1.0472,
- "upper": 2.0944,
+ "lower": -2.2,
+ "upper": -1,
+ },
+ Stompy.legs.right.hip_roll: {
+ "lower": -2.39,
+ "upper": -1,
},
Stompy.legs.right.knee_pitch: {
- "lower": -4.18879,
- "upper": 0,
+ "lower": 1.54,
+ "upper": 3,
},
Stompy.legs.right.ankle_pitch: {
- "lower": -1.5708,
- "upper": 2.18166,
- },
- Stompy.legs.right.ankle_roll: {
- "lower": -2.26893,
- "upper": -1.22173,
- },
- Stompy.left_arm.elbow_pitch: {
- "lower": 1.4486233,
- "higher": 5.4454273,
- },
- Stompy.right_arm.elbow_pitch: {
- "lower": 1.4486233,
- "higher": 5.4454273,
- },
- Stompy.left_arm.hand.left_finger: {
- "lower": -0.051,
- "upper": 0.0,
- },
- Stompy.left_arm.hand.right_finger: {
"lower": 0,
- "upper": 0.051,
- },
- Stompy.right_arm.hand.left_finger: {
- "lower": -0.051,
- "upper": 0.0,
+ "upper": 1.5,
},
- Stompy.right_arm.hand.right_finger: {
- "lower": 0,
- "upper": 0.051,
+ Stompy.legs.right.ankle_roll: {
+ "lower": 1,
+ "upper": 2.3,
},
}
-
-class StompyFixed(Stompy):
- torso = Torso()
- left_arm = LeftArm()
- right_arm = RightArm()
- legs = Legs()
-
+ # p_gains
@classmethod
- def default_standing(cls) -> Dict[str, float]:
+ def stiffness(cls) -> Dict[str, float]:
return {
- Stompy.torso.roll: 2.58,
- # arms
- Stompy.left_arm.shoulder_pitch: -0.534,
- Stompy.left_arm.shoulder_yaw: 2.54,
- Stompy.left_arm.shoulder_roll: -0.0314,
- Stompy.right_arm.shoulder_pitch: 2.45,
- Stompy.right_arm.shoulder_yaw: 3.77,
- Stompy.right_arm.shoulder_roll: -0.0314,
- Stompy.left_arm.elbow_pitch: 2.35,
- Stompy.right_arm.elbow_pitch: 2.65,
- # hands
- Stompy.left_arm.hand.left_finger: 0.0,
- Stompy.left_arm.hand.right_finger: 0.0,
- Stompy.right_arm.hand.left_finger: 0.0,
- Stompy.right_arm.hand.right_finger: 0.0,
- Stompy.left_arm.hand.wrist_roll: 1.79,
- Stompy.left_arm.hand.wrist_pitch: 1.35,
- Stompy.left_arm.hand.wrist_yaw: 1.07,
- Stompy.right_arm.hand.wrist_roll: -2.13,
- Stompy.right_arm.hand.wrist_pitch: 1.79,
- Stompy.right_arm.hand.wrist_yaw: -0.251,
- # legs
- Stompy.legs.left.hip_pitch: -1.6,
- Stompy.legs.left.hip_roll: 1.41,
- Stompy.legs.left.hip_yaw: -2.12,
- Stompy.legs.left.knee_pitch: 2.01,
- Stompy.legs.left.ankle_pitch: 0.238,
- Stompy.legs.left.ankle_roll: 1.85,
- Stompy.legs.right.hip_pitch: 1.76,
- Stompy.legs.right.hip_roll: -1.54,
- Stompy.legs.right.hip_yaw: 0.967,
- Stompy.legs.right.knee_pitch: 2.07,
- Stompy.legs.right.ankle_pitch: 0.377,
- Stompy.legs.right.ankle_roll: 1.92,
+ "hip pitch": 250,
+ "hip yaw": 150,
+ "hip roll": 150,
+ "knee pitch": 150,
+ "ankle pitch": 45,
+ "ankle roll": 45,
}
+ # d_gains
@classmethod
- def default_limits(cls) -> Dict[str, Dict[str, float]]:
+ def damping(cls) -> Dict[str, float]:
return {
- Stompy.torso.roll: {
- "lower": 2.53,
- "upper": 2.63,
- },
- Stompy.left_arm.shoulder_pitch: {
- "lower": -0.584,
- "upper": -0.484,
- },
- Stompy.left_arm.shoulder_yaw: {
- "lower": 2.49,
- "upper": 2.59,
- },
- Stompy.left_arm.shoulder_roll: {
- "lower": -0.0814,
- "upper": 0.0186,
- },
- Stompy.right_arm.shoulder_pitch: {
- "lower": 2.40,
- "upper": 2.50,
- },
- Stompy.right_arm.shoulder_yaw: {
- "lower": 3.72,
- "upper": 3.82,
- },
- Stompy.right_arm.shoulder_roll: {
- "lower": -0.0814,
- "upper": 0.0186,
- },
- Stompy.left_arm.elbow_pitch: {
- "lower": 2.30,
- "upper": 2.40,
- },
- Stompy.right_arm.elbow_pitch: {
- "lower": 2.60,
- "upper": 2.70,
- },
- Stompy.left_arm.hand.left_finger: {
- "lower": -0.05,
- "upper": 0.05,
- },
- Stompy.left_arm.hand.right_finger: {
- "lower": -0.05,
- "upper": 0.05,
- },
- Stompy.right_arm.hand.left_finger: {
- "lower": -0.05,
- "upper": 0.05,
- },
- Stompy.right_arm.hand.right_finger: {
- "lower": -0.05,
- "upper": 0.05,
- },
- Stompy.left_arm.hand.wrist_roll: {
- "lower": 1.74,
- "upper": 1.84,
- },
- Stompy.left_arm.hand.wrist_pitch: {
- "lower": 1.30,
- "upper": 1.40,
- },
- Stompy.left_arm.hand.wrist_yaw: {
- "lower": 1.02,
- "upper": 1.12,
- },
- Stompy.right_arm.hand.wrist_roll: {
- "lower": -2.18,
- "upper": -2.08,
- },
- Stompy.right_arm.hand.wrist_pitch: {
- "lower": 1.74,
- "upper": 1.84,
- },
- Stompy.right_arm.hand.wrist_yaw: {
- "lower": -0.301,
- "upper": -0.201,
- },
- Stompy.legs.left.hip_pitch: {
- "lower": -1.65,
- "upper": -1.55,
- },
- Stompy.legs.left.hip_roll: {
- "lower": 1.36,
- "upper": 1.46,
- },
- Stompy.legs.left.hip_yaw: {
- "lower": -2.17,
- "upper": -2.07,
- },
- Stompy.legs.left.knee_pitch: {
- "lower": 1.96,
- "upper": 2.06,
- },
- Stompy.legs.left.ankle_pitch: {
- "lower": 0.188,
- "upper": 0.288,
- },
- Stompy.legs.left.ankle_roll: {
- "lower": 1.80,
- "upper": 1.90,
- },
- Stompy.legs.right.hip_pitch: {
- "lower": 1.71,
- "upper": 1.81,
- },
- Stompy.legs.right.hip_roll: {
- "lower": -1.59,
- "upper": -1.49,
- },
- Stompy.legs.right.hip_yaw: {
- "lower": 0.917,
- "upper": 1.017,
- },
- Stompy.legs.right.knee_pitch: {
- "lower": 2.02,
- "upper": 2.12,
- },
- Stompy.legs.right.ankle_pitch: {
- "lower": 0.327,
- "upper": 0.427,
- },
- Stompy.legs.right.ankle_roll: {
- "lower": 1.87,
- "upper": 1.97,
- },
+ "hip pitch": 15,
+ "hip yaw": 10,
+ "hip roll": 10,
+ "knee pitch": 10,
+ "ankle pitch": 3,
+ "ankle roll": 3,
}
+ # pos_limits
+ @classmethod
+ def effort(cls) -> Dict[str, float]:
+ return {
+ "hip pitch": 150,
+ "hip yaw": 90,
+ "hip roll": 90,
+ "knee pitch": 90,
+ "ankle pitch": 24,
+ "ankle roll": 24,
+ }
-class ImuTorso(Torso):
- pitch = "torso_1_x8_1_dof_x8"
- pelvis_tx = "pelvis_tx"
- pelvis_tz = "pelvis_tz"
- pelvis_ty = "pelvis_ty"
- tilt = "pelvis_tilt"
- list = "pelvis_list"
- rotation = "pelvis_rotation"
-
-
-class MjcfStompy(Stompy):
- torso = ImuTorso()
- left_arm = LeftArm()
- right_arm = RightArm()
- legs = Legs()
-
- # test the model
+ # vel_limits
@classmethod
- def default_standing(cls) -> Dict[str, float]:
+ def velocity(cls) -> Dict[str, float]:
return {
- Stompy.torso.roll: 2.58,
- # arms
- Stompy.left_arm.shoulder_pitch: -0.534,
- Stompy.left_arm.shoulder_yaw: 2.54,
- Stompy.left_arm.shoulder_roll: -0.0314,
- Stompy.right_arm.shoulder_pitch: 2.45,
- Stompy.right_arm.shoulder_yaw: 3.77,
- Stompy.right_arm.shoulder_roll: -0.0314,
- Stompy.left_arm.elbow_pitch: 2.35,
- Stompy.right_arm.elbow_pitch: 2.65,
- # hands
- Stompy.left_arm.hand.left_finger: 0.0,
- Stompy.left_arm.hand.right_finger: 0.0,
- Stompy.right_arm.hand.left_finger: 0.0,
- Stompy.right_arm.hand.right_finger: 0.0,
- Stompy.left_arm.hand.wrist_roll: 1.79,
- Stompy.left_arm.hand.wrist_pitch: 1.35,
- Stompy.left_arm.hand.wrist_yaw: 1.07,
- Stompy.right_arm.hand.wrist_roll: -2.13,
- Stompy.right_arm.hand.wrist_pitch: 1.79,
- Stompy.right_arm.hand.wrist_yaw: -0.251,
- # legs
- Stompy.legs.left.hip_pitch: -1.6,
- Stompy.legs.left.hip_roll: 1.41,
- Stompy.legs.left.hip_yaw: -2.12,
- Stompy.legs.left.knee_pitch: 2.01,
- Stompy.legs.left.ankle_pitch: 0.238,
- Stompy.legs.left.ankle_roll: 1.85,
- Stompy.legs.right.hip_pitch: 1.76,
- Stompy.legs.right.hip_roll: -1.54,
- Stompy.legs.right.hip_yaw: 0.967,
- Stompy.legs.right.knee_pitch: 2.07,
- Stompy.legs.right.ankle_pitch: 0.377,
- Stompy.legs.right.ankle_roll: 1.92,
+ "hip pitch": 25,
+ "hip yaw": 40,
+ "hip roll": 40,
+ "knee pitch": 40,
+ "ankle pitch": 40,
+ "ankle roll": 40,
}
@classmethod
- def default_limits(cls) -> Dict[str, Dict[str, float]]:
+ def friction(cls) -> Dict[str, float]:
return {
- Stompy.torso.roll: {
- "lower": 2.53,
- "upper": 2.63,
- },
- Stompy.left_arm.shoulder_pitch: {
- "lower": -0.584,
- "upper": -0.484,
- },
- Stompy.left_arm.shoulder_yaw: {
- "lower": 2.49,
- "upper": 2.59,
- },
- Stompy.left_arm.shoulder_roll: {
- "lower": -0.0814,
- "upper": 0.0186,
- },
- Stompy.right_arm.shoulder_pitch: {
- "lower": 2.40,
- "upper": 2.50,
- },
- Stompy.right_arm.shoulder_yaw: {
- "lower": 3.72,
- "upper": 3.82,
- },
- Stompy.right_arm.shoulder_roll: {
- "lower": -0.0814,
- "upper": 0.0186,
- },
- Stompy.left_arm.elbow_pitch: {
- "lower": 2.30,
- "upper": 2.40,
- },
- Stompy.right_arm.elbow_pitch: {
- "lower": 2.60,
- "upper": 2.70,
- },
- Stompy.left_arm.hand.left_finger: {
- "lower": -0.05,
- "upper": 0.05,
- },
- Stompy.left_arm.hand.right_finger: {
- "lower": -0.05,
- "upper": 0.05,
- },
- Stompy.right_arm.hand.left_finger: {
- "lower": -0.05,
- "upper": 0.05,
- },
- Stompy.right_arm.hand.right_finger: {
- "lower": -0.05,
- "upper": 0.05,
- },
- Stompy.left_arm.hand.wrist_roll: {
- "lower": 1.74,
- "upper": 1.84,
- },
- Stompy.left_arm.hand.wrist_pitch: {
- "lower": 1.30,
- "upper": 1.40,
- },
- Stompy.left_arm.hand.wrist_yaw: {
- "lower": 1.02,
- "upper": 1.12,
- },
- Stompy.right_arm.hand.wrist_roll: {
- "lower": -2.18,
- "upper": -2.08,
- },
- Stompy.right_arm.hand.wrist_pitch: {
- "lower": 1.74,
- "upper": 1.84,
- },
- Stompy.right_arm.hand.wrist_yaw: {
- "lower": -0.301,
- "upper": -0.201,
- },
- Stompy.legs.left.hip_pitch: {
- "lower": -1.65,
- "upper": -1.55,
- },
- Stompy.legs.left.hip_roll: {
- "lower": 1.36,
- "upper": 1.46,
- },
- Stompy.legs.left.hip_yaw: {
- "lower": -2.17,
- "upper": -2.07,
- },
- Stompy.legs.left.knee_pitch: {
- "lower": 1.96,
- "upper": 2.06,
- },
- Stompy.legs.left.ankle_pitch: {
- "lower": 0.188,
- "upper": 0.288,
- },
- Stompy.legs.left.ankle_roll: {
- "lower": 1.80,
- "upper": 1.90,
- },
- Stompy.legs.right.hip_pitch: {
- "lower": 1.71,
- "upper": 1.81,
- },
- Stompy.legs.right.hip_roll: {
- "lower": -1.59,
- "upper": -1.49,
- },
- Stompy.legs.right.hip_yaw: {
- "lower": 0.917,
- "upper": 1.017,
- },
- Stompy.legs.right.knee_pitch: {
- "lower": 2.02,
- "upper": 2.12,
- },
- Stompy.legs.right.ankle_pitch: {
- "lower": 0.327,
- "upper": 0.427,
- },
- Stompy.legs.right.ankle_roll: {
- "lower": 1.87,
- "upper": 1.97,
- },
+ "hip pitch": 0.0,
+ "hip yaw": 0.0,
+ "hip roll": 0.0,
+ "knee pitch": 0.0,
+ "ankle pitch": 0.0,
+ "ankle roll": 0.1,
}
diff --git a/sim/stompy_legs/joints.py b/sim/stompy_legs/joints.py
index e66f217c..6756f89e 100644
--- a/sim/stompy_legs/joints.py
+++ b/sim/stompy_legs/joints.py
@@ -85,16 +85,16 @@ def default_positions(cls) -> Dict[str, float]:
@classmethod
def default_standing(cls) -> Dict[str, float]:
return {
- # # # legs
+ # legs
Stompy.legs.left.hip_pitch: 1.61,
- Stompy.legs.left.hip_roll: 0,
Stompy.legs.left.hip_yaw: 1,
+ Stompy.legs.left.hip_roll: 0,
Stompy.legs.left.knee_pitch: 2.05,
Stompy.legs.left.ankle_pitch: 0.33,
Stompy.legs.left.ankle_roll: 1.73,
Stompy.legs.right.hip_pitch: 0,
- Stompy.legs.right.hip_roll: -1.6,
Stompy.legs.right.hip_yaw: -2.15,
+ Stompy.legs.right.hip_roll: -1.6,
Stompy.legs.right.knee_pitch: 2.16,
Stompy.legs.right.ankle_pitch: 0.5,
Stompy.legs.right.ankle_roll: 1.72,
@@ -107,45 +107,45 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]:
"lower": 0.5,
"upper": 2.69,
},
- Stompy.legs.left.hip_roll: {
- "lower": -0.5,
- "upper": 0.5,
- },
Stompy.legs.left.hip_yaw: {
"lower": 0.5,
"upper": 1.19,
},
+ Stompy.legs.left.hip_roll: {
+ "lower": -0.5,
+ "upper": 0.5,
+ },
Stompy.legs.left.knee_pitch: {
- "lower": 1,
- "upper": 3.1,
+ "lower": 0.5,
+ "upper": 2.5,
},
Stompy.legs.left.ankle_pitch: {
- "lower": -0.3,
- "upper": 1.13,
+ "lower": -0.8,
+ "upper": 0.6,
},
Stompy.legs.left.ankle_roll: {
- "lower": 1.3,
- "upper": 2,
+ "lower": 1,
+ "upper": 2.3,
},
Stompy.legs.right.hip_pitch: {
"lower": -1,
"upper": 1,
},
- Stompy.legs.right.hip_roll: {
- "lower": -2.39,
- "upper": -1,
- },
Stompy.legs.right.hip_yaw: {
"lower": -2.2,
"upper": -1,
},
+ Stompy.legs.right.hip_roll: {
+ "lower": -2.39,
+ "upper": -1,
+ },
Stompy.legs.right.knee_pitch: {
"lower": 1.54,
"upper": 3,
},
Stompy.legs.right.ankle_pitch: {
- "lower": -0.5,
- "upper": 0.94,
+ "lower": 0,
+ "upper": 1.5,
},
Stompy.legs.right.ankle_roll: {
"lower": 1,
@@ -153,6 +153,65 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]:
},
}
+ # p_gains
+ @classmethod
+ def stiffness(cls) -> Dict[str, float]:
+ return {
+ "hip pitch": 250,
+ "hip yaw": 150,
+ "hip roll": 150,
+ "knee pitch": 150,
+ "ankle pitch": 45,
+ "ankle roll": 45,
+ }
+
+ # d_gains
+ @classmethod
+ def damping(cls) -> Dict[str, float]:
+ return {
+ "hip pitch": 15,
+ "hip yaw": 10,
+ "hip roll": 10,
+ "knee pitch": 10,
+ "ankle pitch": 3,
+ "ankle roll": 3,
+ }
+
+ # pos_limits
+ @classmethod
+ def effort(cls) -> Dict[str, float]:
+ return {
+ "hip pitch": 150,
+ "hip yaw": 90,
+ "hip roll": 90,
+ "knee pitch": 90,
+ "ankle pitch": 24,
+ "ankle roll": 24,
+ }
+
+ # vel_limits
+ @classmethod
+ def velocity(cls) -> Dict[str, float]:
+ return {
+ "hip pitch": 25,
+ "hip yaw": 40,
+ "hip roll": 40,
+ "knee pitch": 40,
+ "ankle pitch": 40,
+ "ankle roll": 40,
+ }
+
+ @classmethod
+ def friction(cls) -> Dict[str, float]:
+ return {
+ "hip pitch": 0.0,
+ "hip yaw": 0.0,
+ "hip roll": 0.0,
+ "knee pitch": 0.0,
+ "ankle pitch": 0.0,
+ "ankle roll": 0.1,
+ }
+
def print_joints() -> None:
joints = Stompy.all_joints()
diff --git a/sim/stompy_legs/robot.urdf b/sim/stompy_legs/robot.urdf
new file mode 100644
index 00000000..321b1df3
--- /dev/null
+++ b/sim/stompy_legs/robot.urdf
@@ -0,0 +1,151 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/sim/stompy_legs/robot.xml b/sim/stompy_legs/robot.xml
new file mode 100644
index 00000000..9fa4358a
--- /dev/null
+++ b/sim/stompy_legs/robot.xml
@@ -0,0 +1,150 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sim/stompy_legs/robot_fixed.urdf b/sim/stompy_legs/robot_fixed.urdf
index aa14d37c..63c033c8 100644
--- a/sim/stompy_legs/robot_fixed.urdf
+++ b/sim/stompy_legs/robot_fixed.urdf
@@ -1,1263 +1,150 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/sim/stompy_legs/robot_fixed.xml b/sim/stompy_legs/robot_fixed.xml
index 827e661d..21185b2d 100644
--- a/sim/stompy_legs/robot_fixed.xml
+++ b/sim/stompy_legs/robot_fixed.xml
@@ -1,22 +1,17 @@
-
+
-
-
+
+
-
-
-
-
+
-
+
+
@@ -69,16 +64,13 @@
-
+
-
-
-
-
-
+
+
-
+
@@ -88,10 +80,9 @@
-
-
+
@@ -100,7 +91,7 @@
-
+
@@ -111,7 +102,7 @@
-
+
@@ -120,7 +111,7 @@
-
+
@@ -131,7 +122,7 @@
-
+
@@ -142,24 +133,22 @@
-
+
-
-
+
-
-
+
@@ -168,7 +157,7 @@
-
+
@@ -179,7 +168,7 @@
-
+
@@ -188,7 +177,7 @@
-
+
@@ -199,7 +188,7 @@
-
+
@@ -210,14 +199,13 @@
-
+
-
-
+
@@ -227,53 +215,53 @@
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
-
+
-
+
-
+
-
+
-
+
-
+
@@ -281,6 +269,6 @@
-
+
-
+
\ No newline at end of file