29 bp::class_<ConstantCostTpl<Scalar>, bp::bases<CostAbstract>>(
30 "ConstantCost",
"A constant cost term.",
31 bp::init<shared_ptr<Manifold>, int, Scalar>(
32 bp::args(
"self",
"space",
"nu",
"value")))
36 bp::class_<QuadraticCost, bp::bases<CostAbstract>>(
38 "Quadratic cost in both state and control - only for Euclidean spaces.",
40 .def(bp::init<ConstMatrixRef, ConstMatrixRef, ConstMatrixRef,
41 ConstVectorRef, ConstVectorRef>(
42 bp::args(
"self",
"w_x",
"w_u",
"w_cross",
"interp_x",
"interp_u")))
43 .def(bp::init<ConstMatrixRef, ConstMatrixRef, ConstVectorRef,
45 bp::args(
"self",
"w_x",
"w_u",
"interp_x",
"interp_u")))
46 .def(bp::init<ConstMatrixRef, ConstMatrixRef>(
47 "Constructor with just weights (no cross-term).",
48 bp::args(
"self",
"w_x",
"w_u")))
49 .def(bp::init<ConstMatrixRef, ConstMatrixRef, ConstMatrixRef>(
50 "Constructor with just weights (with cross-term).",
51 bp::args(
"self",
"w_x",
"w_u",
"w_cross")))
52 .def_readwrite(
"w_x", &QuadraticCost::Wxx_,
"Weights on the state.")
53 .def_readwrite(
"w_u", &QuadraticCost::Wuu_,
"Weights on the control.")
54 .def_readwrite(
"interp_x", &QuadraticCost::interp_x)
55 .def_readwrite(
"interp_u", &QuadraticCost::interp_u)
56 .add_property(
"has_cross_term", &QuadraticCost::hasCrossTerm,
57 "Whether there is a cross term.")
58 .add_property(
"weights_cross", &QuadraticCost::getCrossWeights,
59 &QuadraticCost::setCrossWeight,
"Cross term weight.")
62 bp::class_<QuadraticCost::Data, bp::bases<CostData>>(
63 "QuadraticCostData",
"Quadratic cost data.", bp::no_init);
76 bp::register_ptr_to_python<CostPtr>();
78 bp::class_<PyCostFunction<>, boost::noncopyable>(
79 "CostAbstract",
"Base class for cost functions.", bp::no_init)
80 .def(bp::init<shared_ptr<Manifold>,
const int>(
81 bp::args(
"self",
"space",
"nu")))
82 .def(
"evaluate", bp::pure_virtual(&CostAbstract::evaluate),
83 bp::args(
"self",
"x",
"u",
"data"),
"Evaluate the cost function.")
84 .def(
"computeGradients",
85 bp::pure_virtual(&CostAbstract::computeGradients),
86 bp::args(
"self",
"x",
"u",
"data"),
87 "Compute the cost function gradients.")
88 .def(
"computeHessians", bp::pure_virtual(&CostAbstract::computeHessians),
89 bp::args(
"self",
"x",
"u",
"data"),
90 "Compute the cost function hessians.")
91 .def_readonly(
"space", &CostAbstract::space)
92 .add_property(
"nx", &CostAbstract::nx)
93 .add_property(
"ndx", &CostAbstract::ndx)
94 .add_property(
"nu", &CostAbstract::nu)
95 .def(CreateDataPythonVisitor<CostAbstract>());
97 bp::register_ptr_to_python<shared_ptr<CostData>>();
98 bp::class_<CostDataWrapper, boost::noncopyable>(
99 "CostData",
"Cost function data struct.", bp::no_init)
100 .def(bp::init<const int, const int>(bp::args(
"self",
"ndx",
"nu")))
101 .def(bp::init<const CostAbstract &>(bp::args(
"self",
"cost")))
102 .def_readwrite(
"value", &CostData::value_)
103 .def_readwrite(
"grad", &CostData::grad_)
104 .def_readwrite(
"hess", &CostData::hess_)
106 "Lx", bp::make_getter(&CostData::Lx_,
107 bp::return_value_policy<bp::return_by_value>()))
109 "Lu", bp::make_getter(&CostData::Lu_,
110 bp::return_value_policy<bp::return_by_value>()))
111 .add_property(
"Lxx", bp::make_getter(
113 bp::return_value_policy<bp::return_by_value>()))
114 .add_property(
"Lxu", bp::make_getter(
116 bp::return_value_policy<bp::return_by_value>()))
117 .add_property(
"Lux", bp::make_getter(
119 bp::return_value_policy<bp::return_by_value>()))
120 .add_property(
"Luu", bp::make_getter(
122 bp::return_value_policy<bp::return_by_value>()));
124 StdVectorPythonVisitor<std::vector<CostPtr>,
true>::expose(
125 "StdVec_CostAbstract");
126 StdVectorPythonVisitor<std::vector<shared_ptr<CostData>>,
true>::expose(
CostAbstractTpl< Scalar > CostAbstract
ManifoldAbstractTpl< Scalar > Manifold
CostDataAbstractTpl< Scalar > CostData