From b63f7052bb89a761552c0661eb59e1c50526f94e Mon Sep 17 00:00:00 2001 From: Benjamin Bolte Date: Fri, 20 Sep 2024 04:35:20 +0800 Subject: [PATCH 1/3] better joint limits --- kol/onshape/download.py | 32 ++++++++++++++++++++++---------- 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/kol/onshape/download.py b/kol/onshape/download.py index 4da6138..8e6677b 100644 --- a/kol/onshape/download.py +++ b/kol/onshape/download.py @@ -1067,15 +1067,21 @@ def get_effort_and_velocity(name: str, default_effort: float, default_velocity: parent, child = doc.key_namer(joint.parent, "link"), doc.key_namer(joint.child, "link") mimic_joint = doc.mate_relations.get(joint.joint_key) - if joint_limits is None: - min_value, max_value = ( + min_value: float | None = None + max_value: float | None = None + if joint_limits is not None: + min_value = resolve(joint_limits.axial_z_min_expression) + max_value = resolve(joint_limits.axial_z_max_expression) + if min_value is None or max_value is None: + new_min_value, new_max_value = ( (None, None) if config.default_revolute_joint_limits is None else config.default_revolute_joint_limits ) - else: - min_value = resolve(joint_limits.axial_z_min_expression) - max_value = resolve(joint_limits.axial_z_max_expression) + if min_value is None: + min_value = new_min_value + if max_value is None: + max_value = new_max_value if min_value is None or max_value is None: raise ValueError(f"Revolute joint {name} ({parent} -> {child}) does not have limits defined.") @@ -1113,15 +1119,21 @@ def get_effort_and_velocity(name: str, default_effort: float, default_velocity: parent, child = doc.key_namer(joint.parent, "link"), doc.key_namer(joint.child, "link") mimic_joint = doc.mate_relations.get(joint.joint_key) - if joint_limits is None: - min_value, max_value = ( + min_value: float | None = None + max_value: float | None = None + if joint_limits is not None: + min_value = resolve(joint_limits.axial_z_min_expression) + max_value = resolve(joint_limits.axial_z_max_expression) + if min_value is None or max_value is None: + new_min_value, new_max_value = ( (None, None) if config.default_prismatic_joint_limits is None else config.default_prismatic_joint_limits ) - else: - min_value = resolve(joint_limits.axial_z_min_expression) - max_value = resolve(joint_limits.axial_z_max_expression) + if min_value is None: + min_value = new_min_value + if max_value is None: + max_value = new_max_value if min_value is None or max_value is None: raise ValueError(f"Slider joint {name} ({parent} -> {child}) does not have limits defined.") From 452f27eac707ef31a8197449552a1dd398d5570a Mon Sep 17 00:00:00 2001 From: Benjamin Bolte Date: Fri, 20 Sep 2024 04:44:45 +0800 Subject: [PATCH 2/3] fix lint --- kol/onshape/download.py | 54 ++++++++++++++++++----------------------- 1 file changed, 24 insertions(+), 30 deletions(-) diff --git a/kol/onshape/download.py b/kol/onshape/download.py index 8e6677b..6f9ad3f 100644 --- a/kol/onshape/download.py +++ b/kol/onshape/download.py @@ -1049,6 +1049,28 @@ def get_effort_and_velocity(name: str, default_effort: float, default_velocity: break return effort, velocity + def get_joint_limits( + joint_limits: JointLimits, + default_limits: tuple[float, float] | None, + is_axial: bool, + ) -> tuple[float | None, float | None]: + min_value: float | None = None + max_value: float | None = None + if joint_limits is not None: + if is_axial: + min_value = resolve(joint_limits.axial_z_min_expression) + max_value = resolve(joint_limits.axial_z_max_expression) + else: + min_value = resolve(joint_limits.z_min_expression) + max_value = resolve(joint_limits.z_max_expression) + if min_value is None or max_value is None: + new_min_value, new_max_value = (None, None) if default_limits is None else default_limits + if min_value is None: + min_value = new_min_value + if max_value is None: + max_value = new_max_value + return min_value, max_value + name = doc.key_namer(joint.joint_key, "joint") origin = urdf.Origin.from_matrix(parent_stl_origin_to_mate_tf) mate_type = joint.mate_type @@ -1067,21 +1089,7 @@ def get_effort_and_velocity(name: str, default_effort: float, default_velocity: parent, child = doc.key_namer(joint.parent, "link"), doc.key_namer(joint.child, "link") mimic_joint = doc.mate_relations.get(joint.joint_key) - min_value: float | None = None - max_value: float | None = None - if joint_limits is not None: - min_value = resolve(joint_limits.axial_z_min_expression) - max_value = resolve(joint_limits.axial_z_max_expression) - if min_value is None or max_value is None: - new_min_value, new_max_value = ( - (None, None) - if config.default_revolute_joint_limits is None - else config.default_revolute_joint_limits - ) - if min_value is None: - min_value = new_min_value - if max_value is None: - max_value = new_max_value + min_value, max_value = get_joint_limits(joint_limits, config.default_revolute_joint_limits, is_axial=False) if min_value is None or max_value is None: raise ValueError(f"Revolute joint {name} ({parent} -> {child}) does not have limits defined.") @@ -1119,21 +1127,7 @@ def get_effort_and_velocity(name: str, default_effort: float, default_velocity: parent, child = doc.key_namer(joint.parent, "link"), doc.key_namer(joint.child, "link") mimic_joint = doc.mate_relations.get(joint.joint_key) - min_value: float | None = None - max_value: float | None = None - if joint_limits is not None: - min_value = resolve(joint_limits.axial_z_min_expression) - max_value = resolve(joint_limits.axial_z_max_expression) - if min_value is None or max_value is None: - new_min_value, new_max_value = ( - (None, None) - if config.default_prismatic_joint_limits is None - else config.default_prismatic_joint_limits - ) - if min_value is None: - min_value = new_min_value - if max_value is None: - max_value = new_max_value + min_value, max_value = get_joint_limits(joint_limits, config.default_prismatic_joint_limits, is_axial=True) if min_value is None or max_value is None: raise ValueError(f"Slider joint {name} ({parent} -> {child}) does not have limits defined.") From 14f2bbcba599928df66c276c48f52a5aedbbdc80 Mon Sep 17 00:00:00 2001 From: Benjamin Bolte Date: Fri, 20 Sep 2024 04:55:46 +0800 Subject: [PATCH 3/3] fix lint --- kol/onshape/download.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kol/onshape/download.py b/kol/onshape/download.py index 6f9ad3f..750900c 100644 --- a/kol/onshape/download.py +++ b/kol/onshape/download.py @@ -1050,7 +1050,7 @@ def get_effort_and_velocity(name: str, default_effort: float, default_velocity: return effort, velocity def get_joint_limits( - joint_limits: JointLimits, + joint_limits: JointLimits | None, default_limits: tuple[float, float] | None, is_axial: bool, ) -> tuple[float | None, float | None]: