您能否通过将 Pydrake 代码转换为 C++ 版本来演示一个关于符号变量和 Jacobian 的非常简单的 C++ 示例？

Question

我正在尝试将以下 pydrake 代码转换为 C++ 版本。不幸的是，我迷失在非常严格的 C++ API 文档中。你能帮忙把下面的代码转换成C++版本的教程吗？非常感谢！

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

Answer 1

既然你用了名字“cost”，我想你想在 drake 的 MathematicalProgram 中使用它作为成本，所以我创建了 MyCost class 可以在 Drake 的 MathematicalProgram 中使用。如果您以后不想使用 MathematicalProgram，您可以只使用模板化函数 DoEvalGeneric 而无需 class MyCost.

这是C++伪代码（我没有编译或运行代码，所以代码中很可能有错误，但你明白了）

#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";
}

我在这里创建了一个模板函数 DoEvalGeneirc 来处理三种不同的标量类型 double、AutoDiffXd 和符号表达式。您可以在 drake 代码库中看到类似的模式 https://github.com/RobotLocomotion/drake/blob/6ee5e9325821277a62bd5cd5456ccf02ca25dab7/solvers/cost.cc#L14-L33

如果您不需要在 drake MathematicalProgram 中使用 cost，那么您可以在 C++ 中创建 cost 函数，如

#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;
}

然后你可以在 cost_stage 的 return 参数上调用 Jacobian。

您能否通过将 Pydrake 代码转换为 C++ 版本来演示一个关于符号变量和 Jacobian 的非常简单的 C++ 示例？

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

drake