Merge pull request #128 from ichumuh/compose_expression

compose forward kinematics expression

Author: LucaKro

src/semantic_digital_twin/spatial_computations/ik_solver.py

@@ -481,7 +481,7 @@ def build_goal_constraints(
         self, active_symbols: List[cas.Symbol]
     ) -> Tuple[cas.Expression, cas.Expression]:
         """Build position and rotation goal constraints."""
-        root_T_tip = self.world._forward_kinematic_manager.compose_expression(
+        root_T_tip = self.world.compose_forward_kinematics_expression(
             self.root, self.tip
         )
 

src/semantic_digital_twin/world.py

@@ -1609,6 +1609,18 @@ def compute_forward_kinematics(
         """
         return self._forward_kinematic_manager.compute(root, tip)
 
+    def compose_forward_kinematics_expression(
+        self, root: KinematicStructureEntity, tip: KinematicStructureEntity
+    ) -> cas.TransformationMatrix:
+        """
+        :param root: The root KinematicStructureEntity in the kinematic chain.
+            It determines the starting point of the forward kinematics calculation.
+        :param tip: The tip KinematicStructureEntity in the kinematic chain.
+            It determines the endpoint of the forward kinematics calculation.
+        :return: An expression representing the computed forward kinematics of the tip KinematicStructureEntity relative to the root KinematicStructureEntity.
+        """
+        return self._forward_kinematic_manager.compose_expression(root, tip)
+
     def compute_forward_kinematics_np(
         self, root: KinematicStructureEntity, tip: KinematicStructureEntity
     ) -> NpMatrix4x4:

test/test_robots/test_world_pr2.py

@@ -170,7 +170,7 @@ def test_compute_fk_np_l_elbow_flex_joint_pr2(pr2_world):
         PrefixedName("l_upper_arm_link")
     )
 
-    fk_expr = pr2_world._forward_kinematic_manager.compose_expression(root, tip)
+    fk_expr = pr2_world.compose_forward_kinematics_expression(root, tip)
     fk_expr_compiled = fk_expr.compile()
     fk2 = fk_expr_compiled(
         symbol_manager.resolve_symbols(*fk_expr_compiled.symbol_parameters)

test/test_worlds/test_world.py

@@ -259,7 +259,7 @@ def test_compute_fk_expression(world_setup):
     world.state[connection.dof.name].position = 1.0
     world.notify_state_change()
     fk = world.compute_forward_kinematics_np(r2, l2)
-    fk_expr = world._forward_kinematic_manager.compose_expression(r2, l2)
+    fk_expr = world.compose_forward_kinematics_expression(r2, l2)
     fk_expr_compiled = fk_expr.compile()
     fk2 = fk_expr_compiled(
         *symbol_manager.resolve_symbols(fk_expr_compiled.symbol_parameters)
@@ -850,6 +850,7 @@ def test_symbol_removal():
         c_root_bf = OmniDrive.create_with_dofs(parent=body1, child=body2, world=world1)
         world1.add_connection(c_root_bf)
 
+
 def test_set_static_collision_config():
     w = World()
 
@@ -869,6 +870,4 @@ def test_set_static_collision_config():
         collision_config = CollisionCheckingConfig(
             buffer_zone_distance=0.05, violated_distance=0.0, max_avoided_bodies=4
         )
-        connection.set_static_collision_config_for_direct_child_bodies(
-            collision_config
-        )
+        connection.set_static_collision_config_for_direct_child_bodies(collision_config)