How to work with Symbolic Expression Calculation in C++ drake?

Chen Wang 0
Viewed 24

I used to work with pydrake and recently I want to use drake in C++. I am not experienced with C++. I find that the symbolic expression calculation in C++ drake has some strange behaviors. For example, in pydrake, I can have

prog = MathematicalProgram()
v = prog.NewContinuousVariables(7, 'v')
prog.AddCost((J_G.dot(v) - V_G_desired).dot(J_G.dot(v) - V_G_desired) + 0.01* v.dot(v))

which works fine. However, in C++, if I write similarly,

drake::solvers::MathematicalProgram prog;
auto v = prog.NewContinuousVariables(7, "v");
prog.AddCost((J_V_WS*v - V_WS).dot(J_V_WS*v - V_WS) + 0.01*v.dot(v));

I would get a long error message:

In file included from external/eigen/include/_usr_include_eigen3/Eigen/Core:372,
                 from bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/common/autodiff.h:12,
                 from bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/common/default_scalars.h:3,
                 from bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/systems/framework/leaf_system.h:14,
                 from ./kuka/diffIK_controller.h:3,
                 from kuka/diffIK_controller.cc:3:
external/eigen/include/_usr_include_eigen3/Eigen/src/Core/GenericPacketMath.h: In instantiation of 'Packet Eigen::internal::pmul(const Packet&, const Packet&) [with Packet = drake::symbolic::Variable]':
external/eigen/include/_usr_include_eigen3/Eigen/src/Core/util/BlasUtil.h:75:116:   required from 'Scalar Eigen::internal::conj_helper<Scalar, Scalar, false, false>::pmul(const Scalar&, const Scalar&) const [with Scalar = drake::symbolic::Variable]'
external/eigen/include/_usr_include_eigen3/Eigen/src/Core/functors/BinaryFunctors.h:120:66:   required from 'const result_type Eigen::internal::scalar_conj_product_op<LhsScalar, RhsScalar>::operator()(const LhsScalar&, const RhsScalar&) const [with LhsScalar = drake::symbolic::Variable; RhsScalar = drake::symbolic::Variable; Eigen::internal::scalar_conj_product_op<LhsScalar, RhsScalar>::result_type = drake::symbolic::Expression]'
external/eigen/include/_usr_include_eigen3/Eigen/src/Core/CoreEvaluators.h:713:74:   required from 'Eigen::internal::binary_evaluator<Eigen::CwiseBinaryOp<BinaryOp, Lhs, Rhs>, Eigen::internal::IndexBased, Eigen::internal::IndexBased>::CoeffReturnType Eigen::internal::binary_evaluator<Eigen::CwiseBinaryOp<BinaryOp, Lhs, Rhs>, Eigen::internal::IndexBased, Eigen::internal::IndexBased>::coeff(Eigen::Index, Eigen::Index) const [with BinaryOp = Eigen::internal::scalar_conj_product_op<drake::symbolic::Variable, drake::symbolic::Variable>; Lhs = const Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1>; Rhs = const Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1>; typename Eigen::internal::traits<typename Eigen::CwiseBinaryOp<BinaryOp, Lhs, Rhs>::Rhs>::Scalar = drake::symbolic::Variable; typename Eigen::internal::traits<typename Eigen::CwiseBinaryOp<BinaryOp, Lhs, Rhs>::Lhs>::Scalar = drake::symbolic::Variable; Eigen::internal::binary_evaluator<Eigen::CwiseBinaryOp<BinaryOp, Lhs, Rhs>, Eigen::internal::IndexBased, Eigen::internal::IndexBased>::CoeffReturnType = const drake::symbolic::Expression; Eigen::Index = long int]'
external/eigen/include/_usr_include_eigen3/Eigen/src/Core/Redux.h:381:84:   required from 'Eigen::internal::redux_evaluator<_XprType>::CoeffReturnType Eigen::internal::redux_evaluator<_XprType>::coeffByOuterInner(Eigen::Index, Eigen::Index) const [with _XprType = Eigen::CwiseBinaryOp<Eigen::internal::scalar_conj_product_op<drake::symbolic::Variable, drake::symbolic::Variable>, const Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1>, const Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1> >; Eigen::internal::redux_evaluator<_XprType>::CoeffReturnType = const drake::symbolic::Expression; Eigen::Index = long int]'
external/eigen/include/_usr_include_eigen3/Eigen/src/Core/Redux.h:194:9:   required from 'static Eigen::internal::redux_impl<Func, Derived, 0, 0>::Scalar Eigen::internal::redux_impl<Func, Derived, 0, 0>::run(const Derived&, const Func&) [with Func = Eigen::internal::scalar_sum_op<drake::symbolic::Expression, drake::symbolic::Expression>; Derived = Eigen::internal::redux_evaluator<Eigen::CwiseBinaryOp<Eigen::internal::scalar_conj_product_op<drake::symbolic::Variable, drake::symbolic::Variable>, const Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1>, const Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1> > >; Eigen::internal::redux_impl<Func, Derived, 0, 0>::Scalar = drake::symbolic::Expression]'
external/eigen/include/_usr_include_eigen3/Eigen/src/Core/Redux.h:418:56:   required from 'typename Eigen::internal::traits<T>::Scalar Eigen::DenseBase<Derived>::redux(const Func&) const [with BinaryOp = Eigen::internal::scalar_sum_op<drake::symbolic::Expression, drake::symbolic::Expression>; Derived = Eigen::CwiseBinaryOp<Eigen::internal::scalar_conj_product_op<drake::symbolic::Variable, drake::symbolic::Variable>, const Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1>, const Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1> >; typename Eigen::internal::traits<T>::Scalar = drake::symbolic::Expression]'
external/eigen/include/_usr_include_eigen3/Eigen/src/Core/Redux.h:453:73:   required from 'typename Eigen::internal::traits<T>::Scalar Eigen::DenseBase<Derived>::sum() const [with Derived = Eigen::CwiseBinaryOp<Eigen::internal::scalar_conj_product_op<drake::symbolic::Variable, drake::symbolic::Variable>, const Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1>, const Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1> >; typename Eigen::internal::traits<T>::Scalar = drake::symbolic::Expression]'
external/eigen/include/_usr_include_eigen3/Eigen/src/Core/Dot.h:37:52:   required from 'static Eigen::internal::dot_nocheck<T, U, NeedToTranspose>::ResScalar Eigen::internal::dot_nocheck<T, U, NeedToTranspose>::run(const Eigen::MatrixBase<Derived>&, const Eigen::MatrixBase<U>&) [with T = Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1>; U = Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1>; bool NeedToTranspose = false; Eigen::internal::dot_nocheck<T, U, NeedToTranspose>::ResScalar = drake::symbolic::Expression]'
external/eigen/include/_usr_include_eigen3/Eigen/src/Core/Dot.h:84:58:   required from 'typename Eigen::ScalarBinaryOpTraits<typename Eigen::internal::traits<T>::Scalar, typename Eigen::internal::traits<OtherDerived>::Scalar>::ReturnType Eigen::MatrixBase<Derived>::dot(const Eigen::MatrixBase<OtherDerived>&) const [with OtherDerived = Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1>; Derived = Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1>; typename Eigen::ScalarBinaryOpTraits<typename Eigen::internal::traits<T>::Scalar, typename Eigen::internal::traits<OtherDerived>::Scalar>::ReturnType = drake::symbolic::Expression]'
kuka/diffIK_controller.cc:52:67:   required from 'drake::VectorX<T> DiffIKController<T>::CalcDiffIK(drake::MatrixX<double>, drake::VectorX<double>) const [with T = double; drake::VectorX<T> = Eigen::Matrix<double, -1, 1>; drake::MatrixX<double> = Eigen::Matrix<double, -1, -1>; drake::VectorX<double> = Eigen::Matrix<double, -1, 1>]'
kuka/diffIK_controller.cc:67:16:   required from here
external/eigen/include/_usr_include_eigen3/Eigen/src/Core/GenericPacketMath.h:171:37: error: could not convert 'drake::symbolic::operator*(drake::symbolic::Expression, const drake::symbolic::Expression&)(drake::symbolic::Expression((* & b)))' from 'drake::symbolic::Expression' to 'drake::symbolic::Variable'
  171 |         const Packet& b) { return a*b; }
      |                                     ^
      |                                     |
      |                                     drake::symbolic::Expression
Target //kuka:diffIK_test failed to build
Use --verbose_failures to see the command lines of failed build steps.
INFO: Elapsed time: 5.876s, Critical Path: 5.72s
INFO: 2 processes: 2 internal.
FAILED: Build did NOT complete successfully
FAILED: Build did NOT complete successfully

I find that by simply adding a paraenthesis around 0.01*v would solve this error:

prog.AddCost((J_V_WS*v - V_WS).dot(J_V_WS*v - V_WS) + (0.01*v).dot(v));

However, use the transpose method to do the inner product would still have issues:

prog.AddCost((J_V_WS*v - V_WS).transpose()*(J_V_WS*v - V_WS) + (0.01*v).transpose()*(v));

The error info is:

kuka/diffIK_controller.cc failed: (Exit 1): gcc failed: error executing command /usr/bin/gcc -U_FORTIFY_SOURCE -fstack-protector -Wall -Wunused-but-set-parameter -Wno-free-nonheap-object -fno-omit-frame-pointer -g0 -O2 '-D_FORTIFY_SOURCE=1' -DNDEBUG -ffunction-sections ... (remaining 319 arguments skipped)

Use --sandbox_debug to see verbose messages from the sandbox
kuka/diffIK_controller.cc: In member function 'drake::VectorX<T> DiffIKController<T>::CalcDiffIK(drake::MatrixX<double>, drake::VectorX<double>) const':
kuka/diffIK_controller.cc:53:90: error: no matching function for call to 'drake::solvers::MathematicalProgram::AddCost(const Eigen::CwiseBinaryOp<Eigen::internal::scalar_sum_op<drake::symbolic::Expression, drake::symbolic::Expression>, const Eigen::Product<Eigen::Transpose<const Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> > >, Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> >, 0>, const Eigen::Matrix<drake::symbolic::Expression, 1, 1, 0, 1, 1> >)'
   53 |   prog.AddCost((J_V_WS*v - V_WS).transpose()*(J_V_WS*v - V_WS) + (0.01*v).transpose()*(v));
      |                                                                                          ^
In file included from kuka/diffIK_controller.cc:5:
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:863:17: note: candidate: 'drake::solvers::Binding<drake::solvers::Cost> drake::solvers::MathematicalProgram::AddCost(const drake::solvers::Binding<drake::solvers::Cost>&)'
  863 |   Binding<Cost> AddCost(const Binding<Cost>& binding);
      |                 ^~~~~~~
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:863:46: note:   no known conversion for argument 1 from 'const Eigen::CwiseBinaryOp<Eigen::internal::scalar_sum_op<drake::symbolic::Expression, drake::symbolic::Expression>, const Eigen::Product<Eigen::Transpose<const Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> > >, Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> >, 0>, const Eigen::Matrix<drake::symbolic::Expression, 1, 1, 0, 1, 1> >' to 'const drake::solvers::Binding<drake::solvers::Cost>&'
  863 |   Binding<Cost> AddCost(const Binding<Cost>& binding);
      |                         ~~~~~~~~~~~~~~~~~~~~~^~~~~~~
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:873:8: note: candidate: 'template<class C> auto drake::solvers::MathematicalProgram::AddCost(const std::shared_ptr<_Tp>&, const Eigen::Ref<const Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1> >&)'
  873 |   auto AddCost(const std::shared_ptr<C>& obj,
      |        ^~~~~~~
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:873:8: note:   template argument deduction/substitution failed:
kuka/diffIK_controller.cc:53:90: note:   'const Eigen::CwiseBinaryOp<Eigen::internal::scalar_sum_op<drake::symbolic::Expression, drake::symbolic::Expression>, const Eigen::Product<Eigen::Transpose<const Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> > >, Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> >, 0>, const Eigen::Matrix<drake::symbolic::Expression, 1, 1, 0, 1, 1> >' is not derived from 'const std::shared_ptr<_Tp>'
   53 |   prog.AddCost((J_V_WS*v - V_WS).transpose()*(J_V_WS*v - V_WS) + (0.01*v).transpose()*(v));
      |                                                                                          ^
In file included from kuka/diffIK_controller.cc:5:
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:888:8: note: candidate: 'template<class C> auto drake::solvers::MathematicalProgram::AddCost(const std::shared_ptr<_Tp>&, const VariableRefList&)'
  888 |   auto AddCost(const std::shared_ptr<C>& obj, const VariableRefList& vars) {
      |        ^~~~~~~
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:888:8: note:   template argument deduction/substitution failed:
kuka/diffIK_controller.cc:53:90: note:   'const Eigen::CwiseBinaryOp<Eigen::internal::scalar_sum_op<drake::symbolic::Expression, drake::symbolic::Expression>, const Eigen::Product<Eigen::Transpose<const Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> > >, Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> >, 0>, const Eigen::Matrix<drake::symbolic::Expression, 1, 1, 0, 1, 1> >' is not derived from 'const std::shared_ptr<_Tp>'
   53 |   prog.AddCost((J_V_WS*v - V_WS).transpose()*(J_V_WS*v - V_WS) + (0.01*v).transpose()*(v));
      |                                                                                          ^
In file included from kuka/diffIK_controller.cc:5:
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:911:3: note: candidate: 'template<class F> std::enable_if_t<drake::solvers::internal::is_cost_functor_candidate<F>::value, drake::solvers::Binding<drake::solvers::Cost> > drake::solvers::MathematicalProgram::AddCost(F&&, const VariableRefList&)'
  911 |   AddCost(F&& f, const VariableRefList& vars) {
      |   ^~~~~~~
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:911:3: note:   template argument deduction/substitution failed:
kuka/diffIK_controller.cc:53:90: note:   candidate expects 2 arguments, 1 provided
   53 |   prog.AddCost((J_V_WS*v - V_WS).transpose()*(J_V_WS*v - V_WS) + (0.01*v).transpose()*(v));
      |                                                                                          ^
In file included from kuka/diffIK_controller.cc:5:
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:925:3: note: candidate: 'template<class F> std::enable_if_t<drake::solvers::internal::is_cost_functor_candidate<F>::value, drake::solvers::Binding<drake::solvers::Cost> > drake::solvers::MathematicalProgram::AddCost(F&&, const Eigen::Ref<const Eigen::Matrix<drake::symbolic::Variable, -1, 1, 0, -1, 1> >&)'
  925 |   AddCost(F&& f, const Eigen::Ref<const VectorXDecisionVariable>& vars) {
      |   ^~~~~~~
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:925:3: note:   template argument deduction/substitution failed:
kuka/diffIK_controller.cc:53:90: note:   candidate expects 2 arguments, 1 provided
   53 |   prog.AddCost((J_V_WS*v - V_WS).transpose()*(J_V_WS*v - V_WS) + (0.01*v).transpose()*(v));
      |                                                                                          ^
In file included from kuka/diffIK_controller.cc:5:
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:940:3: note: candidate: 'template<class F, class Vars> std::enable_if_t<drake::solvers::internal::assert_if_is_constraint<F>::value, drake::solvers::Binding<drake::solvers::Cost> > drake::solvers::MathematicalProgram::AddCost(F&&, Vars&&)'
  940 |   AddCost(F&&, Vars&&) {
      |   ^~~~~~~
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:940:3: note:   template argument deduction/substitution failed:
kuka/diffIK_controller.cc:53:90: note:   candidate expects 2 arguments, 1 provided
   53 |   prog.AddCost((J_V_WS*v - V_WS).transpose()*(J_V_WS*v - V_WS) + (0.01*v).transpose()*(v));
      |                                                                                          ^
In file included from kuka/diffIK_controller.cc:5:
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:951:23: note: candidate: 'drake::solvers::Binding<drake::solvers::LinearCost> drake::solvers::MathematicalProgram::AddCost(const drake::solvers::Binding<drake::solvers::LinearCost>&)'
  951 |   Binding<LinearCost> AddCost(const Binding<LinearCost>& binding);
      |                       ^~~~~~~
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:951:58: note:   no known conversion for argument 1 from 'const Eigen::CwiseBinaryOp<Eigen::internal::scalar_sum_op<drake::symbolic::Expression, drake::symbolic::Expression>, const Eigen::Product<Eigen::Transpose<const Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> > >, Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> >, 0>, const Eigen::Matrix<drake::symbolic::Expression, 1, 1, 0, 1, 1> >' to 'const drake::solvers::Binding<drake::solvers::LinearCost>&'
  951 |   Binding<LinearCost> AddCost(const Binding<LinearCost>& binding);
      |                               ~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:1001:26: note: candidate: 'drake::solvers::Binding<drake::solvers::QuadraticCost> drake::solvers::MathematicalProgram::AddCost(const drake::solvers::Binding<drake::solvers::QuadraticCost>&)'
 1001 |   Binding<QuadraticCost> AddCost(const Binding<QuadraticCost>& binding);
      |                          ^~~~~~~
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:1001:64: note:   no known conversion for argument 1 from 'const Eigen::CwiseBinaryOp<Eigen::internal::scalar_sum_op<drake::symbolic::Expression, drake::symbolic::Expression>, const Eigen::Product<Eigen::Transpose<const Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> > >, Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> >, 0>, const Eigen::Matrix<drake::symbolic::Expression, 1, 1, 0, 1, 1> >' to 'const drake::solvers::Binding<drake::solvers::QuadraticCost>&'
 1001 |   Binding<QuadraticCost> AddCost(const Binding<QuadraticCost>& binding);
      |                                  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:1107:23: note: candidate: 'drake::solvers::Binding<drake::solvers::L2NormCost> drake::solvers::MathematicalProgram::AddCost(const drake::solvers::Binding<drake::solvers::L2NormCost>&)'
 1107 |   Binding<L2NormCost> AddCost(const Binding<L2NormCost>& binding);
      |                       ^~~~~~~
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:1107:58: note:   no known conversion for argument 1 from 'const Eigen::CwiseBinaryOp<Eigen::internal::scalar_sum_op<drake::symbolic::Expression, drake::symbolic::Expression>, const Eigen::Product<Eigen::Transpose<const Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> > >, Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> >, 0>, const Eigen::Matrix<drake::symbolic::Expression, 1, 1, 0, 1, 1> >' to 'const drake::solvers::Binding<drake::solvers::L2NormCost>&'
 1107 |   Binding<L2NormCost> AddCost(const Binding<L2NormCost>& binding);
      |                               ~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:1170:17: note: candidate: 'drake::solvers::Binding<drake::solvers::Cost> drake::solvers::MathematicalProgram::AddCost(const drake::symbolic::Expression&)'
 1170 |   Binding<Cost> AddCost(const symbolic::Expression& e);
      |                 ^~~~~~~
bazel-out/k8-opt/bin/external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/solvers/mathematical_program.h:1170:53: note:   no known conversion for argument 1 from 'const Eigen::CwiseBinaryOp<Eigen::internal::scalar_sum_op<drake::symbolic::Expression, drake::symbolic::Expression>, const Eigen::Product<Eigen::Transpose<const Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> > >, Eigen::CwiseBinaryOp<Eigen::internal::scalar_difference_op<drake::symbolic::Expression, double>, const Eigen::Matrix<drake::symbolic::Expression, -1, 1, 0, -1, 1>, const Eigen::Matrix<double, -1, 1> >, 0>, const Eigen::Matrix<drake::symbolic::Expression, 1, 1, 0, 1, 1> >' to 'const drake::symbolic::Expression&'
 1170 |   Binding<Cost> AddCost(const symbolic::Expression& e);
      |                         ~~~~~~~~~~~~~~~~~~~~~~~~~~~~^
Target //kuka:diffIK_test failed to build
Use --verbose_failures to see the command lines of failed build steps.
INFO: Elapsed time: 5.757s, Critical Path: 5.61s
INFO: 2 processes: 2 internal.
FAILED: Build did NOT complete successfully
FAILED: Build did NOT complete successfully

What's more, I find that

prog.AddCost(v.dot(v))

would cause error while

prog.AddCost(v.transpose() * v);

would work fine.

Why would the symbolic calculations in C++ drake have above behaviors? Is there some guidance on how to do symbolic calculations with drake in C++? Thank you very much for your help.

drake
1 Answers

We have two different class, symbolic::Variable and symbolic::Expression. We first create symbolic::Variable (for example using prog.NewContinuousVariables()), and then do some algebraic operation on these symbolic::Variable objects to get a symbolic::Expression.

In pydrake, we can convert symbolic::Variable to symbolic::Expression automatically. But in C++, the user has to explicitly do that conversion. I modified your code here

prog.AddCost((J_V_WS*v - V_WS).dot(J_V_WS*v - V_WS) + 0.01*v.cast<symbolic::Expression>().dot(v));

Note that I did v.cast<symbolic::Expression>().dot(v) to explicitly change v from a vector of symbolic::Variable to a vector of symbolic::Expression.

As you said, doing (0.01* v) also solves the problem, that is because when we have a double scalar multiply a symbolic::Variable, we end up with a symbolic::Expression.

Hongkai Dai0
Related