Updates trajopt planner to make it compatible with Ubuntu 20.04-ROS Noetic and other minor updates

robowflex_dart/include/robowflex_dart/gui.h

@@ -91,8 +91,8 @@ namespace robowflex
                 InteractiveCallback callback{};                         ///< Callback function on motion.
                 dart::dynamics::Frame *parent{dart::dynamics::Frame::World()};  ///< Parent frame.
                 double size{0.2};                                               ///< Size of marker.
-                double thickness{2.};      ///< Thickness of marker arrows.
-                bool obstructable{false};  ///< Is this frame obstructable?
+                double thickness{2.};                ///< Thickness of marker arrows.
+                bool obstructable{false};            ///< Is this frame obstructable?
 
                 bool linear[3]{true, true, true};    ///< Linear position controls enabled.
                 bool rotation[3]{true, true, true};  ///< Rotation ring controls enabled.
@@ -203,14 +203,14 @@ namespace robowflex
             const WorldPtr &getWorldConst() const;
 
         private:
-            WorldPtr world_;                  ///< World to visualize.
-            WindowWidgetPtr widget_;          ///< IMGUI widget.
-            std::vector<WidgetPtr> widgets_;  ///< Other widgets;
+            WorldPtr world_;                                   ///< World to visualize.
+            WindowWidgetPtr widget_;                           ///< IMGUI widget.
+            std::vector<WidgetPtr> widgets_;                   ///< Other widgets;
 
             std::shared_ptr<std::thread> animation_{nullptr};  ///< Animation thread.
 
-            ::osg::ref_ptr<Window> node_;  ///< OSG Node.
-            Viewer viewer_;                ///< Viewer
+            ::osg::ref_ptr<Window> node_;                      ///< OSG Node.
+            Viewer viewer_;                                    ///< Viewer
         };
 
         /** \brief Abstract class for IMGUI Widget.
@@ -531,21 +531,21 @@ namespace robowflex
             const double rotation_alpha_{0.6};   ///< Rotation bound alpha.
             const double rotation_width_{0.05};  ///< Rotation bound width.
 
-            const float max_position_{5.0f};  ///< Max position value.
-            const float drag_step_{0.01f};    ///< Slider drag amount.
+            const float max_position_{5.0f};     ///< Max position value.
+            const float drag_step_{0.01f};       ///< Slider drag amount.
 
-            float position_[3];  ///< GUI frame position.
-            float rotation_[3];  ///< GUI frame rotation.
+            float position_[3];                  ///< GUI frame position.
+            float rotation_[3];                  ///< GUI frame rotation.
 
-            float xp_[2];  ///< GUI X position bounds.
-            float yp_[2];  ///< GUI Y position bounds.
-            float zp_[2];  ///< GUI Z position bounds.
+            float xp_[2];                        ///< GUI X position bounds.
+            float yp_[2];                        ///< GUI Y position bounds.
+            float zp_[2];                        ///< GUI Z position bounds.
 
-            float xr_[2];  ///< GUI X orientation bounds.
-            float yr_[2];  ///< GUI Y orientation bounds.
-            float zr_[2];  ///< GUI Z orientation bounds.
+            float xr_[2];                        ///< GUI X orientation bounds.
+            float yr_[2];                        ///< GUI Y orientation bounds.
+            float zr_[2];                        ///< GUI Z orientation bounds.
 
-            float inner_radius{0.2};  ///< GUI Rotation bound inner radius.
+            float inner_radius{0.2};             ///< GUI Rotation bound inner radius.
 
             /** \} */
         };
@@ -587,16 +587,16 @@ namespace robowflex
             const int max_iteration_{200};       ///< Max iteration value.
             const float drag_tolerance_{0.01f};  ///< Slider drag for tolerance.
 
-            bool track_tsr_{false};     ///< Track the TSR by solving IK.
-            bool use_gradient_{false};  ///< Use gradient solving instead of built-in.
-            bool need_solve_{false};    ///< A solve is requested.
+            bool track_tsr_{false};              ///< Track the TSR by solving IK.
+            bool use_gradient_{false};           ///< Use gradient solving instead of built-in.
+            bool need_solve_{false};             ///< A solve is requested.
 
-            float step_;       ///< GUI gradient step size.
-            float limit_;      ///< GUI gradient limit.
-            float damping_;    ///< GUI SVD damping.
-            float tolerance_;  ///< GUI solver tolerance
-            int maxIter_;      ///< GUI maximum allowed iterations.
-            int item_{0};      ///< GUI solver.
+            float step_;                         ///< GUI gradient step size.
+            float limit_;                        ///< GUI gradient limit.
+            float damping_;                      ///< GUI SVD damping.
+            float tolerance_;                    ///< GUI solver tolerance
+            int maxIter_;                        ///< GUI maximum allowed iterations.
+            int item_{0};                        ///< GUI solver.
 
             /** \} */
 

robowflex_dart/include/robowflex_dart/joints.h

@@ -202,20 +202,20 @@ namespace robowflex
             /** \} */
 
         protected:
-            StateSpace *space_;  ///< State space this joint is for.
-            ompl::RNG &rng_;     ///< Random number generator.
+            StateSpace *space_;                 ///< State space this joint is for.
+            ompl::RNG &rng_;                    ///< Random number generator.
 
             std::vector<std::size_t> indices_;  ///< Indices this joint corresponds to.
             std::vector<std::string> dofs_;     ///< Controlled DoF names.
 
-            unsigned int skelIndex_;   ///< Index of skeleton.
-            unsigned int jointIndex_;  ///< Index of joint.
+            unsigned int skelIndex_;            ///< Index of skeleton.
+            unsigned int jointIndex_;           ///< Index of joint.
 
-            unsigned int startInSpace_;  ///< Start index in space configuration.
-            unsigned int sizeInSpace_;   ///< Size of joint in space configuration.
+            unsigned int startInSpace_;         ///< Start index in space configuration.
+            unsigned int sizeInSpace_;          ///< Size of joint in space configuration.
 
-            unsigned int startIndex_;  ///< Start index in joint.
-            unsigned int numDof_;      ///< Number of DoF this joint controls.
+            unsigned int startIndex_;           ///< Start index in joint.
+            unsigned int numDof_;               ///< Number of DoF this joint controls.
         };
 
         /** \brief A real vector joint of \a n dimensions.

robowflex_dart/include/robowflex_dart/planner.h

@@ -69,9 +69,10 @@ namespace robowflex
              *  \return Allocated planner.
              */
             template <typename T, typename... Args>
-            PlannerAllocator makePlanner(Args &&... args)
+            PlannerAllocator makePlanner(Args &&...args)
             {
-                return [&]() -> ompl::base::PlannerPtr {
+                return [&]() -> ompl::base::PlannerPtr
+                {
                     if (builder)
                     {
                         auto p = std::make_shared<T>(builder->info, std::forward<Args>(args)...);
@@ -94,7 +95,7 @@ namespace robowflex
             WorldPtr world_;            ///< DART world containing robot and scene.
             ompl::base::GoalPtr goal_;  ///< Current motion planning goal.
         };
-    }  // namespace darts
+    }                                   // namespace darts
 }  // namespace robowflex
 
 #endif

robowflex_dart/include/robowflex_dart/planning.h

@@ -411,10 +411,10 @@ namespace robowflex
             ompl::geometric::SimpleSetupPtr ss{nullptr};     ///< Simple Setup.
             WorldPtr world;                                  ///< World used for planning.
 
-            std::vector<TSRPtr> path_constraints;  ///< Path Constraints.
-            TSRConstraintPtr constraint{nullptr};  ///< OMPL Constraint for Path Constraints.
+            std::vector<TSRPtr> path_constraints;            ///< Path Constraints.
+            TSRConstraintPtr constraint{nullptr};            ///< OMPL Constraint for Path Constraints.
 
-            Eigen::VectorXd start;  ///< Start configuration.
+            Eigen::VectorXd start;                           ///< Start configuration.
 
             /** \brief Hyperparameter options.
              */

robowflex_dart/include/robowflex_dart/robot.h

@@ -303,7 +303,7 @@ namespace robowflex
                                                                                           ///< named states.
             std::map<std::string, std::vector<std::size_t>> group_indices_;  ///< Indices of group joints.
 
-            robowflex::RobotPtr robot_{nullptr};  ///< Robowflex robot.
+            robowflex::RobotPtr robot_{nullptr};                             ///< Robowflex robot.
         };
 
         /** \brief Load a robot from a URDF and SRDF (i.e., a MoveIt enabled robot).

robowflex_dart/include/robowflex_dart/space.h

@@ -254,21 +254,21 @@ namespace robowflex
             void addJoint(const std::string &group_name, const JointPtr &joint);
             void addJointToGroup(const std::string &group_name, const JointPtr &joint);
 
-            WorldPtr world_;  ///< World to use for planning.
+            WorldPtr world_;                              ///< World to use for planning.
 
             std::set<dart::dynamics::Joint *> jointset_;  ///< Set of joints used in planning.
             std::vector<std::size_t> indices_;            ///< Vector of indices for planning.
 
-            bool metric_{true};  ///< Is this space all Rn?
+            bool metric_{true};                           ///< Is this space all Rn?
 
-            std::vector<JointPtr> joints_;  ///< Vector of all joints used in planning.
+            std::vector<JointPtr> joints_;                ///< Vector of all joints used in planning.
 
-            ompl::RNG rng_;  ///< Random number generator.
+            ompl::RNG rng_;                               ///< Random number generator.
 
             std::map<std::string, std::vector<JointPtr>> group_joints_;  ///< Joints belonging to a group.
             std::map<std::string, std::size_t> group_dimension_;         ///< Dimension of the group.
         };
-    }  // namespace darts
+    }                                                                    // namespace darts
 }  // namespace robowflex
 
 #endif

robowflex_dart/include/robowflex_dart/structure.h

@@ -117,7 +117,6 @@ namespace robowflex
              */
             void dumpGraphViz(std::ostream &out, bool standalone = true);
 
-
             /** \} */
 
             /** \name Getting and Setting Configurations

robowflex_dart/include/robowflex_dart/tsr.h

@@ -83,7 +83,7 @@ namespace robowflex
                     std::string frame{magic::ROOT_FRAME};  ///< Name of base frame.
                 } base;                                    ///< Base frame.
 
-                RobotPose pose{RobotPose::Identity()};  ///< Pose of TSR.
+                RobotPose pose{RobotPose::Identity()};     ///< Pose of TSR.
 
                 struct
                 {
@@ -689,8 +689,8 @@ namespace robowflex
              */
             void computeBijection();
 
-            WorldPtr world_;      ///< Underlying world.
-            Specification spec_;  ///< TSR specification.
+            WorldPtr world_;                    ///< Underlying world.
+            Specification spec_;                ///< TSR specification.
 
             std::size_t skel_index_;            ///< Index of controlled skeleton.
             std::vector<std::size_t> indices_;  ///< Controlled indices.
@@ -983,20 +983,20 @@ namespace robowflex
             WorldPtr world_;                      ///< World to use.
             std::set<std::size_t> skel_indices_;  ///< All skeleton indices used by members of the set.
 
-            bool qr_{false};        ///< If true, use QR in gradient solve. Else, SVD.
-            bool damped_{true};     ///< If true, use damped SVD.
-            double step_{1.0};      ///< Step scaling in gradient.
-            double limit_{1.};      ///< Step size limit.
-            double damping_{1e-8};  ///< Damping factor.
+            bool qr_{false};                      ///< If true, use QR in gradient solve. Else, SVD.
+            bool damped_{true};                   ///< If true, use damped SVD.
+            double step_{1.0};                    ///< Step scaling in gradient.
+            double limit_{1.};                    ///< Step size limit.
+            double damping_{1e-8};                ///< Damping factor.
             double tolerance_{magic::DEFAULT_IK_TOLERANCE};  ///< Tolerance for solving.
             std::size_t maxIter_{50};                        ///< Maximum iterations to use for solving.
 
-            std::vector<TSRPtr> tsrs_;     ///< Set of TSRs
-            std::vector<double> weights_;  ///< Weights on TSRs
-            std::size_t dimension_{0};     ///< Total error dimension of set.
+            std::vector<TSRPtr> tsrs_;                       ///< Set of TSRs
+            std::vector<double> weights_;                    ///< Weights on TSRs
+            std::size_t dimension_{0};                       ///< Total error dimension of set.
 
-            Eigen::VectorXd upper_;  ///< Upper bounds on world configuration.
-            Eigen::VectorXd lower_;  ///< Lower bounds on world configuration.
+            Eigen::VectorXd upper_;                          ///< Upper bounds on world configuration.
+            Eigen::VectorXd lower_;                          ///< Lower bounds on world configuration.
         };
 
         /** \cond IGNORE */
@@ -1058,7 +1058,7 @@ namespace robowflex
             StateSpacePtr space_;  ///< Robot state space.
             TSRSetPtr tsr_;        ///< Set of TSR constraints.
         };
-    }  // namespace darts
+    }                              // namespace darts
 }  // namespace robowflex
 
 #endif

robowflex_dart/include/robowflex_dart/world.h

@@ -232,10 +232,10 @@ namespace robowflex
              */
             void removeSkeletonCollider(const std::string &name, const dart::dynamics::SkeletonPtr &skeleton);
 
-            dart::simulation::WorldPtr world_;  ///< Underlying world.
+            dart::simulation::WorldPtr world_;                ///< Underlying world.
 
-            Eigen::Vector3d low_{-5, -5, -5};  ///< Lower workspace bounds.
-            Eigen::Vector3d high_{5, 5, 5};    ///< Upper workspace bounds.
+            Eigen::Vector3d low_{-5, -5, -5};                 ///< Lower workspace bounds.
+            Eigen::Vector3d high_{5, 5, 5};                   ///< Upper workspace bounds.
 
             std::map<std::string, RobotPtr> robots_;          ///< Robots in world.
             std::map<std::string, StructurePtr> structures_;  ///< Structures in world.
@@ -258,9 +258,9 @@ namespace robowflex
             dart::collision::CollisionDetectorPtr collider_;                     ///< Collision checker.
             const std::string name_;                                             ///< Name of world.
 
-            std::recursive_mutex mutex_;  ///< Internal lock.
+            std::recursive_mutex mutex_;                                         ///< Internal lock.
         };
-    }  // namespace darts
+    }                                                                            // namespace darts
 }  // namespace robowflex
 
 #endif

robowflex_dart/scripts/fetch_bimanual.cpp

@@ -38,7 +38,8 @@ int main(int /*argc*/, char ** /*argv*/)
     //
     // Initial goal - touch fingertips
     //
-    const auto &touch = [&] {
+    const auto &touch = [&]
+    {
         darts::PlanBuilder builder(world);
         builder.addGroup("fetch1", "arm_with_torso");
         builder.addGroup("fetch2", "arm_with_torso");
@@ -87,7 +88,8 @@ int main(int /*argc*/, char ** /*argv*/)
         return solved;
     };
 
-    const auto &dance = [&] {
+    const auto &dance = [&]
+    {
         darts::PlanBuilder builder(world);
         builder.addGroup("fetch1", "arm_with_torso");
         builder.addGroup("fetch2", "arm_with_torso");
@@ -132,16 +134,18 @@ int main(int /*argc*/, char ** /*argv*/)
         return solved;
     };
 
-    window.run([&]() {
-        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-        while (true)
+    window.run(
+        [&]()
         {
             std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-
-            auto solved = touch();
-            if (solved)
-                dance();
-        }
-    });
+            while (true)
+            {
+                std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+                auto solved = touch();
+                if (solved)
+                    dance();
+            }
+        });
     return 0;
 }

robowflex_dart/scripts/fetch_ik.cpp

@@ -68,26 +68,28 @@ int main(int /*argc*/, char ** /*argv*/)
     builder.setup();
     builder.ss->print();
 
-    window.run([&]() {
-        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
-        while (true)
+    window.run(
+        [&]()
         {
-            goal->startSampling();
-            ompl::base::PlannerStatus solved = builder.ss->solve(30.0);
-            goal->stopSampling();
-
-            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-            if (solved)
+            std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+            while (true)
             {
-                RBX_INFO("Found solution!");
-                window.animatePath(builder, builder.getSolutionPath());
+                goal->startSampling();
+                ompl::base::PlannerStatus solved = builder.ss->solve(30.0);
+                goal->stopSampling();
+
+                std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+                if (solved)
+                {
+                    RBX_INFO("Found solution!");
+                    window.animatePath(builder, builder.getSolutionPath());
+                }
+                else
+                    RBX_WARN("No solution found");
+
+                builder.ss->clear();
             }
-            else
-                RBX_WARN("No solution found");
-
-            builder.ss->clear();
-        }
-    });
+        });
 
     return 0;
 }