如何使用C++实现自定义约束？

Question

我正在尝试使用 C++ 重新实现 littledog.ipynb。我发现很难翻译函数 velocity_dynamics_constraint 并且有 3 个问题

ad_velocity_dynamics_context的作用是什么？我们可以忽略它吗？
如何使用 C++ 重新实现 velocity_dynamics_constraint？我是否必须像 class VelocityDynamicsConstraint : public drake::solvers::Constraint 这样创建一个新的 class？三个更简单的实现方式吗？
为什么要考虑isinstance(vars[0], AutoDiffXd)条件？

# Some code from https://github.com/RussTedrake/underactuated/blob/master/examples/littledog.ipynb
ad_velocity_dynamics_context = [
    ad_plant.CreateDefaultContext() for i in range(N)
]

def velocity_dynamics_constraint(vars, context_index):
    h, q, v, qn = np.split(vars, [1, 1+nq, 1+nq+nv])
    if isinstance(vars[0], AutoDiffXd):
        if not autoDiffArrayEqual(
                q,
                ad_plant.GetPositions(
                    ad_velocity_dynamics_context[context_index])):
            ad_plant.SetPositions(
                ad_velocity_dynamics_context[context_index], q)
        v_from_qdot = ad_plant.MapQDotToVelocity(
            ad_velocity_dynamics_context[context_index], (qn - q) / h)
    else:
        if not np.array_equal(q, plant.GetPositions(
                context[context_index])):
            plant.SetPositions(context[context_index], q)
        v_from_qdot = plant.MapQDotToVelocity(context[context_index],
                                              (qn - q) / h)
    return v - v_from_qdot
for n in range(N-1):
    prog.AddConstraint(partial(velocity_dynamics_constraint,
                                context_index=n),
                        lb=[0] * nv,
                        ub=[0] * nv,
                        vars=np.concatenate(
                            ([h[n]], q[:, n], v[:, n], q[:, n + 1]))

Answer 1

What is the function of ad_velocity_dynamics_context? Can we ignore it?

上下文缓存给定 q, v, u 的中间计算结果。对同一组 q, v, u 施加多个约束是很常见的（例如，考虑在最后时刻，我们通常对最终状态有运动学约束，比如机器人脚必须落在地面上并且它的质心在某个位置。同时我们对最终状态有速度动力学约束）。因此，这些不同的约束可以共享一些中间计算结果，例如每个相邻链接之间的刚性变换。因此我们将结果缓存在 ad_velocity_dynamics_context 中，这个 ad_velocity_dynamics_context 可以在我们施加其他约束时使用。

How to reimplement velocity_dynamics_constraint using C++? Do I have to create a new class like class VelocityDynamicsConstraint : public drake::solvers::Constraint? Is there any easier way to implement it?

没错，您需要创建一个新的 class VelocityDynamicsConstraint。实现此 class 的主要挑战是为三种标量类型（double、AutoDiffXd、symbolic::Expression）编写三个重载 DoEval 函数。大家可以参考PositionConstraint作为参考。目前你可以忽略用 MultibodyPlant<double> 调用 DoEval(const Eigen::Ref<const AutoDiffXd>&, AutoDiffXd*) 的情况，只用 MultibodyPlant<AutoDiffXd>.

实现这个 DoEval 函数

Why we need to consider isinstance(vars[0], AutoDiffXd) condition?

因为当标量类型为 AutoDiffXd 时，我们不仅要将 q 的值与上下文中存储的值进行比较，还要比较其梯度。如果它们不同，那么我们需要调用 SetPositions 来重新计算缓存。当标量类型为double时，我们只需要比较值即可。

如何使用C++实现自定义约束？

How to implement custom constraint using C++?

drake