Issue in updating hw metadata in model in which not all links are parametrizable #427
Description
Description
When trying to update the hw parameters of a model in which some of the links have been skipped (since they are not supported, for example due to multiple visuals), we get an error. We should account for the fact that the parametrizable links are always a subset of the links of a model (one of the edge cases is that the two sets coincide but this is only one possibility) when handling them.
Repro
import os
from pathlib import Path
os.environ["JAXSIM_LOGGING_LEVEL"] = "debug"
import jax.numpy as jnp
import jaxsim.api as js
urdf_file = <path-to-urdf>
model = js.model.JaxSimModel.build_from_model_description(
model_description=urdf_file,
time_step=0.001,
is_urdf=True,
)
# model = js.model.reduce(model=model, considered_joints=JOINT_NAMES)
base_position = jnp.array([0.0, 0.0, 1.5], dtype=jnp.float32)
data = js.data.JaxSimModelData.build(
model=model,
base_position=base_position,
)
print(f"Link names: \n{model.link_names()}")
print(f"Joint names: \n{model.joint_names()}")
# Define the link used in the loss function.
link_name = "torso_1"
link_idx = js.link.name_to_idx(model, link_name=link_name)
# Define the initial hardware parameters (scaling factors).
initial_dims = jnp.ones(
(
model.number_of_links(),
3,
)
) # Initial dimensions (1.0 for all links).
initial_density = jnp.ones(
(model.number_of_links(),)
) # Initial density (1.0 for all links).
s = jnp.ones(
(
model.number_of_links(),
3,
)
) # Initial dimensions (1.0 for all links).
s = s.at[link_idx].set(jnp.array([2.0, 1.0, 1.0])) # manual scaling
scaling_factors = js.kin_dyn_parameters.ScalingFactors(
dims=s,
density=initial_density,
)
# Update the model with the optimized scaling factors.
optimized_model: js.model.JaxSimModel = js.model.update_hw_parameters(
model=model, scaling_factors=scaling_factors
)URDF
<?xml version="1.0" encoding="utf-8"?>
<robot name="humanoid">
<link name="root">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.15"/>
<inertia ixx="2.4e-05" ixy="0.0" ixz="0.0" iyy="2.4e-05" iyz="0.0" izz="2.4e-05"/>
</inertial>
<visual name="root_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</visual>
<collision name="root_collision">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
</link>
<link name="torso_1">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.125"/>
<inertia ixx="3.125000000000001e-05" ixy="0.0" ixz="0.0" iyy="3.125000000000001e-05" iyz="0.0" izz="3.125000000000001e-05"/>
</inertial>
<visual name="torso_1_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.025"/>
</geometry>
</visual>
<collision name="torso_1_collision">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.025"/>
</geometry>
</collision>
</link>
<link name="torso_2">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.125"/>
<inertia ixx="1.7578125000000005e-05" ixy="0.0" ixz="0.0" iyy="1.7578125000000005e-05" iyz="0.0" izz="1.7578125000000005e-05"/>
</inertial>
<visual name="torso_2_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.018750000000000003"/>
</geometry>
</visual>
</link>
<link name="head">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.1"/>
<inertia ixx="1.6000000000000003e-05" ixy="0.0" ixz="0.0" iyy="1.6000000000000003e-05" iyz="0.0" izz="1.6000000000000003e-05"/>
</inertial>
<visual name="head_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</visual>
<collision name="head_collision">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
</link>
<link name="left_shoulder">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.025"/>
<inertia ixx="3.600000000000001e-07" ixy="0.0" ixz="0.0" iyy="3.600000000000001e-07" iyz="0.0" izz="3.600000000000001e-07"/>
</inertial>
<visual name="left_shoulder_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.006"/>
</geometry>
</visual>
<collision name="left_shoulder_collision">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.006"/>
</geometry>
</collision>
</link>
<link name="left_upper_arm">
<inertial>
<origin xyz="0.0 0.0 -0.025" rpy="0.0 0.0 0.0"/>
<mass value="0.075"/>
<inertia ixx="1.6300000000000003e-05" ixy="0.0" ixz="0.0" iyy="1.6300000000000003e-05" iyz="0.0" izz="1.35e-06"/>
</inertial>
<visual name="left_upper_arm_visual">
<origin xyz="0.0 0.0 -0.025" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.006" length="0.05"/>
</geometry>
</visual>
<visual name="left_elbow_visual">
<origin xyz="0.0 0.0 -0.05" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.006600000000000001"/>
</geometry>
</visual>
<collision name="left_upper_arm_collision">
<origin xyz="0.0 0.0 -0.025" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.006" length="0.05"/>
</geometry>
</collision>
</link>
<link name="left_forearm">
<inertial>
<origin xyz="0.0 0.0 -0.02" rpy="0.0 0.0 0.0"/>
<mass value="0.075"/>
<inertia ixx="1.046875e-05" ixy="0.0" ixz="0.0" iyy="1.046875e-05" iyz="0.0" izz="9.375e-07"/>
</inertial>
<visual name="left_forearm_visual">
<origin xyz="0.0 0.0 -0.02" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.005" length="0.04"/>
</geometry>
</visual>
<collision name="left_forearm_collision">
<origin xyz="0.0 0.0 -0.02" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.005" length="0.04"/>
</geometry>
</collision>
</link>
<link name="left_hand">
<inertial>
<origin xyz="0.0 0.0 -0.0175" rpy="0.0 0.0 0.0"/>
<mass value="0.025"/>
<inertia ixx="1.510416666666667e-06" ixy="0.0" ixz="0.0" iyy="2.1354166666666666e-06" iyz="0.0" izz="1.0416666666666667e-06"/>
</inertial>
<visual name="left_hand_visual">
<origin xyz="0.0 0.0 -0.0175" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="0.02 0.01 0.025"/>
</geometry>
</visual>
<visual name="left_wrist_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.005"/>
</geometry>
</visual>
<collision name="left_hand_collision">
<origin xyz="0.0 0.0 -0.0175" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="0.02 0.01 0.025"/>
</geometry>
</collision>
</link>
<link name="right_shoulder">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.025"/>
<inertia ixx="3.600000000000001e-07" ixy="0.0" ixz="0.0" iyy="3.600000000000001e-07" iyz="0.0" izz="3.600000000000001e-07"/>
</inertial>
<visual name="right_shoulder_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.006"/>
</geometry>
</visual>
<collision name="right_shoulder_collision">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.006"/>
</geometry>
</collision>
</link>
<link name="right_upper_arm">
<inertial>
<origin xyz="0.0 0.0 -0.025" rpy="0.0 0.0 0.0"/>
<mass value="0.075"/>
<inertia ixx="1.6300000000000003e-05" ixy="0.0" ixz="0.0" iyy="1.6300000000000003e-05" iyz="0.0" izz="1.35e-06"/>
</inertial>
<visual name="right_upper_arm_visual">
<origin xyz="0.0 0.0 -0.025" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.006" length="0.05"/>
</geometry>
</visual>
<visual name="right_elbow_visual">
<origin xyz="0.0 0.0 -0.05" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.006600000000000001"/>
</geometry>
</visual>
<collision name="right_upper_arm_collision">
<origin xyz="0.0 0.0 -0.025" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.006" length="0.05"/>
</geometry>
</collision>
</link>
<link name="right_forearm">
<inertial>
<origin xyz="0.0 0.0 -0.02" rpy="0.0 0.0 0.0"/>
<mass value="0.075"/>
<inertia ixx="1.046875e-05" ixy="0.0" ixz="0.0" iyy="1.046875e-05" iyz="0.0" izz="9.375e-07"/>
</inertial>
<visual name="right_forearm_visual">
<origin xyz="0.0 0.0 -0.02" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.005" length="0.04"/>
</geometry>
</visual>
<collision name="right_forearm_collision">
<origin xyz="0.0 0.0 -0.02" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.005" length="0.04"/>
</geometry>
</collision>
</link>
<link name="right_hand">
<inertial>
<origin xyz="0.0 0.0 -0.0175" rpy="0.0 0.0 0.0"/>
<mass value="0.025"/>
<inertia ixx="1.510416666666667e-06" ixy="0.0" ixz="0.0" iyy="2.1354166666666666e-06" iyz="0.0" izz="1.0416666666666667e-06"/>
</inertial>
<visual name="right_hand_visual">
<origin xyz="0.0 0.0 -0.0175" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="0.02 0.01 0.025"/>
</geometry>
</visual>
<visual name="right_wrist_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.005"/>
</geometry>
</visual>
<collision name="right_hand_collision">
<origin xyz="0.0 0.0 -0.0175" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="0.02 0.01 0.025"/>
</geometry>
</collision>
</link>
<link name="left_hip_1">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0125"/>
<inertia ixx="3.2000000000000006e-07" ixy="0.0" ixz="0.0" iyy="3.2000000000000006e-07" iyz="0.0" izz="3.2000000000000006e-07"/>
</inertial>
<visual name="left_hip_1_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.008"/>
</geometry>
</visual>
<collision name="left_hip_1_collision">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.008"/>
</geometry>
</collision>
</link>
<link name="left_hip_2">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0125"/>
<inertia ixx="1.8000000000000005e-07" ixy="0.0" ixz="0.0" iyy="1.8000000000000005e-07" iyz="0.0" izz="1.8000000000000005e-07"/>
</inertial>
<visual name="left_hip_2_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.006"/>
</geometry>
</visual>
</link>
<link name="left_upper_leg">
<inertial>
<origin xyz="0.0 0.0 -0.0325" rpy="0.0 0.0 0.0"/>
<mass value="0.15"/>
<inertia ixx="5.5212500000000014e-05" ixy="0.0" ixz="0.0" iyy="5.5212500000000014e-05" iyz="0.0" izz="4.8e-06"/>
</inertial>
<visual name="left_upper_leg_visual">
<origin xyz="0.0 0.0 -0.0325" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.008" length="0.065"/>
</geometry>
</visual>
<visual name="left_knee_visual">
<origin xyz="0.0 0.0 -0.065" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.0088"/>
</geometry>
</visual>
<collision name="left_upper_leg_collision">
<origin xyz="0.0 0.0 -0.0325" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.008" length="0.065"/>
</geometry>
</collision>
</link>
<link name="left_lower_leg">
<inertial>
<origin xyz="0.0 0.0 -0.0325" rpy="0.0 0.0 0.0"/>
<mass value="0.075"/>
<inertia ixx="2.7460937500000003e-05" ixy="0.0" ixz="0.0" iyy="2.7460937500000003e-05" iyz="0.0" izz="2.109375e-06"/>
</inertial>
<visual name="left_lower_leg_visual">
<origin xyz="0.0 0.0 -0.0325" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.0075" length="0.065"/>
</geometry>
</visual>
<collision name="left_lower_leg_collision">
<origin xyz="0.0 0.0 -0.0325" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.0075" length="0.065"/>
</geometry>
</collision>
</link>
<link name="left_ankle_1">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0125"/>
<inertia ixx="2.8125000000000006e-07" ixy="0.0" ixz="0.0" iyy="2.8125000000000006e-07" iyz="0.0" izz="2.8125000000000006e-07"/>
</inertial>
<visual name="left_ankle_1_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.0075"/>
</geometry>
</visual>
<collision name="left_ankle_1_collision">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.0075"/>
</geometry>
</collision>
</link>
<link name="left_ankle_2">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0125"/>
<inertia ixx="1.5820312500000003e-07" ixy="0.0" ixz="0.0" iyy="1.5820312500000003e-07" iyz="0.0" izz="1.5820312500000003e-07"/>
</inertial>
<visual name="left_ankle_2_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.005625"/>
</geometry>
</visual>
</link>
<link name="left_foot">
<inertial>
<origin xyz="0.0075 0.0 -0.0125" rpy="0.0 0.0 0.0"/>
<mass value="0.025"/>
<inertia ixx="1.0416666666666667e-06" ixy="0.0" ixz="0.0" iyy="2.0833333333333334e-06" iyz="0.0" izz="2.7083333333333334e-06"/>
</inertial>
<visual name="left_foot_visual">
<origin xyz="0.0075 0.0 -0.0125" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="0.03 0.02 0.01"/>
</geometry>
</visual>
<collision name="left_foot_collision">
<origin xyz="0.0075 0.0 -0.0125" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="0.03 0.02 0.01"/>
</geometry>
</collision>
</link>
<link name="right_hip_1">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0125"/>
<inertia ixx="3.2000000000000006e-07" ixy="0.0" ixz="0.0" iyy="3.2000000000000006e-07" iyz="0.0" izz="3.2000000000000006e-07"/>
</inertial>
<visual name="right_hip_1_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.008"/>
</geometry>
</visual>
<collision name="right_hip_1_collision">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.008"/>
</geometry>
</collision>
</link>
<link name="right_hip_2">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0125"/>
<inertia ixx="1.8000000000000005e-07" ixy="0.0" ixz="0.0" iyy="1.8000000000000005e-07" iyz="0.0" izz="1.8000000000000005e-07"/>
</inertial>
<visual name="right_hip_2_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.006"/>
</geometry>
</visual>
</link>
<link name="right_upper_leg">
<inertial>
<origin xyz="0.0 0.0 -0.0325" rpy="0.0 0.0 0.0"/>
<mass value="0.15"/>
<inertia ixx="5.5212500000000014e-05" ixy="0.0" ixz="0.0" iyy="5.5212500000000014e-05" iyz="0.0" izz="4.8e-06"/>
</inertial>
<visual name="right_upper_leg_visual">
<origin xyz="0.0 0.0 -0.0325" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.008" length="0.065"/>
</geometry>
</visual>
<visual name="right_knee_visual">
<origin xyz="0.0 0.0 -0.065" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.0088"/>
</geometry>
</visual>
<collision name="right_upper_leg_collision">
<origin xyz="0.0 0.0 -0.0325" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.008" length="0.065"/>
</geometry>
</collision>
</link>
<link name="right_lower_leg">
<inertial>
<origin xyz="0.0 0.0 -0.0325" rpy="0.0 0.0 0.0"/>
<mass value="0.075"/>
<inertia ixx="2.7460937500000003e-05" ixy="0.0" ixz="0.0" iyy="2.7460937500000003e-05" iyz="0.0" izz="2.109375e-06"/>
</inertial>
<visual name="right_lower_leg_visual">
<origin xyz="0.0 0.0 -0.0325" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.0075" length="0.065"/>
</geometry>
</visual>
<collision name="right_lower_leg_collision">
<origin xyz="0.0 0.0 -0.0325" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.0075" length="0.065"/>
</geometry>
</collision>
</link>
<link name="right_ankle_1">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0125"/>
<inertia ixx="2.8125000000000006e-07" ixy="0.0" ixz="0.0" iyy="2.8125000000000006e-07" iyz="0.0" izz="2.8125000000000006e-07"/>
</inertial>
<visual name="right_ankle_1_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.0075"/>
</geometry>
</visual>
<collision name="right_ankle_1_collision">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.0075"/>
</geometry>
</collision>
</link>
<link name="right_ankle_2">
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0125"/>
<inertia ixx="1.5820312500000003e-07" ixy="0.0" ixz="0.0" iyy="1.5820312500000003e-07" iyz="0.0" izz="1.5820312500000003e-07"/>
</inertial>
<visual name="right_ankle_2_visual">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.005625"/>
</geometry>
</visual>
</link>
<link name="right_foot">
<inertial>
<origin xyz="0.0075 0.0 -0.0125" rpy="0.0 0.0 0.0"/>
<mass value="0.025"/>
<inertia ixx="1.0416666666666667e-06" ixy="0.0" ixz="0.0" iyy="2.0833333333333334e-06" iyz="0.0" izz="2.7083333333333334e-06"/>
</inertial>
<visual name="right_foot_visual">
<origin xyz="0.0075 0.0 -0.0125" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="0.03 0.02 0.01"/>
</geometry>
</visual>
<collision name="right_foot_collision">
<origin xyz="0.0075 0.0 -0.0125" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="0.03 0.02 0.01"/>
</geometry>
</collision>
</link>
<link name="contact_left_heel_inner">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1.001e-06"/>
<inertia ixx="1.001e-06" ixy="0" ixz="0" iyy="1.001e-06" iyz="0" izz="1.001e-06"/>
</inertial>
<visual name="contact_left_heel_inner_visual">
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</visual>
</link>
<link name="contact_left_heel_outer">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1.001e-06"/>
<inertia ixx="1.001e-06" ixy="0" ixz="0" iyy="1.001e-06" iyz="0" izz="1.001e-06"/>
</inertial>
<visual name="contact_left_heel_outer_visual">
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</visual>
</link>
<link name="contact_left_toe_inner">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1.001e-06"/>
<inertia ixx="1.001e-06" ixy="0" ixz="0" iyy="1.001e-06" iyz="0" izz="1.001e-06"/>
</inertial>
<visual name="contact_left_toe_inner_visual">
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</visual>
</link>
<link name="contact_left_toe_outer">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1.001e-06"/>
<inertia ixx="1.001e-06" ixy="0" ixz="0" iyy="1.001e-06" iyz="0" izz="1.001e-06"/>
</inertial>
<visual name="contact_left_toe_outer_visual">
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</visual>
</link>
<link name="contact_left_sole_center">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1.001e-06"/>
<inertia ixx="1.001e-06" ixy="0" ixz="0" iyy="1.001e-06" iyz="0" izz="1.001e-06"/>
</inertial>
<visual name="contact_left_sole_center_visual">
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</visual>
</link>
<link name="contact_right_heel_outer">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1.001e-06"/>
<inertia ixx="1.001e-06" ixy="0" ixz="0" iyy="1.001e-06" iyz="0" izz="1.001e-06"/>
</inertial>
<visual name="contact_right_heel_outer_visual">
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</visual>
</link>
<link name="contact_right_heel_inner">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1.001e-06"/>
<inertia ixx="1.001e-06" ixy="0" ixz="0" iyy="1.001e-06" iyz="0" izz="1.001e-06"/>
</inertial>
<visual name="contact_right_heel_inner_visual">
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</visual>
</link>
<link name="contact_right_toe_outer">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1.001e-06"/>
<inertia ixx="1.001e-06" ixy="0" ixz="0" iyy="1.001e-06" iyz="0" izz="1.001e-06"/>
</inertial>
<visual name="contact_right_toe_outer_visual">
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</visual>
</link>
<link name="contact_right_toe_inner">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1.001e-06"/>
<inertia ixx="1.001e-06" ixy="0" ixz="0" iyy="1.001e-06" iyz="0" izz="1.001e-06"/>
</inertial>
<visual name="contact_right_toe_inner_visual">
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</visual>
</link>
<link name="contact_right_sole_center">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1.001e-06"/>
<inertia ixx="1.001e-06" ixy="0" ixz="0" iyy="1.001e-06" iyz="0" izz="1.001e-06"/>
</inertial>
<visual name="contact_right_sole_center_visual">
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</visual>
</link>
<joint name="spine_pitch" type="revolute">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<parent link="torso_1"/>
<child link="torso_2"/>
<axis xyz="0 1 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-0.3490658503988659" upper="0.5235987755982988"/>
</joint>
<joint name="spine_yaw" type="revolute">
<origin xyz="0.0 0.0 0.06" rpy="0.0 0.0 0.0"/>
<parent link="root"/>
<child link="torso_1"/>
<axis xyz="0 0 1"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-0.5235987755982988" upper="0.5235987755982988"/>
</joint>
<joint name="neck" type="fixed">
<origin xyz="0.0 0.0 0.05249999999999999" rpy="0.0 0.0 0.0"/>
<parent link="torso_2"/>
<child link="head"/>
</joint>
<joint name="left_shoulder_roll" type="revolute">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<parent link="left_shoulder"/>
<child link="left_upper_arm"/>
<axis xyz="1 0 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="0.0" upper="1.5707963267948966"/>
</joint>
<joint name="left_shoulder_pitch" type="revolute">
<origin xyz="0.0 0.036000000000000004 0.018750000000000017" rpy="0.0 0.0 0.0"/>
<parent link="torso_2"/>
<child link="left_shoulder"/>
<axis xyz="0 1 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-2.0943951023931953" upper="0.7853981633974483"/>
</joint>
<joint name="left_elbow" type="revolute">
<origin xyz="0.0 0.0 -0.05000000000000002" rpy="0.0 0.0 0.0"/>
<parent link="left_upper_arm"/>
<child link="left_forearm"/>
<axis xyz="0 1 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-2.8797932657906435" upper="0.0"/>
</joint>
<joint name="left_wrist" type="fixed">
<origin xyz="0.0 0.0 -0.03999999999999998" rpy="0.0 0.0 0.0"/>
<parent link="left_forearm"/>
<child link="left_hand"/>
</joint>
<joint name="right_shoulder_roll" type="revolute">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<parent link="right_shoulder"/>
<child link="right_upper_arm"/>
<axis xyz="1 0 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-1.5707963267948966" upper="0.0"/>
</joint>
<joint name="right_shoulder_pitch" type="revolute">
<origin xyz="0.0 -0.036000000000000004 0.018750000000000017" rpy="0.0 0.0 0.0"/>
<parent link="torso_2"/>
<child link="right_shoulder"/>
<axis xyz="0 1 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-2.0943951023931953" upper="0.7853981633974483"/>
</joint>
<joint name="right_elbow" type="revolute">
<origin xyz="0.0 0.0 -0.05000000000000002" rpy="0.0 0.0 0.0"/>
<parent link="right_upper_arm"/>
<child link="right_forearm"/>
<axis xyz="0 1 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-2.8797932657906435" upper="0.0"/>
</joint>
<joint name="right_wrist" type="fixed">
<origin xyz="0.0 0.0 -0.03999999999999998" rpy="0.0 0.0 0.0"/>
<parent link="right_forearm"/>
<child link="right_hand"/>
</joint>
<joint name="left_hip_roll" type="revolute">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<parent link="left_hip_1"/>
<child link="left_hip_2"/>
<axis xyz="1 0 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-0.5235987755982988" upper="0.7853981633974483"/>
</joint>
<joint name="left_hip_pitch" type="revolute">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<parent link="left_hip_2"/>
<child link="left_upper_leg"/>
<axis xyz="0 1 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-2.356194490192345" upper="0.7853981633974483"/>
</joint>
<joint name="left_hip_yaw" type="revolute">
<origin xyz="0.0 0.02 -0.01999999999999999" rpy="0.0 0.0 0.0"/>
<parent link="root"/>
<child link="left_hip_1"/>
<axis xyz="0 0 1"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-0.3490658503988659" upper="0.7853981633974483"/>
</joint>
<joint name="left_knee" type="revolute">
<origin xyz="0.0 0.0 -0.06500000000000002" rpy="0.0 0.0 0.0"/>
<parent link="left_upper_leg"/>
<child link="left_lower_leg"/>
<axis xyz="0 1 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="0.0" upper="2.8797932657906435"/>
</joint>
<joint name="left_ankle_roll" type="revolute">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<parent link="left_ankle_1"/>
<child link="left_ankle_2"/>
<axis xyz="1 0 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-0.5235987755982988" upper="0.5235987755982988"/>
</joint>
<joint name="left_ankle_pitch" type="revolute">
<origin xyz="0.0 0.0 -0.06500000000000002" rpy="0.0 0.0 0.0"/>
<parent link="left_lower_leg"/>
<child link="left_ankle_1"/>
<axis xyz="0 1 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-0.5235987755982988" upper="1.3089969389957472"/>
</joint>
<joint name="left_ankle_yaw" type="revolute">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<parent link="left_ankle_2"/>
<child link="left_foot"/>
<axis xyz="0 0 1"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-0.5235987755982988" upper="0.5235987755982988"/>
</joint>
<joint name="right_hip_roll" type="revolute">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<parent link="right_hip_1"/>
<child link="right_hip_2"/>
<axis xyz="1 0 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-0.7853981633974483" upper="0.5235987755982988"/>
</joint>
<joint name="right_hip_pitch" type="revolute">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<parent link="right_hip_2"/>
<child link="right_upper_leg"/>
<axis xyz="0 1 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-2.356194490192345" upper="0.7853981633974483"/>
</joint>
<joint name="right_hip_yaw" type="revolute">
<origin xyz="0.0 -0.02 -0.01999999999999999" rpy="0.0 0.0 0.0"/>
<parent link="root"/>
<child link="right_hip_1"/>
<axis xyz="0 0 1"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-0.7853981633974483" upper="0.3490658503988659"/>
</joint>
<joint name="right_knee" type="revolute">
<origin xyz="0.0 0.0 -0.06500000000000002" rpy="0.0 0.0 0.0"/>
<parent link="right_upper_leg"/>
<child link="right_lower_leg"/>
<axis xyz="0 1 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="0.0" upper="2.8797932657906435"/>
</joint>
<joint name="right_ankle_roll" type="revolute">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<parent link="right_ankle_1"/>
<child link="right_ankle_2"/>
<axis xyz="1 0 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-0.5235987755982988" upper="0.5235987755982988"/>
</joint>
<joint name="right_ankle_pitch" type="revolute">
<origin xyz="0.0 0.0 -0.06500000000000002" rpy="0.0 0.0 0.0"/>
<parent link="right_lower_leg"/>
<child link="right_ankle_1"/>
<axis xyz="0 1 0"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-0.5235987755982988" upper="1.3089969389957472"/>
</joint>
<joint name="right_ankle_yaw" type="revolute">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<parent link="right_ankle_2"/>
<child link="right_foot"/>
<axis xyz="0 0 1"/>
<limit effort="3.4028235e+38" velocity="3.4028235e+38" lower="-0.5235987755982988" upper="0.5235987755982988"/>
</joint>
<joint name="left_foot_to_contact_left_heel_inner" type="fixed">
<parent link="left_foot"/>
<child link="contact_left_heel_inner"/>
<origin xyz="-0.0075 -0.01 -0.017500000000000016" rpy="0.0 0.0 0.0"/>
</joint>
<joint name="left_foot_to_contact_left_heel_outer" type="fixed">
<parent link="left_foot"/>
<child link="contact_left_heel_outer"/>
<origin xyz="-0.0075 0.009999999999999998 -0.017500000000000016" rpy="0.0 0.0 0.0"/>
</joint>
<joint name="left_foot_to_contact_left_toe_inner" type="fixed">
<parent link="left_foot"/>
<child link="contact_left_toe_inner"/>
<origin xyz="0.0225 -0.01 -0.017500000000000016" rpy="0.0 0.0 0.0"/>
</joint>
<joint name="left_foot_to_contact_left_toe_outer" type="fixed">
<parent link="left_foot"/>
<child link="contact_left_toe_outer"/>
<origin xyz="0.0225 0.009999999999999998 -0.017500000000000016" rpy="0.0 0.0 0.0"/>
</joint>
<joint name="left_foot_to_contact_left_sole_center" type="fixed">
<parent link="left_foot"/>
<child link="contact_left_sole_center"/>
<origin xyz="0.0075 0.0 -0.017500000000000016" rpy="0.0 0.0 0.0"/>
</joint>
<joint name="right_foot_to_contact_right_heel_outer" type="fixed">
<parent link="right_foot"/>
<child link="contact_right_heel_outer"/>
<origin xyz="-0.0075 -0.009999999999999998 -0.017500000000000016" rpy="0.0 0.0 0.0"/>
</joint>
<joint name="right_foot_to_contact_right_heel_inner" type="fixed">
<parent link="right_foot"/>
<child link="contact_right_heel_inner"/>
<origin xyz="-0.0075 0.01 -0.017500000000000016" rpy="0.0 0.0 0.0"/>
</joint>
<joint name="right_foot_to_contact_right_toe_outer" type="fixed">
<parent link="right_foot"/>
<child link="contact_right_toe_outer"/>
<origin xyz="0.0225 -0.009999999999999998 -0.017500000000000016" rpy="0.0 0.0 0.0"/>
</joint>
<joint name="right_foot_to_contact_right_toe_inner" type="fixed">
<parent link="right_foot"/>
<child link="contact_right_toe_inner"/>
<origin xyz="0.0225 0.01 -0.017500000000000016" rpy="0.0 0.0 0.0"/>
</joint>
<joint name="right_foot_to_contact_right_sole_center" type="fixed">
<parent link="right_foot"/>
<child link="contact_right_sole_center"/>
<origin xyz="0.0075 0.0 -0.017500000000000016" rpy="0.0 0.0 0.0"/>
</joint>
<gazebo reference="left_foot_to_contact_left_heel_inner">
<preserveFixedJoint>true</preserveFixedJoint>
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<gazebo reference="left_foot_to_contact_left_heel_outer">
<preserveFixedJoint>true</preserveFixedJoint>
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<gazebo reference="left_foot_to_contact_left_toe_inner">
<preserveFixedJoint>true</preserveFixedJoint>
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<gazebo reference="left_foot_to_contact_left_toe_outer">
<preserveFixedJoint>true</preserveFixedJoint>
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<gazebo reference="left_foot_to_contact_left_sole_center">
<preserveFixedJoint>true</preserveFixedJoint>
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<gazebo reference="right_foot_to_contact_right_heel_outer">
<preserveFixedJoint>true</preserveFixedJoint>
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<gazebo reference="right_foot_to_contact_right_heel_inner">
<preserveFixedJoint>true</preserveFixedJoint>
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<gazebo reference="right_foot_to_contact_right_toe_outer">
<preserveFixedJoint>true</preserveFixedJoint>
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<gazebo reference="right_foot_to_contact_right_toe_inner">
<preserveFixedJoint>true</preserveFixedJoint>
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<gazebo reference="right_foot_to_contact_right_sole_center">
<preserveFixedJoint>true</preserveFixedJoint>
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
</robot>
Error:
---------------------------------------------------------------------------
ValueError Traceback (most recent call last)
Cell In[1], line 73
66 scaling_factors = js.kin_dyn_parameters.ScalingFactors(
67 dims=s,
68 density=initial_density,
69 )
72 # Update the model with the optimized scaling factors.
---> 73 optimized_model: js.model.JaxSimModel = js.model.update_hw_parameters(
74 model=model, scaling_factors=scaling_factors
75 )
[... skipping hidden 13 frame]
File ~/repos/jaxsim/src/jaxsim/api/common.py:34, in named_scope.<locals>.wrapper(*args, **kwargs)
31 @functools.wraps(fn)
32 def wrapper(*args: _P.args, **kwargs: _P.kwargs) -> _R:
33 with jax.named_scope(name or fn.__name__):
---> 34 return fn(*args, **kwargs)
File ~/repos/jaxsim/src/jaxsim/api/model.py:2337, in update_hw_parameters(model, scaling_factors)
2334 hw_link_metadata: HwLinkMetadata = kin_dyn_params.hw_link_metadata
2336 # Apply scaling to hw_link_metadata using vmap
-> 2337 updated_hw_link_metadata = jax.vmap(HwLinkMetadata.apply_scaling)(
2338 hw_link_metadata, scaling_factors
2339 )
2341 # Compute mass and inertia once and unpack the results
2342 m_updated, I_com_updated = jax.vmap(HwLinkMetadata.compute_mass_and_inertia)(
2343 updated_hw_link_metadata
2344 )
[... skipping hidden 2 frame]
File ~/repos/component_darwin/.pixi/envs/default/lib/python3.13/site-packages/jax/_src/api.py:1091, in _mapped_axis_size(fn, tree, vals, dims, name)
1089 else:
1090 msg.append(f" * some axes ({ct} of them) had size {sz}, e.g. axis {ax} of {ex};\n")
-> 1091 raise ValueError(''.join(msg)[:-2])
ValueError: vmap got inconsistent sizes for array axes to be mapped:
* most axes (7 of them) had size 16, e.g. axis 0 of argument hw_metadata[<flat index 0>] of type int64[16];
* some axes (2 of them) had size 23, e.g. axis 0 of argument scaling_factors[<flat index 0>] of type float64[23,3]