Inverse kinematics
The main function solve inverse kinematics is solve_ik()
. Here is for
instance how it appears in a closed-loop inverse kinematics:
rate = RateLimiter(frequency=100.0)
while True:
# [...] <- update task targets here
velocity = solve_ik(configuration, tasks, rate.dt, solver=solver)
configuration.integrate_inplace(velocity, rate.dt)
rate.sleep()
See the examples/
folder in the repository for complete use cases.
- pink.solve_ik.solve_ik(configuration, tasks, dt, solver, barriers=None, damping=1e-12, safety_break=True, **kwargs)
Compute a velocity tangent to the current robot configuration.
The computed velocity satisfies at (weighted) best the set of kinematic tasks given in argument.
- Parameters:
configuration (
Configuration
) – Robot configuration to read kinematics from.tasks (
Iterable
[Task
]) – List of kinematic tasks.dt (
float
) – Integration timestep in [s].solver (
str
) – Backend quadratic programming (QP) solver.damping (
float
) – weight of Tikhonov (everywhere) regularization. Its unit is \([\mathrm{cost}]^2 / [\mathrm{tangent}]\) where \([\mathrm{tangent}]\) is “the” unit of robot velocities. Improves numerical stability, but larger values slow down all tasks.barriers (
Optional
[Iterable
[Barrier
]]) – Collection of barriers functions.safety_break (
bool
) – If True, stop execution and raise an exception if the current configuration is outside limits. If False, print a warning and continue execution.kwargs – Keyword arguments to forward to the backend QP solver.
- Return type:
- Returns:
Velocity \(v\) in tangent space.
- Raises:
NotWithinConfigurationLimits – if the current configuration is not within limits.
Note
Our Tikhonov damping is isotropic despite tangent velocities not being homogeneous. If it helps we can add a tangent-space scaling to damp the floating base differently from joint angular velocities.
It is also possible to ask Pink to only build the underlying inverse kinematics
problem via the build_ik()
function:
- pink.solve_ik.build_ik(configuration, tasks, dt, damping=1e-12, barriers=None)
Build quadratic program from current configuration and tasks.
This quadratic program is, in standard form:
\[\begin{split}\begin{split}\begin{array}{ll} \underset{\Delta q}{\mbox{minimize}} & \frac{1}{2} {\Delta q}^T H {\Delta q} + c^T {\Delta q} \\ \mbox{subject to} & G {\Delta q} \leq h \end{array}\end{split}\end{split}\]where \(\Delta q\) is a vector of joint displacements corresponding to the joint velocities \(v = {\Delta q} / {\mathrm{d}t}\).
- Parameters:
configuration (
Configuration
) – Robot configuration to read kinematics from.tasks (
Iterable
[Task
]) – List of kinematic tasks.dt (
float
) – Integration timestep in [s].damping (
float
) – weight of Tikhonov (everywhere) regularization. Its unit is \([\mathrm{cost}]^2 / [\mathrm{tangent}]\) where \([\mathrm{tangent}]\) is “the” unit of robot velocities. Improves numerical stability, but larger values slow down all tasks.barriers (
Optional
[Iterable
[Barrier
]]) – Collection of barriers.
- Return type:
- Returns:
Quadratic program of the inverse kinematics problem.