Drake: Integrate Mass Matrix and Bias Term in Optimization Problem

Question

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 ?

score 2 · Accepted Answer · answered Sep 28 '22 at 15:28

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.

This is working, but I still can't solve the problem very effectively. Only SNOPT was able to solve and the best I could do was solve in < 5 Hz with a very small Horizon (0.1s). Otherwise, it gets super slow. I also tried MOSEK but I it says it doesn't support GenericConstraint. The rest of the constraints are state boundaries and differentation rules, and the cost is quadratic. Any ideas ? — DRakovitis, Oct 13 '22 at 09:39
You do have a nonlinear optimization problem, due to the nonlinear dynamics, so I don't think Mosek can solve your problem (Mosek can solve convex or mixed-integer convex problems). — Hongkai Dai, Oct 13 '22 at 18:45
I would suggest to think about a different formulation of the problem. Currently you are trying to find q, v, vdot simultaneously, and this becomes a nonconvex nonlinear optimization problem. For the IIWA robot arm, you could consider to first solve each individual q[t] separately (through inverse kinematics for example), and then solve a time-optimal path planning problem to find v and vdot. This time-optimal path planning problem can be solved through convex optimization. Check https://arxiv.org/pdf/1707.07239.pdf for a recent review on time-optimal path planning. — Hongkai Dai, Oct 13 '22 at 18:47

Drake: Integrate Mass Matrix and Bias Term in Optimization Problem

1 Answers1