Skip to content

Commit

Permalink
Merge pull request #82 from ymontmarin/master
Browse files Browse the repository at this point in the history 
Use a simplified formulation of the relative task
  • Loading branch information
@stephane-caron
stephane-caron authored Apr 2, 2024
2 parents af01adb + a22dda2 commit 1bcfd48
Show file tree
Hide file tree
Showing 3 changed files with 162 additions and 139 deletions.
1 change: 1 addition & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
92 changes: 57 additions & 35 deletions pink/tasks/relative_frame_task.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down Expand Up @@ -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]
Expand All @@ -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,
Expand Down Expand Up @@ -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
Expand All @@ -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:
Expand All @@ -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
Expand All @@ -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

Expand Down
208 changes: 104 additions & 104 deletions tests/test_relative_frame_task.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
)
)
Expand Down Expand Up @@ -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,
],
]
)
Expand Down

0 comments on commit 1bcfd48

Please sign in to comment.