pinker 0.1.0b0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- pinker/__init__.py +35 -0
- pinker/barriers/__init__.py +13 -0
- pinker/barriers/barrier.py +275 -0
- pinker/barriers/body_spherical_barrier.py +186 -0
- pinker/barriers/position_barrier.py +150 -0
- pinker/configuration.py +265 -0
- pinker/exceptions.py +113 -0
- pinker/kinematics/.clang-format +12 -0
- pinker/kinematics/__init__.py +126 -0
- pinker/kinematics/_kinematics_c.c +1698 -0
- pinker/kinematics/algorithms.py +448 -0
- pinker/kinematics/geometry.py +109 -0
- pinker/kinematics/joints.py +326 -0
- pinker/kinematics/model.py +610 -0
- pinker/kinematics/robot_wrapper.py +86 -0
- pinker/kinematics/se3.py +421 -0
- pinker/kinematics/so3.py +143 -0
- pinker/kinematics/urdf.py +480 -0
- pinker/limits/__init__.py +29 -0
- pinker/limits/acceleration_limit.py +198 -0
- pinker/limits/configuration_limit.py +143 -0
- pinker/limits/floating_base_velocity_limit.py +146 -0
- pinker/limits/limit.py +42 -0
- pinker/limits/velocity_limit.py +119 -0
- pinker/loaders.py +143 -0
- pinker/solve_ik.py +262 -0
- pinker/tasks/__init__.py +33 -0
- pinker/tasks/com_task.py +156 -0
- pinker/tasks/damping_task.py +43 -0
- pinker/tasks/frame_task.py +259 -0
- pinker/tasks/joint_coupling_task.py +110 -0
- pinker/tasks/joint_velocity_task.py +110 -0
- pinker/tasks/linear_holonomic_task.py +199 -0
- pinker/tasks/low_acceleration_task.py +81 -0
- pinker/tasks/manipulability_task.py +424 -0
- pinker/tasks/omniwheel_task.py +74 -0
- pinker/tasks/posture_task.py +134 -0
- pinker/tasks/relative_frame_task.py +261 -0
- pinker/tasks/rolling_task.py +148 -0
- pinker/tasks/task.py +168 -0
- pinker/utils.py +44 -0
- pinker/visualizer.py +295 -0
- pinker-0.1.0b0.dist-info/METADATA +277 -0
- pinker-0.1.0b0.dist-info/RECORD +47 -0
- pinker-0.1.0b0.dist-info/WHEEL +4 -0
- pinker-0.1.0b0.dist-info/licenses/LICENSE +201 -0
- pinker-0.1.0b0.dist-info/licenses/NOTICE +9 -0
pinker/__init__.py
ADDED
|
@@ -0,0 +1,35 @@
|
|
|
1
|
+
# SPDX-License-Identifier: Apache-2.0
|
|
2
|
+
|
|
3
|
+
"""Inverse kinematics for articulated robot models."""
|
|
4
|
+
|
|
5
|
+
from .configuration import Configuration
|
|
6
|
+
from .exceptions import PinkerError
|
|
7
|
+
from .kinematics import custom_configuration
|
|
8
|
+
from .loaders import load_robot_description, load_robot_urdf
|
|
9
|
+
from .solve_ik import build_ik, solve_ik
|
|
10
|
+
from .tasks import (
|
|
11
|
+
FrameTask,
|
|
12
|
+
JointCouplingTask,
|
|
13
|
+
JointVelocityTask,
|
|
14
|
+
LinearHolonomicTask,
|
|
15
|
+
PostureTask,
|
|
16
|
+
Task,
|
|
17
|
+
)
|
|
18
|
+
|
|
19
|
+
__version__ = "0.1.0-beta"
|
|
20
|
+
|
|
21
|
+
__all__ = [
|
|
22
|
+
"Configuration",
|
|
23
|
+
"FrameTask",
|
|
24
|
+
"JointCouplingTask",
|
|
25
|
+
"JointVelocityTask",
|
|
26
|
+
"LinearHolonomicTask",
|
|
27
|
+
"PinkerError",
|
|
28
|
+
"PostureTask",
|
|
29
|
+
"Task",
|
|
30
|
+
"build_ik",
|
|
31
|
+
"custom_configuration",
|
|
32
|
+
"load_robot_description",
|
|
33
|
+
"load_robot_urdf",
|
|
34
|
+
"solve_ik",
|
|
35
|
+
]
|
|
@@ -0,0 +1,13 @@
|
|
|
1
|
+
# SPDX-License-Identifier: Apache-2.0
|
|
2
|
+
|
|
3
|
+
"""Control Barrier Functions."""
|
|
4
|
+
|
|
5
|
+
from .barrier import Barrier
|
|
6
|
+
from .body_spherical_barrier import BodySphericalBarrier
|
|
7
|
+
from .position_barrier import PositionBarrier
|
|
8
|
+
|
|
9
|
+
__all__ = [
|
|
10
|
+
"Barrier",
|
|
11
|
+
"PositionBarrier",
|
|
12
|
+
"BodySphericalBarrier",
|
|
13
|
+
]
|
|
@@ -0,0 +1,275 @@
|
|
|
1
|
+
# SPDX-License-Identifier: Apache-2.0
|
|
2
|
+
|
|
3
|
+
"""All barriers derive from the :class:`Barrier` base class.
|
|
4
|
+
|
|
5
|
+
The formalism used in this implementation is written down in
|
|
6
|
+
https://web.archive.org/web/20241125170734/https://simeon-ned.com/blog/2024/cbf/
|
|
7
|
+
"""
|
|
8
|
+
|
|
9
|
+
import abc
|
|
10
|
+
from typing import Callable, Optional, Tuple, Union
|
|
11
|
+
|
|
12
|
+
import numpy as np
|
|
13
|
+
|
|
14
|
+
from ..configuration import Configuration
|
|
15
|
+
|
|
16
|
+
|
|
17
|
+
class Barrier(abc.ABC):
|
|
18
|
+
r"""Abstract base class for barrier.
|
|
19
|
+
|
|
20
|
+
A barrier is a function :math:`h(q)` that
|
|
21
|
+
satisfies the following condition:
|
|
22
|
+
|
|
23
|
+
.. math::
|
|
24
|
+
|
|
25
|
+
\frac{\partial h_j}{\partial q}
|
|
26
|
+
\dot{q} +\alpha_j(h_j(q))
|
|
27
|
+
\geq 0, \quad \forall j
|
|
28
|
+
|
|
29
|
+
where :math:`\frac{\partial h_j}{\partial q}`
|
|
30
|
+
are the Jacobians of the constraint functions, :math:`\dot{q}`
|
|
31
|
+
is the joint velocity vector, and :math:`\alpha_j` are extended
|
|
32
|
+
`class kappa <https://en.wikipedia.org/wiki/Class_kappa_function>`__
|
|
33
|
+
functions.
|
|
34
|
+
|
|
35
|
+
On top of that, following `this article
|
|
36
|
+
<https://arxiv.org/pdf/2404.12329>`__ barriers utilize safe displacement
|
|
37
|
+
term is added to the cost of the optimization problem:
|
|
38
|
+
|
|
39
|
+
.. math::
|
|
40
|
+
|
|
41
|
+
\frac{r}{2\|J_h\|^{2}}\|dq-dq_{\text{safe}}(q)\|^{2},
|
|
42
|
+
|
|
43
|
+
where :math:`J_h` is the Jacobian of the barrier function, dq is the
|
|
44
|
+
joint displacement vector, and :math:`dq_{\text{safe}}(q)` is the safe
|
|
45
|
+
displacement vector.
|
|
46
|
+
|
|
47
|
+
Attributes:
|
|
48
|
+
dim: Dimension of the barrier.
|
|
49
|
+
gain: linear barrier gain.
|
|
50
|
+
gain_function: function, that defines stabilization term as nonlinear
|
|
51
|
+
function of barrier. Defaults to the (linear) identity function.
|
|
52
|
+
safe_displacement: Safe backup displacement.
|
|
53
|
+
safe_displacement_gain: positive gain for safe backup displacement.
|
|
54
|
+
"""
|
|
55
|
+
|
|
56
|
+
gain: np.ndarray
|
|
57
|
+
gain_function: Callable[[float], float]
|
|
58
|
+
safe_displacement: np.ndarray
|
|
59
|
+
safe_displacement_gain: float
|
|
60
|
+
|
|
61
|
+
def __init__(
|
|
62
|
+
self,
|
|
63
|
+
dim: int,
|
|
64
|
+
gain: Union[float, np.ndarray] = 1.0,
|
|
65
|
+
gain_function: Optional[Callable[[float], float]] = None,
|
|
66
|
+
safe_displacement_gain: float = 0.0,
|
|
67
|
+
):
|
|
68
|
+
"""Initialize the barrier.
|
|
69
|
+
|
|
70
|
+
Args:
|
|
71
|
+
dim: Dimension of the barrier.
|
|
72
|
+
gain: barrier gain. Defaults to 1.0.
|
|
73
|
+
gain_function: gain function.
|
|
74
|
+
Defaults to the identity function.
|
|
75
|
+
safe_displacement_gain: gain for the safe backup displacement
|
|
76
|
+
cost term. Defaults to 3.0.
|
|
77
|
+
"""
|
|
78
|
+
self.dim = dim
|
|
79
|
+
self.gain = (
|
|
80
|
+
gain if isinstance(gain, np.ndarray) else np.ones(dim) * gain
|
|
81
|
+
)
|
|
82
|
+
|
|
83
|
+
self.gain_function = (
|
|
84
|
+
gain_function if gain_function is not None else lambda x: x
|
|
85
|
+
)
|
|
86
|
+
self.safe_displacement = np.zeros(self.dim)
|
|
87
|
+
self.safe_displacement_gain = safe_displacement_gain
|
|
88
|
+
|
|
89
|
+
# Cached values to avoid recomputation
|
|
90
|
+
self.__q_cache: Optional[np.ndarray] = None
|
|
91
|
+
self.__jac_cache: Optional[np.ndarray] = None
|
|
92
|
+
self.__barrier_cache: Optional[np.ndarray] = None
|
|
93
|
+
|
|
94
|
+
@abc.abstractmethod
|
|
95
|
+
def compute_barrier(self, configuration: Configuration) -> np.ndarray:
|
|
96
|
+
r"""Compute the value of the barrier function.
|
|
97
|
+
|
|
98
|
+
The barrier function :math:`h(q)`
|
|
99
|
+
is a vector-valued function that represents the safety constraints.
|
|
100
|
+
It should be designed such that the set
|
|
101
|
+
:math:`\{q : h(q) \geq 0\}`
|
|
102
|
+
represents the safe region of the configuration space.
|
|
103
|
+
|
|
104
|
+
Args:
|
|
105
|
+
configuration: Robot configuration :math:`q`.
|
|
106
|
+
|
|
107
|
+
Returns:
|
|
108
|
+
Value of the barrier function :math:`h(q)`.
|
|
109
|
+
"""
|
|
110
|
+
|
|
111
|
+
@abc.abstractmethod
|
|
112
|
+
def compute_jacobian(self, configuration: Configuration) -> np.ndarray:
|
|
113
|
+
r"""Compute the Jacobian matrix of the barrier function.
|
|
114
|
+
|
|
115
|
+
The Jacobian matrix
|
|
116
|
+
:math:`\frac{\partial h}{\partial q}(q)`
|
|
117
|
+
of the barrier function with respect to the configuration variables is
|
|
118
|
+
required for the computation of the barrier condition.
|
|
119
|
+
|
|
120
|
+
Args:
|
|
121
|
+
configuration: Robot configuration :math:`q`.
|
|
122
|
+
|
|
123
|
+
Returns:
|
|
124
|
+
Jacobian matrix
|
|
125
|
+
:math:`\frac{\partial h}{\partial q}(q)`.
|
|
126
|
+
"""
|
|
127
|
+
|
|
128
|
+
def compute_safe_displacement(
|
|
129
|
+
self, configuration: Configuration
|
|
130
|
+
) -> np.ndarray:
|
|
131
|
+
r"""Compute the safe backup displacement.
|
|
132
|
+
|
|
133
|
+
The safe backup control displacement :math:`dq_{safe}(q)`
|
|
134
|
+
is a joint displacement vector that can guarantee that system
|
|
135
|
+
would stay in safety set.
|
|
136
|
+
|
|
137
|
+
By default, it is set to zero, since it could not violate safety set.
|
|
138
|
+
|
|
139
|
+
Args:
|
|
140
|
+
configuration: Robot configuration :math:`q`.
|
|
141
|
+
|
|
142
|
+
Returns:
|
|
143
|
+
Safe backup joint velocities
|
|
144
|
+
:math:`\dot{q}_{safe}(q)`.
|
|
145
|
+
"""
|
|
146
|
+
return np.zeros(configuration.model.nv)
|
|
147
|
+
|
|
148
|
+
def compute_qp_objective(
|
|
149
|
+
self,
|
|
150
|
+
configuration: Configuration,
|
|
151
|
+
) -> Tuple[np.ndarray, np.ndarray]:
|
|
152
|
+
r"""Compute the quadratic objective function for the barrier-based QP.
|
|
153
|
+
|
|
154
|
+
The quadratic objective function includes a regularization term
|
|
155
|
+
based on the safe backup policy:
|
|
156
|
+
|
|
157
|
+
.. math::
|
|
158
|
+
|
|
159
|
+
\gamma(q)\left\| \dot{q}-
|
|
160
|
+
\dot{q}_{safe}(q)\right\|^{2}
|
|
161
|
+
|
|
162
|
+
where :math:`\gamma(q)` is a configuration-dependent weight and
|
|
163
|
+
:math:`\dot{q}_{safe}(q)` is the safe backup policy.
|
|
164
|
+
|
|
165
|
+
Note:
|
|
166
|
+
If `safe_displacement_gain` is set to zero, the regularization
|
|
167
|
+
term is not included. Jacobian and barrier values are cached
|
|
168
|
+
to avoid recomputation.
|
|
169
|
+
|
|
170
|
+
Args:
|
|
171
|
+
configuration: Robot configuration :math:`q`.
|
|
172
|
+
dt: Time step for discrete-time implementation. Defaults to 1e-3.
|
|
173
|
+
|
|
174
|
+
Returns:
|
|
175
|
+
Tuple containing the quadratic objective matrix (H) and linear
|
|
176
|
+
objective vector (c).
|
|
177
|
+
"""
|
|
178
|
+
if (
|
|
179
|
+
self.__q_cache is None
|
|
180
|
+
or self.__jac_cache is None
|
|
181
|
+
or self.__barrier_cache is None
|
|
182
|
+
) or not np.allclose(self.__q_cache, configuration.q):
|
|
183
|
+
self.__q_cache = configuration.q
|
|
184
|
+
self.__jac_cache = self.compute_jacobian(configuration)
|
|
185
|
+
self.__barrier_cache = self.compute_barrier(configuration)
|
|
186
|
+
|
|
187
|
+
H = np.zeros((configuration.model.nv, configuration.model.nv))
|
|
188
|
+
c = np.zeros(configuration.model.nv)
|
|
189
|
+
|
|
190
|
+
if self.safe_displacement_gain > 1e-6:
|
|
191
|
+
self.safe_displacement = self.compute_safe_displacement(
|
|
192
|
+
configuration
|
|
193
|
+
)
|
|
194
|
+
jac_squared_norm = np.linalg.norm(self.__jac_cache) ** 2
|
|
195
|
+
gain_over_jacobian = self.safe_displacement_gain / jac_squared_norm
|
|
196
|
+
|
|
197
|
+
H += gain_over_jacobian * np.eye(configuration.model.nv)
|
|
198
|
+
c += -gain_over_jacobian * self.safe_displacement
|
|
199
|
+
|
|
200
|
+
return (H, c)
|
|
201
|
+
|
|
202
|
+
def compute_qp_inequalities(
|
|
203
|
+
self,
|
|
204
|
+
configuration: Configuration,
|
|
205
|
+
dt: float = 1e-3,
|
|
206
|
+
) -> Tuple[np.ndarray, np.ndarray]:
|
|
207
|
+
r"""Compute the linear inequality constraints for the barrier-based QP.
|
|
208
|
+
|
|
209
|
+
The linear inequality constraints enforce the barrier conditions:
|
|
210
|
+
|
|
211
|
+
.. math::
|
|
212
|
+
|
|
213
|
+
\frac{\partial h_j}
|
|
214
|
+
{\partial q} \dot{q} +
|
|
215
|
+
\alpha_j(h_j(q)) \geq 0, \quad \forall j
|
|
216
|
+
|
|
217
|
+
where :math:`\frac{\partial h_j}{\partial q}`
|
|
218
|
+
are the Jacobians of the constraint functions,
|
|
219
|
+
:math:`\dot{q}` is the joint velocity vector,
|
|
220
|
+
and :math:`\alpha_j` are extended class K functions.
|
|
221
|
+
|
|
222
|
+
Note:
|
|
223
|
+
Jacobian and barrier values are cached to avoid recomputation.
|
|
224
|
+
|
|
225
|
+
Args:
|
|
226
|
+
configuration: Robot configuration :math:`q`.
|
|
227
|
+
dt: Time step for discrete-time implementation. Defaults to 1e-3.
|
|
228
|
+
|
|
229
|
+
Returns:
|
|
230
|
+
Tuple containing the inequality constraint matrix (G)
|
|
231
|
+
and vector (h).
|
|
232
|
+
"""
|
|
233
|
+
if (
|
|
234
|
+
self.__q_cache is None
|
|
235
|
+
or self.__jac_cache is None
|
|
236
|
+
or self.__barrier_cache is None
|
|
237
|
+
or not np.allclose(self.__q_cache, configuration.q)
|
|
238
|
+
):
|
|
239
|
+
self.__q_cache = configuration.q
|
|
240
|
+
self.__jac_cache = self.compute_jacobian(configuration)
|
|
241
|
+
self.__barrier_cache = self.compute_barrier(configuration)
|
|
242
|
+
|
|
243
|
+
G = -self.__jac_cache / dt
|
|
244
|
+
h = np.array(
|
|
245
|
+
[
|
|
246
|
+
self.gain[i] * self.gain_function(self.__barrier_cache[i])
|
|
247
|
+
for i in range(self.dim)
|
|
248
|
+
]
|
|
249
|
+
)
|
|
250
|
+
|
|
251
|
+
return (G, h)
|
|
252
|
+
|
|
253
|
+
def __repr__(self) -> str:
|
|
254
|
+
"""Return a string representation of the barrier.
|
|
255
|
+
|
|
256
|
+
Returns:
|
|
257
|
+
str: String representation.
|
|
258
|
+
"""
|
|
259
|
+
safe_displacement = (
|
|
260
|
+
"no"
|
|
261
|
+
if np.allclose(self.safe_displacement_gain, 0)
|
|
262
|
+
else self.safe_displacement
|
|
263
|
+
)
|
|
264
|
+
safe_displacement_gain = (
|
|
265
|
+
"no"
|
|
266
|
+
if np.allclose(self.safe_displacement_gain, 0)
|
|
267
|
+
else self.safe_displacement_gain
|
|
268
|
+
)
|
|
269
|
+
return (
|
|
270
|
+
f"Barrier("
|
|
271
|
+
f"gain={self.gain}, "
|
|
272
|
+
f"{safe_displacement=}, "
|
|
273
|
+
f"{safe_displacement_gain=}, "
|
|
274
|
+
f"dim={self.dim})"
|
|
275
|
+
)
|
|
@@ -0,0 +1,186 @@
|
|
|
1
|
+
# SPDX-License-Identifier: Apache-2.0
|
|
2
|
+
|
|
3
|
+
"""Barrier based on the distance between two frames."""
|
|
4
|
+
|
|
5
|
+
from typing import Tuple, Union
|
|
6
|
+
|
|
7
|
+
import numpy as np
|
|
8
|
+
|
|
9
|
+
from ..configuration import Configuration
|
|
10
|
+
from ..exceptions import NegativeMinimumDistance
|
|
11
|
+
from .barrier import Barrier
|
|
12
|
+
|
|
13
|
+
|
|
14
|
+
class BodySphericalBarrier(Barrier):
|
|
15
|
+
r"""A barrier based on the distance between two frames.
|
|
16
|
+
|
|
17
|
+
Defines a barrier function based on the Euclidean distance between two
|
|
18
|
+
specified frames. It allows for the specification of a minimum distance
|
|
19
|
+
threshold.
|
|
20
|
+
|
|
21
|
+
The barrier function is defined as:
|
|
22
|
+
|
|
23
|
+
.. math::
|
|
24
|
+
|
|
25
|
+
h(q) = \|p_1(q) -
|
|
26
|
+
p_2(q)\|^2 - d_{min}^2
|
|
27
|
+
|
|
28
|
+
where :math:`p_1(q)` and
|
|
29
|
+
:math:`p_2(q)` are the positions of the two
|
|
30
|
+
frames in the world coordinate system, and :math:`d_{min}`
|
|
31
|
+
is the minimum distance threshold.
|
|
32
|
+
|
|
33
|
+
Attributes:
|
|
34
|
+
frames: Tuple of two frame names.
|
|
35
|
+
d_min: Minimum distance threshold.
|
|
36
|
+
"""
|
|
37
|
+
|
|
38
|
+
frames: Tuple[str, str]
|
|
39
|
+
d_min: float
|
|
40
|
+
|
|
41
|
+
def __init__(
|
|
42
|
+
self,
|
|
43
|
+
frames: Tuple[str, str],
|
|
44
|
+
d_min: float,
|
|
45
|
+
gain: Union[float, np.ndarray] = 1.0,
|
|
46
|
+
safe_displacement_gain: float = 3.0,
|
|
47
|
+
):
|
|
48
|
+
"""Initialize the BodySphericalBarrier.
|
|
49
|
+
|
|
50
|
+
Args:
|
|
51
|
+
frames: Tuple of two frame names.
|
|
52
|
+
d_min: Minimum distance threshold.
|
|
53
|
+
gain: Barrier gain. Defaults to 1.0.
|
|
54
|
+
safe_displacement_gain: gain for the safe backup displacement
|
|
55
|
+
cost term. Defaults to 3.0.
|
|
56
|
+
"""
|
|
57
|
+
if d_min < 0.0:
|
|
58
|
+
raise NegativeMinimumDistance(
|
|
59
|
+
"The minimum distance threshold must be non-negative."
|
|
60
|
+
)
|
|
61
|
+
super().__init__(
|
|
62
|
+
dim=1,
|
|
63
|
+
gain=gain,
|
|
64
|
+
gain_function=lambda h: h / (1 + np.abs(h)),
|
|
65
|
+
safe_displacement_gain=safe_displacement_gain,
|
|
66
|
+
)
|
|
67
|
+
self.frames = frames
|
|
68
|
+
self.d_min = d_min
|
|
69
|
+
|
|
70
|
+
def compute_barrier(self, configuration: Configuration) -> np.ndarray:
|
|
71
|
+
r"""Compute the value of the barrier function.
|
|
72
|
+
|
|
73
|
+
The barrier function is computed based on the Euclidean distance
|
|
74
|
+
between the two specified frames. It considers the minimum distance
|
|
75
|
+
threshold.
|
|
76
|
+
|
|
77
|
+
The barrier function is given by:
|
|
78
|
+
|
|
79
|
+
.. math::
|
|
80
|
+
|
|
81
|
+
h(q) = \|p_1(q) -
|
|
82
|
+
p_2(q)\|^2 - d_{min}^2
|
|
83
|
+
|
|
84
|
+
where :math:`p_1(q)` and
|
|
85
|
+
:math:`p_2(q)` are the positions of the two
|
|
86
|
+
frames in the world coordinate system, and :math:`d_{min}`
|
|
87
|
+
is the minimum distance threshold.
|
|
88
|
+
|
|
89
|
+
Args:
|
|
90
|
+
configuration: Robot configuration :math:`q`.
|
|
91
|
+
|
|
92
|
+
Returns:
|
|
93
|
+
Value of the barrier function
|
|
94
|
+
:math:`h(q)`.
|
|
95
|
+
"""
|
|
96
|
+
pos1_world, pos2_world = self._get_frame_positions(configuration)
|
|
97
|
+
return np.array(
|
|
98
|
+
[
|
|
99
|
+
(pos1_world - pos2_world).dot(pos1_world - pos2_world)
|
|
100
|
+
- self.d_min**2
|
|
101
|
+
]
|
|
102
|
+
)
|
|
103
|
+
|
|
104
|
+
def compute_jacobian(self, configuration: Configuration) -> np.ndarray:
|
|
105
|
+
r"""Compute the Jacobian matrix of the barrier function.
|
|
106
|
+
|
|
107
|
+
The Jacobian matrix is computed based on the position Jacobians of the
|
|
108
|
+
two specified frames. The Jacobians are transformed to align with the
|
|
109
|
+
world coordinate system.
|
|
110
|
+
|
|
111
|
+
The Jacobian matrix is given by:
|
|
112
|
+
|
|
113
|
+
.. math::
|
|
114
|
+
|
|
115
|
+
\frac{\partial h}{\partial q}(q) =
|
|
116
|
+
2(p_1(q) -
|
|
117
|
+
p_2(q))^T \begin{bmatrix}
|
|
118
|
+
\frac{\partial p_1}{\partial q}
|
|
119
|
+
(q) \\
|
|
120
|
+
-\frac{\partial p_2}{\partial q}
|
|
121
|
+
(q)
|
|
122
|
+
\end{bmatrix}
|
|
123
|
+
|
|
124
|
+
where :math:`\frac{\partial p_1}{\partial q}(q)`
|
|
125
|
+
and :math:`\frac{\partial p_2}{\partial q}(q)`
|
|
126
|
+
are the position Jacobians of the two frames.
|
|
127
|
+
|
|
128
|
+
Args:
|
|
129
|
+
configuration: Robot configuration :math:`q`.
|
|
130
|
+
|
|
131
|
+
Returns:
|
|
132
|
+
Jacobian matrix
|
|
133
|
+
:math:`\frac{\partial h}{\partial q}(q)`.
|
|
134
|
+
"""
|
|
135
|
+
pos1_world, pos2_world = self._get_frame_positions(configuration)
|
|
136
|
+
pos1_jac, pos2_jac = self._get_frame_jacobians(configuration)
|
|
137
|
+
|
|
138
|
+
dh_dx = 2 * (pos1_world - pos2_world)
|
|
139
|
+
dx_dq = pos1_jac - pos2_jac
|
|
140
|
+
return dh_dx.T @ dx_dq
|
|
141
|
+
|
|
142
|
+
def _get_frame_positions(
|
|
143
|
+
self, configuration: Configuration
|
|
144
|
+
) -> Tuple[np.ndarray, np.ndarray]:
|
|
145
|
+
"""Get the positions of the two frames in the world coordinate system.
|
|
146
|
+
|
|
147
|
+
Args:
|
|
148
|
+
configuration: Robot configuration.
|
|
149
|
+
|
|
150
|
+
Returns:
|
|
151
|
+
Tuple of position vectors of the two frames in the world
|
|
152
|
+
coordinate system.
|
|
153
|
+
"""
|
|
154
|
+
pos1_world = configuration.get_transform_frame_to_world(
|
|
155
|
+
self.frames[0]
|
|
156
|
+
).translation
|
|
157
|
+
pos2_world = configuration.get_transform_frame_to_world(
|
|
158
|
+
self.frames[1]
|
|
159
|
+
).translation
|
|
160
|
+
return pos1_world, pos2_world
|
|
161
|
+
|
|
162
|
+
def _get_frame_jacobians(
|
|
163
|
+
self, configuration: Configuration
|
|
164
|
+
) -> Tuple[np.ndarray, np.ndarray]:
|
|
165
|
+
"""Get the position Jacobians of the two frames in the world frame.
|
|
166
|
+
|
|
167
|
+
Args:
|
|
168
|
+
configuration: Robot configuration.
|
|
169
|
+
|
|
170
|
+
Returns:
|
|
171
|
+
Tuple of position Jacobian matrices of the two frames in the world
|
|
172
|
+
coordinate system.
|
|
173
|
+
"""
|
|
174
|
+
pos1_jac = configuration.get_frame_jacobian(self.frames[0])[:3]
|
|
175
|
+
rotation1 = configuration.get_transform_frame_to_world(
|
|
176
|
+
self.frames[0]
|
|
177
|
+
).rotation
|
|
178
|
+
pos1_jac = rotation1 @ pos1_jac
|
|
179
|
+
|
|
180
|
+
pos2_jac = configuration.get_frame_jacobian(self.frames[1])[:3]
|
|
181
|
+
rotation2 = configuration.get_transform_frame_to_world(
|
|
182
|
+
self.frames[1]
|
|
183
|
+
).rotation
|
|
184
|
+
pos2_jac = rotation2 @ pos2_jac
|
|
185
|
+
|
|
186
|
+
return pos1_jac, pos2_jac
|
|
@@ -0,0 +1,150 @@
|
|
|
1
|
+
# SPDX-License-Identifier: Apache-2.0
|
|
2
|
+
|
|
3
|
+
"""Frame position barrier."""
|
|
4
|
+
|
|
5
|
+
from typing import List, Optional, Union
|
|
6
|
+
|
|
7
|
+
import numpy as np
|
|
8
|
+
|
|
9
|
+
from ..configuration import Configuration
|
|
10
|
+
from ..exceptions import NoPositionLimitProvided
|
|
11
|
+
from .barrier import Barrier
|
|
12
|
+
|
|
13
|
+
|
|
14
|
+
class PositionBarrier(Barrier):
|
|
15
|
+
r"""A position-based barrier.
|
|
16
|
+
|
|
17
|
+
Defines a barrier function based on the position of a
|
|
18
|
+
specified frame in the world coordinate system. It allows for the
|
|
19
|
+
specification of minimum and maximum position bounds along selected axes.
|
|
20
|
+
|
|
21
|
+
Attributes:
|
|
22
|
+
frame: Name of the frame to monitor.
|
|
23
|
+
indices: Indices of the position components to consider.
|
|
24
|
+
p_min: Minimum position bounds.
|
|
25
|
+
p_max: Maximum position bounds.
|
|
26
|
+
"""
|
|
27
|
+
|
|
28
|
+
frame: str
|
|
29
|
+
indices: List[int]
|
|
30
|
+
p_min: Optional[np.ndarray]
|
|
31
|
+
p_max: Optional[np.ndarray]
|
|
32
|
+
|
|
33
|
+
def __init__(
|
|
34
|
+
self,
|
|
35
|
+
frame: str,
|
|
36
|
+
indices: Optional[List[int]] = None,
|
|
37
|
+
p_min: Optional[np.ndarray] = None,
|
|
38
|
+
p_max: Optional[np.ndarray] = None,
|
|
39
|
+
gain: Union[float, np.ndarray] = 1.0,
|
|
40
|
+
safe_displacement_gain: float = 0.0,
|
|
41
|
+
):
|
|
42
|
+
"""Initialize a position barrier.
|
|
43
|
+
|
|
44
|
+
Args:
|
|
45
|
+
frame: name of the frame to monitor.
|
|
46
|
+
indices: indices with limits.
|
|
47
|
+
Indices from 0 to 2 stand for coordinates x to z respectively.
|
|
48
|
+
Defaults to all dimensions.
|
|
49
|
+
p_min: minimum position limit.
|
|
50
|
+
Defaults to None, meaning no minimum limit is applied.
|
|
51
|
+
p_max: maximum position limit.
|
|
52
|
+
Defaults to None, meaning no maximum limit is applied.
|
|
53
|
+
gain: regularization gain.
|
|
54
|
+
Defaults to 1.0.
|
|
55
|
+
safe_displacement_gain: gain for the safe backup displacement
|
|
56
|
+
cost term. Defaults to 0.0.
|
|
57
|
+
|
|
58
|
+
Raises:
|
|
59
|
+
NoPositionLimitProvided: neither minimum nor maximum
|
|
60
|
+
position limit is provided.
|
|
61
|
+
"""
|
|
62
|
+
indices = [0, 1, 2] if indices is None else indices
|
|
63
|
+
|
|
64
|
+
if p_min is None and p_max is None:
|
|
65
|
+
raise NoPositionLimitProvided(
|
|
66
|
+
f"Position barrier for frame {frame} requires either \
|
|
67
|
+
p_min or p_max"
|
|
68
|
+
)
|
|
69
|
+
|
|
70
|
+
dim = 0
|
|
71
|
+
if p_min is not None:
|
|
72
|
+
dim += len(indices)
|
|
73
|
+
if p_max is not None:
|
|
74
|
+
dim += len(indices)
|
|
75
|
+
|
|
76
|
+
# If both p_min and p_max are specified, they must have
|
|
77
|
+
# the same gains repeated along the dimension
|
|
78
|
+
if isinstance(gain, np.ndarray) and len(gain) != dim:
|
|
79
|
+
gain = np.tile(gain, 2)
|
|
80
|
+
|
|
81
|
+
super().__init__(
|
|
82
|
+
dim,
|
|
83
|
+
gain=gain,
|
|
84
|
+
safe_displacement_gain=safe_displacement_gain,
|
|
85
|
+
)
|
|
86
|
+
|
|
87
|
+
self.indices = indices
|
|
88
|
+
self.frame = frame
|
|
89
|
+
self.p_min = p_min
|
|
90
|
+
self.p_max = p_max
|
|
91
|
+
|
|
92
|
+
def compute_barrier(self, configuration: Configuration) -> np.ndarray:
|
|
93
|
+
r"""Compute the value of the barrier function.
|
|
94
|
+
|
|
95
|
+
The barrier function is computed based on the position of the specified
|
|
96
|
+
frame in the world coordinate system. It considers the minimum and
|
|
97
|
+
maximum position bounds along the selected axes.
|
|
98
|
+
|
|
99
|
+
Args:
|
|
100
|
+
configuration: Robot configuration :math:`q`.
|
|
101
|
+
|
|
102
|
+
Returns:
|
|
103
|
+
Value of the barrier function
|
|
104
|
+
:math:`h(q)`.
|
|
105
|
+
"""
|
|
106
|
+
pos_world = configuration.get_transform_frame_to_world(
|
|
107
|
+
self.frame
|
|
108
|
+
).translation
|
|
109
|
+
barriers = []
|
|
110
|
+
if self.p_min is not None:
|
|
111
|
+
barriers.append(pos_world[self.indices] - self.p_min)
|
|
112
|
+
if self.p_max is not None:
|
|
113
|
+
barriers.append(self.p_max - pos_world[self.indices])
|
|
114
|
+
|
|
115
|
+
return np.concatenate(barriers)
|
|
116
|
+
|
|
117
|
+
def compute_jacobian(self, configuration: Configuration) -> np.ndarray:
|
|
118
|
+
r"""Compute the Jacobian matrix of the barrier function.
|
|
119
|
+
|
|
120
|
+
The Jacobian matrix is computed based on the position Jacobian of the
|
|
121
|
+
specified frame. The Jacobian is transformed to align with the world
|
|
122
|
+
coordinate system and only the selected indices are considered.
|
|
123
|
+
|
|
124
|
+
Args:
|
|
125
|
+
configuration: Robot configuration :math:`q`.
|
|
126
|
+
|
|
127
|
+
Returns:
|
|
128
|
+
Jacobian matrix
|
|
129
|
+
:math:`\frac{\partial h}{\partial q}(q)`.
|
|
130
|
+
"""
|
|
131
|
+
pos_jacobian_in_frame = configuration.get_frame_jacobian(self.frame)[
|
|
132
|
+
:3
|
|
133
|
+
]
|
|
134
|
+
# Transform jacobian to world aligned frame
|
|
135
|
+
rotation = configuration.get_transform_frame_to_world(
|
|
136
|
+
self.frame
|
|
137
|
+
).rotation
|
|
138
|
+
# Apply rotation to trasfrom from world aligned to local frame
|
|
139
|
+
# and select only relevant indices
|
|
140
|
+
pos_jacobian_in_world = (rotation @ pos_jacobian_in_frame)[
|
|
141
|
+
self.indices
|
|
142
|
+
]
|
|
143
|
+
|
|
144
|
+
jacobians = []
|
|
145
|
+
if self.p_min is not None:
|
|
146
|
+
jacobians.append(pos_jacobian_in_world.copy())
|
|
147
|
+
if self.p_max is not None:
|
|
148
|
+
jacobians.append(-pos_jacobian_in_world.copy())
|
|
149
|
+
|
|
150
|
+
return np.vstack(jacobians)
|