是否有与 PostureChangeConstraint 等效的 MultiBody？

Question

我试图将解决方案限制在每个关节的最大 angular 距离内，我认为使用 RigidBodyTree，我可以只添加一个 PostureChangeConstraint，但是是否有等效项多体树？除了可能使用向量之间的角度之外？

Answer 1

您可以通过 MathematicalProgram::AddLinearConstraint 添加该约束。例如，假设您有一个轨迹优化问题 prog，并且您想要约束 q[n+1] 和 q[n] 之间的差异小于界限 max_delta_q。你可以这样做

nq = plant.num_positions();
A = Eigen::MatrixXd(nq, 2*nq);
A << Eigen::MatrixXd::Identity(nq), -Eigen::MatrixXd::Identity(nq)
// Add the constraint -max_delta_q <= q[n+1] - q[n] <= max_delta_q
prog.AddLinearConstraint(A, -max_delta_q, max_delta_q, {q[n], q[n+1]});

是否有与 PostureChangeConstraint 等效的 MultiBody？

Is there a MultiBody equivalent of PostureChangeConstraint?

drake