Could you demo a very simple C++ example about Symbolic variable and Jacobian by converting Pydrake code to C++ version?

Question

I am trying to convert the following pydrake code to C++ version. Unfortunately,I get lost in the very rigorous C++ API documentation. Could you help to convert the following code into C++ version for a tutorial? Thank you so much!

import pydrake.math as drake_math
import pydrake.symbolic as sym

def cost_stage(x):
    m = sym if x.dtype == object else np # Check type for autodiff
    cost = m.sqrt(x[0]**2 + x[1]**2 ) 
    return cost

x_sym = np.array([sym.Variable("x_{}".format(i)) for i in range(n_x)])
x = x_sym
l = cost_stage(x)
self.l_x = sym.Jacobian([l], x).ravel()

Do you want to use this cost in drake's MathematicalProgram, or you just want the symbolic expression on the cost Jacobian? — Hongkai Dai, Jan 23 '21 at 20:17
I guess the answer is the latter. The above code is part of iLQR exercise which does a lot of derivatives for Q-function expansion(We get the quadaratic cost function finally). — Wc Chang, Jan 24 '21 at 01:16
Then you just need the template DoEvalGeneric function as I wrote in the comment. — Hongkai Dai, Jan 24 '21 at 01:39
I just updated my answer below, that uses a templated function `cost_stage`. — Hongkai Dai, Jan 24 '21 at 01:47

Hongkai Dai · Accepted Answer · 2021-01-24T19:33:21.953

Since you used the name "cost", I suppose you want to use this as a cost in drake's MathematicalProgram, so I created MyCost class which can be used in Drake's MathematicalProgram. If you don't want to use MathematicalProgram later, you could just use the templated function DoEvalGeneric only without the class MyCost.

Here is the C++ pseudo-code (I didn't compile or run the code, so it is highly likely there are bugs in the code, but you get the idea)

#include "drake/solvers/cost.h"
#include "drake/common/symbolic.h"

class MyCost : public drake::solvers::Cost {
 public:
  MyCost() {}
 
 protected:
  void DoEval(const Eigen::Ref<const Eigen::VectorXd>& x, Eigen::VectorXd* y) const override {
    DoEvalGeneric<double>(x, y);
  }

  void DoEval(const Eigen::Ref<const drake::AutoDiffVecXd>& x, drake::AutoDiffVecXd* y) const override {
    DoEvalGeneric<drake::AutoDiffXd>(x, y)
  }
  
  void DoEval(const Eigen::Ref<const drake::VectorX<drake::symbolic::Variable>>& x, drake::VectorX<drake::symbolic::Expression>* y) const override {
    DoEvalGeneric<drake::symbolic::Expression>(x.cast<drake::symbolic::Expression>(), y);
  }

 private:
  template <typename T>
  void DoEvalGeneric(const Eigen::Ref<const drake::VectorX<T>>& x, drake::VectorX<T>* y) const {
    y->resize(1);
    using std::sqrt
    (*y)(0) = sqrt(x[0] * x[0] + x[1] * x[1]);
  }
};

void main() {
  const drake::symbolic::Variable x0{"x0"};
  const drake::symbolic::Variable x1{"x1"};
  drake::Vector2<drake::symbolic::Variable> x(x0, x1);
  MyCost cost{};
  drake::VectorX<drake::symbolic::Expression> y;
  cost.Eval(x, &y);
  std::cout << y(0).Jacobian(x) << "\n";
}

Here I created a templated function DoEvalGeneirc to handle the three different scalar types double, AutoDiffXd and symbolic expression. You could see similar patterns in the drake codebase https://github.com/RobotLocomotion/drake/blob/6ee5e9325821277a62bd5cd5456ccf02ca25dab7/solvers/cost.cc#L14-L33

If you don't need to use cost in drake MathematicalProgram, then you can create your cost function in C++ as

#include "drake/common/symbolic.h"

#include <Eigen/Core>


template <typename Derived>
typename Derived::Scalar cost_stage(const Derived& x) {
  using std::sqrt;
  return sqrt(x[0] * x[0] + x[1] * x[1]);
}

int main() {
  const drake::symbolic::Variable x0{"x0"};
  const drake::symbolic::Variable x1{"x1"};
  drake::Vector2<drake::symbolic::Variable> x(x0, x1);
  const drake::symbolic::Expression l = cost_stage(x.cast<drake::symbolic::Expression>());
  std::cout << l.Jacobian(x) << "\n";
  return 0;
}

Then you can call Jacobian on the return argument of cost_stage.

Thank you for providing this code. It is a great help to me. — Wc Chang, Jan 24 '21 at 01:47
After a lot of effort, I still get two errors, I appreciate it if you can help to correct them. — Wc Chang, Jan 24 '21 at 11:22
error 1: missing 'typename' prior to dependent type name 'Derived::Scalar' Derived::Scalar cost_stage(const Eigen::Ref>& x) { — Wc Chang, Jan 24 '21 at 11:22
error 2: no matching function for call to 'cost_stage' const drake::symbolic::Expression l = cost_stage(x.cast()); — Wc Chang, Jan 24 '21 at 11:22
I updated that code above, it compiles and runs on my machine. — Hongkai Dai, Jan 24 '21 at 19:33
It works on my computer now. Thanks! By the way, I search "cast" in the API documentation, it supports several methods in math and multibody class, but not includes vector or symbolic class. It is quite interesting. — Wc Chang, Jan 25 '21 at 00:28

Could you demo a very simple C++ example about Symbolic variable and Jacobian by converting Pydrake code to C++ version?

1 Answers1