CODEWITHSUNDEEP
drake
Wc Chang
Wc Chang

Reputation: 97

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

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()

Upvotes: 0

Views: 357

Answers (1)

Hongkai Dai
Hongkai Dai

Reputation: 2766

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.

Upvotes: 2

Related Questions