Kinematics wip

Rhoban · Nov 10, 2022 · abe0691 · abe0691
1 parent 1866409
commit abe0691
Show file tree

Hide file tree

Showing 22 changed files with 783 additions and 562 deletions.
diff --git a/.clang-format b/.clang-format
@@ -0,0 +1,67 @@
+---
+BasedOnStyle:  Google
+AccessModifierOffset: -2
+ConstructorInitializerIndentWidth: 2
+AlignEscapedNewlinesLeft: false
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: None
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakTemplateDeclarations: true
+AlwaysBreakBeforeMultilineStrings: false
+BreakBeforeBinaryOperators: false
+BreakBeforeTernaryOperators: false
+BreakConstructorInitializersBeforeComma: true
+BinPackParameters: true
+ColumnLimit:    120
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+DerivePointerBinding: false
+PointerBindsToType: true
+ExperimentalAutoDetectBinPacking: false
+IndentCaseLabels: true
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 60
+PenaltyBreakString: 1
+PenaltyBreakFirstLessLess: 1000
+PenaltyExcessCharacter: 1000
+PenaltyReturnTypeOnItsOwnLine: 90
+SpacesBeforeTrailingComments: 2
+Cpp11BracedListStyle: false
+Standard:        Auto
+IndentWidth:     2
+TabWidth:        2
+UseTab:          Never
+IndentFunctionDeclarationAfterType: false
+SpacesInParentheses: false
+SpacesInAngles:  false
+SpaceInEmptyParentheses: false
+SpacesInCStyleCastParentheses: false
+SpaceAfterControlStatementKeyword: true
+SpaceBeforeAssignmentOperators: true
+ContinuationIndentWidth: 4
+SortIncludes: false
+SpaceAfterCStyleCast: false
+
+# Configure each individual brace in BraceWrapping
+BreakBeforeBraces: Custom
+
+# Control of individual brace wrapping cases
+BraceWrapping: {
+    AfterClass: 'true'
+    AfterControlStatement: 'true'
+    AfterEnum : 'true'
+    AfterFunction : 'true'
+    AfterNamespace : 'true'
+    AfterStruct : 'true'
+    AfterUnion : 'true'
+    BeforeCatch : 'true'
+    BeforeElse : 'true'
+    IndentBraces : 'false'
+}
+...
+
diff --git a/bindings/expose-eigen.cpp b/bindings/expose-eigen.cpp
@@ -6,8 +6,9 @@
 
 using namespace boost::python;
 
-void exposeAffine3d() {
-  auto toString = +[](const Eigen::Affine3d &a) {
+void exposeAffine3d()
+{
+  auto toString = +[](const Eigen::Affine3d& a) {
     std::ostringstream oss;
     oss << "[Affine3d]" << std::endl;
     oss << "t: " << a.translation().transpose() << std::endl;
@@ -18,30 +19,23 @@ void exposeAffine3d() {
   class_<Eigen::Affine3d>("Affine3d")
       .def(
           "from_matrix",
-          +[](const Eigen::Matrix4d &m) {
+          +[](const Eigen::Matrix4d& m) {
             Eigen::Affine3d result;
             result.matrix() = m;
             return result;
           })
       .staticmethod("from_matrix")
       .add_property(
-          "mat",
-          +[](const Eigen::Affine3d &a) { return (Eigen::Matrix4d)a.matrix(); },
-          +[](Eigen::Affine3d &a, const Eigen::Matrix4d &m) { a.matrix() = m; })
+          "mat", +[](const Eigen::Affine3d& a) { return (Eigen::Matrix4d)a.matrix(); },
+          +[](Eigen::Affine3d& a, const Eigen::Matrix4d& m) { a.matrix() = m; })
       .add_property(
-          "t",
-          +[](const Eigen::Affine3d &a) {
-            return (Eigen::Vector3d)a.translation();
-          },
-          +[](Eigen::Affine3d &a, const Eigen::Vector3d &t) {
-            a.translation() = t;
-          })
+          "t", +[](const Eigen::Affine3d& a) { return (Eigen::Vector3d)a.translation(); },
+          +[](Eigen::Affine3d& a, const Eigen::Vector3d& t) { a.translation() = t; })
       .add_property(
-          "R",
-          +[](const Eigen::Affine3d &a) { return (Eigen::Matrix3d)a.linear(); },
-          +[](Eigen::Affine3d &a, const Eigen::Matrix3d &m) { a.linear() = m; })
+          "R", +[](const Eigen::Affine3d& a) { return (Eigen::Matrix3d)a.linear(); },
+          +[](Eigen::Affine3d& a, const Eigen::Matrix3d& m) { a.linear() = m; })
       .def(
-          "inv", +[](const Eigen::Affine3d &a) { return a.inverse(); })
+          "inv", +[](const Eigen::Affine3d& a) { return a.inverse(); })
       .def("__repr__", toString)
       .def("__str__", toString)
       .def(self * other<Eigen::Vector3d>())

diff --git a/bindings/expose-footsteps.cpp b/bindings/expose-footsteps.cpp
@@ -10,7 +10,8 @@
 using namespace boost::python;
 using namespace placo;
 
-void exposeFootsteps() {
+void exposeFootsteps()
+{
   enum_<FootstepsPlanner::Side>("FootstepsPlanner_Side")
       .value("left", FootstepsPlanner::Side::Left)
       .value("right", FootstepsPlanner::Side::Right);
@@ -24,15 +25,11 @@ void exposeFootsteps() {
       .def("support_polygon", &FootstepsPlanner::Support::support_polygon)
       .add_property("footsteps", &FootstepsPlanner::Support::footsteps);
 
-  class_<FootstepsPlannerNaive>(
-      "FootstepsPlannerNaive",
-      init<std::string, Eigen::Affine3d, Eigen::Affine3d, double>())
+  class_<FootstepsPlannerNaive>("FootstepsPlannerNaive", init<std::string, Eigen::Affine3d, Eigen::Affine3d, double>())
       .def("plan", &FootstepsPlannerNaive::plan)
       .def("make_double_supports", &FootstepsPlannerNaive::make_double_supports)
-      .add_property("foot_width", &FootstepsPlannerNaive::foot_width,
-                    &FootstepsPlannerNaive::foot_width)
-      .add_property("foot_length", &FootstepsPlannerNaive::foot_length,
-                    &FootstepsPlannerNaive::foot_length);
+      .add_property("foot_width", &FootstepsPlannerNaive::foot_width, &FootstepsPlannerNaive::foot_width)
+      .add_property("foot_length", &FootstepsPlannerNaive::foot_length, &FootstepsPlannerNaive::foot_length);
 
   // Exposing vector of footsteps
   exposeStdVector<FootstepsPlanner::Footstep>("Footsteps");

diff --git a/bindings/expose-jerk-planner.cpp b/bindings/expose-jerk-planner.cpp
@@ -13,15 +13,16 @@
 using namespace boost::python;
 using namespace placo;
 
-void exposeJerkPlanner() {
+void exposeJerkPlanner()
+{
   enum_<JerkPlanner::ConstraintType>("ConstraintType")
       .value("position", JerkPlanner::ConstraintType::Position)
       .value("velocity", JerkPlanner::ConstraintType::Velocity)
       .value("acceleration", JerkPlanner::ConstraintType::Acceleration)
       .value("zmp", JerkPlanner::ConstraintType::ZMP)
       .value("dcm", JerkPlanner::ConstraintType::DCM);
 
-  class_<JerkPlanner::Constraint>("Constraint", init<JerkPlanner &>())
+  class_<JerkPlanner::Constraint>("Constraint", init<JerkPlanner&>())
       .def("is_active", &JerkPlanner::Constraint::is_active);
 
   class_<JerkPlanner::JerkTrajectory2D>("JerkTrajectory2D", init<double>())
@@ -30,7 +31,7 @@ void exposeJerkPlanner() {
       .def("vel", &JerkPlanner::JerkTrajectory2D::vel)
       .def("acc", &JerkPlanner::JerkTrajectory2D::acc);
 
-  auto toString = +[](const JerkPlanner &planner) {
+  auto toString = +[](const JerkPlanner& planner) {
     std::ostringstream oss;
     oss << "[JerkPlanner]" << std::endl;
     oss << "* steps: " << planner.N << std::endl;
@@ -40,12 +41,10 @@ void exposeJerkPlanner() {
     return oss.str();
   };
 
-  class_<JerkPlanner>("JerkPlanner",
-                      init<int, JerkPlanner::State, double, double>())
+  class_<JerkPlanner>("JerkPlanner", init<int, JerkPlanner::State, double, double>())
       .def("add_equality_constraint", &JerkPlanner::add_equality_constraint)
       .def("add_lower_than_constraint", &JerkPlanner::add_lower_than_constraint)
-      .def("add_greater_than_constraint",
-           &JerkPlanner::add_greater_than_constraint)
+      .def("add_greater_than_constraint", &JerkPlanner::add_greater_than_constraint)
       .def("add_polygon_constraint", &JerkPlanner::add_polygon_constraint)
       .def("add_limit_constraint", &JerkPlanner::add_limit_constraint)
       .def("plan", &JerkPlanner::plan)

diff --git a/bindings/expose-kinematics.cpp b/bindings/expose-kinematics.cpp
@@ -9,34 +9,33 @@
 using namespace boost::python;
 using namespace placo;
 
-void exposeKinematics() {
-
-  class_<KinematicsSolver>("KinematicsSolver", init<MobileRobot &>())
+void exposeKinematics()
+{
+  class_<KinematicsSolver>("KinematicsSolver", init<MobileRobot&>())
       // Position and CoM task
-      .def<void (KinematicsSolver::*)(std::string, Eigen::Vector3d, std::string,
-                                      double)>(
+      .def<void (KinematicsSolver::*)(std::string, Eigen::Vector3d, std::string, double)>(
           "add_position_task", &KinematicsSolver::add_position_task)
-      .def<void (KinematicsSolver::*)(Eigen::Vector3d, std::string, double)>(
-          "add_com_task", &KinematicsSolver::add_com_task)
+      .def<void (KinematicsSolver::*)(Eigen::Vector3d, std::string, double)>("add_com_task",
+                                                                             &KinematicsSolver::add_com_task)
 
       // Orientation task
-      .def<void (KinematicsSolver::*)(std::string, Eigen::Matrix3d, std::string,
-                                      double)>(
+      .def<void (KinematicsSolver::*)(std::string, Eigen::Matrix3d, std::string, double)>(
           "add_orientation_task", &KinematicsSolver::add_orientation_task)
 
       // Frame task
-      .def<void (KinematicsSolver::*)(std::string, Eigen::Affine3d, std::string,
-                                      double, double)>(
+      .def<void (KinematicsSolver::*)(std::string, Eigen::Affine3d, std::string, double, double)>(
           "add_frame_task", &KinematicsSolver::add_frame_task)
 
       // Pose task
-      .def<void (KinematicsSolver::*)(std::string, Eigen::Affine3d, std::string,
-                                      double)>("add_pose_task",
-                                               &KinematicsSolver::add_pose_task)
+      .def<void (KinematicsSolver::*)(std::string, Eigen::Affine3d, std::string, double)>(
+          "add_pose_task", &KinematicsSolver::add_pose_task)
 
       // Regularization task
-      .def("add_regularization_task",
-           &KinematicsSolver::add_regularization_task)
+      .def("add_regularization_task", &KinematicsSolver::add_regularization_task)
+
+      // Masking/unmasking DoFs
+      .def("mask_dof", &KinematicsSolver::mask_dof)
+      .def("unmask_dof", &KinematicsSolver::unmask_dof)
 
       .def("solve", &KinematicsSolver::solve);
 

diff --git a/bindings/expose-mobile-robot.cpp b/bindings/expose-mobile-robot.cpp
@@ -9,18 +9,15 @@
 using namespace boost::python;
 using namespace placo;
 
-void exposeMobileRobot() {
+void exposeMobileRobot()
+{
   class_<MobileRobot::State>("MobileRobot_State")
       .add_property(
-          "q", +[](const MobileRobot::State &state) { return state.q; },
-          +[](MobileRobot::State &state, const Eigen::VectorXd &q) {
-            state.q = q;
-          })
+          "q", +[](const MobileRobot::State& state) { return state.q; },
+          +[](MobileRobot::State& state, const Eigen::VectorXd& q) { state.q = q; })
       .add_property(
-          "qd", +[](const MobileRobot::State &state) { return state.qd; },
-          +[](MobileRobot::State &state, const Eigen::VectorXd &qd) {
-            state.qd = qd;
-          });
+          "qd", +[](const MobileRobot::State& state) { return state.qd; },
+          +[](MobileRobot::State& state, const Eigen::VectorXd& qd) { state.qd = qd; });
 
   class_<MobileRobot::Collision>("Collision")
       .add_property("bodyA", &MobileRobot::Collision::bodyA)
@@ -43,12 +40,10 @@ void exposeMobileRobot() {
       .def("joint_names", &MobileRobot::joint_names)
       .def("frame_names", &MobileRobot::frame_names)
       .def("self_collisions", &MobileRobot::self_collisions)
-      .def<Eigen::Affine3d (MobileRobot::*)(const std::string &)>(
-          "get_T_world_frame", &MobileRobot::get_T_world_frame)
-      .def<void (MobileRobot::*)(const std::string &, Eigen::Affine3d)>(
-          "set_T_world_frame", &MobileRobot::set_T_world_frame)
-      .def<Eigen::MatrixXd (MobileRobot::*)(const std::string &)>(
-          "frame_jacobian", &MobileRobot::frame_jacobian)
+      .def<Eigen::Affine3d (MobileRobot::*)(const std::string&)>("get_T_world_frame", &MobileRobot::get_T_world_frame)
+      .def<void (MobileRobot::*)(const std::string&, Eigen::Affine3d)>("set_T_world_frame",
+                                                                       &MobileRobot::set_T_world_frame)
+      .def<Eigen::MatrixXd (MobileRobot::*)(const std::string&)>("frame_jacobian", &MobileRobot::frame_jacobian)
       .def("com_jacobian", &MobileRobot::com_jacobian);
 
   exposeStdVector<MobileRobot::Collision>("vector_Collision");

diff --git a/bindings/expose-utils.cpp b/bindings/expose-utils.cpp
@@ -14,7 +14,7 @@ void exposeUtils()
   def("frame_yaw", &placo::frame_yaw);
   def("frame", &placo::frame);
   def("flatten_on_floor", &placo::flatten_on_floor);
-  
+
   exposeStdVector<int>("vector_int");
   exposeStdVector<double>("vector_double");
   exposeStdVector<std::string>("vector_string");

diff --git a/bindings/expose-utils.hpp b/bindings/expose-utils.hpp
@@ -6,33 +6,33 @@ using namespace boost::python;
 
 // From http://bazaar.launchpad.net/~yade-dev/yade/trunk/view/head:/py/wrapper/customConverters.cpp#L127
 // Allow conversions from list to some std::vector
-template <typename containedType> struct custom_vector_from_seq {
-  custom_vector_from_seq() {
-    converter::registry::push_back(&convertible, &construct,
-                                   type_id<std::vector<containedType>>());
+template <typename containedType>
+struct custom_vector_from_seq
+{
+  custom_vector_from_seq()
+  {
+    converter::registry::push_back(&convertible, &construct, type_id<std::vector<containedType>>());
   }
-  static void *convertible(PyObject *obj_ptr) {
+  static void* convertible(PyObject* obj_ptr)
+  {
     // the second condition is important, for some reason otherwise there were
     // attempted conversions of Body to list which failed afterwards.
-    if (!PySequence_Check(obj_ptr) ||
-        !PyObject_HasAttrString(obj_ptr, "__len__"))
+    if (!PySequence_Check(obj_ptr) || !PyObject_HasAttrString(obj_ptr, "__len__"))
       return 0;
     return obj_ptr;
   }
-  static void construct(PyObject *obj_ptr,
-                        converter::rvalue_from_python_stage1_data *data) {
-    void *storage =
-        ((converter::rvalue_from_python_storage<std::vector<containedType>>
-              *)(data))
-            ->storage.bytes;
+  static void construct(PyObject* obj_ptr, converter::rvalue_from_python_stage1_data* data)
+  {
+    void* storage = ((converter::rvalue_from_python_storage<std::vector<containedType>>*)(data))->storage.bytes;
     new (storage) std::vector<containedType>();
-    std::vector<containedType> *v = (std::vector<containedType> *)(storage);
+    std::vector<containedType>* v = (std::vector<containedType>*)(storage);
     int l = PySequence_Size(obj_ptr);
     if (l < 0)
       abort(); /*std::cerr<<"l="<<l<<";
                   "<<typeid(containedType).name()<<std::endl;*/
     v->reserve(l);
-    for (int i = 0; i < l; i++) {
+    for (int i = 0; i < l; i++)
+    {
       v->push_back(extract<containedType>(PySequence_GetItem(obj_ptr, i)));
     }
     data->convertible = storage;
@@ -44,7 +44,9 @@ template <typename containedType> struct custom_vector_from_seq {
  * @tparam T type
  * @param name name of the type as exposed in Python
  */
-template <typename T> void exposeStdVector(const std::string &class_name) {
+template <typename T>
+void exposeStdVector(const std::string& class_name)
+{
   typedef typename std::vector<T> vector_T;
 
   class_<vector_T>(class_name.c_str()).def(vector_indexing_suite<vector_T>());

diff --git a/bindings/module.cpp b/bindings/module.cpp
@@ -8,7 +8,8 @@
 
 #include "module.h"
 
-BOOST_PYTHON_MODULE(placo) {
+BOOST_PYTHON_MODULE(placo)
+{
   using namespace boost::python;
 
   exposeAffine3d();

diff --git a/python/examples-affine3d.py b/python/examples-affine3d.py
@@ -18,4 +18,4 @@
 print(frame * frame)
 
 print("* A frame times its inverse")
-print(frame * frame.inv())
+print(frame * frame.inv())