pyexotica.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2020, University of Edinburgh, University of Oxford
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
34 #ifdef MSGPACK_FOUND
36 #endif
38 #undef NDEBUG
39 #include <pybind11/eigen.h>
40 #include <pybind11/pybind11.h>
41 #include <pybind11/stl.h>
42 
43 #include <octomap/OcTree.h>
44 #include <ros/package.h>
45 
46 #include <unsupported/Eigen/CXX11/Tensor>
47 
48 #if PY_MAJOR_VERSION < 3
49 #define PY_OLDSTANDARD
50 #endif
51 
52 #ifndef PY_OLDSTANDARD
53 bool PyInt_Check(PyObject* value_py) { return PyLong_Check(value_py); }
54 long PyInt_AsLong(PyObject* value_py) { return PyLong_AsLong(value_py); }
55 #endif
56 
57 bool IsPyString(PyObject* value_py)
58 {
59 #ifndef PY_OLDSTANDARD
60  return PyUnicode_Check(value_py);
61 #else
62  return PyString_Check(value_py) || PyUnicode_Check(value_py);
63 #endif
64 }
65 
66 std::string PyAsStdString(PyObject* value_py)
67 {
68 #ifndef PY_OLDSTANDARD
69  PyObject* tmp = PyUnicode_AsASCIIString(value_py);
70  std::string ret = std::string(PyBytes_AsString(tmp));
71  Py_DECREF(tmp);
72  return ret;
73 #else
74  return std::string(PyString_AsString(value_py));
75 #endif
76 }
77 
78 PyObject* StdStringAsPy(std::string value)
79 {
80 #ifndef PY_OLDSTANDARD
81  return PyUnicode_DecodeASCII(value.c_str(), value.size(), "");
82 #else
83  return PyString_FromString(value.c_str());
84 #endif
85 }
86 
87 using namespace exotica;
88 namespace py = pybind11;
89 
90 std::map<std::string, Initializer> known_initializers;
91 
93 {
94 #if PY_MAJOR_VERSION <= 2
95  PyObject* module = PyImport_ImportModule("StringIO");
96  if (!module) ThrowPretty("Can't load StringIO module.");
97  PyObject* cls = PyObject_GetAttrString(module, "StringIO");
98  if (!cls) ThrowPretty("Can't load StringIO class.");
99 #else
100  PyObject* module = PyImport_ImportModule("io");
101  if (!module) ThrowPretty("Can't load io module.");
102  PyObject* cls = PyObject_GetAttrString(module, "BytesIO");
103  if (!cls) ThrowPretty("Can't load BytesIO class.");
104 #endif
105  PyObject* stringio = PyObject_CallObject(cls, NULL);
106  if (!stringio) ThrowPretty("Can't create StringIO object.");
107  Py_DECREF(module);
108  Py_DECREF(cls);
109  return stringio;
110 }
111 
112 #define ROS_MESSAGE_WRAPPER(MessageType) \
113  namespace pybind11 \
114  { \
115  namespace detail \
116  { \
117  template <> \
118  struct type_caster<MessageType> \
119  { \
120  public: \
121  PYBIND11_TYPE_CASTER(MessageType, _("genpy.Message")); \
122  \
123  bool load(handle src, bool) \
124  { \
125  PyObject* stringio = CreateStringIOObject(); \
126  if (!stringio) ThrowPretty("Can't create StringIO instance."); \
127  PyObject* result = \
128  PyObject_CallMethod(src.ptr(), "serialize", "O", stringio); \
129  if (!result) ThrowPretty("Can't serialize."); \
130  result = PyObject_CallMethod(stringio, "getvalue", nullptr); \
131  if (!result) ThrowPretty("Can't get buffer."); \
132  char* data = PyByteArray_AsString(PyByteArray_FromObject(result)); \
133  int len = PyByteArray_Size(result); \
134  unsigned char* udata = new unsigned char[len]; \
135  for (int i = 0; i < len; ++i) \
136  udata[i] = static_cast<unsigned char>(data[i]); \
137  ros::serialization::IStream stream(udata, len); \
138  ros::serialization::deserialize<MessageType>(stream, value); \
139  delete[] udata; \
140  delete[] data; \
141  Py_DECREF(stringio); \
142  Py_DECREF(result); \
143  return !PyErr_Occurred(); \
144  } \
145  \
146  static handle cast(MessageType src, \
147  return_value_policy /* policy /, handle / parent */) \
148  { \
149  ros::message_traits::DataType<MessageType::Type> type; \
150  ThrowPretty("Can't create python object from message of type '" \
151  << type.value() << "'!"); \
152  } \
153  }; \
154  } \
155  }
156 #include <moveit_msgs/PlanningScene.h>
157 #include <moveit_msgs/PlanningSceneWorld.h>
158 ROS_MESSAGE_WRAPPER(moveit_msgs::PlanningScene);
159 ROS_MESSAGE_WRAPPER(moveit_msgs::PlanningSceneWorld);
160 
162 {
163  return Initializer(init);
164 }
165 
166 std::pair<Initializer, Initializer> LoadFromXML(std::string file_name, const std::string& solver_name = "", const std::string& problem_name = "", bool parsePathAsXML = false)
167 {
168  Initializer solver, problem;
169  XMLLoader::Load(file_name, solver, problem, solver_name, problem_name, parsePathAsXML);
170  return std::pair<Initializer, Initializer>(solver, problem);
171 }
172 
173 void AddInitializers(py::module& module)
174 {
175  py::module inits = module.def_submodule("Initializers", "Initializers for core EXOTica classes.");
176  inits.def("Initializer", &CreateInitializer);
177  std::vector<Initializer> initializers = Setup::GetInitializers();
178  for (const Initializer& i : initializers)
179  {
180  const std::string full_name = i.GetName();
181  const std::string name = full_name.substr(8); // This removes the prefix "exotica/"
182  known_initializers[full_name] = CreateInitializer(i);
183  inits.def((name + "Initializer").c_str(), [full_name]() { return CreateInitializer(known_initializers[full_name]); }, (name + "Initializer constructor.").c_str());
184  }
185 
186  inits.def("load_xml", (Initializer(*)(std::string, bool)) & XMLLoader::Load, "Loads initializer from XML", py::arg("xml"), py::arg("parseAsXMLString") = false);
187  inits.def("load_xml_full", &LoadFromXML, "Loads initializer from XML", py::arg("xml"), py::arg("solver_name") = std::string(""), py::arg("problem_name") = std::string(""), py::arg("parseAsXMLString") = false);
188 }
189 
190 namespace pybind11
191 {
192 namespace detail
193 {
194 template <>
195 struct type_caster<Initializer>
196 {
197 public:
198  PYBIND11_TYPE_CASTER(Initializer, _("Initializer"));
199 
200  bool AddPropertyFromDict(Property& target, PyObject* value_py)
201  {
202  if (target.GetType() == "std::string" || target.GetType() == GetTypeName(typeid(std::string)))
203  {
204  target.Set(PyAsStdString(value_py));
205  return true;
206  }
207  else if (target.GetType() == "int")
208  {
209  if (IsPyString(value_py))
210  {
211  target.Set(ParseInt(PyAsStdString(value_py)));
212  return true;
213  }
214  else if (PyInt_Check(value_py) || PyLong_Check(value_py))
215  {
216  target.Set((int)PyInt_AsLong(value_py));
217  return true;
218  }
219  }
220  else if (target.GetType() == "long")
221  {
222  if (IsPyString(value_py))
223  {
224  target.Set((long)ParseInt(PyAsStdString(value_py)));
225  return true;
226  }
227  else if (PyInt_Check(value_py))
228  {
229  target.Set(PyInt_AsLong(value_py));
230  return true;
231  }
232  else
233  {
234  ThrowPretty("to be implemented - please open an issue.");
235  }
236  }
237  else if (target.GetType() == "double")
238  {
239  if (IsPyString(value_py))
240  {
241  target.Set(ParseDouble(PyAsStdString(value_py)));
242  return true;
243  }
244  else if (PyFloat_Check(value_py))
245  {
246  target.Set(PyFloat_AsDouble(value_py));
247  return true;
248  }
249  }
250  else if (target.GetType() == "Eigen::Matrix<double, -1, 1, 0, -1, 1>")
251  {
252  if (IsPyString(value_py))
253  {
254  target.Set(ParseVector<double, Eigen::Dynamic>(PyAsStdString(value_py)));
255  }
256  else
257  {
258  target.Set(py::cast<Eigen::VectorXd>(value_py));
259  }
260  return true;
261  }
262  else if (target.GetType() == "Eigen::Matrix<int, -1, 1, 0, -1, 1>")
263  {
264  if (IsPyString(value_py))
265  {
266  target.Set(ParseVector<int, Eigen::Dynamic>(PyAsStdString(value_py)));
267  }
268  else
269  {
270  target.Set(py::cast<Eigen::VectorXi>(value_py));
271  }
272  return true;
273  }
274  else if (target.GetType() == "Eigen::Matrix<double, 2, 1, 0, 2, 1>")
275  {
276  if (IsPyString(value_py))
277  {
278  target.Set(ParseVector<double, 2>(PyAsStdString(value_py)));
279  }
280  else
281  {
282  target.Set(py::cast<Eigen::Vector2d>(value_py));
283  }
284  return true;
285  }
286  else if (target.GetType() == "Eigen::Matrix<double, 3, 1, 0, 3, 1>")
287  {
288  if (IsPyString(value_py))
289  {
290  target.Set(ParseVector<double, 3>(PyAsStdString(value_py)));
291  }
292  else
293  {
294  target.Set(py::cast<Eigen::Vector3d>(value_py));
295  }
296  return true;
297  }
298  else if (target.GetType() == GetTypeName(typeid(std::vector<int>)))
299  {
300  if (IsPyString(value_py))
301  {
302  target.Set(ParseIntList(PyAsStdString(value_py)));
303  }
304  else
305  {
306  target.Set(py::cast<std::vector<int>>(value_py));
307  }
308  return true;
309  }
310  else if (target.GetType() == GetTypeName(typeid(std::vector<std::string>)))
311  {
312  if (IsPyString(value_py))
313  {
314  target.Set(ParseList(PyAsStdString(value_py)));
315  }
316  else
317  {
318  target.Set(py::cast<std::vector<std::string>>(value_py));
319  }
320  return true;
321  }
322  else if (target.GetType() == "bool")
323  {
324  if (IsPyString(value_py))
325  {
326  target.Set(ParseBool(PyAsStdString(value_py)));
327  return true;
328  }
329  else if (PyBool_Check(value_py))
330  {
331  target.Set(PyObject_IsTrue(value_py) == 1);
332  return true;
333  }
334  }
335  else if (target.GetType() == "exotica::Initializer")
336  {
337  if (PyList_Check(value_py))
338  {
339  Initializer tmp;
340  int n = PyList_Size(value_py);
341  if (n == 1)
342  {
343  if (!PyToInit(PyList_GetItem(value_py, 0), tmp))
344  {
345  WARNING("Could not create initializer :'(");
346  return false;
347  }
348  }
349  else
350  {
351  WARNING("List size is greater than 1 - this should not happen: " << n);
352  }
353  target.Set(tmp);
354  }
355  else
356  {
357  Initializer tmp;
358  if (!PyToInit(value_py, tmp))
359  {
360  WARNING("Could not convert Python value to exotica::Initializer");
361  return false;
362  }
363  target.Set(tmp);
364  }
365  return true;
366  }
367  else if (target.IsInitializerVectorType())
368  {
369  if (PyList_Check(value_py))
370  {
371  int n = PyList_Size(value_py);
372  std::vector<Initializer> vec(n);
373  for (int i = 0; i < n; ++i)
374  {
375  if (!PyToInit(PyList_GetItem(value_py, i), vec[i]))
376  {
377  WARNING("Could not parse initializer in vector of initializers: #" << i);
378  return false;
379  }
380  }
381  target.Set(vec);
382  return true;
383  }
384  else
385  {
386  HIGHLIGHT("InitializerVectorType failed PyList_Check");
387  }
388  }
389  else
390  {
391  HIGHLIGHT("Skipping unsupported type '" << target.GetType() << "'");
392  }
393 
394  return false;
395  }
396 
397  bool PyToInit(PyObject* source, Initializer& ret)
398  {
399  if (!PyTuple_CheckExact(source))
400  {
401  WARNING_NAMED("PyToInit", "Failed exact tuple check.");
402  return false;
403  }
404 
405  int tuple_size = PyTuple_Size(source);
406  if (tuple_size < 1 || tuple_size > 2)
407  {
408  WARNING_NAMED("PyToInit", "Wrong sized tuple for exotica::Initializer: " << tuple_size);
409  return false;
410  }
411 
412  PyObject* const name_py = PyTuple_GetItem(source, 0);
413  if (!IsPyString(name_py))
414  {
415  WARNING_NAMED("PyToInit", "First element of exotica::Initializer-tuple is not a string.");
416  return false;
417  }
418  const std::string initializer_name = PyAsStdString(name_py);
419 
420  const auto& it = known_initializers.find(initializer_name);
421  if (it == known_initializers.end())
422  {
423  HIGHLIGHT("Unknown initializer type '" << initializer_name << "'");
424  return false;
425  }
426  ret = Initializer(it->second);
427 
428  if (tuple_size == 2)
429  {
430  PyObject* const dict = PyTuple_GetItem(source, 1);
431  if (!PyDict_Check(dict))
432  {
433  WARNING_NAMED("PyToInit", "Second element of exotica::Initializer-tuple is not a dict.");
434  return false;
435  }
436 
437  PyObject *key, *value_py;
438  Py_ssize_t pos = 0;
439 
440  while (PyDict_Next(dict, &pos, &key, &value_py))
441  {
442  const std::string key_str = PyAsStdString(key);
443 
444  if (ret.properties_.count(key_str))
445  {
446  if (!AddPropertyFromDict(ret.properties_.at(key_str), value_py))
447  {
448  HIGHLIGHT("Failed to add property '" << key_str << "'");
449  return false;
450  }
451  }
452  else
453  {
454  // 2020-11-04: Replaced the ignoring behaviour with a warning that still adds the property to the initializer.
455  // This resolves issue #719: Sometimes (e.g. for SphereCollision ), we do casting to derived types inside a TaskMap.
456  // This requires having the properties added to the generic Initializer, even if the base initializer does not contain them.
457  // HIGHLIGHT(initializer_name << ": Ignoring property '" << key_str << "'")
458  ret.AddProperty(Property(key_str, false, boost::any(PyAsStdString(value_py))));
459  // WARNING("Adding property '" << key_str << "' even though Initializer type '" << initializer_name << "' does not know this property.");
460  }
461  }
462  }
463  return true;
464  }
465 
466  bool load(handle src, bool)
467  {
468  return PyToInit(src.ptr(), value);
469  }
470 
471  static PyObject* InitializerToTuple(const Initializer& src)
472  {
473  PyObject* dict = PyDict_New();
474  for (auto& prop : src.properties_)
475  {
476  addPropertyToDict(dict, prop.first, prop.second);
477  }
478  PyObject* name = StdStringAsPy(src.GetName());
479  PyObject* tup = PyTuple_Pack(2, name, dict);
480  Py_DECREF(dict);
481  Py_DECREF(name);
482  return tup;
483  }
484 
485  static void addPropertyToDict(PyObject* dict, const std::string& name, const Property& prop)
486  {
487  if (prop.GetType() == "std::string" || prop.GetType() == GetTypeName(typeid(std::string)))
488  {
489  PyDict_SetItemString(dict, name.c_str(), py::cast(boost::any_cast<std::string>(prop.Get())).ptr());
490  }
491  else if (prop.GetType() == "int")
492  {
493  PyDict_SetItemString(dict, name.c_str(), py::cast(boost::any_cast<int>(prop.Get())).ptr());
494  }
495  else if (prop.GetType() == "long")
496  {
497  PyDict_SetItemString(dict, name.c_str(), py::cast(boost::any_cast<long>(prop.Get())).ptr());
498  }
499  else if (prop.GetType() == "double")
500  {
501  PyDict_SetItemString(dict, name.c_str(), py::cast(boost::any_cast<double>(prop.Get())).ptr());
502  }
503  else if (prop.GetType() == "Eigen::Matrix<double, -1, 1, 0, -1, 1>")
504  {
505  PyDict_SetItemString(dict, name.c_str(), py::cast(boost::any_cast<Eigen::VectorXd>(prop.Get())).ptr());
506  }
507  else if (prop.GetType() == "Eigen::Matrix<int, -1, 1, 0, -1, 1>")
508  {
509  PyDict_SetItemString(dict, name.c_str(), py::cast(boost::any_cast<Eigen::VectorXi>(prop.Get())).ptr());
510  }
511  else if (prop.GetType() == "Eigen::Matrix<double, 2, 1, 0, 2, 1>")
512  {
513  PyDict_SetItemString(dict, name.c_str(), py::cast(boost::any_cast<Eigen::Vector2d>(prop.Get())).ptr());
514  }
515  else if (prop.GetType() == "Eigen::Matrix<double, 3, 1, 0, 3, 1>")
516  {
517  PyDict_SetItemString(dict, name.c_str(), py::cast(boost::any_cast<Eigen::Vector3d>(prop.Get())).ptr());
518  }
519  else if (prop.GetType() == GetTypeName(typeid(std::vector<int>)))
520  {
521  PyDict_SetItemString(dict, name.c_str(), py::cast(boost::any_cast<std::vector<int>>(prop.Get())).ptr());
522  }
523  else if (prop.GetType() == GetTypeName(typeid(std::vector<std::string>)))
524  {
525  PyDict_SetItemString(dict, name.c_str(), py::cast(boost::any_cast<std::vector<std::string>>(prop.Get())).ptr());
526  }
527  else if (prop.GetType() == "bool")
528  {
529  PyDict_SetItemString(dict, name.c_str(), py::cast(boost::any_cast<bool>(prop.Get())).ptr());
530  }
531  else if (prop.GetType() == "exotica::Initializer")
532  {
533  PyObject* init = InitializerToTuple(boost::any_cast<Initializer>(prop.Get()));
534  PyDict_SetItemString(dict, name.c_str(), init);
535  Py_DECREF(init);
536  }
537  else if (prop.IsInitializerVectorType())
538  {
539  PyObject* vec = PyList_New(0);
540  for (Initializer& i : boost::any_cast<std::vector<Initializer>>(prop.Get()))
541  {
542  PyObject* init = InitializerToTuple(i);
543  PyList_Append(vec, init);
544  Py_DECREF(init);
545  }
546  PyDict_SetItemString(dict, name.c_str(), vec);
547  Py_DECREF(vec);
548  }
549  else
550  {
551  HIGHLIGHT("Skipping unsupported type '" << prop.GetType() << "'");
552  }
553  }
554 
555  static handle cast(Initializer src, return_value_policy /* policy */, handle /* parent */)
556  {
557  return handle(InitializerToTuple(src));
558  }
559 };
560 } // namespace detail
561 } // namespace pybind11
562 
563 template <class T>
565  pybind11::array_t<T> inArray)
566 {
567  // request a buffer descriptor from Python
568  pybind11::buffer_info buffer_info = inArray.request();
569 
570  // extract data an shape of input array
571  T* data = static_cast<T*>(buffer_info.ptr);
572  std::vector<ssize_t> shape = buffer_info.shape;
573 
574  // wrap ndarray in Eigen::Map:
575  // the second template argument is the rank of the tensor and has to be
576  // known at compile time
577  Eigen::TensorMap<Eigen::Tensor<T, 3, Eigen::RowMajor>> in_tensor(
578  data, shape[0], shape[1], shape[2]);
579 
580  Hessian hessian = Hessian::Constant(shape[0], Eigen::MatrixXd::Zero(shape[1], shape[2]));
581  for (int i = 0; i < shape[0]; i++)
582  {
583  for (int j = 0; j < shape[1]; j++)
584  {
585  for (int k = 0; k < shape[2]; k++)
586  {
587  hessian(i)(j, k) = in_tensor(i, j, k);
588  }
589  }
590  }
591  return hessian;
592 }
593 
594 template <class T>
595 py::handle hessian_array(
596  Hessian& inp)
597 {
598  std::vector<ssize_t> shape(3);
599  shape[0] = inp.rows();
600  if (shape[0] > 0)
601  {
602  shape[1] = inp(0).rows();
603  shape[2] = inp(0).cols();
604  }
605  else
606  {
607  shape[1] = shape[2] = 0;
608  }
609 
610  pybind11::array_t<T> array(
611  shape, // shape
612  {shape[1] * shape[2] * sizeof(T), shape[2] * sizeof(T), sizeof(T)}); // stride
613 
614  Eigen::TensorMap<Eigen::Tensor<T, 3, Eigen::RowMajor>> in_tensor(
615  array.mutable_data(), shape[0], shape[1], shape[2]);
616  for (int i = 0; i < shape[0]; i++)
617  {
618  for (int j = 0; j < shape[1]; j++)
619  {
620  for (int k = 0; k < shape[2]; k++)
621  {
622  in_tensor(i, j, k) = inp(i)(j, k);
623  }
624  }
625  }
626  return array.release();
627 }
628 
629 namespace pybind11
630 {
631 namespace detail
632 {
633 template <>
634 struct type_caster<Hessian>
635 {
636 public:
642  PYBIND11_TYPE_CASTER(Hessian, _("Hessian"));
643 
649  bool load(handle src, bool)
650  {
651  value = array_hessian(py::cast<pybind11::array_t<Hessian::Scalar::Scalar>>(src.ptr()));
652  return true;
653  }
654 
662  static handle cast(Hessian src, return_value_policy /* policy */, handle /* parent */)
663  {
664  return hessian_array<Hessian::Scalar::Scalar>(src);
665  }
666 };
667 } // namespace detail
668 } // namespace pybind11
669 
671 {
672  module.doc() = "Exotica Python wrapper";
673 
674  py::class_<Setup, std::unique_ptr<Setup, py::nodelete>> setup(module, "Setup");
675  setup.def(py::init([]() { return Setup::Instance().get(); }));
676  setup.def_static("get_solvers", &Setup::GetSolvers, "Returns a list of available solvers.");
677  setup.def_static("get_problems", &Setup::GetProblems, "Returns a list of available problems.");
678  setup.def_static("get_maps", &Setup::GetMaps, "Returns a list of available task maps.");
679  setup.def_static("get_collision_scenes", &Setup::GetCollisionScenes, "Returns a list of available collision scene plug-ins.");
680  setup.def_static("get_dynamics_solvers", &Setup::GetDynamicsSolvers, "Returns a list of available dynamics solvers plug-ins.");
681  setup.def_static("create_solver", [](const Initializer& init) { return Setup::CreateSolver(init); }, py::return_value_policy::take_ownership); // "Creates an instance of the solver identified by name parameter.", py::arg("solverType"), py::arg("prependExoticaNamespace"));
682  setup.def_static("create_problem", [](const Initializer& init) { return Setup::CreateProblem(init); }, py::return_value_policy::take_ownership); // "Creates an instance of the problem identified by name parameter.", py::arg("problemType"), py::arg("prependExoticaNamespace"));
683  setup.def_static("create_scene", [](const Initializer& init) { return Setup::CreateScene(init); }, py::return_value_policy::take_ownership);
684  setup.def_static("create_dynamics_solver", [](const Initializer& init) { return Setup::CreateDynamicsSolver(init); }, py::return_value_policy::take_ownership);
685  setup.def_static("print_supported_classes", &Setup::PrintSupportedClasses, "Print a list of available plug-ins sorted by class.");
686  setup.def_static("get_initializers", &Setup::GetInitializers, py::return_value_policy::copy, "Returns a list of available initializers with all available parameters/arguments.");
687  setup.def_static("get_package_path", &ros::package::getPath, "ROS package path resolution.");
688  setup.def_static("init_ros",
689  [](const std::string& name, const bool& anonymous) {
690  int argc = 0;
691  if (anonymous)
692  {
693  ros::init(argc, nullptr, name, ros::init_options::AnonymousName);
694  }
695  else
696  {
697  ros::init(argc, nullptr, name);
698  }
699  Server::InitRos(std::make_shared<ros::NodeHandle>("~"));
700  },
701  "Initializes an internal ROS node for publishing debug information from Exotica (i.e., activates ROS features). Options are setting the name and whether to spawn an anonymous node.",
702  py::arg("name") = "exotica", py::arg("anonymous") = false);
703  setup.def_static("load_solver", &XMLLoader::LoadSolver, "Instantiate solver and problem from an XML file containing both a solver and problem initializer.", py::arg("filepath"));
704  setup.def_static("load_solver_standalone", &XMLLoader::LoadSolverStandalone, "Instantiate only a solver from an XML file containing solely a solver initializer.", py::arg("filepath"));
705  setup.def_static("load_problem", &XMLLoader::LoadProblem, "Instantiate only a problem from an XML file containing solely a problem initializer.", py::arg("filepath"));
706 
707  py::module tools = module.def_submodule("Tools");
708  tools.def("parse_path", &ParsePath);
709  tools.def("parse_bool", &ParseBool);
710  tools.def("parse_double", &ParseDouble);
711  tools.def("parse_vector", &ParseVector<double, Eigen::Dynamic>);
712  tools.def("parse_list", &ParseList);
713  tools.def("parse_int", &ParseInt);
714  tools.def("parse_int_list", &ParseIntList);
715  tools.def("load_obj", [](const std::string& path) { Eigen::VectorXi tri; Eigen::VectorXd vert; LoadOBJ(LoadFile(path), tri, vert); return py::make_tuple(tri, vert); });
716  tools.def("load_octree", &LoadOctree);
717  tools.def("load_octree_as_shape", &LoadOctreeAsShape);
718  tools.def("save_matrix", &SaveMatrix);
719  tools.def("VectorTransform", &Eigen::VectorTransform);
720  tools.def("IdentityTransform", &Eigen::IdentityTransform);
721  tools.def("load_file", &LoadFile);
722  tools.def("path_exists", &PathExists);
723  tools.def("create_composite_trajectory", [](Eigen::MatrixXdRefConst data, double radius) {
724  return Trajectory(data, radius).ToString();
725  },
726  py::arg("data"), py::arg("max_radius") = 1.0);
727 
728  py::module sparse_costs = tools.def_submodule("SparseCosts")
729  .def("huber_cost", &huber_cost)
730  .def("huber_jacobian", &huber_jacobian)
731  .def("huber_hessian", &huber_hessian)
732  .def("smooth_l1_cost", &smooth_l1_cost)
733  .def("smooth_l1_jacobian", &smooth_l1_jacobian)
734  .def("smooth_l1_hessian", &smooth_l1_hessian)
735  .def("pseudo_huber_cost", &pseudo_huber_cost)
736  .def("pseudo_huber_jacobian", &pseudo_huber_jacobian)
737  .def("pseudo_huber_hessian", &pseudo_huber_hessian);
738 
739  py::class_<Timer, std::shared_ptr<Timer>> timer(module, "Timer");
740  timer.def(py::init());
741  timer.def("reset", &Timer::Reset);
742  timer.def("get_duration", &Timer::GetDuration);
743 
744  py::class_<Object, std::shared_ptr<Object>> object(module, "Object");
745  object.def_property_readonly("type", &Object::type, "Object type");
746  object.def_property_readonly("name", &Object::GetObjectName, "Object name");
747  object.def("__repr__", &Object::Print, "String representation of the object", py::arg("prepend") = std::string(""));
748  object.def_readwrite("namespace", &Object::ns_);
749  object.def_readwrite("debug_mode", &Object::debug_);
750 
751  py::enum_<TerminationCriterion>(module, "TerminationCriterion")
752  .value("NotStarted", TerminationCriterion::NotStarted)
753  .value("IterationLimit", TerminationCriterion::IterationLimit)
754  .value("BacktrackIterationLimit", TerminationCriterion::BacktrackIterationLimit)
755  .value("StepTolerance", TerminationCriterion::StepTolerance)
756  .value("FunctionTolerance", TerminationCriterion::FunctionTolerance)
757  .value("GradientTolerance", TerminationCriterion::GradientTolerance)
758  .value("Divergence", TerminationCriterion::Divergence)
759  .value("UserDefined", TerminationCriterion::UserDefined)
760  .value("Convergence", TerminationCriterion::Convergence)
761  .export_values();
762 
763  py::enum_<RotationType>(module, "RotationType")
764  .value("Quaternion", RotationType::QUATERNION)
765  .value("RPY", RotationType::RPY)
766  .value("ZYZ", RotationType::ZYZ)
767  .value("ZYX", RotationType::ZYX)
768  .value("AngleAxis", RotationType::ANGLE_AXIS)
769  .value("Matrix", RotationType::MATRIX)
770  .export_values();
771 
772  py::enum_<BaseType>(module, "BaseType")
773  .value("Fixed", BaseType::FIXED)
774  .value("Floating", BaseType::FLOATING)
775  .value("Planar", BaseType::PLANAR)
776  .export_values();
777 
778  py::class_<KDL::Frame> kdl_frame(module, "KDLFrame");
779  kdl_frame.def(py::init());
780  kdl_frame.def(py::init([](Eigen::MatrixXd other) { return GetFrameFromMatrix(other); }));
781  kdl_frame.def(py::init([](Eigen::VectorXd other) { return GetFrame(other); }));
782  kdl_frame.def(py::init([](const KDL::Frame& other) { return KDL::Frame(other); }));
783  kdl_frame.def("__repr__", [](KDL::Frame* me) { return "KDL::Frame " + ToString(*me); });
784  kdl_frame.def("get_rpy", [](KDL::Frame* me) { return GetRotationAsVector(*me, RotationType::RPY); });
785  kdl_frame.def("get_zyz", [](KDL::Frame* me) { return GetRotationAsVector(*me, RotationType::ZYZ); });
786  kdl_frame.def("get_zyx", [](KDL::Frame* me) { return GetRotationAsVector(*me, RotationType::ZYX); });
787  kdl_frame.def("get_angle_axis", [](KDL::Frame* me) { return GetRotationAsVector(*me, RotationType::ANGLE_AXIS); });
788  kdl_frame.def("get_quaternion", [](KDL::Frame* me) { return GetRotationAsVector(*me, RotationType::QUATERNION); });
789  kdl_frame.def("get_translation", [](KDL::Frame* me) { Eigen::Vector3d tmp; for (int i = 0; i < 3; ++i) { tmp[i] = me->p.data[i]; } return tmp; });
790  kdl_frame.def("get_translation_and_rpy", [](KDL::Frame* me) { return GetFrameAsVector(*me, RotationType::RPY); });
791  kdl_frame.def("get_translation_and_zyz", [](KDL::Frame* me) { return GetFrameAsVector(*me, RotationType::ZYZ); });
792  kdl_frame.def("get_translation_and_zyx", [](KDL::Frame* me) { return GetFrameAsVector(*me, RotationType::ZYX); });
793  kdl_frame.def("get_translation_and_angle_axis", [](KDL::Frame* me) { return GetFrameAsVector(*me, RotationType::ANGLE_AXIS); });
794  kdl_frame.def("get_translation_and_quaternion", [](KDL::Frame* me) { return GetFrameAsVector(*me, RotationType::QUATERNION); });
795  kdl_frame.def("get_frame", [](KDL::Frame* me) { return GetFrame(*me); });
796  kdl_frame.def("inverse", (KDL::Frame(KDL::Frame::*)() const) & KDL::Frame::Inverse);
797  kdl_frame.def("__mul__", [](const KDL::Frame& A, const KDL::Frame& B) { return A * B; }, py::is_operator());
798  kdl_frame.def_readwrite("p", &KDL::Frame::p);
799  kdl_frame.def_static("interpolate", [](KDL::Frame* A, KDL::Frame* B, double alpha) { return KDL::addDelta(*A, KDL::diff(*A, *B) * alpha); });
800  kdl_frame.def_static("diff", [](KDL::Frame* A, KDL::Frame* B) { Eigen::VectorXd ret(6); KDL::Twist t = KDL::diff(*A, *B); for(int i=0; i<6; ++i) ret(i) = t[i]; return ret; });
801  py::implicitly_convertible<Eigen::MatrixXd, KDL::Frame>();
802  py::implicitly_convertible<Eigen::VectorXd, KDL::Frame>();
803 
804  py::class_<KDL::Vector>(module, "KDLVector")
805  .def(py::init())
806  .def(py::init<double, double, double>(),
807  py::arg("x") = 0,
808  py::arg("y") = 0,
809  py::arg("z") = 0)
810  .def("x", [](KDL::Vector& v) -> double& { return v[0]; })
811  .def("y", [](KDL::Vector& v) -> double& { return v[1]; })
812  .def("z", [](KDL::Vector& v) -> double& { return v[2]; })
813  .def_static("Zero", &KDL::Vector::Zero);
814 
815  py::class_<KDL::RotationalInertia>(module, "KDLRotationalInertia")
816  .def(py::init<double, double, double, double, double, double>(),
817  py::arg("Ixx") = 0,
818  py::arg("Iyy") = 0,
819  py::arg("Izz") = 0,
820  py::arg("Ixy") = 0,
821  py::arg("Ixz") = 0,
822  py::arg("Iyz") = 0)
823  .def_static("Zero", &KDL::RotationalInertia::Zero);
824 
825  py::class_<KDL::RigidBodyInertia>(module, "KDLRigidBodyInertia")
826  .def(py::init<double, KDL::Vector&, KDL::RotationalInertia&>(),
827  py::arg("m") = 0,
828  py::arg("oc") = KDL::Vector::Zero(),
829  py::arg("Ic") = KDL::RotationalInertia::Zero())
830  .def_static("Zero", &KDL::RigidBodyInertia::Zero);
831 
832  py::class_<TaskMap, std::shared_ptr<TaskMap>, Object>(module, "TaskMap")
833  .def_readonly("id", &TaskMap::id)
834  .def_readonly("start", &TaskMap::start)
835  .def_readonly("length", &TaskMap::length)
836  .def_readonly("startJ", &TaskMap::start_jacobian)
837  .def_readonly("lengthJ", &TaskMap::length_jacobian)
838  .def("task_space_dim", (int (TaskMap::*)()) & TaskMap::TaskSpaceDim)
839  .def("task_space_jacobian_dim", &TaskMap::TaskSpaceJacobianDim);
840 
841  py::class_<TaskIndexing>(module, "TaskIndexing")
842  .def_readonly("id", &TaskIndexing::id)
843  .def_readonly("start", &TaskIndexing::start)
844  .def_readonly("length", &TaskIndexing::length)
845  .def_readonly("startJ", &TaskIndexing::start_jacobian)
846  .def_readonly("lengthJ", &TaskIndexing::length_jacobian);
847 
848  py::class_<TimeIndexedTask, std::shared_ptr<TimeIndexedTask>>(module, "TimeIndexedTask")
849  .def_readonly("indexing", &TimeIndexedTask::indexing)
850  .def_readonly("length_Phi", &TimeIndexedTask::length_Phi)
851  .def_readonly("length_jacobian", &TimeIndexedTask::length_jacobian)
852  .def_readonly("num_tasks", &TimeIndexedTask::num_tasks)
853  .def_readonly("y", &TimeIndexedTask::y)
854  .def_readonly("ydiff", &TimeIndexedTask::ydiff)
855  .def_readonly("Phi", &TimeIndexedTask::Phi)
856  .def_readonly("rho", &TimeIndexedTask::rho)
857  .def_readonly("hessian", &TimeIndexedTask::hessian) // Kinematic
858  .def_readonly("jacobian", &TimeIndexedTask::jacobian) // Kinematic
859  .def_readonly("dPhi_dx", &TimeIndexedTask::dPhi_dx) // Dynamic
860  .def_readonly("dPhi_du", &TimeIndexedTask::dPhi_du) // Dynamic
861  .def_readonly("ddPhi_ddx", &TimeIndexedTask::ddPhi_ddx) // Dynamic
862  .def_readonly("ddPhi_ddu", &TimeIndexedTask::ddPhi_ddu) // Dynamic
863  .def_readonly("ddPhi_dxdu", &TimeIndexedTask::ddPhi_dxdu) // Dynamic
864  .def_readonly("S", &TimeIndexedTask::S)
865  .def_readonly("T", &TimeIndexedTask::T)
866  .def_readonly("tasks", &TimeIndexedTask::tasks)
867  .def_readonly("task_maps", &TimeIndexedTask::task_maps)
868  .def("set_goal", &TimeIndexedTask::SetGoal)
869  .def("get_goal", &TimeIndexedTask::GetGoal)
870  .def("set_rho", &TimeIndexedTask::SetRho)
871  .def("get_rho", &TimeIndexedTask::GetRho)
872  .def("get_task_error", &TimeIndexedTask::GetTaskError)
873  .def("get_S", &TimeIndexedTask::GetS);
874 
875  py::class_<EndPoseTask, std::shared_ptr<EndPoseTask>>(module, "EndPoseTask")
876  .def_readonly("length_Phi", &EndPoseTask::length_Phi)
877  .def_readonly("length_jacobian", &EndPoseTask::length_jacobian)
878  .def_readonly("num_tasks", &EndPoseTask::num_tasks)
879  .def_readonly("y", &EndPoseTask::y)
880  .def_readonly("ydiff", &EndPoseTask::ydiff)
881  .def_readonly("Phi", &EndPoseTask::Phi)
882  // .def_readonly("hessian", &EndPoseTask::hessian)
883  .def_readonly("jacobian", &EndPoseTask::jacobian)
884  .def_readonly("S", &EndPoseTask::S)
885  .def_readonly("tasks", &EndPoseTask::tasks)
886  .def_readonly("task_maps", &EndPoseTask::task_maps)
887  .def("get_task_error", &EndPoseTask::GetTaskError)
888  .def("set_goal", &EndPoseTask::SetGoal)
889  .def("get_goal", &EndPoseTask::GetGoal)
890  .def("set_rho", &EndPoseTask::SetRho)
891  .def("get_rho", &EndPoseTask::GetRho);
892 
893  py::class_<SamplingTask, std::shared_ptr<SamplingTask>>(module, "SamplingTask")
894  .def_readonly("length_Phi", &SamplingTask::length_Phi)
895  .def_readonly("length_jacobian", &SamplingTask::length_jacobian)
896  .def_readonly("num_tasks", &SamplingTask::num_tasks)
897  .def_readonly("y", &SamplingTask::y)
898  .def_readonly("ydiff", &SamplingTask::ydiff)
899  .def_readonly("Phi", &SamplingTask::Phi)
900  .def_readonly("S", &SamplingTask::S)
901  .def_readonly("tasks", &SamplingTask::tasks)
902  .def_readonly("task_maps", &SamplingTask::task_maps)
903  .def("set_goal", &SamplingTask::SetGoal)
904  .def("get_goal", &SamplingTask::GetGoal)
905  .def("set_rho", &SamplingTask::SetRho)
906  .def("get_rho", &SamplingTask::GetRho);
907 
908  py::class_<TaskSpaceVector, std::shared_ptr<TaskSpaceVector>> task_space_vector(module, "TaskSpaceVector");
909  task_space_vector.def("set_zero", &TaskSpaceVector::SetZero);
910  task_space_vector.def_readonly("data", &TaskSpaceVector::data);
911  task_space_vector.def("__sub__", &TaskSpaceVector::operator-, py::is_operator());
912  task_space_vector.def("__repr__", [](TaskSpaceVector* instance) { return ((std::ostringstream&)(std::ostringstream("") << "TaskSpaceVector (" << instance->data.transpose() << ")")).str(); });
913 
914  py::class_<MotionSolver, std::shared_ptr<MotionSolver>, Object> motion_solver(module, "MotionSolver");
915  motion_solver.def_property("max_iterations", &MotionSolver::GetNumberOfMaxIterations, &MotionSolver::SetNumberOfMaxIterations);
916  motion_solver.def("get_planning_time", &MotionSolver::GetPlanningTime);
917  motion_solver.def("specify_problem", &MotionSolver::SpecifyProblem, "Assign problem to the solver", py::arg("planning_problem"));
918  motion_solver.def(
919  "solve", [](std::shared_ptr<MotionSolver> sol) {
920  Eigen::MatrixXd ret;
921  sol->Solve(ret);
922  return ret;
923  },
924  "Solve the problem");
925  motion_solver.def("get_problem", &MotionSolver::GetProblem);
926 
927  py::class_<FeedbackMotionSolver, std::shared_ptr<FeedbackMotionSolver>, MotionSolver> feedback_motion_solver(module, "FeedbackMotionSolver");
928  feedback_motion_solver.def("get_feedback_control", &FeedbackMotionSolver::GetFeedbackControl);
929 
930  py::class_<PlanningProblem, std::shared_ptr<PlanningProblem>, Object>(module, "PlanningProblem")
931  .def("get_tasks", &PlanningProblem::GetTasks, py::return_value_policy::reference_internal)
932  .def("get_task_maps", &PlanningProblem::GetTaskMaps, py::return_value_policy::reference_internal)
933  .def("get_scene", &PlanningProblem::GetScene, py::return_value_policy::reference_internal)
934  .def("__repr__", &PlanningProblem::Print, "String representation of the object", py::arg("prepend") = std::string(""))
935  .def_readonly("N", &PlanningProblem::N)
936  .def_property_readonly("num_positions", &PlanningProblem::get_num_positions) // deprecated
937  .def_property_readonly("num_velocities", &PlanningProblem::get_num_velocities) // deprecated
938  .def_property_readonly("num_controls", &PlanningProblem::get_num_controls) // deprecated
939  .def_property("start_state", &PlanningProblem::GetStartState, &PlanningProblem::SetStartState)
940  .def_property("start_time", &PlanningProblem::GetStartTime, &PlanningProblem::SetStartTime)
941  .def("get_number_of_problem_updates", &PlanningProblem::GetNumberOfProblemUpdates)
942  .def("reset_number_of_problem_updates", &PlanningProblem::ResetNumberOfProblemUpdates)
943  .def("get_cost_evolution", (std::pair<std::vector<double>, std::vector<double>>(PlanningProblem::*)() const) & PlanningProblem::GetCostEvolution)
944  .def("get_number_of_iterations", &PlanningProblem::GetNumberOfIterations)
945  .def("pre_update", &PlanningProblem::PreUpdate)
946  .def("is_valid", &PlanningProblem::IsValid)
947  .def("apply_start_state", &PlanningProblem::ApplyStartState)
948  .def_readonly("termination_criterion", &PlanningProblem::termination_criterion);
949 
950  // Problem types
951  py::module prob = module.def_submodule("Problems", "Problem types");
952 
953  py::class_<UnconstrainedTimeIndexedProblem, std::shared_ptr<UnconstrainedTimeIndexedProblem>, PlanningProblem> unconstrained_time_indexed_problem(prob, "UnconstrainedTimeIndexedProblem");
954  unconstrained_time_indexed_problem.def("get_duration", &UnconstrainedTimeIndexedProblem::GetDuration);
955  unconstrained_time_indexed_problem.def("update", (void (UnconstrainedTimeIndexedProblem::*)(Eigen::VectorXdRefConst, int)) & UnconstrainedTimeIndexedProblem::Update);
956  unconstrained_time_indexed_problem.def("update", (void (UnconstrainedTimeIndexedProblem::*)(Eigen::VectorXdRefConst)) & UnconstrainedTimeIndexedProblem::Update);
957  unconstrained_time_indexed_problem.def("set_goal", &UnconstrainedTimeIndexedProblem::SetGoal);
958  unconstrained_time_indexed_problem.def("set_rho", &UnconstrainedTimeIndexedProblem::SetRho);
959  unconstrained_time_indexed_problem.def("get_goal", &UnconstrainedTimeIndexedProblem::GetGoal);
960  unconstrained_time_indexed_problem.def("get_rho", &UnconstrainedTimeIndexedProblem::GetRho);
961  unconstrained_time_indexed_problem.def_property("tau", &UnconstrainedTimeIndexedProblem::GetTau, &UnconstrainedTimeIndexedProblem::SetTau);
962  unconstrained_time_indexed_problem.def_readwrite("W", &UnconstrainedTimeIndexedProblem::W);
963  unconstrained_time_indexed_problem.def_property("initial_trajectory", &UnconstrainedTimeIndexedProblem::GetInitialTrajectory, &UnconstrainedTimeIndexedProblem::SetInitialTrajectory);
964  unconstrained_time_indexed_problem.def_property("T", &UnconstrainedTimeIndexedProblem::GetT, &UnconstrainedTimeIndexedProblem::SetT);
965  unconstrained_time_indexed_problem.def_readonly("length_Phi", &UnconstrainedTimeIndexedProblem::length_Phi);
966  unconstrained_time_indexed_problem.def_readonly("length_jacobian", &UnconstrainedTimeIndexedProblem::length_jacobian);
967  unconstrained_time_indexed_problem.def_readonly("num_tasks", &UnconstrainedTimeIndexedProblem::num_tasks);
968  unconstrained_time_indexed_problem.def_readonly("Phi", &UnconstrainedTimeIndexedProblem::Phi);
969  unconstrained_time_indexed_problem.def_readonly("jacobian", &UnconstrainedTimeIndexedProblem::jacobian);
970  unconstrained_time_indexed_problem.def("get_scalar_task_cost", &UnconstrainedTimeIndexedProblem::GetScalarTaskCost);
971  unconstrained_time_indexed_problem.def("get_scalar_task_jacobian", &UnconstrainedTimeIndexedProblem::GetScalarTaskJacobian);
972  unconstrained_time_indexed_problem.def("get_scalar_transition_cost", &UnconstrainedTimeIndexedProblem::GetScalarTransitionCost);
973  unconstrained_time_indexed_problem.def("get_scalar_transition_jacobian", &UnconstrainedTimeIndexedProblem::GetScalarTransitionJacobian);
974  unconstrained_time_indexed_problem.def_readonly("cost", &UnconstrainedTimeIndexedProblem::cost);
975 
976  py::class_<TimeIndexedProblem, std::shared_ptr<TimeIndexedProblem>, PlanningProblem> time_indexed_problem(prob, "TimeIndexedProblem");
977  time_indexed_problem.def("get_duration", &TimeIndexedProblem::GetDuration);
978  time_indexed_problem.def("update", (void (TimeIndexedProblem::*)(Eigen::VectorXdRefConst, int)) & TimeIndexedProblem::Update);
979  time_indexed_problem.def("update", (void (TimeIndexedProblem::*)(Eigen::VectorXdRefConst)) & TimeIndexedProblem::Update);
980  time_indexed_problem.def("set_goal", &TimeIndexedProblem::SetGoal);
981  time_indexed_problem.def("set_rho", &TimeIndexedProblem::SetRho);
982  time_indexed_problem.def("get_goal", &TimeIndexedProblem::GetGoal);
983  time_indexed_problem.def("get_rho", &TimeIndexedProblem::GetRho);
984  time_indexed_problem.def("set_goal_eq", &TimeIndexedProblem::SetGoalEQ);
985  time_indexed_problem.def("set_rho_eq", &TimeIndexedProblem::SetRhoEQ);
986  time_indexed_problem.def("get_goal_eq", &TimeIndexedProblem::GetGoalEQ);
987  time_indexed_problem.def("get_rho_eq", &TimeIndexedProblem::GetRhoEQ);
988  time_indexed_problem.def("set_goal_neq", &TimeIndexedProblem::SetGoalNEQ);
989  time_indexed_problem.def("set_rho_neq", &TimeIndexedProblem::SetRhoNEQ);
990  time_indexed_problem.def("get_goal_neq", &TimeIndexedProblem::GetGoalNEQ);
991  time_indexed_problem.def("get_rho_neq", &TimeIndexedProblem::GetRhoNEQ);
992  time_indexed_problem.def_property("tau", &TimeIndexedProblem::GetTau, &TimeIndexedProblem::SetTau);
993  time_indexed_problem.def_property("q_dot_max", &TimeIndexedProblem::GetJointVelocityLimits, &TimeIndexedProblem::SetJointVelocityLimits);
994  time_indexed_problem.def_readwrite("W", &TimeIndexedProblem::W);
995  time_indexed_problem.def_readwrite("use_bounds", &TimeIndexedProblem::use_bounds);
996  time_indexed_problem.def_property("initial_trajectory", &TimeIndexedProblem::GetInitialTrajectory, &TimeIndexedProblem::SetInitialTrajectory);
997  time_indexed_problem.def_property("T", &TimeIndexedProblem::GetT, &TimeIndexedProblem::SetT);
998  time_indexed_problem.def_readonly("length_Phi", &TimeIndexedProblem::length_Phi);
999  time_indexed_problem.def_readonly("length_jacobian", &TimeIndexedProblem::length_jacobian);
1000  time_indexed_problem.def_readonly("num_tasks", &TimeIndexedProblem::num_tasks);
1001  time_indexed_problem.def_readonly("Phi", &TimeIndexedProblem::Phi);
1002  time_indexed_problem.def_readonly("jacobian", &TimeIndexedProblem::jacobian);
1003  time_indexed_problem.def("get_cost", &TimeIndexedProblem::GetCost);
1004  time_indexed_problem.def("get_cost_jacobian", &TimeIndexedProblem::GetCostJacobian);
1005  time_indexed_problem.def("get_scalar_task_cost", &TimeIndexedProblem::GetScalarTaskCost);
1006  time_indexed_problem.def("get_scalar_task_jacobian", &TimeIndexedProblem::GetScalarTaskJacobian);
1007  time_indexed_problem.def("get_scalar_transition_cost", &TimeIndexedProblem::GetScalarTransitionCost);
1008  time_indexed_problem.def("get_scalar_transition_jacobian", &TimeIndexedProblem::GetScalarTransitionJacobian);
1009  time_indexed_problem.def("get_equality", (Eigen::VectorXd(TimeIndexedProblem::*)() const) & TimeIndexedProblem::GetEquality);
1010  time_indexed_problem.def("get_equality", (Eigen::VectorXd(TimeIndexedProblem::*)(int) const) & TimeIndexedProblem::GetEquality);
1011  time_indexed_problem.def("get_equality_jacobian", (Eigen::SparseMatrix<double>(TimeIndexedProblem::*)() const) & TimeIndexedProblem::GetEqualityJacobian);
1012  time_indexed_problem.def("get_equality_jacobian", (Eigen::MatrixXd(TimeIndexedProblem::*)(int) const) & TimeIndexedProblem::GetEqualityJacobian);
1013  time_indexed_problem.def("get_inequality", (Eigen::VectorXd(TimeIndexedProblem::*)() const) & TimeIndexedProblem::GetInequality);
1014  time_indexed_problem.def("get_inequality", (Eigen::VectorXd(TimeIndexedProblem::*)(int) const) & TimeIndexedProblem::GetInequality);
1015  time_indexed_problem.def("get_inequality_jacobian", (Eigen::SparseMatrix<double>(TimeIndexedProblem::*)() const) & TimeIndexedProblem::GetInequalityJacobian);
1016  time_indexed_problem.def("get_inequality_jacobian", (Eigen::MatrixXd(TimeIndexedProblem::*)(int) const) & TimeIndexedProblem::GetInequalityJacobian);
1017  time_indexed_problem.def("get_bounds", &TimeIndexedProblem::GetBounds);
1018  time_indexed_problem.def("get_joint_velocity_limits", &TimeIndexedProblem::GetJointVelocityLimits);
1019  time_indexed_problem.def_readonly("cost", &TimeIndexedProblem::cost);
1020  time_indexed_problem.def_readonly("inequality", &TimeIndexedProblem::inequality);
1021  time_indexed_problem.def_readonly("equality", &TimeIndexedProblem::equality);
1022 
1023  py::class_<BoundedTimeIndexedProblem, std::shared_ptr<BoundedTimeIndexedProblem>, PlanningProblem> bounded_time_indexed_problem(prob, "BoundedTimeIndexedProblem");
1024  bounded_time_indexed_problem.def("get_duration", &BoundedTimeIndexedProblem::GetDuration);
1025  bounded_time_indexed_problem.def("update", (void (BoundedTimeIndexedProblem::*)(Eigen::VectorXdRefConst, int)) & BoundedTimeIndexedProblem::Update);
1026  bounded_time_indexed_problem.def("update", (void (BoundedTimeIndexedProblem::*)(Eigen::VectorXdRefConst)) & BoundedTimeIndexedProblem::Update);
1027  bounded_time_indexed_problem.def("set_goal", &BoundedTimeIndexedProblem::SetGoal);
1028  bounded_time_indexed_problem.def("set_rho", &BoundedTimeIndexedProblem::SetRho);
1029  bounded_time_indexed_problem.def("get_goal", &BoundedTimeIndexedProblem::GetGoal);
1030  bounded_time_indexed_problem.def("get_rho", &BoundedTimeIndexedProblem::GetRho);
1031  bounded_time_indexed_problem.def_property("tau", &BoundedTimeIndexedProblem::GetTau, &BoundedTimeIndexedProblem::SetTau);
1032  bounded_time_indexed_problem.def_readwrite("W", &BoundedTimeIndexedProblem::W);
1033  bounded_time_indexed_problem.def_property("initial_trajectory", &BoundedTimeIndexedProblem::GetInitialTrajectory, &BoundedTimeIndexedProblem::SetInitialTrajectory);
1034  bounded_time_indexed_problem.def_property("T", &BoundedTimeIndexedProblem::GetT, &BoundedTimeIndexedProblem::SetT);
1035  bounded_time_indexed_problem.def_readonly("length_Phi", &BoundedTimeIndexedProblem::length_Phi);
1036  bounded_time_indexed_problem.def_readonly("length_jacobian", &BoundedTimeIndexedProblem::length_jacobian);
1037  bounded_time_indexed_problem.def_readonly("num_tasks", &BoundedTimeIndexedProblem::num_tasks);
1038  bounded_time_indexed_problem.def_readonly("Phi", &BoundedTimeIndexedProblem::Phi);
1039  bounded_time_indexed_problem.def_readonly("jacobian", &BoundedTimeIndexedProblem::jacobian);
1040  bounded_time_indexed_problem.def("get_scalar_task_cost", &BoundedTimeIndexedProblem::GetScalarTaskCost);
1041  bounded_time_indexed_problem.def("get_scalar_task_jacobian", &BoundedTimeIndexedProblem::GetScalarTaskJacobian);
1042  bounded_time_indexed_problem.def("get_scalar_transition_cost", &BoundedTimeIndexedProblem::GetScalarTransitionCost);
1043  bounded_time_indexed_problem.def("get_scalar_transition_jacobian", &BoundedTimeIndexedProblem::GetScalarTransitionJacobian);
1044  bounded_time_indexed_problem.def("get_bounds", &BoundedTimeIndexedProblem::GetBounds);
1045  bounded_time_indexed_problem.def_readonly("cost", &BoundedTimeIndexedProblem::cost);
1046 
1047  py::class_<UnconstrainedEndPoseProblem, std::shared_ptr<UnconstrainedEndPoseProblem>, PlanningProblem> unconstrained_end_pose_problem(prob, "UnconstrainedEndPoseProblem");
1048  unconstrained_end_pose_problem.def("update", &UnconstrainedEndPoseProblem::Update);
1049  unconstrained_end_pose_problem.def("set_goal", &UnconstrainedEndPoseProblem::SetGoal);
1050  unconstrained_end_pose_problem.def("set_rho", &UnconstrainedEndPoseProblem::SetRho);
1051  unconstrained_end_pose_problem.def("get_goal", &UnconstrainedEndPoseProblem::GetGoal);
1052  unconstrained_end_pose_problem.def("get_rho", &UnconstrainedEndPoseProblem::GetRho);
1053  unconstrained_end_pose_problem.def_readwrite("W", &UnconstrainedEndPoseProblem::W);
1054  unconstrained_end_pose_problem.def_readonly("length_Phi", &UnconstrainedEndPoseProblem::length_Phi);
1055  unconstrained_end_pose_problem.def_readonly("length_jacobian", &UnconstrainedEndPoseProblem::length_jacobian);
1056  unconstrained_end_pose_problem.def_readonly("num_tasks", &UnconstrainedEndPoseProblem::num_tasks);
1057  unconstrained_end_pose_problem.def_readonly("Phi", &UnconstrainedEndPoseProblem::Phi);
1058  unconstrained_end_pose_problem.def_readonly("jacobian", &UnconstrainedEndPoseProblem::jacobian);
1059  unconstrained_end_pose_problem.def_property_readonly("ydiff", [](UnconstrainedEndPoseProblem* prob) { return prob->cost.ydiff; });
1060  unconstrained_end_pose_problem.def_property("q_nominal", &UnconstrainedEndPoseProblem::GetNominalPose, &UnconstrainedEndPoseProblem::SetNominalPose);
1061  unconstrained_end_pose_problem.def("get_scalar_cost", &UnconstrainedEndPoseProblem::GetScalarCost);
1062  unconstrained_end_pose_problem.def("get_scalar_jacobian", &UnconstrainedEndPoseProblem::GetScalarJacobian);
1063  unconstrained_end_pose_problem.def("get_scalar_task_cost", &UnconstrainedEndPoseProblem::GetScalarTaskCost);
1064  unconstrained_end_pose_problem.def_readonly("cost", &UnconstrainedEndPoseProblem::cost);
1065 
1066  py::class_<EndPoseProblem, std::shared_ptr<EndPoseProblem>, PlanningProblem> end_pose_problem(prob, "EndPoseProblem");
1067  end_pose_problem.def("update", &EndPoseProblem::Update);
1068  end_pose_problem.def("pre_update", &EndPoseProblem::PreUpdate);
1069  end_pose_problem.def("set_goal", &EndPoseProblem::SetGoal);
1070  end_pose_problem.def("set_rho", &EndPoseProblem::SetRho);
1071  end_pose_problem.def("get_goal", &EndPoseProblem::GetGoal);
1072  end_pose_problem.def("get_rho", &EndPoseProblem::GetRho);
1073  end_pose_problem.def("set_goal_eq", &EndPoseProblem::SetGoalEQ);
1074  end_pose_problem.def("set_rho_eq", &EndPoseProblem::SetRhoEQ);
1075  end_pose_problem.def("get_goal_eq", &EndPoseProblem::GetGoalEQ);
1076  end_pose_problem.def("get_rho_eq", &EndPoseProblem::GetRhoEQ);
1077  end_pose_problem.def("set_goal_neq", &EndPoseProblem::SetGoalNEQ);
1078  end_pose_problem.def("set_rho_neq", &EndPoseProblem::SetRhoNEQ);
1079  end_pose_problem.def("get_goal_neq", &EndPoseProblem::GetGoalNEQ);
1080  end_pose_problem.def("get_rho_neq", &EndPoseProblem::GetRhoNEQ);
1081  end_pose_problem.def_readwrite("W", &EndPoseProblem::W);
1082  end_pose_problem.def_readwrite("use_bounds", &EndPoseProblem::use_bounds);
1083  end_pose_problem.def_readonly("length_Phi", &EndPoseProblem::length_Phi);
1084  end_pose_problem.def_readonly("length_jacobian", &EndPoseProblem::length_jacobian);
1085  end_pose_problem.def_readonly("num_tasks", &EndPoseProblem::num_tasks);
1086  end_pose_problem.def_readonly("Phi", &EndPoseProblem::Phi);
1087  end_pose_problem.def_readonly("jacobian", &EndPoseProblem::jacobian);
1088  end_pose_problem.def("get_scalar_cost", &EndPoseProblem::GetScalarCost);
1089  end_pose_problem.def("get_scalar_jacobian", &EndPoseProblem::GetScalarJacobian);
1090  end_pose_problem.def("get_scalar_task_cost", &EndPoseProblem::GetScalarTaskCost);
1091  end_pose_problem.def("get_equality", &EndPoseProblem::GetEquality);
1092  end_pose_problem.def("get_equality_jacobian", &EndPoseProblem::GetEqualityJacobian);
1093  end_pose_problem.def("get_inequality", &EndPoseProblem::GetInequality);
1094  end_pose_problem.def("get_inequality_jacobian", &EndPoseProblem::GetInequalityJacobian);
1095  end_pose_problem.def("get_bounds", &EndPoseProblem::GetBounds);
1096  end_pose_problem.def_readonly("cost", &EndPoseProblem::cost);
1097  end_pose_problem.def_readonly("inequality", &EndPoseProblem::inequality);
1098  end_pose_problem.def_readonly("equality", &EndPoseProblem::equality);
1099 
1100  py::class_<BoundedEndPoseProblem, std::shared_ptr<BoundedEndPoseProblem>, PlanningProblem> bounded_end_pose_problem(prob, "BoundedEndPoseProblem");
1101  bounded_end_pose_problem.def("update", &BoundedEndPoseProblem::Update);
1102  bounded_end_pose_problem.def("set_goal", &BoundedEndPoseProblem::SetGoal);
1103  bounded_end_pose_problem.def("set_rho", &BoundedEndPoseProblem::SetRho);
1104  bounded_end_pose_problem.def("get_goal", &BoundedEndPoseProblem::GetGoal);
1105  bounded_end_pose_problem.def("get_rho", &BoundedEndPoseProblem::GetRho);
1106  bounded_end_pose_problem.def_readwrite("W", &BoundedEndPoseProblem::W);
1107  bounded_end_pose_problem.def_readonly("length_Phi", &BoundedEndPoseProblem::length_Phi);
1108  bounded_end_pose_problem.def_readonly("length_jacobian", &BoundedEndPoseProblem::length_jacobian);
1109  bounded_end_pose_problem.def_readonly("num_tasks", &BoundedEndPoseProblem::num_tasks);
1110  bounded_end_pose_problem.def_readonly("Phi", &BoundedEndPoseProblem::Phi);
1111  bounded_end_pose_problem.def_readonly("jacobian", &BoundedEndPoseProblem::jacobian);
1112  bounded_end_pose_problem.def("get_scalar_cost", &BoundedEndPoseProblem::GetScalarCost);
1113  bounded_end_pose_problem.def("get_scalar_jacobian", &BoundedEndPoseProblem::GetScalarJacobian);
1114  bounded_end_pose_problem.def("get_scalar_task_cost", &BoundedEndPoseProblem::GetScalarTaskCost);
1115  bounded_end_pose_problem.def("get_bounds", &BoundedEndPoseProblem::GetBounds);
1116  bounded_end_pose_problem.def_readonly("cost", &BoundedEndPoseProblem::cost);
1117 
1118  py::class_<SamplingProblem, std::shared_ptr<SamplingProblem>, PlanningProblem> sampling_problem(prob, "SamplingProblem");
1119  sampling_problem.def("update", &SamplingProblem::Update);
1120  sampling_problem.def_property("goal_state", &SamplingProblem::GetGoalState, &SamplingProblem::SetGoalState);
1121  sampling_problem.def("get_space_dim", &SamplingProblem::GetSpaceDim);
1122  sampling_problem.def("get_bounds", &SamplingProblem::GetBounds);
1123  sampling_problem.def_readonly("num_tasks", &SamplingProblem::num_tasks);
1124  sampling_problem.def_readonly("Phi", &SamplingProblem::Phi);
1125  sampling_problem.def_readonly("inequality", &SamplingProblem::inequality);
1126  sampling_problem.def_readonly("equality", &SamplingProblem::equality);
1127  sampling_problem.def("set_goal_eq", &SamplingProblem::SetGoalEQ);
1128  sampling_problem.def("set_rho_eq", &SamplingProblem::SetRhoEQ);
1129  sampling_problem.def("get_goal_eq", &SamplingProblem::GetGoalEQ);
1130  sampling_problem.def("get_rho_eq", &SamplingProblem::GetRhoEQ);
1131  sampling_problem.def("set_goal_neq", &SamplingProblem::SetGoalNEQ);
1132  sampling_problem.def("set_rho_neq", &SamplingProblem::SetRhoNEQ);
1133  sampling_problem.def("get_goal_neq", &SamplingProblem::GetGoalNEQ);
1134  sampling_problem.def("get_rho_neq", &SamplingProblem::GetRhoNEQ);
1135 
1136  py::class_<TimeIndexedSamplingProblem, std::shared_ptr<TimeIndexedSamplingProblem>, PlanningProblem> time_indexed_sampling_problem(prob, "TimeIndexedSamplingProblem");
1137  time_indexed_sampling_problem.def("update", &TimeIndexedSamplingProblem::Update);
1138  time_indexed_sampling_problem.def("get_space_dim", &TimeIndexedSamplingProblem::GetSpaceDim);
1139  time_indexed_sampling_problem.def("get_bounds", &TimeIndexedSamplingProblem::GetBounds);
1140  time_indexed_sampling_problem.def_property("goal_state", &TimeIndexedSamplingProblem::GetGoalState, &TimeIndexedSamplingProblem::SetGoalState);
1141  time_indexed_sampling_problem.def_property("goal_time", &TimeIndexedSamplingProblem::GetGoalTime, &TimeIndexedSamplingProblem::SetGoalTime);
1142  time_indexed_sampling_problem.def_readonly("num_tasks", &TimeIndexedSamplingProblem::num_tasks);
1143  time_indexed_sampling_problem.def_readonly("Phi", &TimeIndexedSamplingProblem::Phi);
1144  time_indexed_sampling_problem.def_readonly("inequality", &TimeIndexedSamplingProblem::inequality);
1145  time_indexed_sampling_problem.def_readonly("equality", &TimeIndexedSamplingProblem::equality);
1146  time_indexed_sampling_problem.def("set_goal_eq", &TimeIndexedSamplingProblem::SetGoalEQ);
1147  time_indexed_sampling_problem.def("set_rho_eq", &TimeIndexedSamplingProblem::SetRhoEQ);
1148  time_indexed_sampling_problem.def("get_goal_eq", &TimeIndexedSamplingProblem::GetGoalEQ);
1149  time_indexed_sampling_problem.def("get_rho_eq", &TimeIndexedSamplingProblem::GetRhoEQ);
1150  time_indexed_sampling_problem.def("set_goal_neq", &TimeIndexedSamplingProblem::SetGoalNEQ);
1151  time_indexed_sampling_problem.def("set_rho_neq", &TimeIndexedSamplingProblem::SetRhoNEQ);
1152  time_indexed_sampling_problem.def("get_goal_neq", &TimeIndexedSamplingProblem::GetGoalNEQ);
1153  time_indexed_sampling_problem.def("get_rho_neq", &TimeIndexedSamplingProblem::GetRhoNEQ);
1154  time_indexed_sampling_problem.def("is_valid", (bool (TimeIndexedSamplingProblem::*)(Eigen::VectorXdRefConst, const double&)) & TimeIndexedSamplingProblem::IsValid);
1155 
1156  py::enum_<ControlCostLossTermType>(module, "ControlCostLossTermType")
1157  // BimodalHuber = 3, SuperHuber = 4, <-- skipped as not actively used right now.
1158  .value("Undefined", ControlCostLossTermType::Undefined)
1159  .value("L2", ControlCostLossTermType::L2)
1160  .value("SmoothL1", ControlCostLossTermType::SmoothL1)
1161  .value("Huber", ControlCostLossTermType::Huber)
1162  .value("PseudoHuber", ControlCostLossTermType::PseudoHuber)
1163  .export_values();
1164 
1165  py::class_<DynamicTimeIndexedShootingProblem, std::shared_ptr<DynamicTimeIndexedShootingProblem>, PlanningProblem>(prob, "DynamicTimeIndexedShootingProblem")
1168  .def("update_terminal_state", &DynamicTimeIndexedShootingProblem::UpdateTerminalState)
1169  .def_property("X", static_cast<const Eigen::MatrixXd& (DynamicTimeIndexedShootingProblem::*)(void)const>(&DynamicTimeIndexedShootingProblem::get_X), &DynamicTimeIndexedShootingProblem::set_X)
1170  .def_property("U", static_cast<const Eigen::MatrixXd& (DynamicTimeIndexedShootingProblem::*)(void)const>(&DynamicTimeIndexedShootingProblem::get_U), &DynamicTimeIndexedShootingProblem::set_U)
1172  .def_property_readonly("tau", &DynamicTimeIndexedShootingProblem::get_tau)
1178  .def_readonly("Phi", &DynamicTimeIndexedShootingProblem::Phi)
1179  .def_readonly("dPhi_dx", &DynamicTimeIndexedShootingProblem::dPhi_dx)
1180  .def_readonly("cost", &DynamicTimeIndexedShootingProblem::cost)
1181  .def("get_state_cost", &DynamicTimeIndexedShootingProblem::GetStateCost)
1182  .def("get_state_cost_jacobian", &DynamicTimeIndexedShootingProblem::GetStateCostJacobian)
1183  .def("get_state_cost_hessian", &DynamicTimeIndexedShootingProblem::GetStateCostHessian)
1184  .def("get_control_cost", &DynamicTimeIndexedShootingProblem::GetControlCost)
1185  .def("get_control_cost_jacobian", &DynamicTimeIndexedShootingProblem::GetControlCostJacobian)
1186  .def("get_control_cost_hessian", &DynamicTimeIndexedShootingProblem::GetControlCostHessian)
1187  .def("get_state_cost_hessian", &DynamicTimeIndexedShootingProblem::GetStateControlCostHessian);
1188 
1189  py::class_<CollisionProxy, std::shared_ptr<CollisionProxy>> collision_proxy(module, "CollisionProxy");
1190  collision_proxy.def(py::init());
1191  collision_proxy.def_readonly("contact_1", &CollisionProxy::contact1);
1192  collision_proxy.def_readonly("contact_2", &CollisionProxy::contact2);
1193  collision_proxy.def_readonly("normal_1", &CollisionProxy::normal1);
1194  collision_proxy.def_readonly("normal_2", &CollisionProxy::normal2);
1195  collision_proxy.def_readonly("distance", &CollisionProxy::distance);
1196  collision_proxy.def_property_readonly("object_1", [](CollisionProxy* instance) { return (instance->e1 && instance->e2) ? instance->e1->segment.getName() : std::string(""); });
1197  collision_proxy.def_property_readonly("object_2", [](CollisionProxy* instance) { return (instance->e1 && instance->e2) ? instance->e2->segment.getName() : std::string(""); });
1198  collision_proxy.def_property_readonly("transform_1", [](CollisionProxy* instance) { return (instance->e1 && instance->e2) ? instance->e1->frame : KDL::Frame(); });
1199  collision_proxy.def_property_readonly("transform_2", [](CollisionProxy* instance) { return (instance->e1 && instance->e2) ? instance->e2->frame : KDL::Frame(); });
1200  collision_proxy.def("__repr__", &CollisionProxy::Print);
1201 
1202  py::class_<ContinuousCollisionProxy, std::shared_ptr<ContinuousCollisionProxy>> continuous_collision_proxy(module, "ContinuousCollisionProxy");
1203  continuous_collision_proxy.def(py::init());
1204  continuous_collision_proxy.def_readonly("contact_transform_1", &ContinuousCollisionProxy::contact_tf1);
1205  continuous_collision_proxy.def_readonly("contact_transform_2", &ContinuousCollisionProxy::contact_tf2);
1206  continuous_collision_proxy.def_readonly("in_collision", &ContinuousCollisionProxy::in_collision);
1207  continuous_collision_proxy.def_readonly("time_of_contact", &ContinuousCollisionProxy::time_of_contact);
1208  continuous_collision_proxy.def_property_readonly("object_1", [](ContinuousCollisionProxy* instance) { return (instance->e1 && instance->e2) ? instance->e1->segment.getName() : std::string(""); });
1209  continuous_collision_proxy.def_property_readonly("object_2", [](ContinuousCollisionProxy* instance) { return (instance->e1 && instance->e2) ? instance->e2->segment.getName() : std::string(""); });
1210  continuous_collision_proxy.def_property_readonly("transform_1", [](ContinuousCollisionProxy* instance) { return (instance->e1 && instance->e2) ? instance->e1->frame : KDL::Frame(); });
1211  continuous_collision_proxy.def_property_readonly("transform_2", [](ContinuousCollisionProxy* instance) { return (instance->e1 && instance->e2) ? instance->e2->frame : KDL::Frame(); });
1212  continuous_collision_proxy.def("__repr__", &ContinuousCollisionProxy::Print);
1213 
1214  py::class_<Scene, std::shared_ptr<Scene>, Object> scene(module, "Scene");
1215  scene.def_property_readonly("num_positions", &Scene::get_num_positions);
1216  scene.def_property_readonly("num_velocities", &Scene::get_num_velocities);
1217  scene.def_property_readonly("num_controls", &Scene::get_num_controls);
1218  scene.def_property_readonly("num_state", &Scene::get_num_state);
1219  scene.def_property_readonly("num_state_derivative", &Scene::get_num_state_derivative);
1220  scene.def_property_readonly("has_quaternion_floating_base", &Scene::get_has_quaternion_floating_base);
1221  scene.def("update", &Scene::Update, py::arg("x"), py::arg("t") = 0.0);
1222  scene.def("get_controlled_joint_names", (std::vector<std::string>(Scene::*)()) & Scene::GetControlledJointNames);
1223  scene.def("get_controlled_link_names", &Scene::GetControlledLinkNames);
1224  scene.def("get_model_link_names", &Scene::GetModelLinkNames);
1225  scene.def("get_kinematic_tree", &Scene::GetKinematicTree, py::return_value_policy::reference_internal);
1226  scene.def("get_collision_scene", &Scene::GetCollisionScene, py::return_value_policy::reference_internal);
1227  scene.def("get_dynamics_solver", &Scene::GetDynamicsSolver, py::return_value_policy::reference_internal);
1228  scene.def("get_model_joint_names", &Scene::GetModelJointNames);
1229  scene.def("get_model_state", &Scene::GetModelState);
1230  scene.def("get_model_state_map", &Scene::GetModelStateMap);
1231  scene.def("get_tree_names", [](Scene& scene) {
1232  std::vector<std::string> frame_names;
1233  for (const auto& m : scene.GetTreeMap())
1234  {
1235  frame_names.push_back(m.first);
1236  }
1237  return frame_names;
1238  });
1239  scene.def("set_model_state", (void (Scene::*)(Eigen::VectorXdRefConst, double, bool)) & Scene::SetModelState, py::arg("x"), py::arg("t") = 0.0, py::arg("update_trajectory") = false);
1240  scene.def("set_model_state_map", (void (Scene::*)(const std::map<std::string, double>&, double, bool)) & Scene::SetModelState, py::arg("x"), py::arg("t") = 0.0, py::arg("update_trajectory") = false);
1241  scene.def("get_controlled_state", &Scene::GetControlledState);
1242  scene.def("publish_scene", &Scene::PublishScene);
1243  scene.def("publish_proxies", &Scene::PublishProxies);
1244  scene.def("update_planning_scene", &Scene::UpdatePlanningScene);
1245  scene.def("load_scene",
1246  (void (Scene::*)(const std::string&, const KDL::Frame&, bool)) & Scene::LoadScene,
1247  py::arg("scene_string"),
1248  py::arg("offset_transform") = kdl_frame(),
1249  py::arg("update_collision_scene") = true);
1250  scene.def("load_scene_file",
1251  (void (Scene::*)(const std::string&, const KDL::Frame&, bool)) & Scene::LoadSceneFile,
1252  py::arg("file_name"),
1253  py::arg("offset_transform") = kdl_frame(),
1254  py::arg("update_collision_scene") = true);
1255  scene.def("get_scene", &Scene::GetScene);
1256  scene.def("clean_scene", &Scene::CleanScene);
1257  scene.def("is_state_valid", [](Scene* instance, bool self, double safe_distance) { return instance->GetCollisionScene()->IsStateValid(self, safe_distance); }, py::arg("check_self_collision") = true, py::arg("safe_distance") = 0.0);
1258  scene.def("is_collision_free", [](Scene* instance, const std::string& o1, const std::string& o2, double safe_distance) { return instance->GetCollisionScene()->IsCollisionFree(o1, o2, safe_distance); }, py::arg("object_1"), py::arg("object_2"), py::arg("safe_distance") = 0.0);
1259  scene.def("is_allowed_to_collide", [](Scene* instance, const std::string& o1, const std::string& o2, bool self) { return instance->GetCollisionScene()->IsAllowedToCollide(o1, o2, self); }, py::arg("object_1"), py::arg("object_2"), py::arg("check_self_collision") = true);
1260  scene.def("get_collision_distance", [](Scene* instance, bool self) { return instance->GetCollisionScene()->GetCollisionDistance(self); }, py::arg("check_self_collision") = true);
1261  scene.def("get_collision_distance", [](Scene* instance, const std::string& o1, const std::string& o2) { return instance->GetCollisionScene()->GetCollisionDistance(o1, o2); }, py::arg("object_1"), py::arg("object_2"));
1262  scene.def("get_collision_distance",
1263  [](Scene* instance, const std::string& o1, const bool& self) {
1264  return instance->GetCollisionScene()->GetCollisionDistance(o1, self);
1265  },
1266  py::arg("object_1"), py::arg("check_self_collision") = true);
1267  scene.def("get_collision_distance",
1268  [](Scene* instance, const std::vector<std::string>& objects, const bool& self) {
1269  return instance->GetCollisionScene()->GetCollisionDistance(objects, self);
1270  },
1271  py::arg("objects"), py::arg("check_self_collision") = true);
1272  scene.def("update_planning_scene_world",
1273  [](Scene* instance, moveit_msgs::PlanningSceneWorld& world) {
1274  moveit_msgs::PlanningSceneWorldConstPtr my_ptr(
1275  new moveit_msgs::PlanningSceneWorld(world));
1276  instance->UpdatePlanningSceneWorld(my_ptr);
1277  });
1278  scene.def("update_collision_objects", &Scene::UpdateCollisionObjects);
1279  scene.def("get_collision_robot_links", [](Scene* instance) { return instance->GetCollisionScene()->GetCollisionRobotLinks(); });
1280  scene.def("get_collision_world_links", [](Scene* instance) { return instance->GetCollisionScene()->GetCollisionWorldLinks(); });
1281  scene.def("get_root_frame_name", &Scene::GetRootFrameName);
1282  scene.def("get_root_joint_name", &Scene::GetRootJointName);
1283  scene.def("attach_object", &Scene::AttachObject);
1284  scene.def("attach_object_local", (void (Scene::*)(const std::string& name, const std::string& parent, const KDL::Frame& pose)) & Scene::AttachObjectLocal);
1285  scene.def("attach_object_local", (void (Scene::*)(const std::string& name, const std::string& parent, const Eigen::VectorXd& pose)) & Scene::AttachObjectLocal);
1286  scene.def("detach_object", &Scene::DetachObject);
1287  scene.def("has_attached_object", &Scene::HasAttachedObject);
1288  scene.def("fk", [](Scene* instance, const std::string& e1, const KDL::Frame& o1, const std::string& e2, const KDL::Frame& o2) { return instance->GetKinematicTree().FK(e1, o1, e2, o2); });
1289  scene.def("fk", [](Scene* instance, const std::string& e1, const std::string& e2) { return instance->GetKinematicTree().FK(e1, KDL::Frame(), e2, KDL::Frame()); });
1290  scene.def("fk", [](Scene* instance, const std::string& e1) { return instance->GetKinematicTree().FK(e1, KDL::Frame(), "", KDL::Frame()); });
1291  scene.def("jacobian", [](Scene* instance, const std::string& e1, const KDL::Frame& o1, const std::string& e2, const KDL::Frame& o2) { return instance->GetKinematicTree().Jacobian(e1, o1, e2, o2); });
1292  scene.def("jacobian", [](Scene* instance, const std::string& e1, const std::string& e2) { return instance->GetKinematicTree().Jacobian(e1, KDL::Frame(), e2, KDL::Frame()); });
1293  scene.def("jacobian", [](Scene* instance, const std::string& e1) { return instance->GetKinematicTree().Jacobian(e1, KDL::Frame(), "", KDL::Frame()); });
1294  scene.def("hessian", [](Scene* instance, const std::string& e1, const KDL::Frame& o1, const std::string& e2, const KDL::Frame& o2) { return instance->GetKinematicTree().Hessian(e1, o1, e2, o2); });
1295  scene.def("hessian", [](Scene* instance, const std::string& e1, const std::string& e2) { return instance->GetKinematicTree().Hessian(e1, KDL::Frame(), e2, KDL::Frame()); });
1296  scene.def("hessian", [](Scene* instance, const std::string& e1) { return instance->GetKinematicTree().Hessian(e1, KDL::Frame(), "", KDL::Frame()); });
1297  scene.def("add_trajectory_from_file", &Scene::AddTrajectoryFromFile);
1298  scene.def("add_trajectory", (void (Scene::*)(const std::string&, const std::string&)) & Scene::AddTrajectory);
1299  scene.def("get_trajectory", [](Scene* instance, const std::string& link) { return instance->GetTrajectory(link)->ToString(); });
1300  scene.def("remove_trajectory", &Scene::RemoveTrajectory);
1301  scene.def("update_scene_frames", &Scene::UpdateSceneFrames);
1302  scene.def("add_object", [](Scene* instance, const std::string& name, const KDL::Frame& transform, const std::string& parent, const std::string& shape_resource_path, const Eigen::Vector3d scale, const Eigen::Vector4d color, const bool update_collision_scene) { instance->AddObject(name, transform, parent, shape_resource_path, scale, KDL::RigidBodyInertia::Zero(), color, update_collision_scene); },
1303  py::arg("name"),
1304  py::arg("transform") = KDL::Frame(),
1305  py::arg("parent") = "",
1306  py::arg("shape_resource_path"),
1307  py::arg("scale") = Eigen::Vector3d::Ones(),
1308  py::arg("color") = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0),
1309  py::arg("update_collision_scene") = true);
1310  scene.def("add_object", (void (Scene::*)(const std::string&, const KDL::Frame&, const std::string&, shapes::ShapeConstPtr, const KDL::RigidBodyInertia&, const Eigen::Vector4d&, const bool)) & Scene::AddObject,
1311  py::arg("name"),
1312  py::arg("transform") = KDL::Frame(),
1313  py::arg("parent") = std::string(),
1314  py::arg("shape"),
1315  py::arg("inertia") = KDL::RigidBodyInertia::Zero(),
1316  py::arg("color") = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0),
1317  py::arg("update_collision_scene") = true);
1318  scene.def("add_object_to_environment", &Scene::AddObjectToEnvironment,
1319  py::arg("name"),
1320  py::arg("transform") = KDL::Frame(),
1321  py::arg("shape"),
1322  py::arg("color") = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0),
1323  py::arg("update_collision_scene") = true);
1324  scene.def("remove_object", &Scene::RemoveObject);
1325  scene.def_property_readonly("model_link_to_collision_link_map", &Scene::GetModelLinkToCollisionLinkMap);
1326  scene.def_property_readonly("controlled_joint_to_collision_link_map", &Scene::GetControlledJointToCollisionLinkMap);
1327  scene.def_property_readonly("world_links_to_exclude_from_collision_scene", &Scene::get_world_links_to_exclude_from_collision_scene);
1328 
1329  py::class_<CollisionScene, std::shared_ptr<CollisionScene>> collision_scene(module, "CollisionScene");
1330  // TODO: expose IsStateValid, IsCollisionFree, GetCollisionDistance, GetCollisionWorldLinks, GetCollisionRobotLinks, GetTranslation
1331  collision_scene.def_property("always_externally_updated_collision_scene", &CollisionScene::GetAlwaysExternallyUpdatedCollisionScene, &CollisionScene::SetAlwaysExternallyUpdatedCollisionScene);
1332  collision_scene.def_property("replace_primitive_shapes_with_meshes", &CollisionScene::GetReplacePrimitiveShapesWithMeshes, &CollisionScene::SetReplacePrimitiveShapesWithMeshes);
1333  collision_scene.def_property("replace_cylinders_with_capsules", &CollisionScene::get_replace_cylinders_with_capsules, &CollisionScene::set_replace_cylinders_with_capsules);
1334  collision_scene.def_property("robot_link_scale", &CollisionScene::GetRobotLinkScale, &CollisionScene::SetRobotLinkScale);
1335  collision_scene.def_property("world_link_scale", &CollisionScene::GetWorldLinkScale, &CollisionScene::SetWorldLinkScale);
1336  collision_scene.def_property("robot_link_padding", &CollisionScene::GetRobotLinkPadding, &CollisionScene::SetRobotLinkPadding);
1337  collision_scene.def_property("world_link_padding", &CollisionScene::GetWorldLinkPadding, &CollisionScene::SetWorldLinkPadding);
1338  collision_scene.def("update_collision_object_transforms", &CollisionScene::UpdateCollisionObjectTransforms);
1339  collision_scene.def("continuous_collision_check", &CollisionScene::ContinuousCollisionCheck);
1340  collision_scene.def("get_robot_to_robot_collision_distance", &CollisionScene::GetRobotToRobotCollisionDistance);
1341  collision_scene.def("get_robot_to_world_collision_distance", &CollisionScene::GetRobotToWorldCollisionDistance);
1342  collision_scene.def("get_translation", &CollisionScene::GetTranslation);
1343 
1344  py::class_<VisualizationMoveIt> visualization_moveit(module, "VisualizationMoveIt");
1345  visualization_moveit.def(py::init<ScenePtr>());
1346  visualization_moveit.def("display_trajectory", &VisualizationMoveIt::DisplayTrajectory);
1347 #ifdef MSGPACK_FOUND
1348  py::class_<VisualizationMeshcat> visualization_meshcat(module, "VisualizationMeshcat");
1349  visualization_meshcat.def(py::init<ScenePtr, const std::string&, bool, const std::string&>(), py::arg("scene"), py::arg("url"), py::arg("use_mesh_materials") = true, py::arg("file_url") = "");
1350  visualization_meshcat.def("display_scene", &VisualizationMeshcat::DisplayScene, py::arg("use_mesh_materials") = true);
1351  visualization_meshcat.def("display_state", &VisualizationMeshcat::DisplayState, py::arg("state"), py::arg("t") = 0.0);
1352  visualization_meshcat.def("display_trajectory", &VisualizationMeshcat::DisplayTrajectory, py::arg("trajectory"), py::arg("dt") = 1.0);
1353  visualization_meshcat.def("get_web_url", &VisualizationMeshcat::GetWebURL);
1354  visualization_meshcat.def("get_file_url", &VisualizationMeshcat::GetFileURL);
1355  visualization_meshcat.def("delete", &VisualizationMeshcat::Delete, py::arg("path") = "");
1356  visualization_meshcat.def("set_property", py::overload_cast<const std::string&, const std::string&, const double&>(&VisualizationMeshcat::SetProperty), py::arg("path"), py::arg("property"), py::arg("value"));
1357  visualization_meshcat.def("set_property", py::overload_cast<const std::string&, const std::string&, const std::string&>(&VisualizationMeshcat::SetProperty), py::arg("path"), py::arg("property"), py::arg("value"));
1358  visualization_meshcat.def("set_property", py::overload_cast<const std::string&, const std::string&, const bool&>(&VisualizationMeshcat::SetProperty), py::arg("path"), py::arg("property"), py::arg("value"));
1359  visualization_meshcat.def("set_property", py::overload_cast<const std::string&, const std::string&, const Eigen::Vector3d&>(&VisualizationMeshcat::SetProperty), py::arg("path"), py::arg("property"), py::arg("value"));
1360  visualization_meshcat.def("set_property", py::overload_cast<const std::string&, const std::string&, const Eigen::Vector4d&>(&VisualizationMeshcat::SetProperty), py::arg("path"), py::arg("property"), py::arg("value"));
1361 #endif
1362 
1363  py::module kin = module.def_submodule("Kinematics", "Kinematics submodule.");
1364  py::class_<KinematicTree, std::shared_ptr<KinematicTree>> kinematic_tree(kin, "KinematicTree");
1365  kinematic_tree.def_readwrite("debug_mode", &KinematicTree::debug);
1366  kinematic_tree.def("publish_frames", &KinematicTree::PublishFrames, py::arg("tf_prefix") = "exotica");
1367  kinematic_tree.def("get_root_frame_name", &KinematicTree::GetRootFrameName);
1368  kinematic_tree.def("get_root_joint_name", &KinematicTree::GetRootJointName);
1369  kinematic_tree.def("get_kinematic_chain", &KinematicTree::GetKinematicChain);
1370  kinematic_tree.def("get_kinematic_chain_links", &KinematicTree::GetKinematicChainLinks);
1371  kinematic_tree.def("get_model_base_type", &KinematicTree::GetModelBaseType);
1372  kinematic_tree.def("get_controlled_base_type", &KinematicTree::GetControlledBaseType);
1373  kinematic_tree.def("get_controlled_link_mass", &KinematicTree::GetControlledLinkMass);
1374  kinematic_tree.def("get_collision_object_types", &KinematicTree::GetCollisionObjectTypes);
1375  kinematic_tree.def("set_seed", &KinematicTree::SetSeed);
1376  kinematic_tree.def("get_random_controlled_state", &KinematicTree::GetRandomControlledState);
1377  kinematic_tree.def("get_num_model_joints", &KinematicTree::GetNumModelJoints);
1378  kinematic_tree.def("get_num_controlled_joints", &KinematicTree::GetNumControlledJoints);
1379  kinematic_tree.def("find_kinematic_element_by_name", &KinematicTree::FindKinematicElementByName);
1380 
1381  // joints and links that describe the full state of the robot
1382  kinematic_tree.def("get_model_link_names", &KinematicTree::GetModelLinkNames);
1383  kinematic_tree.def("get_model_joint_names", &KinematicTree::GetModelJointNames);
1384 
1385  // subset of model joints and links that can be controlled
1386  kinematic_tree.def("get_controlled_link_names", &KinematicTree::GetControlledLinkNames);
1387  kinematic_tree.def("get_controlled_joint_names", &KinematicTree::GetControlledJointNames);
1388 
1389  // Joint Limits
1390  kinematic_tree.def("get_joint_limits", &KinematicTree::GetJointLimits);
1391  kinematic_tree.def("reset_joint_limits", &KinematicTree::ResetJointLimits);
1392  kinematic_tree.def("set_joint_limits_lower", &KinematicTree::SetJointLimitsLower);
1393  kinematic_tree.def("set_joint_limits_upper", &KinematicTree::SetJointLimitsUpper);
1394  kinematic_tree.def("set_joint_velocity_limits", &KinematicTree::SetJointVelocityLimits);
1395  kinematic_tree.def("set_joint_acceleration_limits", &KinematicTree::SetJointAccelerationLimits);
1396  kinematic_tree.def("get_velocity_limits", &KinematicTree::GetVelocityLimits);
1397  kinematic_tree.def("has_acceleration_limits", &KinematicTree::HasAccelerationLimits);
1398  kinematic_tree.def("get_acceleration_limits", &KinematicTree::GetAccelerationLimits);
1399  kinematic_tree.def("set_floating_base_limits_pos_xyz_euler_zyx", (void (KinematicTree::*)(const std::vector<double>&, const std::vector<double>&)) & KinematicTree::SetFloatingBaseLimitsPosXYZEulerZYX);
1400  kinematic_tree.def("set_floating_base_limits_pos_xyz_euler_zyx", (void (KinematicTree::*)(const std::vector<double>&, const std::vector<double>&, const std::vector<double>&, const std::vector<double>&)) & KinematicTree::SetFloatingBaseLimitsPosXYZEulerZYX);
1401  kinematic_tree.def("set_planar_base_limits_pos_xy_euler_z", (void (KinematicTree::*)(const std::vector<double>&, const std::vector<double>&)) & KinematicTree::SetPlanarBaseLimitsPosXYEulerZ);
1402  kinematic_tree.def("set_planar_base_limits_pos_xy_euler_z", (void (KinematicTree::*)(const std::vector<double>&, const std::vector<double>&, const std::vector<double>&, const std::vector<double>&)) & KinematicTree::SetPlanarBaseLimitsPosXYEulerZ);
1403  kinematic_tree.def("get_used_joint_limits", &KinematicTree::GetUsedJointLimits);
1404 
1405  // Get full tree
1406  kinematic_tree.def("get_model_tree", &KinematicTree::GetModelTree);
1407  kinematic_tree.def("get_tree", [](KinematicTree* kt) {
1408  auto tree_weak_ptr = kt->GetTree();
1409  std::vector<std::shared_ptr<KinematicElement>> tree_shared_ptr;
1410  tree_shared_ptr.reserve(tree_weak_ptr.size());
1411  for (auto e : tree_weak_ptr)
1412  tree_shared_ptr.emplace_back(e.lock());
1413  return tree_shared_ptr;
1414  });
1415 
1416  // KinematicElement
1417  py::class_<KinematicElement, std::shared_ptr<KinematicElement>> kinematic_element(kin, "KinematicElement");
1418  kinematic_element.def("get_pose", &KinematicElement::GetPose);
1419  kinematic_element.def_readonly("id", &KinematicElement::id);
1420  kinematic_element.def("get_segment_name", [](KinematicElement* element) { return element->segment.getName(); });
1421  kinematic_element.def("get_joint_name", [](KinematicElement* element) { return element->segment.getJoint().getName(); });
1422  kinematic_element.def("get_parent_name", [](KinematicElement* element) { auto parent = element->parent.lock(); if (parent) { return parent->segment.getName(); } else { return std::string("no_parent"); } });
1423  kinematic_element.def("get_mass", [](KinematicElement* element) { return element->segment.getInertia().getMass(); });
1424  kinematic_element.def_readonly("control_id", &KinematicElement::control_id);
1425  kinematic_element.def_readonly("is_controlled", &KinematicElement::is_controlled);
1426  kinematic_element.def_readonly("parent_name", &KinematicElement::parent_name);
1427  kinematic_element.def_readonly("joint_limits", &KinematicElement::joint_limits);
1428  kinematic_element.def_readonly("is_robot_link", &KinematicElement::is_robot_link);
1429  kinematic_element.def_readonly("shape", &KinematicElement::shape);
1430 
1431  // TODO: KinematicRequestFlags
1432 
1433  // TODO: KinematicFrame
1434 
1435  // KinematicResponse
1436  py::class_<KinematicResponse, std::shared_ptr<KinematicResponse>> kinematic_response(kin, "KinematicResponse");
1437  kinematic_response.def_property_readonly("Phi", [](KinematicResponse* instance) {
1438  std::vector<KDL::Frame> vec;
1439  for (unsigned int i = 0; i < instance->Phi.cols(); ++i)
1440  vec.push_back(instance->Phi(i));
1441  return vec;
1442  });
1443 
1444  py::enum_<Integrator>(module, "Integrator")
1445  .value("RK1", Integrator::RK1)
1446  .value("SymplecticEuler", Integrator::SymplecticEuler)
1447  .value("RK2", Integrator::RK2)
1448  .value("RK4", Integrator::RK4)
1449  .export_values();
1450 
1451  py::class_<DynamicsSolver, std::shared_ptr<DynamicsSolver>, Object>(module, "DynamicsSolver")
1452  .def("F", &DynamicsSolver::F)
1453  .def("f", &DynamicsSolver::f)
1454  .def("fx", &DynamicsSolver::fx)
1455  .def("fu", &DynamicsSolver::fu)
1456  .def("fx_fd", &DynamicsSolver::fx_fd)
1457  .def("fu_fd", &DynamicsSolver::fu_fd)
1458  .def_property_readonly("nq", &DynamicsSolver::get_num_positions)
1459  .def_property_readonly("nv", &DynamicsSolver::get_num_velocities)
1460  .def_property_readonly("nx", &DynamicsSolver::get_num_state)
1461  .def_property_readonly("ndx", &DynamicsSolver::get_num_state_derivative)
1462  .def_property_readonly("nu", &DynamicsSolver::get_num_controls)
1463  .def_property_readonly("has_second_order_derivatives", &DynamicsSolver::get_has_second_order_derivatives)
1464  .def_property("integrator", &DynamicsSolver::get_integrator, &DynamicsSolver::set_integrator)
1465  .def("get_position", &DynamicsSolver::GetPosition)
1466  .def("simulate", &DynamicsSolver::Simulate)
1467  .def("state_delta", &DynamicsSolver::StateDelta)
1468  .def("state_delta_derivative", &DynamicsSolver::dStateDelta)
1469  .def("state_delta_second_derivative", &DynamicsSolver::ddStateDelta)
1470  .def("compute_derivatives", &DynamicsSolver::ComputeDerivatives)
1471  .def("get_Fx", &DynamicsSolver::get_Fx)
1472  .def("get_Fu", &DynamicsSolver::get_Fu)
1473  .def("get_fx", &DynamicsSolver::get_fx)
1474  .def("get_fu", &DynamicsSolver::get_fu)
1475  .def("integrate", [](DynamicsSolver* instance, Eigen::VectorXdRefConst x, Eigen::VectorXdRefConst u, const double dt) {
1476  Eigen::VectorXd xout(instance->get_num_positions() + instance->get_num_velocities());
1477  instance->Integrate(x, u, dt, xout);
1478  return xout;
1479  })
1480  .def_property_readonly("dt", &DynamicsSolver::get_dt, "dt");
1481 
1484 
1485  // shape base class
1486  py::class_<shapes::Shape, shapes::ShapePtr>(module, "Shape")
1487  .def("scale", &shapes::Shape::scale)
1488  .def("padd", &shapes::Shape::padd)
1489  .def("scaleAndPadd", &shapes::Shape::scaleAndPadd)
1490  .def("isFixed", &shapes::Shape::isFixed)
1491  .def_readonly("type", &shapes::Shape::type);
1492 
1493  py::class_<shapes::Sphere, shapes::Shape, std::shared_ptr<shapes::Sphere>>(module, "Sphere")
1494  .def(py::init())
1495  .def(py::init<double>())
1496  .def_readonly_static("name", &shapes::Sphere::STRING_NAME)
1497  .def_readwrite("radius", &shapes::Sphere::radius);
1498 
1499  py::class_<shapes::Cylinder, shapes::Shape, std::shared_ptr<shapes::Cylinder>>(module, "Cylinder")
1500  .def(py::init())
1501  .def(py::init<double, double>())
1502  .def_readonly_static("name", &shapes::Cylinder::STRING_NAME)
1503  .def_readwrite("radius", &shapes::Cylinder::radius)
1504  .def_readwrite("length", &shapes::Cylinder::length);
1505 
1506  py::class_<shapes::Cone, shapes::Shape, std::shared_ptr<shapes::Cone>>(module, "Cone")
1507  .def(py::init())
1508  .def(py::init<double, double>())
1509  .def_readonly_static("name", &shapes::Cone::STRING_NAME)
1510  .def_readwrite("radius", &shapes::Cone::radius)
1511  .def_readwrite("length", &shapes::Cone::length);
1512 
1513  py::class_<shapes::Box, shapes::Shape, std::shared_ptr<shapes::Box>>(module, "Box")
1514  .def(py::init())
1515  .def(py::init<double, double, double>())
1516  .def_readonly_static("name", &shapes::Box::STRING_NAME);
1517 
1518  py::class_<shapes::Plane, shapes::Shape, std::shared_ptr<shapes::Plane>>(module, "Plane")
1519  .def(py::init())
1520  .def(py::init<double, double, double, double>())
1521  .def_readonly_static("name", &shapes::Plane::STRING_NAME)
1522  .def("isFixed", &shapes::Plane::isFixed)
1523  .def_readwrite("a", &shapes::Plane::a)
1524  .def_readwrite("b", &shapes::Plane::b)
1525  .def_readwrite("c", &shapes::Plane::c)
1526  .def_readwrite("d", &shapes::Plane::d);
1527 
1528  py::class_<shapes::Mesh, shapes::Shape, std::shared_ptr<shapes::Mesh>>(module, "Mesh")
1529  .def(py::init())
1530  .def(py::init<unsigned int, unsigned int>())
1531  .def("computeTriangleNormals", &shapes::Mesh::computeTriangleNormals)
1532  .def("computeVertexNormals", &shapes::Mesh::computeVertexNormals)
1533  .def("mergeVertices", &shapes::Mesh::mergeVertices)
1534  .def_readonly("vertex_count", &shapes::Mesh::vertex_count)
1535  .def_readonly("triangle_count", &shapes::Mesh::triangle_count);
1536 
1537  py::class_<shapes::OcTree, shapes::Shape, std::shared_ptr<shapes::OcTree>>(module, "OcTree")
1538  .def(py::init())
1539  .def(py::init<const std::shared_ptr<const octomap::OcTree>&>());
1540 
1541  py::enum_<shapes::ShapeType>(module, "ShapeType")
1542  .value("UNKNOWN_SHAPE", shapes::ShapeType::UNKNOWN_SHAPE)
1543  .value("SPHERE", shapes::ShapeType::SPHERE)
1544  .value("CYLINDER", shapes::ShapeType::CYLINDER)
1545  .value("CONE", shapes::ShapeType::CONE)
1546  .value("BOX", shapes::ShapeType::BOX)
1547  .value("PLANE", shapes::ShapeType::PLANE)
1548  .value("MESH", shapes::ShapeType::MESH)
1549  .value("OCTREE", shapes::ShapeType::OCTREE)
1550  .export_values();
1551 
1552  py::enum_<ArgumentPosition>(module, "ArgumentPosition")
1553  .value("ARG0", ArgumentPosition::ARG0)
1554  .value("ARG1", ArgumentPosition::ARG1)
1555  .value("ARG2", ArgumentPosition::ARG2)
1556  .value("ARG3", ArgumentPosition::ARG3)
1557  .value("ARG4", ArgumentPosition::ARG4)
1558  .export_values();
1559 
1560  module.attr("version") = std::string(exotica::version);
1561  module.attr("branch") = std::string(exotica::branch);
1562 
1563  py::class_<BoxQPSolution>(module, "BoxQPSolution")
1564  .def_readonly("Hff_inv", &BoxQPSolution::Hff_inv)
1565  .def_readonly("x", &BoxQPSolution::x)
1566  .def_readonly("free_idx", &BoxQPSolution::free_idx)
1567  .def_readonly("clamped_idx", &BoxQPSolution::clamped_idx);
1568 
1569  module.def("box_qp",
1570  (BoxQPSolution(*)(const Eigen::MatrixXd& H, const Eigen::VectorXd& q,
1571  const Eigen::VectorXd& b_low, const Eigen::VectorXd& b_high,
1572  const Eigen::VectorXd& x_init, const double gamma,
1573  const int max_iterations, const double epsilon, const double lambda,
1574  bool use_polynomial_linesearch,
1575  bool use_cholesky_factorization)) &
1576  BoxQP,
1577  py::arg("H"), py::arg("q"), py::arg("b_low"), py::arg("b_high"), py::arg("x_init"), py::arg("gamma"), py::arg("max_iterations"), py::arg("epsilon"), py::arg("lambda"), py::arg("use_polynomial_linesearch") = true, py::arg("use_cholesky_factorization") = true);
1578 
1579  module.def("box_qp_old",
1580  (BoxQPSolution(*)(const Eigen::MatrixXd& H, const Eigen::VectorXd& q,
1581  const Eigen::VectorXd& b_low, const Eigen::VectorXd& b_high,
1582  const Eigen::VectorXd& x_init, const double gamma,
1583  const int max_iterations, const double epsilon, const double lambda,
1584  bool use_polynomial_linesearch,
1585  bool use_cholesky_factorization)) &
1586  ExoticaBoxQP,
1587  py::arg("H"), py::arg("q"), py::arg("b_low"), py::arg("b_high"), py::arg("x_init"), py::arg("gamma"), py::arg("max_iterations"), py::arg("epsilon"), py::arg("lambda"), py::arg("use_polynomial_linesearch") = false, py::arg("use_cholesky_factorization") = false);
1588 
1589  AddInitializers(module);
1590 
1591  auto cleanup_exotica = []() {
1592  known_initializers.clear();
1593  Setup::Destroy();
1594  };
1595  module.add_object("_cleanup", py::capsule(cleanup_exotica));
1596 }
std::string GetObjectName()
PyObject * StdStringAsPy(std::string value)
Definition: pyexotica.cpp:78
void SetGoalEQ(const std::string &task_name, Eigen::VectorXdRefConst goal, int t=0)
bool PathExists(const std::string &path)
std::shared_ptr< octomap::OcTree > LoadOctree(const std::string &file_path)
void AddTrajectory(const std::string &link, const std::string &traj)
std::vector< TaskSpaceVector > y
virtual void scaleAndPadd(double scale, double padd)=0
virtual std::vector< CollisionProxy > GetRobotToRobotCollisionDistance(double check_margin)
static std::vector< std::string > GetDynamicsSolvers()
void LoadOBJ(const std::string &data, Eigen::VectorXi &tri, Eigen::VectorXd &vert)
const Eigen::VectorXd & GetAccelerationLimits() const
#define HIGHLIGHT(x)
void UpdateCollisionObjects()
void Update(Eigen::VectorXdRefConst u, int t)
static exotica::ScenePtr CreateScene(const Initializer &init)
Eigen::MatrixXd S
Eigen::VectorXd GetTaskError(const std::string &task_name, int t) const
Eigen::MatrixXd jacobian
virtual void UpdateCollisionObjectTransforms()=0
const Ref< const VectorXd > & VectorXdRefConst
const char version[]
void Update(Eigen::VectorXdRefConst x_trajectory_in)
void SetNumberOfMaxIterations(int max_iter)
virtual Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > dStateDelta(const StateVector &x_1, const StateVector &x_2, const ArgumentPosition first_or_second)
void PublishScene()
Eigen::MatrixXd S
bool GetAlwaysExternallyUpdatedCollisionScene() const
IMETHOD doubleVel diff(const doubleVel &a, const doubleVel &b, double dt=1.0)
void SetWorldLinkPadding(const double padding)
const std::set< std::string > & get_world_links_to_exclude_from_collision_scene() const
int get_num_positions() const
virtual Eigen::VectorXd ApplyStartState(bool update_traj=true)
std::vector< std::string > GetControlledJointNames()
double GetScalarTaskCost(int t) const
const bool & get_has_second_order_derivatives() const
void scale(double scale)
std::string GetScene()
void SetRhoEQ(const std::string &task_name, const double &rho)
double GetScalarTaskCost(const std::string &task_name) const
void SetGoalNEQ(const std::string &task_name, Eigen::VectorXdRefConst goal)
void mergeVertices(double threshold)
void SetRho(const std::string &task_name, const double &rho)
virtual Eigen::VectorXd GetFeedbackControl(Eigen::VectorXdRefConst x, int t) const =0
const std::map< std::string, std::vector< std::string > > & GetControlledJointToCollisionLinkMap() const
const StateDerivative & get_Fx() const
double huber_jacobian(double x, double beta)
void DisplayScene(bool use_mesh_materials=true)
bool get_has_quaternion_floating_base() const
#define PyBytes_AsString
Eigen::VectorXd GetInequality() const
Eigen::VectorXd ydiff
std::string ToString()
const std::vector< std::string > & GetControlledLinkNames() const
Eigen::MatrixXd Hff_inv
Eigen::VectorXd GetRandomControlledState()
double ParseDouble(const std::string value)
void LoadSceneFile(const std::string &file_name, const Eigen::Isometry3d &offset=Eigen::Isometry3d::Identity(), bool update_collision_scene=true)
Eigen::VectorXd data
std::vector< std::string > GetModelJointNames()
std::map< std::string, shapes::ShapeType > GetCollisionObjectTypes() const
KDL::Frame GetFrameFromMatrix(Eigen::MatrixXdRefConst val)
Eigen::RowVectorXd GetScalarTaskJacobian(int t) const
double smooth_l1_cost(double x, double beta)
const Eigen::VectorXd & GetGoalState() const
double GetRho(const std::string &task_name, int t) const
std::pair< Initializer, Initializer > LoadFromXML(std::string file_name, const std::string &solver_name="", const std::string &problem_name="", bool parsePathAsXML=false)
Definition: pyexotica.cpp:166
void SetRobotLinkPadding(const double padding)
void set_X(Eigen::MatrixXdRefConst X_in)
void SetRhoNEQ(const std::string &task_name, const double rho, int t=0)
double pseudo_huber_jacobian(double x, double beta)
Eigen::Vector3d normal2
Eigen::VectorXd GetGoal(const std::string &task_name, int t) const
std::string GetRootFrameName()
Eigen::VectorXd GetGoal(const std::string &task_name) const
PyObject * CreateStringIOObject()
Definition: pyexotica.cpp:92
void PublishProxies(const std::vector< CollisionProxy > &proxies)
BaseType GetModelBaseType() const
shapes::ShapeConstPtr shape
std::shared_ptr< KinematicElement > FindKinematicElementByName(const std::string &frame_name)
std::string Print() const
bool AddPropertyFromDict(Property &target, PyObject *value_py)
Definition: pyexotica.cpp:200
#define WARNING_NAMED(name, x)
Eigen::MatrixXd Jacobian(std::shared_ptr< KinematicElement > element_A, const KDL::Frame &offset_a, std::shared_ptr< KinematicElement > element_B, const KDL::Frame &offset_b) const
void set_control_cost_weight(const double control_cost_weight_in)
const Eigen::MatrixXd & GetJointLimits() const
virtual ContinuousCollisionProxy ContinuousCollisionCheck(const std::string &o1, const KDL::Frame &tf1_beg, const KDL::Frame &tf1_end, const std::string &o2, const KDL::Frame &tf2_beg, const KDL::Frame &tf2_end)
int get_num_controls() const
Eigen::VectorXd ydiff
void SetModelState(Eigen::VectorXdRefConst x, double t=0, bool update_traj=true)
Eigen::MatrixXd GetInequalityJacobian()
ShapeType type
static std::shared_ptr< exotica::MotionSolver > CreateSolver(const std::string &type, bool prepend=true)
void SetGoal(const std::string &task_name, Eigen::VectorXdRefConst goal)
unsigned int GetNumberOfProblemUpdates() const
double GetRho(const std::string &task_name) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
StateVector Simulate(const StateVector &x, const ControlVector &u, T t)
void set_loss_type(const ControlCostLossTermType &loss_type_in)
Eigen::VectorXd GetGoalNEQ(const std::string &task_name)
void SetZero(const int n)
bool IsInitializerVectorType() const
double smooth_l1_hessian(double x, double beta)
double GetRobotLinkScale() const
int get_num_positions() const
std::weak_ptr< KinematicElement > parent
int get_num_state() const
void Update(Eigen::VectorXdRefConst x, const double &t)
std::vector< std::string > ParseList(const std::string &value, char token= ',')
std::shared_ptr< DynamicsSolver > GetDynamicsSolver() const
double GetRho(const std::string &task_name)
#define ThrowPretty(m)
const ControlDerivative & get_Fu() const
Eigen::VectorXd GetGoalState() const
double GetScalarTaskCost(const std::string &task_name) const
Eigen::VectorXd GetFrameAsVector(const KDL::Frame &val, RotationType type=RotationType::RPY)
exotica::Hessian Hessian(std::shared_ptr< KinematicElement > element_A, const KDL::Frame &offset_a, std::shared_ptr< KinematicElement > element_B, const KDL::Frame &offset_b) const
Eigen::RowVectorXd GetScalarJacobian() const
double GetRho(const std::string &task_name) const
std::vector< std::string > GetKinematicChainLinks(const std::string &begin, const std::string &end) const
double GetRho(const std::string &task_name, int t=0)
static const std::string STRING_NAME
py::handle hessian_array(Hessian &inp)
Definition: pyexotica.cpp:595
double getMass() const
Eigen::VectorXd GetControlledLinkMass() const
double smooth_l1_jacobian(double x, double beta)
void AddTrajectoryFromFile(const std::string &link, const std::string &traj)
void AddObject(const std::string &name, const KDL::Frame &transform=KDL::Frame(), const std::string &parent="", shapes::ShapeConstPtr shape=shapes::ShapeConstPtr(nullptr), const KDL::RigidBodyInertia &inertia=KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d &color=Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const bool update_collision_scene=true)
Eigen::VectorXd GetTaskError(const std::string &task_name) const
virtual StateVector F(const StateVector &x, const ControlVector &u)
Eigen::MatrixXd GetBounds() const
void LoadScene(const std::string &scene, const Eigen::Isometry3d &offset=Eigen::Isometry3d::Identity(), bool update_collision_scene=true)
void SetGoal(const std::string &task_name, Eigen::VectorXdRefConst goal)
std::vector< Hessian > hessian
virtual void SpecifyProblem(PlanningProblemPtr pointer)
void SetGoalEQ(const std::string &task_name, Eigen::VectorXdRefConst goal)
Eigen::VectorXd GetInequality()
const std::string & GetName() const
std::string LoadFile(const std::string &path)
Integrator get_integrator() const
std::vector< Eigen::VectorXd > rho
bool PyToInit(PyObject *source, Initializer &ret)
Definition: pyexotica.cpp:397
Eigen::VectorXd GetGoalEQ(const std::string &task_name, int t=0)
void SetRho(const std::string &task_name, const double &rho)
TaskSpaceVector Phi
void RemoveObject(const std::string &name)
Eigen::VectorXd GetGoalEQ(const std::string &task_name)
Eigen::MatrixXd GetBounds() const
TerminationCriterion termination_criterion
TaskMapMap & GetTaskMaps()
KDL::Frame GetPose(const double &x=0.0)
TaskSpaceVector Phi
boost::any Get() const
TaskSpaceVector y
Eigen::MatrixXd GetStateCostHessian(int t)
void SetSeed(const uint_fast32_t seed)
void DisplayTrajectory(Eigen::MatrixXdRefConst trajectory)
double GetRhoNEQ(const std::string &task_name)
Eigen::VectorXd GetGoalNEQ(const std::string &task_name, int t=0)
const std::vector< std::string > & GetModelLinkNames() const
const bool & HasAccelerationLimits() const
std::shared_ptr< KinematicElement > e1
void SetGoalState(Eigen::VectorXdRefConst qT)
virtual StateVector StateDelta(const StateVector &x_1, const StateVector &x_2)
void SetRhoNEQ(const std::string &task_name, const double &rho)
static void addPropertyToDict(PyObject *dict, const std::string &name, const Property &prop)
Definition: pyexotica.cpp:485
virtual int TaskSpaceJacobianDim()
void Set(const C val)
const std::string & GetRootJointName() const
static std::vector< Initializer > GetInitializers()
std::string Print(const std::string &prepend) const override
void set_Q(Eigen::MatrixXdRefConst Q_in, int t)
void SetWorldLinkScale(const double scale)
std::string ns_
static void InitRos(std::shared_ptr< ros::NodeHandle > nh, int numThreads=2)
const std::vector< std::weak_ptr< KinematicElement > > & GetTree() const
void SetRho(const std::string &task_name, const double rho_in, int t)
Eigen::VectorXd IdentityTransform()
Eigen::VectorXd GetNominalPose() const
void Update(Eigen::VectorXdRefConst x)
int get_num_velocities() const
void Update(Eigen::VectorXdRefConst x, int t) override
void SetJointLimitsLower(Eigen::VectorXdRefConst lower_in)
static std::shared_ptr< exotica::MotionSolver > LoadSolverStandalone(const std::string &file_name)
double GetRho(const std::string &task_name)
void UpdatePlanningSceneWorld(const moveit_msgs::PlanningSceneWorldConstPtr &world)
struct exotica::BoxQPSolution BoxQPSolution
void SetGoalEQ(const std::string &task_name, Eigen::VectorXdRefConst goal)
const Eigen::VectorXd & GetVelocityLimits() const
unsigned int vertex_count
const std::string & getName() const
const std::string & getName() const
virtual StateVector f(const StateVector &x, const ControlVector &u)=0
std::vector< Hessian > ddPhi_dxdu
void SetJointLimitsUpper(Eigen::VectorXdRefConst upper_in)
void Update(Eigen::VectorXdRefConst x)
virtual Hessian ddStateDelta(const StateVector &x_1, const StateVector &x_2, const ArgumentPosition first_or_second)
void padd(double padding)
Eigen::VectorXd GetStateCostJacobian(int t)
Eigen::MatrixXd jacobian
int GetNumModelJoints() const
static void Destroy()
void set_U(Eigen::MatrixXdRefConst U_in)
std::vector< Eigen::MatrixXd > dPhi_dx
std::string ToString(const KDL::Frame &s)
void SetRhoEQ(const std::string &task_name, const double &rho)
static std::vector< std::string > GetCollisionScenes()
Eigen::VectorXd GetGoalEQ(const std::string &task_name)
void SetAlwaysExternallyUpdatedCollisionScene(const bool value)
Eigen::Vector3d contact1
void SetStartTime(double t)
std::string GetType() const
std::vector< Eigen::VectorXd > ydiff
std::string GetRootJointName()
int ParseInt(const std::string value)
void SetRho(const std::string &task_name, const double rho)
Eigen::VectorXd GetJointVelocityLimits() const
void Update(Eigen::VectorXdRefConst x, double t=0)
TaskSpaceVector y
std::pair< std::vector< double >, std::vector< double > > GetCostEvolution() const
Eigen::Array< Eigen::MatrixXd, Eigen::Dynamic, 1 > Hessian
void set_replace_cylinders_with_capsules(const bool value)
TaskMapMap task_maps
const char branch[]
double GetRhoEQ(const std::string &task_name, int t=0)
void SetGoalNEQ(const std::string &task_name, Eigen::VectorXdRefConst goal, int t=0)
virtual void PreUpdate()
unsigned int triangle_count
Eigen::Vector3d contact2
Eigen::RowVectorXd GetScalarTransitionJacobian(int t) const
KDL::Frame FK(KinematicFrame &frame) const
virtual Eigen::Matrix< T, Eigen::Dynamic, 1 > GetPosition(Eigen::VectorXdRefConst x_in)
virtual bool isFixed() const
void set_X_star(Eigen::MatrixXdRefConst X_star_in)
Eigen::MatrixXd GetControlCostHessian(int t)
void SetRhoNEQ(const std::string &task_name, const double &rho)
double data[3]
void SetNominalPose(Eigen::VectorXdRefConst qNominal_in)
void SetRhoEQ(const std::string &task_name, const double rho, int t=0)
virtual void Integrate(const StateVector &x, const StateVector &dx, const double dt, StateVector &xout)
Eigen::VectorXd GetModelState()
virtual void ComputeDerivatives(const StateVector &x, const ControlVector &u)
void DisplayState(Eigen::VectorXdRefConst state, double t=0.0)
virtual bool IsValid()
Eigen::VectorXd GetGoalNEQ(const std::string &task_name)
static void PrintSupportedClasses()
void AddInitializers(py::module &module)
Definition: pyexotica.cpp:173
const std::vector< std::string > & GetModelJointNames() const
double epsilon
int GetNumberOfIterations() const
const std::string & GetRootFrameName() const
const Eigen::MatrixXd & get_Q(int t) const
void SetGoalState(Eigen::VectorXdRefConst qT)
void SetRhoEQ(const std::string &task_name, const double &rho)
void UpdateTerminalState(Eigen::VectorXdRefConst x)
Eigen::VectorXd GetGoal(const std::string &task_name)
Eigen::RowVectorXd GetScalarJacobian()
double GetRhoEQ(const std::string &task_name)
const Joint & getJoint() const
std::vector< Eigen::MatrixXd > dPhi_du
std::vector< std::string > GetKinematicChain(const std::string &begin, const std::string &end) const
std::vector< double > GetBounds()
const std::vector< std::shared_ptr< KinematicElement > > & GetModelTree() const
std::map< std::string, Initializer > known_initializers
Definition: pyexotica.cpp:90
void DetachObject(const std::string &name)
double GetScalarTransitionCost(int t) const
bool get_replace_cylinders_with_capsules() const
void SetRho(const std::string &task_name, const double &rho)
std::map< std::string, double > GetModelStateMap()
std::vector< int > ParseIntList(const std::string value)
void computeVertexNormals()
void SaveMatrix(std::string file_name, const Eigen::Ref< const Eigen::MatrixXd > mat)
std::shared_ptr< Trajectory > GetTrajectory(const std::string &link)
bool GetReplacePrimitiveShapesWithMeshes() const
void PreUpdate() override
std::map< std::string, Property > properties_
Eigen::RowVectorXd GetCostJacobian() const
static RotationalInertia Zero()
Eigen::VectorXd GetGoal(const std::string &task_name, int t=0)
static RigidBodyInertia Zero()
Eigen::MatrixXd GetBounds() const
void AttachObjectLocal(const std::string &name, const std::string &parent, const KDL::Frame &pose)
Eigen::VectorXd VectorTransform(double px=0.0, double py=0.0, double pz=0.0, double qx=0.0, double qy=0.0, double qz=0.0, double qw=1.0)
Frame Inverse() const
Eigen::MatrixXd GetEqualityJacobian()
virtual std::string type() const
const CollisionScenePtr & GetCollisionScene() const
void AttachObject(const std::string &name, const std::string &parent)
void SetJointAccelerationLimits(Eigen::VectorXdRefConst acceleration_in)
const Ref< const MatrixXd > & MatrixXdRefConst
static std::shared_ptr< exotica::DynamicsSolver > CreateDynamicsSolver(const std::string &type, bool prepend=true)
void SetRhoNEQ(const std::string &task_name, const double &rho)
const ControlCostLossTermType & get_loss_type() const
Eigen::VectorXd GetGoalEQ(const std::string &task_name)
ROSLIB_DECL std::string getPath(const std::string &package_name)
int GetNumControlledJoints() const
void SetJointVelocityLimits(const Eigen::VectorXd &qdot_max_in)
double GetRhoNEQ(const std::string &task_name)
Eigen::VectorXd GetStartState() const
static std::vector< std::string > GetProblems()
void SetGoal(const std::string &task_name, Eigen::VectorXdRefConst goal, int t)
TaskMapVec & GetTasks()
void SetGoal(const std::string &task_name, Eigen::VectorXdRefConst goal, int t=0)
virtual Eigen::Vector3d GetTranslation(const std::string &name)=0
std::vector< size_t > clamped_idx
static handle cast(Hessian src, return_value_policy, handle)
Definition: pyexotica.cpp:662
double GetRhoNEQ(const std::string &task_name, int t=0)
void UpdatePlanningScene(const moveit_msgs::PlanningScene &scene)
void computeTriangleNormals()
double GetRhoEQ(const std::string &task_name)
double huber_cost(double x, double beta)
void AddProperty(const Property &prop)
int get_num_state_derivative() const
void SetPlanarBaseLimitsPosXYEulerZ(const std::vector< double > &lower, const std::vector< double > &upper)
Eigen::VectorXd GetGoal(const std::string &task_name) const
Eigen::VectorXd GetControlCostJacobian(int t)
std::vector< double > joint_limits
const std::vector< std::string > & GetControlledJointNames() const
std::vector< Eigen::MatrixXd > jacobian
double GetRho(const std::string &task_name) const
static const std::string STRING_NAME
std::vector< std::string > GetModelLinkNames()
const StateDerivative & get_fx() const
std::string ParsePath(const std::string &path)
double GetRobotLinkPadding() const
std::vector< std::string > GetControlledLinkNames()
const Eigen::MatrixXd & get_X_star() const
IMETHOD doubleVel addDelta(const doubleVel &a, const doubleVel &da, double dt=1.0)
void SetJointVelocityLimits(Eigen::VectorXdRefConst velocity_in)
std::map< std::string, std::weak_ptr< KinematicElement > > GetTreeMap()
std::vector< Eigen::VectorXd > GetInitialTrajectory() const
virtual std::vector< CollisionProxy > GetRobotToWorldCollisionDistance(double check_margin)
double pseudo_huber_cost(double x, double beta)
exotica::KinematicTree & GetKinematicTree()
void SetGoalNEQ(const std::string &task_name, Eigen::VectorXdRefConst goal)
virtual bool isFixed() const
double GetRhoNEQ(const std::string &task_name)
void UpdateSceneFrames()
void SetInitialTrajectory(const std::vector< Eigen::VectorXd > &q_init_in)
Eigen::RowVectorXd GetScalarJacobian() const
std::vector< double > GetBounds()
#define ROS_MESSAGE_WRAPPER(MessageType)
Definition: pyexotica.cpp:112
std::vector< TaskSpaceVector > Phi
const Eigen::MatrixXd & get_X() const
std::vector< Eigen::MatrixXd > S
Eigen::VectorXd x
std::vector< Hessian > ddPhi_ddu
void SetStartState(Eigen::VectorXdRefConst x)
bool ParseBool(const std::string value)
static std::vector< std::string > GetMaps()
void Update(Eigen::VectorXdRefConst x_in, int t) override
Eigen::VectorXd GetEquality()
double GetWorldLinkScale() const
Eigen::MatrixXd GetS(const std::string &task_name, int t) const
virtual int TaskSpaceDim()=0
static std::vector< std::string > GetSolvers()
n
std::vector< size_t > free_idx
std::vector< Hessian > ddPhi_ddx
std::vector< TaskIndexing > indexing
Eigen::SparseMatrix< double > GetEqualityJacobian() const
Eigen::VectorXd GetGoal(const std::string &task_name) const
static std::shared_ptr< exotica::PlanningProblem > CreateProblem(const std::string &type, bool prepend=true)
PYBIND11_MODULE(_pyexotica, module)
Definition: pyexotica.cpp:670
std::shared_ptr< shapes::Shape > LoadOctreeAsShape(const std::string &file_path)
double GetDuration() const
anonymous
Eigen::VectorXd GetControlledState()
static handle cast(Initializer src, return_value_policy, handle)
Definition: pyexotica.cpp:555
static std::shared_ptr< exotica::MotionSolver > LoadSolver(const std::string &file_name)
double GetStartTime() const
std::string GetTypeName(const std::type_info &type)
void Delete(const std::string &path="")
const ControlDerivative & get_fu() const
Eigen::VectorXd GetGoal(const std::string &task_name)
virtual StateDerivative fx(const StateVector &x, const ControlVector &u)
void SetFloatingBaseLimitsPosXYZEulerZYX(const std::vector< double > &lower, const std::vector< double > &upper)
ScenePtr GetScene() const
void SetGoalEQ(const std::string &task_name, Eigen::VectorXdRefConst goal)
void SetProperty(const std::string &path, const std::string &property, const double &value)
int get_num_controls() const
Eigen::VectorXd GetRotationAsVector(const KDL::Frame &val, RotationType type)
double pseudo_huber_hessian(double x, double beta)
void PublishFrames(const std::string &tf_prefix="exotica")
void Update(Eigen::VectorXdRefConst x)
std::vector< TaskSpaceVector > Phi
static const std::string STRING_NAME
void set_integrator(Integrator integrator_in)
std::string PyAsStdString(PyObject *value_py)
Definition: pyexotica.cpp:66
std::vector< Eigen::MatrixXd > jacobian
BaseType GetControlledBaseType() const
const Eigen::MatrixXd & get_U() const
static std::shared_ptr< exotica::PlanningProblem > LoadProblem(const std::string &file_name)
const std::map< std::string, std::vector< std::string > > & GetModelLinkToCollisionLinkMap() const
std::vector< Eigen::MatrixXd > dPhi_dx
std::shared_ptr< KinematicElement > e2
static const std::string STRING_NAME
double GetRhoEQ(const std::string &task_name)
double huber_hessian(double x, double beta)
static std::shared_ptr< Setup > Instance()
void SetRho(const std::string &task_name, const double rho, int t=0)
double x
KDL::Frame GetFrame(Eigen::VectorXdRefConst val)
Eigen::VectorXd GetEquality() const
ControlDerivative fu_fd(const StateVector &x, const ControlVector &u)
virtual std::string Print(const std::string &prepend) const
void Update(Eigen::VectorXdRefConst x)
static void Load(std::string file_name, Initializer &solver, Initializer &problem, const std::string &solver_name="", const std::string &problem_name="", bool parsePathAsXML=false)
std::vector< TaskSpaceVector > Phi
PlanningProblemPtr GetProblem() const
bool HasAttachedObject(const std::string &name)
void DisplayTrajectory(Eigen::MatrixXdRefConst trajectory, double dt=1.0)
static const std::string STRING_NAME
void SetGoal(const std::string &task_name, Eigen::VectorXdRefConst goal)
#define WARNING(x)
StateDerivative fx_fd(const StateVector &x, const ControlVector &u)
void SetGoalTime(const double &t)
void RemoveTrajectory(const std::string &link)
Hessian array_hessian(pybind11::array_t< T > inArray)
Definition: pyexotica.cpp:564
static Vector Zero()
BoxQPSolution ExoticaBoxQP(const Eigen::MatrixXd &H, const Eigen::VectorXd &q, const Eigen::VectorXd &b_low, const Eigen::VectorXd &b_high, const Eigen::VectorXd &x_init, const double gamma, const int max_iterations, const double epsilon, const double lambda, bool use_polynomial_linesearch=false, bool use_cholesky_factorization=false)
void SetGoal(const std::string &task_name, Eigen::VectorXdRefConst goal)
const RigidBodyInertia & getInertia() const
void SetRobotLinkScale(const double scale)
std::shared_ptr< KinematicElement > e2
BoxQPSolution BoxQP(const Eigen::MatrixXd &H, const Eigen::VectorXd &q, const Eigen::VectorXd &b_low, const Eigen::VectorXd &b_high, const Eigen::VectorXd &x_init, const double th_acceptstep, const int max_iterations, const double th_gradient_tolerance, const double lambda, bool use_polynomial_linesearch=true, bool use_cholesky_factorization=true)
std::map< std::string, std::vector< double > > GetUsedJointLimits() const
std::shared_ptr< KinematicElement > e1
TaskSpaceVector Phi
void AddObjectToEnvironment(const std::string &name, const KDL::Frame &transform=KDL::Frame(), shapes::ShapeConstPtr shape=nullptr, const Eigen::Vector4d &color=Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const bool update_collision_scene=true)
bool IsPyString(PyObject *value_py)
Definition: pyexotica.cpp:57
TaskMapVec tasks
void SetGoal(const std::string &task_name, Eigen::VectorXdRefConst goal)
Eigen::VectorXd GetGoalNEQ(const std::string &task_name)
void SetTau(const double tau_in)
double GetScalarTaskCost(const std::string &task_name) const
void SetReplacePrimitiveShapesWithMeshes(const bool value)
Initializer CreateInitializer(const Initializer &init)
Definition: pyexotica.cpp:161
void SetRho(const std::string &task_name, const double rho_in)
static PyObject * InitializerToTuple(const Initializer &src)
Definition: pyexotica.cpp:471
Eigen::Vector3d normal1
double GetWorldLinkPadding() const
void SetGoalNEQ(const std::string &task_name, Eigen::VectorXdRefConst goal)
std::shared_ptr< const Shape > ShapeConstPtr
virtual ControlDerivative fu(const StateVector &x, const ControlVector &u)
Eigen::SparseMatrix< double > GetInequalityJacobian() const
int get_num_velocities() const


exotica_python
Author(s):
autogenerated on Mon Feb 22 2021 03:33:27