Skip to content

Commit

Permalink
Merge pull request #238 from ami-iit/fix/remove_is_urdf
Browse files Browse the repository at this point in the history
Remove `is_urdf` argument when building the model
  • Loading branch information
diegoferigo authored Sep 25, 2024
2 parents 100b60c + cf1ff07 commit 8080f3c
Show file tree
Hide file tree
Showing 9 changed files with 40 additions and 49 deletions.
2 changes: 1 addition & 1 deletion environment.yml
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ dependencies:
- jax-dataclasses >= 1.4.0
- pptree
- qpax
- rod >= 0.3.0
- rod >= 0.3.3
- typing_extensions # python<3.12
# ====================================
# Optional dependencies from setup.cfg
Expand Down
2 changes: 1 addition & 1 deletion examples/PD_controller.ipynb
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@
"num_steps = int(integration_time / dt)\n",
"\n",
"model = js.model.JaxSimModel.build_from_model_description(\n",
" model_description=model_urdf_string, is_urdf=True\n",
" model_description=model_urdf_string\n",
")\n",
"data = js.data.JaxSimModelData.build(model=model)\n",
"integrator = integrators.fixed_step.RungeKutta4SO3.build(\n",
Expand Down
2 changes: 1 addition & 1 deletion pyproject.toml
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ dependencies = [
"jax_dataclasses >= 1.4.0",
"pptree",
"qpax",
"rod >= 0.3.0",
"rod >= 0.3.3",
"typing_extensions ; python_version < '3.12'",
]

Expand Down
6 changes: 3 additions & 3 deletions src/jaxsim/api/model.py
Original file line number Diff line number Diff line change
Expand Up @@ -106,8 +106,8 @@ def build_from_model_description(
terrain:
The optional terrain to consider.
is_urdf:
Whether the model description is a URDF or an SDF. This is
automatically inferred if the model description is a path to a file.
The optional flag to force the model description to be parsed as a
URDF or a SDF. This is otherwise automatically inferred.
considered_joints:
The list of joints to consider. If None, all joints are considered.
Expand All @@ -120,7 +120,7 @@ def build_from_model_description(
# Parse the input resource (either a path to file or a string with the URDF/SDF)
# and build the -intermediate- model description.
intermediate_description = jaxsim.parsers.rod.build_model_description(
model_description=model_description, is_urdf=is_urdf
model_description=model_description
)

# Lump links together if not all joints are considered.
Expand Down
2 changes: 1 addition & 1 deletion src/jaxsim/mujoco/loaders.py
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ def load_rod_model(
Args:
model_description: The URDF/SDF file or ROD model to load.
is_urdf: Whether the model description is a URDF file.
is_urdf: Whether to force parsing the model description as a URDF file.
model_name: The name of the model to load from the resource.
Returns:
Expand Down
52 changes: 30 additions & 22 deletions src/jaxsim/parsers/rod/parser.py
Original file line number Diff line number Diff line change
Expand Up @@ -33,38 +33,45 @@ class SDFData(NamedTuple):


def extract_model_data(
model_description: pathlib.Path | str | rod.Model,
model_description: pathlib.Path | str | rod.Model | rod.Sdf,
model_name: str | None = None,
is_urdf: bool | None = None,
) -> SDFData:
"""
Extract data from an SDF/URDF resource useful to build a JaxSim model.
Args:
model_description: A path to an SDF/URDF file, a string containing its content,
or a pre-parsed/pre-built rod model.
model_description:
A path to an SDF/URDF file, a string containing its content, or
a pre-parsed/pre-built rod model.
model_name: The name of the model to extract from the SDF resource.
is_urdf: Whether the SDF resource is a URDF file. Needed only if model_description
is a URDF string.
is_urdf:
Whether to force parsing the resource as a URDF file. Automatically
detected if not provided.
Returns:
The extracted model data.
"""

if isinstance(model_description, rod.Model):
sdf_model = model_description
else:
# Parse the SDF resource.
sdf_element = rod.Sdf.load(sdf=model_description, is_urdf=is_urdf)

if len(sdf_element.models()) == 0:
raise RuntimeError("Failed to find any model in SDF resource")

# Assume the SDF resource has only one model, or the desired model name is given.
sdf_models = {m.name: m for m in sdf_element.models()}
sdf_model = (
sdf_element.models()[0] if len(sdf_models) == 1 else sdf_models[model_name]
)
match model_description:
case rod.Model():
sdf_model = model_description
case rod.Sdf() | str() | pathlib.Path():
sdf_element = (
model_description
if isinstance(model_description, rod.Sdf)
else rod.Sdf.load(sdf=model_description, is_urdf=is_urdf)
)
if not sdf_element.models():
raise RuntimeError("Failed to find any model in SDF resource")

# Assume the SDF resource has only one model, or the desired model name is given.
sdf_models = {m.name: m for m in sdf_element.models()}
sdf_model = (
sdf_element.models()[0]
if len(sdf_models) == 1
else sdf_models[model_name]
)

# Log model name.
logging.debug(msg=f"Found model '{sdf_model.name}' in SDF resource")
Expand Down Expand Up @@ -344,16 +351,17 @@ def extract_model_data(

def build_model_description(
model_description: pathlib.Path | str | rod.Model,
is_urdf: bool | None = False,
is_urdf: bool | None = None,
) -> descriptions.ModelDescription:
"""
Builds a model description from an SDF/URDF resource.
Args:
model_description: A path to an SDF/URDF file, a string containing its content,
or a pre-parsed/pre-built rod model.
is_urdf: Whether the SDF resource is a URDF file. Needed only if model_description
is a URDF string.
is_urdf: Whether the force parsing the resource as a URDF file. Automatically
detected if not provided.
Returns:
The parsed model description.
"""
Expand Down
15 changes: 0 additions & 15 deletions tests/conftest.py
Original file line number Diff line number Diff line change
Expand Up @@ -83,24 +83,9 @@ def build_jaxsim_model(
A JaxSim model built from the provided description.
"""

is_urdf = None

# If the provided description is a string, automatically detect if it
# contains the content of a URDF or SDF file.
if isinstance(model_description, str):
if "<robot" in model_description:
is_urdf = True

elif "<sdf>" in model_description:
is_urdf = False

else:
is_urdf = None

# Build the JaxSim model.
model = js.model.JaxSimModel.build_from_model_description(
model_description=model_description,
is_urdf=is_urdf,
)

return model
Expand Down
2 changes: 1 addition & 1 deletion tests/test_api_contact.py
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ def test_contact_jacobian_derivative(
W_p_Ci = model.kin_dyn_parameters.contact_parameters.point

# Load the model in ROD.
rod_model = rod.Sdf.load(sdf=model.built_from, is_urdf=True).model
rod_model = rod.Sdf.load(sdf=model.built_from).model

# Add dummy frames on the contact points.
for idx, (link_name, W_p_C) in enumerate(
Expand Down
6 changes: 2 additions & 4 deletions tests/test_pytree.py
Original file line number Diff line number Diff line change
Expand Up @@ -14,14 +14,12 @@ def test_call_jit_compiled_function_passing_different_objects(

# Create a first model from the URDF.
model1 = js.model.JaxSimModel.build_from_model_description(
model_description=ergocub_model_description_path,
is_urdf=True,
model_description=ergocub_model_description_path
)

# Create a second model from the URDF.
model2 = js.model.JaxSimModel.build_from_model_description(
model_description=ergocub_model_description_path,
is_urdf=True,
model_description=ergocub_model_description_path
)

# The objects should be different, but the comparison should return True.
Expand Down

0 comments on commit 8080f3c

Please sign in to comment.