Use attached tool's frame for all planning operations if any tool is attached

https://forum.compas-framework.org/t/planning-with-tcf-after-attaching-a-tool-ros-backend/581

Author: gonzalocasas

CHANGELOG.rst

@@ -19,7 +19,8 @@ Unreleased
 **Changed**
 
 * Changed behavior of ``Attach Tool`` GH component to only attach the tool but not add it to the planning scene state.
-* Duration class takes floats as ``sec`` variable
+* Duration class takes floats as ``sec`` variable.
+* Changed the behavior of ``forward_kinematics``, ``inverse_kinematics``, ``iter_inverse_kinematics``, ``plan_cartesian_motion`` and constraints construction methods (``orientation_constraint_from_frame``, ``position_constraint_from_frame``, ``constraints_from_frame``) in ``Robot`` class to use the frame of the attached tool if a tool is attached. This behavior can be reverted back (ie. only calculate T0CF) using the flag ``use_attached_tool_frame`` of all these methods.
 
 **Fixed**
 

docs/examples/06_backends_kinematics/files/04_cartesian_path_analytic_pybullet.py

@@ -3,19 +3,21 @@
 
 from compas_fab.backends import AnalyticalPyBulletClient
 
-frames_WCF = [Frame((0.407, 0.073, 0.320), (0.922, 0.000, 0.388), (0.113, 0.956, -0.269)),
-              Frame((0.404, 0.057, 0.324), (0.919, 0.000, 0.394), (0.090, 0.974, -0.210)),
-              Frame((0.390, 0.064, 0.315), (0.891, 0.000, 0.454), (0.116, 0.967, -0.228)),
-              Frame((0.388, 0.079, 0.309), (0.881, 0.000, 0.473), (0.149, 0.949, -0.278)),
-              Frame((0.376, 0.087, 0.299), (0.850, 0.000, 0.528), (0.184, 0.937, -0.296))]
+frames_WCF = [
+    Frame((0.407, 0.073, 0.320), (0.922, 0.000, 0.388), (0.113, 0.956, -0.269)),
+    Frame((0.404, 0.057, 0.324), (0.919, 0.000, 0.394), (0.090, 0.974, -0.210)),
+    Frame((0.390, 0.064, 0.315), (0.891, 0.000, 0.454), (0.116, 0.967, -0.228)),
+    Frame((0.388, 0.079, 0.309), (0.881, 0.000, 0.473), (0.149, 0.949, -0.278)),
+    Frame((0.376, 0.087, 0.299), (0.850, 0.000, 0.528), (0.184, 0.937, -0.296)),
+]
 
-with AnalyticalPyBulletClient(connection_type='direct') as client:
+with AnalyticalPyBulletClient(connection_type="direct") as client:
     robot = client.load_ur5(load_geometry=True)
 
     options = {"solver": "ur5", "check_collision": True}
     start_configuration = list(robot.iter_inverse_kinematics(frames_WCF[0], options=options))[-1]
     trajectory = robot.plan_cartesian_motion(frames_WCF, start_configuration=start_configuration, options=options)
-    assert(trajectory.fraction == 1.)
+    assert trajectory.fraction == 1.0
 
     j = [c.joint_values for c in trajectory.points]
     plt.plot(j)

src/compas_fab/robots/robot.py

@@ -940,7 +940,7 @@ def ensure_geometry(self):
     # constraints
     # ==========================================================================
 
-    def orientation_constraint_from_frame(self, frame_WCF, tolerances_axes, group=None):
+    def orientation_constraint_from_frame(self, frame_WCF, tolerances_axes, group=None, use_attached_tool_frame=True):
         r"""Create an orientation constraint from a frame on the group's end-effector link.
 
         Parameters
@@ -953,6 +953,9 @@ def orientation_constraint_from_frame(self, frame_WCF, tolerances_axes, group=No
         group: :obj:`str`, optional
             The planning group for which we specify the constraint. Defaults to
             the robot's main planning group.
+        use_attached_tool_frame : :obj:`bool`, optional
+            If ``True`` and there is a tool attached to the planning group, it will use its TCF
+            instead of the T0CF to create the constraints. Defaults to ``True``.
 
         Returns
         -------
@@ -982,16 +985,21 @@ def orientation_constraint_from_frame(self, frame_WCF, tolerances_axes, group=No
         OrientationConstraint('ee_link', [0.5, 0.5, 0.5, 0.5], [0.017453292519943295, 0.017453292519943295, 0.017453292519943295], 1.0)
         """
 
+        attached_tool = self.attached_tools.get(group)
+        if use_attached_tool_frame and attached_tool:
+            frame_WCF = self.from_tcf_to_t0cf([frame_WCF], group)[0]
+
         ee_link = self.get_end_effector_link_name(group)
 
         tolerances_axes = list(tolerances_axes)
         if len(tolerances_axes) == 1:
             tolerances_axes *= 3
         elif len(tolerances_axes) != 3:
             raise ValueError("Must give either one or 3 values")
+
         return OrientationConstraint(ee_link, frame_WCF.quaternion, tolerances_axes)
 
-    def position_constraint_from_frame(self, frame_WCF, tolerance_position, group=None):
+    def position_constraint_from_frame(self, frame_WCF, tolerance_position, group=None, use_attached_tool_frame=True):
         """Create a position constraint from a frame on the group's end-effector link.
 
         Parameters
@@ -1004,6 +1012,9 @@ def position_constraint_from_frame(self, frame_WCF, tolerance_position, group=No
         group: :obj:`str`, optional
             The planning group for which we specify the constraint. Defaults to
             the robot's main planning group.
+        use_attached_tool_frame : :obj:`bool`, optional
+            If ``True`` and there is a tool attached to the planning group, it will use its TCF
+            instead of the T0CF to create the constraints. Defaults to ``True``.
 
         Returns
         -------
@@ -1032,11 +1043,15 @@ def position_constraint_from_frame(self, frame_WCF, tolerance_position, group=No
         PositionConstraint('ee_link', BoundingVolume(2, Sphere(Point(0.400, 0.300, 0.400), 0.001)), 1.0)
         """
 
+        attached_tool = self.attached_tools.get(group)
+        if use_attached_tool_frame and attached_tool:
+            frame_WCF = self.from_tcf_to_t0cf([frame_WCF], group)[0]
+
         ee_link = self.get_end_effector_link_name(group)
         sphere = Sphere(frame_WCF.point, tolerance_position)
         return PositionConstraint.from_sphere(ee_link, sphere)
 
-    def constraints_from_frame(self, frame_WCF, tolerance_position, tolerances_axes, group=None):
+    def constraints_from_frame(self, frame_WCF, tolerance_position, tolerances_axes, group=None, use_attached_tool_frame=True):
         r"""Create a position and an orientation constraint from a frame calculated for the group's end-effector link.
 
         Parameters
@@ -1052,6 +1067,9 @@ def constraints_from_frame(self, frame_WCF, tolerance_position, tolerances_axes,
         group: :obj:`str`, optional
             The planning group for which we specify the constraint. Defaults to
             the robot's main planning group.
+        use_attached_tool_frame : :obj:`bool`, optional
+            If ``True`` and there is a tool attached to the planning group, it will use its TCF
+            instead of the T0CF to create the constraints. Defaults to ``True``.
 
         Returns
         -------
@@ -1084,8 +1102,8 @@ def constraints_from_frame(self, frame_WCF, tolerance_position, tolerances_axes,
         [PositionConstraint('ee_link', BoundingVolume(2, Sphere(Point(0.400, 0.300, 0.400), 0.001)), 1.0),
         OrientationConstraint('ee_link', [0.5, 0.5, 0.5, 0.5], [0.017453292519943295, 0.017453292519943295, 0.017453292519943295], 1.0)]
         """
-        pc = self.position_constraint_from_frame(frame_WCF, tolerance_position, group)
-        oc = self.orientation_constraint_from_frame(frame_WCF, tolerances_axes, group)
+        pc = self.position_constraint_from_frame(frame_WCF, tolerance_position, group, use_attached_tool_frame)
+        oc = self.orientation_constraint_from_frame(frame_WCF, tolerances_axes, group, use_attached_tool_frame)
         return [pc, oc]
 
     def constraints_from_configuration(self, configuration, tolerances_above, tolerances_below, group=None):
@@ -1164,7 +1182,7 @@ def constraints_from_configuration(self, configuration, tolerances_above, tolera
     # services
     # ==========================================================================
 
-    def inverse_kinematics(self, frame_WCF, start_configuration=None, group=None, return_full_configuration=False, options=None):
+    def inverse_kinematics(self, frame_WCF, start_configuration=None, group=None, return_full_configuration=False, use_attached_tool_frame=True, options=None):
         """Calculate the robot's inverse kinematic for a given frame.
 
         The inverse kinematic solvers are implemented as generators in order to fit both analytic
@@ -1190,6 +1208,9 @@ def inverse_kinematics(self, frame_WCF, start_configuration=None, group=None, re
         return_full_configuration : :obj:`bool`, optional
             If ``True``, returns a full configuration with all joint values
             specified, including passive ones if available. Defaults to ``False``.
+        use_attached_tool_frame : :obj:`bool`, optional
+            If ``True`` and there is a tool attached to the planning group, it will use its TCF
+            instead of the T0CF to calculate IK. Defaults to ``True``.
         options: :obj:`dict`, optional
             Dictionary containing the key-value pairs of additional options.
             The valid options are specific to the backend in use.
@@ -1221,13 +1242,13 @@ def inverse_kinematics(self, frame_WCF, start_configuration=None, group=None, re
             if solution is not None:
                 return solution
 
-        solutions = self.iter_inverse_kinematics(frame_WCF, start_configuration, group, return_full_configuration, options)
+        solutions = self.iter_inverse_kinematics(frame_WCF, start_configuration, group, return_full_configuration, use_attached_tool_frame, options)
         self._current_ik["request_id"] = request_id
         self._current_ik["solutions"] = solutions
 
         return next(solutions)
 
-    def iter_inverse_kinematics(self, frame_WCF, start_configuration=None, group=None, return_full_configuration=False, options=None):
+    def iter_inverse_kinematics(self, frame_WCF, start_configuration=None, group=None, return_full_configuration=False, use_attached_tool_frame=True, options=None):
         """Iterate over the inverse kinematic solutions of a robot.
 
         This method exposes the generator-based inverse kinematic solvers. Analytics solvers will return
@@ -1250,6 +1271,9 @@ def iter_inverse_kinematics(self, frame_WCF, start_configuration=None, group=Non
         return_full_configuration : :obj:`bool`, optional
             If ``True``, returns a full configuration with all joint values
             specified, including passive ones if available. Defaults to ``False``.
+        use_attached_tool_frame : :obj:`bool`, optional
+            If ``True`` and there is a tool attached to the planning group, it will use its TCF
+            instead of the T0CF to calculate IK. Defaults to ``True``.
         options: :obj:`dict`, optional
             Dictionary containing the key-value pairs of additional options.
             The valid options are specific to the backend in use.
@@ -1281,6 +1305,10 @@ def iter_inverse_kinematics(self, frame_WCF, start_configuration=None, group=Non
 
         start_configuration, start_configuration_scaled = self._check_full_configuration_and_scale(start_configuration)
 
+        attached_tool = self.attached_tools.get(group)
+        if use_attached_tool_frame and attached_tool:
+            frame_WCF = self.from_tcf_to_t0cf([frame_WCF], group)[0]
+
         frame_WCF_scaled = frame_WCF.copy()
         frame_WCF_scaled.point /= self.scale_factor  # must be in meters
 
@@ -1329,7 +1357,7 @@ def inverse_kinematics_deprecated(
             ),
         )
 
-    def forward_kinematics(self, configuration, group=None, options=None):
+    def forward_kinematics(self, configuration, group=None, use_attached_tool_frame=True, options=None):
         """Calculate the robot's forward kinematic.
 
         Parameters
@@ -1341,6 +1369,9 @@ def forward_kinematics(self, configuration, group=None, options=None):
         group: obj:`str`, optional
             The planning group used for the calculation. Defaults to the robot's
             main planning group.
+        use_attached_tool_frame : :obj:`bool`, optional
+            If ``True`` and there is a tool attached to the planning group, FK will return
+            the TCF of the attached tool instead of the T0CF. Defaults to ``True``.
         options: obj:`dict`, optional
             Dictionary containing the following key-value pairs:
 
@@ -1409,12 +1440,17 @@ def forward_kinematics(self, configuration, group=None, options=None):
 
         # Scale and return
         frame_WCF.point *= self.scale_factor
+
+        attached_tool = self.attached_tools.get(group)
+        if use_attached_tool_frame and attached_tool:
+            frame_WCF = self.from_t0cf_to_tcf([frame_WCF], group)[0]
+
         return frame_WCF
 
     def forward_kinematics_deprecated(self, configuration, group=None, backend=None, ee_link=None):
         return self.forward_kinematics(configuration, group, options=dict(solver=backend, link=ee_link))
 
-    def plan_cartesian_motion(self, frames_WCF, start_configuration=None, group=None, options=None):
+    def plan_cartesian_motion(self, frames_WCF, start_configuration=None, group=None, use_attached_tool_frame=True, options=None):
         """Calculate a cartesian motion path (linear in tool space).
 
         Parameters
@@ -1428,6 +1464,9 @@ def plan_cartesian_motion(self, frames_WCF, start_configuration=None, group=None
         group : :obj:`str`, optional
             The planning group used for calculation. Defaults to the robot's
             main planning group.
+        use_attached_tool_frame : :obj:`bool`, optional
+            If ``True`` and there is a tool attached to the planning group, it will use its TCF
+            instead of the T0CF to calculate cartesian paths. Defaults to ``True``.
         options : :obj:`dict`, optional
             Dictionary containing the following key-value pairs:
 
@@ -1484,6 +1523,10 @@ def plan_cartesian_motion(self, frames_WCF, start_configuration=None, group=None
         # that all configurable joints of the whole robot are defined for planning.
         start_configuration, start_configuration_scaled = self._check_full_configuration_and_scale(start_configuration)
 
+        attached_tool = self.attached_tools.get(group)
+        if use_attached_tool_frame and attached_tool:
+            frames_WCF = self.from_tcf_to_t0cf(frames_WCF, group)
+
         frames_WCF_scaled = []
         for frame in frames_WCF:
             frames_WCF_scaled.append(Frame(frame.point * 1.0 / self.scale_factor, frame.xaxis, frame.yaxis))

tests/backends/kinematics/test_inverse_kinematics.py

@@ -4,13 +4,14 @@
 
 import compas_fab
 from compas_fab.backends import AnalyticalInverseKinematics
+from compas_fab.robots import Tool
 from compas_fab.robots.ur5 import Robot
 
 if not compas.IPY:
     from compas_fab.backends import AnalyticalPyBulletClient
 
-urdf_filename = compas_fab.get('universal_robot/ur_description/urdf/ur5.urdf')
-srdf_filename = compas_fab.get('universal_robot/ur5_moveit_config/config/ur5.srdf')
+urdf_filename = compas_fab.get("universal_robot/ur_description/urdf/ur5.urdf")
+srdf_filename = compas_fab.get("universal_robot/ur5_moveit_config/config/ur5.srdf")
 
 
 def test_inverse_kinematics():
@@ -19,10 +20,10 @@ def test_inverse_kinematics():
     frame_WCF = Frame((0.381, 0.093, 0.382), (0.371, -0.292, -0.882), (0.113, 0.956, -0.269))
     # set the solver later
     solutions = list(ik.inverse_kinematics(robot, frame_WCF, start_configuration=None, group=None, options={"solver": "ur5"}))
-    assert(len(solutions) == 8)
+    assert len(solutions) == 8
     joint_positions, _ = solutions[0]
     correct = Configuration.from_revolute_values((0.022, 4.827, 1.508, 1.126, 1.876, 3.163))
-    assert(correct.close_to(Configuration.from_revolute_values(joint_positions)))
+    assert correct.close_to(Configuration.from_revolute_values(joint_positions))
 
 
 def test_inverse_kinematics2():
@@ -31,37 +32,69 @@ def test_inverse_kinematics2():
     frame_WCF = Frame((0.381, 0.093, 0.382), (0.371, -0.292, -0.882), (0.113, 0.956, -0.269))
     # set the solver later
     solutions = list(ik.inverse_kinematics(robot, frame_WCF, start_configuration=None, group=None))
-    assert(len(solutions) == 8)
+    assert len(solutions) == 8
     joint_positions, _ = solutions[0]
     correct = Configuration.from_revolute_values((0.022, 4.827, 1.508, 1.126, 1.876, 3.163))
-    assert(correct.close_to(Configuration.from_revolute_values(joint_positions)))
+    assert correct.close_to(Configuration.from_revolute_values(joint_positions))
 
 
 def test_kinematics_client():
     if compas.IPY:
         return
     frame_WCF = Frame((0.381, 0.093, 0.382), (0.371, -0.292, -0.882), (0.113, 0.956, -0.269))
-    with AnalyticalPyBulletClient(connection_type='direct') as client:
+    with AnalyticalPyBulletClient(connection_type="direct") as client:
         robot = client.load_robot(urdf_filename)
         client.load_semantics(robot, srdf_filename)
         solutions = list(robot.iter_inverse_kinematics(frame_WCF, options={"solver": "ur5", "check_collision": True, "keep_order": False}))
-        assert(len(solutions) == 5)
+        assert len(solutions) == 5
+
+
+def test_kinematics_client_with_attached_tool_but_disabled_usage_of_tcf():
+    if compas.IPY:
+        return
+    frame_WCF = Frame((0.381, 0.093, 0.382), (0.371, -0.292, -0.882), (0.113, 0.956, -0.269))
+    with AnalyticalPyBulletClient(connection_type="direct") as client:
+        robot = client.load_robot(urdf_filename)
+        client.load_semantics(robot, srdf_filename)
+
+        tool_frame = Frame([0.0, 0, 0], [0, 1, 0], [0, 0, 1])
+        robot.attach_tool(Tool(visual=None, frame_in_tool0_frame=tool_frame))
+
+        solutions = list(robot.iter_inverse_kinematics(frame_WCF, use_attached_tool_frame=False, options={"solver": "ur5", "check_collision": True, "keep_order": False}))
+        assert len(solutions) == 5
+
+
+def test_kinematics_client_with_attached_tool():
+    if compas.IPY:
+        return
+    frame_WCF = Frame((0.381, 0.093, 0.382), (0.371, -0.292, -0.882), (0.113, 0.956, -0.269))
+    with AnalyticalPyBulletClient(connection_type="direct") as client:
+        robot = client.load_robot(urdf_filename)
+        client.load_semantics(robot, srdf_filename)
+
+        tool_frame = Frame([0, 0, 0.6], [1, 0, 0], [0, 1, 0])
+        robot.attach_tool(Tool(visual=None, frame_in_tool0_frame=tool_frame))
+
+        solutions = list(robot.iter_inverse_kinematics(frame_WCF, options={"solver": "ur5", "check_collision": True, "keep_order": False}))
+        assert len(solutions) == 4
 
 
 def test_kinematics_cartesian():
     if compas.IPY:
         return
-    frames_WCF = [Frame((0.407, 0.073, 0.320), (0.922, 0.000, 0.388), (0.113, 0.956, -0.269)),
-                  Frame((0.404, 0.057, 0.324), (0.919, 0.000, 0.394), (0.090, 0.974, -0.210)),
-                  Frame((0.390, 0.064, 0.315), (0.891, 0.000, 0.454), (0.116, 0.967, -0.228)),
-                  Frame((0.388, 0.079, 0.309), (0.881, 0.000, 0.473), (0.149, 0.949, -0.278)),
-                  Frame((0.376, 0.087, 0.299), (0.850, 0.000, 0.528), (0.184, 0.937, -0.296))]
+    frames_WCF = [
+        Frame((0.407, 0.073, 0.320), (0.922, 0.000, 0.388), (0.113, 0.956, -0.269)),
+        Frame((0.404, 0.057, 0.324), (0.919, 0.000, 0.394), (0.090, 0.974, -0.210)),
+        Frame((0.390, 0.064, 0.315), (0.891, 0.000, 0.454), (0.116, 0.967, -0.228)),
+        Frame((0.388, 0.079, 0.309), (0.881, 0.000, 0.473), (0.149, 0.949, -0.278)),
+        Frame((0.376, 0.087, 0.299), (0.850, 0.000, 0.528), (0.184, 0.937, -0.296)),
+    ]
 
-    with AnalyticalPyBulletClient(connection_type='direct') as client:
+    with AnalyticalPyBulletClient(connection_type="direct") as client:
         robot = client.load_robot(urdf_filename)
         client.load_semantics(robot, srdf_filename)
 
         options = {"solver": "ur5", "check_collision": True}
         start_configuration = list(robot.iter_inverse_kinematics(frames_WCF[0], options=options))[-1]
         trajectory = robot.plan_cartesian_motion(frames_WCF, start_configuration=start_configuration, options=options)
-        assert(trajectory.fraction == 1.)
+        assert trajectory.fraction == 1.0