diff --git a/README.md b/README.md index f5300ae..4f8c257 100644 --- a/README.md +++ b/README.md @@ -21,28 +21,10 @@ joint_ordering="urdf_order" # DFS preserving raw URDF sibling order joint_ordering="alphabetical_order" # DFS sorting sibling joints by joint name ``` -The legacy `alpha_tie_breaker` argument is still accepted for backward compatibility: -```python -alpha_tie_breaker=False # equivalent to joint_ordering="urdf_order" -alpha_tie_breaker=True # equivalent to joint_ordering="alphabetical_order" -``` -Floating-base parsing also accepts a public input convention flag: -```python -robot = parser.parse( - urdf_filepath, - floating_base=True, - floating_base_convention="pinocchio", -) -``` -Supported values are: -```python -floating_base_convention="pinocchio" # default: q = [x, y, z, qx, qy, qz, qw], v = [vx, vy, vz, wx, wy, wz] -floating_base_convention="legacy" # legacy public input/output order: q = [x, y, z, qw, qx, qy, qz], v = [wx, wy, wz, vx, vy, vz] -``` -Internally the parser and downstream dynamics code normalize floating-base states -into the Pinocchio-style convention so generated code and reference algorithms -stay consistent under the hood. +Floating-base parsing uses pinocchio's layout: `q = [x, y, z, qx, qy, qz, qw, joints]` +and `v = [vx, vy, vz, wx, wy, wz, joints]`; the parser, RBDReference and the generated +code share it, so there is no convention flag. ## Supported joint types: Revolute, continuous, prismatic, fixed, **helical/screw**, planar, translation diff --git a/Robot.py b/Robot.py index d79c2e5..c278b52 100644 --- a/Robot.py +++ b/Robot.py @@ -6,16 +6,13 @@ class Robot: # initialization - def __init__(self, name, floating_base = False, using_quaternion = True, floating_base_convention = "pinocchio"): + def __init__(self, name, floating_base = False, using_quaternion = True): self.name = name self.floating_base = floating_base self.links = [] self.joints = [] self.fixed_joints = [] self.using_quaternion = using_quaternion - if floating_base_convention not in ("pinocchio", "legacy"): - raise ValueError("floating_base_convention must be 'pinocchio' or 'legacy'") - self.floating_base_convention = floating_base_convention self.joint_type_by_id = {} self.joint_type_by_name = {} @@ -67,75 +64,19 @@ def get_joint_index_f(self, joint_id): # floating root 6-wide, spherical/planar 3-wide) as get_joint_index_v. return self.get_joint_index_v(joint_id) - def uses_legacy_floating_base_convention(self): - return self.floating_base and self.floating_base_convention == "legacy" - - def get_floating_base_convention(self): - return self.floating_base_convention - - def get_floating_base_q_input_permutation_to_internal(self): - if not self.uses_legacy_floating_base_convention(): - return None - if self.using_quaternion: - return [0, 1, 2, 4, 5, 6, 3] - return None - - def get_floating_base_q_output_permutation_from_internal(self): - if not self.uses_legacy_floating_base_convention(): - return None - if self.using_quaternion: - return [0, 1, 2, 6, 3, 4, 5] - return None - - def get_floating_base_v_permutation_to_internal(self): - if not self.uses_legacy_floating_base_convention(): - return None - return [3, 4, 5, 0, 1, 2] - - def get_floating_base_v_permutation_from_internal(self): - if not self.uses_legacy_floating_base_convention(): - return None - return [3, 4, 5, 0, 1, 2] - + # The floating-base layout is pinocchio's everywhere (q = [xyz, quaternion xyzw, + # joints], v = [linear, angular, joints]); the four helpers return a float64 copy. def normalize_floating_base_q_input(self, q): - q = np.asarray(q, dtype=np.float64).copy() - permutation = self.get_floating_base_q_input_permutation_to_internal() - if permutation is None: - return q - if q.shape[0] < len(permutation): - raise ValueError("Floating-base quaternion input must have at least 7 entries.") - q[: len(permutation)] = q[permutation] - return q + return np.asarray(q, dtype=np.float64).copy() def normalize_floating_base_v_input(self, vec): - vec = np.asarray(vec, dtype=np.float64).copy() - permutation = self.get_floating_base_v_permutation_to_internal() - if permutation is None: - return vec - if vec.shape[0] < len(permutation): - raise ValueError("Floating-base velocity-like input must have at least 6 entries.") - vec[: len(permutation)] = vec[permutation] - return vec + return np.asarray(vec, dtype=np.float64).copy() def denormalize_floating_base_q_output(self, q): - q = np.asarray(q, dtype=np.float64).copy() - permutation = self.get_floating_base_q_output_permutation_from_internal() - if permutation is None: - return q - if q.shape[0] < len(permutation): - raise ValueError("Floating-base quaternion output must have at least 7 entries.") - q[: len(permutation)] = q[permutation] - return q + return np.asarray(q, dtype=np.float64).copy() def denormalize_floating_base_v_output(self, vec): - vec = np.asarray(vec, dtype=np.float64).copy() - permutation = self.get_floating_base_v_permutation_from_internal() - if permutation is None: - return vec - if vec.shape[0] < len(permutation): - raise ValueError("Floating-base velocity-like output must have at least 6 entries.") - vec[: len(permutation)] = vec[permutation] - return vec + return np.asarray(vec, dtype=np.float64).copy() ################# # Setters # diff --git a/URDFParser.py b/URDFParser.py index 11c7c9d..bf50901 100644 --- a/URDFParser.py +++ b/URDFParser.py @@ -17,9 +17,7 @@ def parse( filename, floating_base = False, using_quaternion = True, - alpha_tie_breaker = None, joint_ordering = "pinocchio_order", - floating_base_convention = "pinocchio", strict_inertial = False, ): """Parse a tree-structured URDF into a Robot, or raise URDFParseError. @@ -47,14 +45,13 @@ def parse( self.soup["name"], floating_base, using_quaternion, - floating_base_convention=floating_base_convention, ) # collect links self.parse_links() # collect joints self.parse_joints() # remove all fixed joints, renumber links and joints, and build parent and subtree lists - resolved_joint_ordering = self.resolve_joint_ordering(alpha_tie_breaker, joint_ordering) + resolved_joint_ordering = self.resolve_joint_ordering(joint_ordering) self.renumber_linksJoints(using_quaternion, resolved_joint_ordering) # report joint ordering to user self.print_joint_order() @@ -71,9 +68,7 @@ def parse( # None for missing files, bad options, or malformed model fields. raise URDFParseError(f"Failed to parse URDF '{filename}': {exc}") from exc - def resolve_joint_ordering(self, alpha_tie_breaker, joint_ordering): - if alpha_tie_breaker is not None: - return "alphabetical_order" if alpha_tie_breaker else "urdf_order" + def resolve_joint_ordering(self, joint_ordering): if joint_ordering not in ("urdf_order", "alphabetical_order", "pinocchio_order"): raise ValueError( "joint_ordering must be one of 'urdf_order', 'alphabetical_order', or 'pinocchio_order'" diff --git a/tests/test_errors.py b/tests/test_errors.py index 3bd5534..eb7d8e9 100644 --- a/tests/test_errors.py +++ b/tests/test_errors.py @@ -95,8 +95,7 @@ def test_missing_robot_element_raises(tmp_path): URDFParser().parse(path) -@pytest.mark.parametrize("option", [dict(joint_ordering="invalid"), - dict(floating_base_convention="invalid")]) +@pytest.mark.parametrize("option", [dict(joint_ordering="invalid")]) def test_invalid_parse_option_is_not_silently_swallowed(tmp_path, option): path = _write_urdf(tmp_path, '' ''