粉色的

基于匹诺曹的关节机器人模型的运动学中的Python。

安装

pip install pin-pink

在 Raspberry Pi 上，您需要从 source安装。

用法

Pink 中的逆运动学由加权任务定义。任务通过要最小化的目标函数（例如 $\Vert {}^{world}p_{foot}^{target}）来表征要实现的目标，例如“将脚框架放在这个位置” - {}^{world}p_{foot} \Vert^2$。机器人需要完成多项任务，其中一些任务可能会发生冲突。冲突是通过将所有目标投射到同一个单元来解决的，比如[cost]，并权衡所有这些标准化的目标。

任务成本

这是一个双足机器人示例，它控制其基础、左接触和右接触框架的位置和方向。为正则化添加了第四个“姿势”任务，为每个关节提供所需的角度：

from pink.tasks import BodyTask, PostureTask

tasks = {
    "base": BodyTask(
        "base",
        position_cost=1.0,              # [cost] / [m]
        orientation_cost=1.0,           # [cost] / [rad]
    ),
    "left_contact": BodyTask(
        "left_contact",
        position_cost=[0.1, 0.0, 0.1],  # [cost] / [m]
        orientation_cost=0.0,           # [cost] / [rad]
    ),
    "right_contact": BodyTask(
        "right_contact",
        position_cost=[0.1, 0.0, 0.1],  # [cost] / [m]
        orientation_cost=0.0,           # [cost] / [rad]
    ),
    "posture": PostureTask(
        cost=1e-3,                      # [cost] / [rad]
    ),
}

方向（类似位置）成本，可以是标量或 3D 向量，指定在整体归一化目标中每个角度误差弧度的“成本”。使用 3D 矢量时，组件沿身体框架的每个轴进行各向异性加权。

任务目标

除了成本之外，大多数任务还采用称为目标的第二组参数，例如身体任务的目标变换或姿势任务的目标配置向量。目标由set_target函数设置：

    tasks["posture"].set_target(
        [1.0, 0.0, 0.0, 0.0] +           # floating base quaternion
        [0.0, 0.0, 0.0] +                # floating base position
        [0.0, 0.2, 0.0, 0.0, -0.2, 0.0]  # joint angles
    )

例如，可以从机器人的中性配置初始化身体任务：

import pink
import upkie_description

robot = pink.models.build_from_urdf(upkie_description.urdf_path)
configuration = pink.apply_configuration(robot, robot.q0)
for body, task in tasks.items():
    if type(task) is BodyTask:
        task.set_target(configuration.get_transform_body_to_world(body))

一旦定义了任务的成本和（如果适用）目标，它就可以用于求解逆运动学。

微分逆运动学

Pink 求解微分逆运动学，这意味着它会输出一个速度，引导机器人模型朝向解决方案配置（而不是求解该配置本身）。如果我们继续整合该速度，并且同时任务目标不改变，我们将收敛到该配置：

dt = 6e-3  # [s]
for t in np.arange(0.0, 42.0, dt):
    velocity = solve_ik(configuration, tasks.values(), dt)  # includes joint limits
    q = configuration.integrate(velocity, dt)
    configuration = pink.apply_configuration(robot, q)
    time.sleep(dt)

如果任务目标不断更新，则不会有稳定的解决方案可以收敛，但模型最多会继续跟踪每个目标。请注意，这solve_ik考虑了从 URDF 读取的关节位置和速度限制。

例子

上述所有步骤在以下示例中进行了说明：

• 人形站立挥手：一种人形模型，在放弃右手腕的同时定位其骨盆和脚部。
• Upkie 蹲伏：一种双足轮式模型，可以上下抬脚。