Drake: Integrate Mass Matrix and Bias Term in Optimization Problem

I am trying to implement Non Linear MPC for a 7-DOF manipulator in drake. To do this, in my constraints, I need to have dynamic parameters like the Mass matrix M(q) and the bias term C(q,q_dot)*q_dot, but those depend on the decision variables q, q_dot.

I tried the following

    // 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
    ...
    

The above code will not work, because plant.SetPositionsAndVelocities(.) doesn't accept symbolic variables.

Is there any way to integrate M,C in my ocp constraints ?

I think you want to impose the following nonlinear nonconvex constraint

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

This constraint is non-convex. We will need to solve it through nonlinear optimization, and evaluate the constraint in every iteration of the nonlinear optimization. We can't do this evaluation using symbolic computation (it would be horribly slow with symbolic computation).

So you will need a constraint evaluator, something like this

// 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]});
}
}

You could refer to the class CentroidalMomentumConstraint on how to construct your own MyConstraint class.