DIRCON rework.

[mposa]

Rewrites DIRCON code to use new evaluators and an impoved API. In addition, contains two related changes:

-Add friction constraints as options to kinematic evaluators -Change multibody_utils CreateContext and ModifyContext to take Eigen::Ref, avoiding a small copy operation. Note that this requires manual template declarations, e.g. CreateContext<double>(...) as template type cannot be inferred automatically.

This PR introduces one new example, and rewrites two existing examples. It does not delete the old code or rewrite all examples, as this should be the product of a later PR once we are happy with the rewrite performance. The examples are: -(new) systems/trajectory_optimization/dircon/test/passive_constrained_pendulum_dircon.cc: A floating-base acrobot constrained via a WorldPointEvaluator and DistanceEvaluator to behave like a passive pendulum. Tests both evaluator types and quaternion constraints. -(updated) examples/PlanarWalker/run_gait_dircon.cc -(updated) examples/Cassie/run_dircon_squatting.cc

Feedback on the code, API, and examples is all appreciated.

Caching

I experimented with a more aggressive Systems-based caching mechanism using individual dependencies on q,v,lambda, but eventually abandoned it in favor of a more narrowly scoped DIRCON-specific cache for xdot calculations only. The more aggressive cache was going to be very difficult to maintain, and essentially required manual determination of sparsity patterns and gradient extraction/creation.

While there's some room for improvement, xdot is definitely the most expensive repeated computation. What this fails to capture, however, is that not every element used in xdot requires differentiation w.r.t. all inputs x,u,lambda. For instance, the bias terms are independent of u and lambda, and the mass matrix only depends on q. Profiling shows that future improvements to numerical gradient calculations could show benefits.

Scaling

I noted, as everyone has before me, extreme sensitivity on runtime w.r.t. scaling parameters on the Cassie example, while using SNOPT. I also tried using IPOPT, where the default automatic scaling seemed to render our manual scaling unnecessary. While SNOPT with optimized-scale parameters is ~2x faster, on limited testing I found IPOPT to be more reliable for the squatting example. Note that the example now includes an option to use IPOPT, along with some option settings. For these options, IPOPT did not converge to dual feasibility, but was primal feasible and had stalled progress on improving the cost (achieving the same cost as SNOPT).

---

This change is 

[mposa]

I also found a bug in the old Dircon code, where certain forces were not being included in the dynamics calculations. Specifically, the old code ignored joint damping. Hopefully resolving https://github.com/RobotLocomotion/drake/issues/13476 will make this easier in the future.

[yangwill]

Only made if about halfway through the review, will continue tomorrow.

[yminchen]

I was hoping to find a clean-ish implementation of a caching system for our numerical differentiation. And not surprising, it doesn't look clean... (especially because dircon is templated) Below is what I came up with.

I think we are only using Drake's context to cache forward kinematics elements, J and JdotV, so the robot's state x is the only cache dependency here. Assuming this is true, we can create n_x + 1 number of Context for each knot point. Dircon constructor could look like:

// Loop over all modes
for (int i_mode = 0; i_mode < num_modes(); i_mode++) {
  ...

  //
  // Create context elements 
  //
  if (typeid(T) == typeid(drake::AutoDiffXd)) {
    // create 1 context per knot point
  } else if (typeid(T) == typeid(double)) {
    // create n_x + 1 context per knot point
  } else {
    DRAKE_UNREACHABLE();
  }

  ...
}

To handle the cache dependency for numerical differentiation, I think we can create a class derived from NonlinearConstraint, and we override the DoEval where we compute gradient numerically. The new constraint class takes in dependent_input_inds which tells the location of x (or x's) in the constraint input. And we add a virtual method EvaluateConstraintWithCache which is similar to EvaluateConstraint except for an extra input which tells the derived class which context it should use among n_x + 1 contexts.

NonlinearConstraintWithCache<T>::NonlinearConstraintWithCache(
    int num_constraints, int num_vars, const VectorXd& lb, const VectorXd& ub,
    const std::set<int> dependent_input_inds, const std::string& description)
    : NonlinearConstraint(num_constraints, num_vars, lb, ub, description) {
  // DRAKE_DEMAND each element of dependent_input_inds < num_vars

  // Create the complement of dependent_input_inds
}

// NonlinearConstraintWithCache has a pure virtual function
// EvaluateConstraintWithCache(const Eigen::Ref<const drake::VectorX<T>>& x,
//                             int cache_id, drake::VectorX<T>* y)

template <>
void NonlinearConstraintWithCache<double>::DoEval(
    const Eigen::Ref<const AutoDiffVecXd>& x, AutoDiffVecXd* y) const {
  MatrixXd original_grad = drake::math::autoDiffToGradientMatrix(x);

  // forward differencing
  VectorXd x_val = drake::math::autoDiffToValueMatrix(x);
  VectorXd y0, yi;
  EvaluateConstraintWithCache(x_val, 0, &y0);

  // We split the gradient calculation into two groups
  MatrixXd dy = MatrixXd(y0.size(), x_val.size());
  for (int idx = 0; idx < dependent_input_inds.size(); idx++) {
    x_val(dependent_input_inds(idx)) += eps_;
    EvaluateConstraintWithCache(x_val, idx, &yi);
    x_val(dependent_input_inds(idx)) -= eps_;
    dy.col(dependent_input_inds(idx)) = (yi - y0) / eps_;
  }
  for (int idx = 0; idx < complement_of_dependent_input_inds.size(); idx++) {
    x_val(complement_of_dependent_input_inds(idx)) += eps_;
    EvaluateConstraintWithCache(x_val, 0, &yi);
    x_val(complement_of_dependent_input_inds(idx)) -= eps_;
    dy.col(complement_of_dependent_input_inds(idx)) = (yi - y0) / eps_;
  }

  ...
}

We can pass std::vector<Context> into DirconCollocationConstraint, ImpactConstraint and CachedAccelerationConstraint to avoid calling two versions of constructor for double and autodiff. Haven't thought through all the details but just to present an idea here

[yminchen]

_multibody/kinematic/kinematic_evaluator_set.cc, line 105 at r5 (raw file):_ Previously, yminchen (Yu-Ming Chen) wrote…

No, it should be vdot from applying chain rule to phi(q)

It looks to me that KinematicEvaluatorSet<T>::CalcTimeDerivativesWithForce always returns xdot. https://github.com/DAIRLab/dairlib/blob/9c8e722/multibody/kinematic/kinematic_evaluator_set.cc#L244 Am I not looking at the correct function?

_multibody/kinematic/world_point_evaluator.cc, line 90 at r5 (raw file):_ Previously, yminchen (Yu-Ming Chen) wrote…

It's not included as a bounding box constraint, but implied by the single nonlinear constraint.

Should we go back to something like the current formulation where Evaluator.CreateXXFrictionConeConstraint actually returns a list of constraints?

Yea, returning a list of constraints should be a good solution for now

_systems/trajectory_optimization/dircon/dircon.h, line 214 at r5 (raw file):_ Previously, yminchen (Yu-Ming Chen) wrote…

Done. I added a getter to a const Context<T>&. Does that work? Exposing the pointer seems pointless, unless you see a reason.

Yes that works!

_systems/trajectory_optimization/dircon/dircon.cc, line 171 at r5 (raw file):_ Previously, yminchen (Yu-Ming Chen) wrote…

See if you think the fix to FindActiveUnion resolved this.

The code looks correct, but the variable name should be row_indices instead of union_indices

[mposa]

Going to merge--but feel free to followup with other comments down the line as you use it.