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

如何解决您能否通过将 Pydrake 代码转换为 C++ 版本来演示一个关于符号变量和雅可比行列式的非常简单的 C++ 示例？

我正在尝试将以下 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()

解决方法

由于您使用了名称“成本”，我想您想将其用作 drake 的 MathematicalProgram 中的成本，因此我创建了可在 Drake 的 MathematicalProgram 中使用的 MyCost 类。如果您以后不想使用 MathematicalProgram，您可以只使用模板函数 DoEvalGeneric，而无需使用类 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;
}

然后您可以对 Jacobian 的返回参数调用 cost_stage。