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 @@ - + - - + + - - - - + - + + \ No newline at end of file