c++ - Drake:在优化问题中集成质量矩阵和偏差项

Question

我正在尝试在 drake 中为 7-DOF 操纵器实现非线性 MPC。为此，在我的约束条件下，我需要具有动态参数，例如质量矩阵 M(q) 和偏差项 C(q,q_dot)*q_dot，但这些参数取决于决策变量 q、q_dot。

我尝试了以下方法

    // finalize plant
    // create builder, diagram, context, plant context
    ...

    // formulate optimazation problem
    drake::solvers::MathematicalProgram prog;

    // create decision variables
    ...
    std::vector<drake::solvers::VectorXDecisionVariable> q_v;
    std::vector<drake::solvers::VectorXDecisionVariable> q_ddot;

    for (int i = 0; i < H; i++) {
        q_v.push_back(prog.NewContinuousVariables<14>(state_var_name));
        q_ddot.push_back(prog.NewContinuousVariables<7>(input_var_name));
    }

    // add cost
    ...

    // add constraints
    ...
    for (int i = 0; i < H; i++) {
        plant.SetPositionsAndVelocities(*plant_context, q_v[i]);
        plant.CalcMassMatrix(*plant_context, M);
        plant.CalcBiasTerm(*plant_context, C_q_dot);
    }

    ...
    
    for (int i = 0; i < H; i++) {
        prog.AddConstraint( M * q_ddot[i] + C_q_dot + G >= lb );
        prog.AddConstraint( M * q_ddot[i] + C_q_dot + G <= ub );
    }

    // solve prog
    ...
    

上面的代码将不起作用，因为plant.SetPositionsAndVelocity(.)不接受符号变量。

有什么方法可以将 M、C 集成到我的 ocp 约束中吗？

Answer 1

我认为你想施加以下非线性非凸约束

lb <= M * qddot + C(q, v) + g(q) <= ub

该约束是非凸的。我们需要通过非线性优化来解决它，并在非线性优化的每次迭代中评估约束。我们无法使用符号计算进行此评估(使用符号计算会非常慢)。

所以你需要一个约束评估器，像这样

// This constraint takes [q;v;vdot] and evaluate
// M * vdot + C(q, v) + g(q)
class MyConstraint : public solvers::Constraint {
 public:
  MyConstraint(const MultibodyPlant<AutoDiffXd>& plant, systems::Context<AutoDiffXd>* context, const Eigen::Ref<const Eigen::VectorXd>& lb, const Eigen::Ref<const Eigen::VectorXd>& ub) : solvers::Constraint(plant.num_velocitiex(), plant.num_positions() + 2 * plant.num_velocities(), lb, ub), plant_{plant}, context_{context} {
  ...
  }

 private:
  void DoEval(const Eigen::Ref<const AutoDiffVecXd>& x, AutoDiffVecXd* y) const {
    ...
  }
  
  MultibodyPlant<AutoDiffXd> plant_;
  systems::Context<AutoDiffXd>* context_;
};

int main() {
...
// Construct the constraint and add it to every time instances
std::vector<std::unique_ptr<systems::Context<AutoDiffXd>>> plant_contexts;
for (int i = 0; i < H; ++i) {
 
 plant_contexts.push_back(plant.CreateDefaultContext());
  prog.AddConstraint(std::make_shared<MyConstraint>(plant, plant_context[i], lb, ub), {q_v[i], qddot[i]});
}
}

可以引用类(class)CentroidalMomentumConstraint了解如何构建您自己的 MyConstraint 类。