Skip to content

Commit

Permalink
fix up default position argument
Browse files Browse the repository at this point in the history
  • Loading branch information
codekansas committed Oct 23, 2024
1 parent 7e4e39b commit 7d93baa
Show file tree
Hide file tree
Showing 3 changed files with 18 additions and 19 deletions.
2 changes: 1 addition & 1 deletion tests/sample/robot_test.xml
Original file line number Diff line number Diff line change
Expand Up @@ -169,6 +169,6 @@
</sensor>

<keyframe>
<key name="default" qpos="0.0 0.0 0.63 0.0 0.0 0.0 1.0 -0.23 0.0 0.0 0.441 -0.258 -0.23 0.0 0.0 0.441 -0.258" />
<key name="default" qpos="0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0" />
</keyframe>
</mujoco>
2 changes: 1 addition & 1 deletion tests/test_conversion.py
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ def test_conversion_no_frc_limit(tmpdir: Path) -> None:
mjcf_path=mjcf_path,
no_frc_limit=True,
copy_meshes=False,
default_position="0.0 0.0 0.63 0.0 0.0 0.0 1.0 -0.23 0.0 0.0 0.441 -0.258 -0.23 0.0 0.0 0.441 -0.258",
default_position=[0.0] * 10,
)

# Compare the outputted MJCF with the expected XML
Expand Down
33 changes: 16 additions & 17 deletions urdf2mjcf/convert.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
import tempfile
import xml.etree.ElementTree as ET
from pathlib import Path
from typing import Union
from typing import List, Union

import mujoco

Expand Down Expand Up @@ -449,17 +449,25 @@ def add_visual_geom_logic(root: ET.Element) -> None:
body.insert(index + 1, new_geom)


def add_default_position(root: ET.Element, default_position: str) -> None:
def add_default_position(root: ET.Element, default_position: List[float]) -> None:
"""Add a keyframe to the root element.
Args:
root: The root element of the MJCF file.
default_position: The default position of the robot.
"""
actuators = root.find("actuator")
if actuators is None:
raise ValueError("No actuators found in the MJCF file.")

num_actuators = len(list(actuators.iter("motor")))
if len(default_position) != num_actuators:
raise ValueError(f"Default position must have {num_actuators} values, got {len(default_position)}.")

keyframe = ET.Element("keyframe")
key = ET.SubElement(keyframe, "key")
key.set("name", "default")
key.set("qpos", default_position)
key.set("qpos", " ".join(map(str, default_position)))
root.append(keyframe)


Expand All @@ -471,7 +479,7 @@ def convert_urdf_to_mjcf(
camera_distance: float = 3.0,
camera_height_offset: float = 0.5,
no_frc_limit: bool = False,
default_position: Union[str, None] = None,
default_position: Union[List[float], None] = None,
) -> None:
"""Convert a URDF file to an MJCF file.
Expand Down Expand Up @@ -552,17 +560,13 @@ def convert_urdf_to_mjcf(
add_compiler(root)
add_option(root)
add_assets(root)
add_cameras(
root,
distance=camera_distance,
height_offset=camera_height_offset,
)
add_cameras(root, distance=camera_distance, height_offset=camera_height_offset)
add_root_body(root)
add_worldbody_elements(root)
add_actuators(root, no_frc_limit)
add_sensors(root)
add_visual_geom_logic(root)
if default_position:
if default_position is not None:
add_default_position(root, default_position)

# Copy mesh files to the output directory.
Expand All @@ -587,12 +591,7 @@ def main() -> None:
parser.add_argument("--camera-distance", type=float, default=3.0, help="Camera distance from the robot.")
parser.add_argument("--camera-height-offset", type=float, default=0.5, help="Camera height offset.")
parser.add_argument("--no-frc-limit", action="store_true", help="Do not include force limit for the actuators.")
parser.add_argument(
"--default-position",
type=str,
default="0.0 0.0 0.63 0.0 0.0 0.0 1.0 -0.23 0.0 0.0 0.441 -0.258 -0.23 0.0 0.0 0.441 -0.258",
help="Default position for the robot.",
)
parser.add_argument("--default-position", type=str, help="Default position for the robot.")
args = parser.parse_args()

convert_urdf_to_mjcf(
Expand All @@ -603,7 +602,7 @@ def main() -> None:
camera_distance=args.camera_distance,
camera_height_offset=args.camera_height_offset,
no_frc_limit=args.no_frc_limit,
default_position=args.default_position,
default_position=None if args.default_position is None else list(map(float, args.default_position.split())),
)


Expand Down

0 comments on commit 7d93baa

Please sign in to comment.