diff --git a/CHANGELOG.md b/CHANGELOG.md index 749e2b2d..e2f189ca 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,7 @@ All notable changes to this project will be documented in this file. ### Changed - Configuration: update function now accepts a configuration vector argument +- Costs: setting cost in relative frame task now accepts plain float ## [2.0.0] - 2024-03-05 diff --git a/pink/tasks/relative_frame_task.py b/pink/tasks/relative_frame_task.py index a7181959..48bf5296 100644 --- a/pink/tasks/relative_frame_task.py +++ b/pink/tasks/relative_frame_task.py @@ -27,6 +27,7 @@ class RelativeFrameTask(Task): transform_target_to_root: Target pose of the frame. """ + cost: np.ndarray frame: str root: str transform_target_to_root: Optional[pin.SE3] @@ -86,15 +87,19 @@ def set_position_cost( """ if isinstance(position_cost, float): assert position_cost >= 0.0 - else: # not isinstance(position_cost, float) - assert all(cost >= 0.0 for cost in position_cost) - if isinstance(self.cost, np.ndarray): - self.cost[0:3] = position_cost - else: # self.cost is not a vector - raise TaskDefinitionError( - "Frame task cost should be a vector, " - f"currently cost={self.cost}" - ) + else: # Should be a ndarray or seq + if not isinstance(position_cost, np.ndarray): # Must be seq + try: + position_cost = np.array(position_cost) + except Exception: # Not a proper float sequence + raise TaskDefinitionError( + "Position task cost should be a float or a " + "seq of float or ndarray of size 1 or 3," + f"currently cost={self.cost}" + ) + assert np.all(position_cost >= 0.0) + + self.cost[0:3] = position_cost def set_orientation_cost( self, orientation_cost: Union[float, Sequence[float], np.ndarray] @@ -109,15 +114,19 @@ def set_orientation_cost( """ if isinstance(orientation_cost, float): assert orientation_cost >= 0.0 - else: # not isinstance(orientation_cost, float) - assert all(cost >= 0.0 for cost in orientation_cost) - if isinstance(self.cost, np.ndarray): - self.cost[3:6] = orientation_cost - else: # self.cost is not a vector - raise TaskDefinitionError( - "Frame task cost should be a vector, " - f"currently cost={self.cost}" - ) + else: # Should be a ndarray or seq + if not isinstance(orientation_cost, np.ndarray): # Must be seq + try: + orientation_cost = np.array(orientation_cost) + except Exception: # Not a proper float sequence + raise TaskDefinitionError( + "Orientation task cost should be a float or a " + "seq of float or ndarray of size 1 or 3," + f"currently cost={self.cost}" + ) + assert np.all(orientation_cost >= 0.0) + + self.cost[3:] = orientation_cost def set_target( self, @@ -151,7 +160,7 @@ def compute_error(self, configuration: Configuration) -> np.ndarray: .. math:: - e(q) := {}_b \xi_{0b} = \log(T_{bt}) + e(q) := \log(T_{tf}) = \log(T_{rt}^{-1} T_{rf}) where :math:`b` denotes our frame, :math:`t` the target frame and :math:`0` the inertial frame. See also :func:`Task.compute_error` for @@ -171,10 +180,10 @@ def compute_error(self, configuration: Configuration) -> np.ndarray: transform_frame_to_root = configuration.get_transform( self.frame, self.root ) - transform_target_to_frame = transform_frame_to_root.actInv( - self.transform_target_to_root + transform_frame_to_target = self.transform_target_to_root.actInv( + transform_frame_to_root ) - error_in_frame: np.ndarray = pin.log(transform_target_to_frame).vector + error_in_frame: np.ndarray = pin.log(transform_frame_to_target).vector return error_in_frame def compute_jacobian(self, configuration: Configuration) -> np.ndarray: @@ -186,8 +195,24 @@ def compute_jacobian(self, configuration: Configuration) -> np.ndarray: .. math:: - J(q) = \text{Jlog}_6(T_{bt}) (\text{Ad}_{T_{tr}) {}_r J_{0r}(q) - - \text{Ad}_{T_{tb}} {}_b J_{0b}(q)) + J(q) = \text{Jlog}(T_{tf}) ( + {}_f J_{0f}(q) - \text{Ad}_{T_{fr}}{}_r J_{0r}(q) + ) + + The proof is as follow if we denote :math:`JT(q)` the Jacobian in the + local frame :math:`T_{tf}(q) = T_{rt}^{-1} T_{0r}^{-1}(q) T_{0f}(q)` + we have + .. math:: + J(q) = \text{Jlog}(T_{tf}) JT + + [Jv]^{up} = T_{tf}^{-1} \dot{T}_{tf} + = T_{ft}T_{tr}T_{r0}\dot{T}_{0f} + - T_{ft}T_{tr}T_{0r}^{-1}\dot{T}_{0r}T_{0r}^{-1}T_{0f} + = T_{0f}^{-1}\dot{T}_{0f} + - T_{fr}(T_{0r}^{-1}\dot{T}_{0r})T_{fr}^{-1} + = [{}_f J_{0f} v - \text{Ad}_{T_{fr}}{}_r J_{0r} v]^{up} + + J = {}_f J_{0f} - \text{Ad}_{T_{fr}}{}_r J_{0r} The formula implemented here is more general than the one detailed in [FrameTaskJacobian]_. See also :func:`Task.compute_jacobian` for more @@ -204,21 +229,18 @@ def compute_jacobian(self, configuration: Configuration) -> np.ndarray: f"target pose of frame '{self.frame}' " f"in frame '{self.root}' is undefined" ) - transform_root_to_frame = configuration.get_transform( - self.root, self.frame + transform_frame_to_root = configuration.get_transform( + self.frame, self.root ) - transform_target_to_frame = transform_root_to_frame.act( - self.transform_target_to_root + transform_frame_to_target = self.transform_target_to_root.actInv( + transform_frame_to_root ) + action_root_to_frame = transform_frame_to_root.actionInverse jacobian_frame_in_frame = configuration.get_frame_jacobian(self.frame) jacobian_root_in_root = configuration.get_frame_jacobian(self.root) - action_frame_to_target = transform_target_to_frame.actionInverse - action_root_to_target = transform_target_to_frame.actInv( - transform_root_to_frame - ).action - J = pin.Jlog6(transform_target_to_frame) @ ( - action_root_to_target @ jacobian_root_in_root - - action_frame_to_target @ jacobian_frame_in_frame + J = pin.Jlog6(transform_frame_to_target) @ ( + jacobian_frame_in_frame + - action_root_to_frame @ jacobian_root_in_root ) return J diff --git a/tests/test_relative_frame_task.py b/tests/test_relative_frame_task.py index ca34a259..d9e276b9 100644 --- a/tests/test_relative_frame_task.py +++ b/tests/test_relative_frame_task.py @@ -98,13 +98,13 @@ def test_matches_frame_task(self): frame_task.set_target_from_configuration(self.configuration) self.assertTrue( np.allclose( - relative_task.compute_error(self.configuration), + -relative_task.compute_error(self.configuration), frame_task.compute_error(self.configuration), ) ) self.assertTrue( np.allclose( - relative_task.compute_jacobian(self.configuration), + -relative_task.compute_jacobian(self.configuration), frame_task.compute_jacobian(self.configuration), ) ) @@ -146,118 +146,118 @@ def test_relative_jacobian(self): J_check = np.array( [ [ - 8.90314314e-01, - -2.62831699e-01, - 5.23328878e-01, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - -3.63284356e-01, - 3.00488917e-01, - 3.00488917e-01, - 3.00488917e-01, - 3.00488917e-01, - 1.21569960e-01, - -5.40643664e-02, - 2.64531107e-03, - 0.00000000e00, - 8.52051844e-02, - 0.00000000e00, + -8.90314314e-01, + 2.62831699e-01, + -5.23328878e-01, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + 3.63284356e-01, + -3.00488917e-01, + -3.00488917e-01, + -3.00488917e-01, + -3.00488917e-01, + -1.21569960e-01, + 5.40643664e-02, + -2.64531107e-03, + -0.00000000e+00, + -8.52051844e-02, + -0.00000000e+00, ], [ - 4.35448031e-01, - 2.52682609e-01, - 3.20948600e-01, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 8.66420482e-01, - -2.59185486e-01, - -2.59185486e-01, - -2.59185486e-01, - -2.59185486e-01, - 1.29231854e-01, - 1.95397417e-01, - 1.29331003e-01, - 0.00000000e00, - 3.40020430e-10, - 0.00000000e00, + -4.35448031e-01, + -2.52682609e-01, + -3.20948600e-01, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -8.66420482e-01, + 2.59185486e-01, + 2.59185486e-01, + 2.59185486e-01, + 2.59185486e-01, + -1.29231854e-01, + -1.95397417e-01, + -1.29331003e-01, + -0.00000000e+00, + -3.40020430e-10, + -0.00000000e+00, ], [ - 1.33136965e-01, - 9.31166481e-01, - 2.43700772e-01, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - -3.42549304e-01, - -9.17893955e-01, - -9.17893955e-01, - -9.17893955e-01, - -9.17893955e-01, - 1.89931943e-01, - -1.30718019e-01, - 1.41489258e-03, - 0.00000000e00, - 1.71149858e-01, - 0.00000000e00, + -1.33136965e-01, + -9.31166481e-01, + -2.43700772e-01, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + 3.42549304e-01, + 9.17893955e-01, + 9.17893955e-01, + 9.17893955e-01, + 9.17893955e-01, + -1.89931943e-01, + 1.30718019e-01, + -1.41489258e-03, + -0.00000000e+00, + -1.71149858e-01, + -0.00000000e+00, ], [ - 0.00000000e00, - 0.00000000e00, - -3.71833209e-01, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - -3.54998241e-01, - -7.69530324e-01, - 4.71641546e-01, - 0.00000000e00, - 2.81651980e-09, - 0.00000000e00, + -0.00000000e+00, + -0.00000000e+00, + 3.71833209e-01, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + 3.54998241e-01, + 7.69530324e-01, + -4.71641546e-01, + -0.00000000e+00, + -2.81651980e-09, + -0.00000000e+00, ], [ - 0.00000000e00, - 0.00000000e00, - 8.64023446e-01, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 8.62832198e-01, - -4.88273308e-01, - -9.25233451e-10, - 0.00000000e00, - 1.00000000e00, - 0.00000000e00, + -0.00000000e+00, + -0.00000000e+00, + -8.64023446e-01, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -8.62832198e-01, + 4.88273308e-01, + 9.25233451e-10, + -0.00000000e+00, + -1.00000000e+00, + -0.00000000e+00, ], [ - 0.00000000e00, - 0.00000000e00, - -3.39416483e-01, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - -3.59856704e-01, - -4.11597224e-01, - -8.81790377e-01, - 0.00000000e00, - 2.57093808e-09, - 0.00000000e00, + -0.00000000e+00, + -0.00000000e+00, + 3.39416483e-01, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + -0.00000000e+00, + 3.59856704e-01, + 4.11597224e-01, + 8.81790377e-01, + -0.00000000e+00, + -2.57093808e-09, + -0.00000000e+00, ], ] )