(PyKDL) expose ChainExternalWrenchEstimator

Author: PeterBowman

python_orocos_kdl/PyKDL/dynamics.cpp

@@ -26,6 +26,7 @@
 #include <iostream>
 #include <iomanip>
 #include <kdl/chaindynparam.hpp>
+#include <kdl/chainexternalwrenchestimator.hpp>
 #include <kdl/jntspaceinertiamatrix.hpp>
 #include <kdl/kinfam_io.hpp>
 #include "PyKDL.h"
@@ -90,4 +91,25 @@ void init_dynamics(pybind11::module &m)
     chain_dyn_param.def("JntToCoriolis", &ChainDynParam::JntToCoriolis, py::arg("q"), py::arg("q_dot"), py::arg("coriolis"));
     chain_dyn_param.def("JntToMass", &ChainDynParam::JntToMass, py::arg("q"), py::arg("H"));
     chain_dyn_param.def("JntToGravity", &ChainDynParam::JntToGravity, py::arg("q"), py::arg("gravity"));
+
+    // --------------------
+    // ChainExternalWrenchEstimator
+    // --------------------
+    py::class_<ChainExternalWrenchEstimator> chain_ext_wrench_estimator(m, "ChainExternalWrenchEstimator");
+    chain_ext_wrench_estimator.def(py::init<const Chain&, const Vector&, const double, const double, const double, const double, const int>(),
+                                   py::arg("chain"), py::arg("gravity"), py::arg("sample_frequency"),
+                                   py::arg("estimation_gain"), py::arg("filter_constant"),
+                                   py::arg("eps")=0.00001, py::arg("maxiter")=150);
+    chain_ext_wrench_estimator.def("setInitialMomentum", &ChainExternalWrenchEstimator::setInitialMomentum,
+                                   py::arg("joint_position"), py::arg("joint_velocity"));
+    chain_ext_wrench_estimator.def("setSVDEps", &ChainExternalWrenchEstimator::setSVDEps, py::arg("eps_in"));
+    chain_ext_wrench_estimator.def("setSVDMaxIter", &ChainExternalWrenchEstimator::setSVDMaxIter, py::arg("maxiter_in"));
+    chain_ext_wrench_estimator.def("JntToExtWrench", &ChainExternalWrenchEstimator::JntToExtWrench,
+                                   py::arg("joint_position"), py::arg("joint_velocity"),
+                                   py::arg("joint_torque"), py::arg("external_wrench"));
+    chain_ext_wrench_estimator.def("getEstimatedJntTorque", &ChainExternalWrenchEstimator::getEstimatedJntTorque,
+                                   py::arg("external_joint_torque"));
+    chain_ext_wrench_estimator.def("updateInternalDataStructures", &ChainExternalWrenchEstimator::updateInternalDataStructures);
+    chain_ext_wrench_estimator.def("strError", &ChainExternalWrenchEstimator::strError,
+                                   py::arg("error"));
 }