format

Author: nim65s

include/hpp/rbprm/contact_generation/algorithm.hh

@@ -17,16 +17,16 @@
 // <http://www.gnu.org/licenses/>.
 
 #ifndef HPP_RBPRM_ALGORITHM_HH
-# define HPP_RBPRM_ALGORITHM_HH
+#define HPP_RBPRM_ALGORITHM_HH
 
-# include <hpp/rbprm/reports.hh>
-# include <hpp/rbprm/contact_generation/contact_generation.hh>
+#include <hpp/rbprm/reports.hh>
+#include <hpp/rbprm/contact_generation/contact_generation.hh>
 
-# include <queue>
+#include <queue>
 
 namespace hpp {
 namespace rbprm {
-namespace contact{
+namespace contact {
 
 /// Generates one step of the contact planner.
 /// First, generates all possible cases of contact maintenance (feasible).
@@ -37,7 +37,6 @@ namespace contact{
 /// \return whether a step was successfully generated
 ContactReport HPP_RBPRM_DLLAPI oneStep(ContactGenHelper& helper);
 
-
 /// Generates a balanced contact configuration, considering the
 /// given current configuration of the robot, and a direction of motion.
 /// Typically used to generate a start and / or goal configuration automatically for a planning problem.
@@ -48,13 +47,15 @@ ContactReport HPP_RBPRM_DLLAPI oneStep(ContactGenHelper& helper);
 /// \param affFilters a vector of strings determining which affordance
 ///  types are to be used in generating contacts for each limb.
 /// \param direction An estimation of the direction of motion of the character.
-/// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
-/// \return a State describing the computed contact configuration, with relevant contact information and balance information.
-hpp::rbprm::State HPP_RBPRM_DLLAPI ComputeContacts(
-  const hpp::rbprm::RbPrmFullBodyPtr_t& body, pinocchio::ConfigurationIn_t configuration,
-  const affMap_t& affordances,
-  const std::map<std::string, std::vector<std::string> >& affFilters, const fcl::Vec3f& direction,
-  const double robustnessTreshold = 0, const fcl::Vec3f& acceleration = fcl::Vec3f(0,0,0));
+/// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the
+/// configuration (0 by default). \return a State describing the computed contact configuration, with relevant contact
+/// information and balance information.
+hpp::rbprm::State HPP_RBPRM_DLLAPI ComputeContacts(const hpp::rbprm::RbPrmFullBodyPtr_t& body,
+                                                   pinocchio::ConfigurationIn_t configuration,
+                                                   const affMap_t& affordances,
+                                                   const std::map<std::string, std::vector<std::string> >& affFilters,
+                                                   const fcl::Vec3f& direction, const double robustnessTreshold = 0,
+                                                   const fcl::Vec3f& acceleration = fcl::Vec3f(0, 0, 0));
 
 /// Generates a balanced contact configuration, considering the
 /// given current configuration of the robot, and a previous, balanced configuration.
@@ -68,21 +69,20 @@ hpp::rbprm::State HPP_RBPRM_DLLAPI ComputeContacts(
 /// \param affFilters a vector of strings determining which affordance
 ///  types are to be used in generating contacts for each limb.
 /// \param direction An estimation of the direction of motion of the character.
-/// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
-/// \param acceleration acceleration on the CoM estimated by the planning
-/// \param comPath : path found by the planning
-/// \param currentPathId : timing inside comPath such that comPath(currentPathId) == configuration (for the freeflyer DOFs)
-/// \return a State describing the computed contact configuration, with relevant contact information and balance information.
-hpp::rbprm::contact::ContactReport HPP_RBPRM_DLLAPI ComputeContacts(
-        const hpp::rbprm::State& previous, const hpp::rbprm::RbPrmFullBodyPtr_t& body,
-        pinocchio::ConfigurationIn_t configuration,
-            const affMap_t& affordances,
-        const std::map<std::string, std::vector<std::string> >& affFilters, const fcl::Vec3f& direction,
-  const double robustnessTreshold = 0,const fcl::Vec3f& acceleration = fcl::Vec3f(0,0,0),
-        const core::PathConstPtr_t& comPath = core::PathPtr_t(),const double currentPathId=0,const bool testReachability=true, const bool quasiStatic=false);
-
+/// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the
+/// configuration (0 by default). \param acceleration acceleration on the CoM estimated by the planning \param comPath
+/// : path found by the planning \param currentPathId : timing inside comPath such that comPath(currentPathId) ==
+/// configuration (for the freeflyer DOFs) \return a State describing the computed contact configuration, with relevant
+/// contact information and balance information.
+hpp::rbprm::contact::ContactReport HPP_RBPRM_DLLAPI
+ComputeContacts(const hpp::rbprm::State& previous, const hpp::rbprm::RbPrmFullBodyPtr_t& body,
+                pinocchio::ConfigurationIn_t configuration, const affMap_t& affordances,
+                const std::map<std::string, std::vector<std::string> >& affFilters, const fcl::Vec3f& direction,
+                const double robustnessTreshold = 0, const fcl::Vec3f& acceleration = fcl::Vec3f(0, 0, 0),
+                const core::PathConstPtr_t& comPath = core::PathPtr_t(), const double currentPathId = 0,
+                const bool testReachability = true, const bool quasiStatic = false);
 
-    } // namespace contact
-  } // namespace rbprm
-} // namespace hpp
-#endif // HPP_RBPRM_ALGORITHM_HH
+}  // namespace contact
+}  // namespace rbprm
+}  // namespace hpp
+#endif  // HPP_RBPRM_ALGORITHM_HH

include/hpp/rbprm/contact_generation/contact_generation.hh

@@ -17,68 +17,59 @@
 // <http://www.gnu.org/licenses/>.
 
 #ifndef HPP_RBPRM_CONTACT_GENERATION_HH
-# define HPP_RBPRM_CONTACT_GENERATION_HH
+#define HPP_RBPRM_CONTACT_GENERATION_HH
 
-# include <hpp/rbprm/rbprm-state.hh>
-# include <hpp/rbprm/rbprm-fullbody.hh>
-# include <hpp/rbprm/projection/projection.hh>
-# include <queue>
+#include <hpp/rbprm/rbprm-state.hh>
+#include <hpp/rbprm/rbprm-fullbody.hh>
+#include <hpp/rbprm/projection/projection.hh>
+#include <queue>
 
 namespace hpp {
 namespace rbprm {
-namespace contact{
+namespace contact {
 
 typedef std::queue<hpp::rbprm::State> Q_State;
-typedef std::pair <hpp::rbprm::State, std::vector<std::string> > ContactState;
+typedef std::pair<hpp::rbprm::State, std::vector<std::string> > ContactState;
 typedef std::queue<ContactState> T_ContactState;
 
-struct ContactGenHelper
-{
-     ContactGenHelper(RbPrmFullBodyPtr_t fb, const State& ps,
-                      pinocchio::ConfigurationIn_t configuration,
-                      const affMap_t& affordances,
-                      const std::map<std::string, std::vector<std::string> >& affFilters,
-                      const double robustnessTreshold = 0,
-                      const std::size_t maxContactBreaks = 1,
-                      const std::size_t maxContactCreations = 1,
-                      const bool checkStabilityMaintain = false,
-                      const bool checkStabilityGenerate = true,
-                      const fcl::Vec3f& direction = fcl::Vec3f(0,0,1),
-                      const fcl::Vec3f& acceleration = fcl::Vec3f(0,0,0),
-                      const bool contactIfFails = false,
-                      const bool stableForOneContact = false,
-                      const core::PathConstPtr_t& comPath=core::PathConstPtr_t(),
-                      const double currentPathId=0);
-    ~ContactGenHelper(){}
-    hpp::rbprm::RbPrmFullBodyPtr_t fullBody_;
-    const hpp::rbprm::State previousState_;
-    const bool checkStabilityMaintain_;
-    bool contactIfFails_;
-    const bool stableForOneContact_;
-    const fcl::Vec3f acceleration_;
-    const fcl::Vec3f direction_;
-    const double robustnessTreshold_;
-    const std::size_t maxContactBreaks_;
-    const std::size_t maxContactCreations_;
-    const affMap_t& affordances_;
-    const std::map<std::string, std::vector<std::string> >& affFilters_;
-    hpp::rbprm::State workingState_;
-    bool checkStabilityGenerate_;
-    Q_State candidates_;
-    const core::PathConstPtr_t comPath_;
-    const double currentPathId_;
-    bool quasiStatic_;
-    bool testReachability_;
-    const bool maximiseContacts_;
-    const bool accept_unreachable_;
-    const bool tryQuasiStatic_;
-    const int reachabilityPointPerPhases_;
-
+struct ContactGenHelper {
+  ContactGenHelper(RbPrmFullBodyPtr_t fb, const State& ps, pinocchio::ConfigurationIn_t configuration,
+                   const affMap_t& affordances, const std::map<std::string, std::vector<std::string> >& affFilters,
+                   const double robustnessTreshold = 0, const std::size_t maxContactBreaks = 1,
+                   const std::size_t maxContactCreations = 1, const bool checkStabilityMaintain = false,
+                   const bool checkStabilityGenerate = true, const fcl::Vec3f& direction = fcl::Vec3f(0, 0, 1),
+                   const fcl::Vec3f& acceleration = fcl::Vec3f(0, 0, 0), const bool contactIfFails = false,
+                   const bool stableForOneContact = false,
+                   const core::PathConstPtr_t& comPath = core::PathConstPtr_t(), const double currentPathId = 0);
+  ~ContactGenHelper() {}
+  hpp::rbprm::RbPrmFullBodyPtr_t fullBody_;
+  const hpp::rbprm::State previousState_;
+  const bool checkStabilityMaintain_;
+  bool contactIfFails_;
+  const bool stableForOneContact_;
+  const fcl::Vec3f acceleration_;
+  const fcl::Vec3f direction_;
+  const double robustnessTreshold_;
+  const std::size_t maxContactBreaks_;
+  const std::size_t maxContactCreations_;
+  const affMap_t& affordances_;
+  const std::map<std::string, std::vector<std::string> >& affFilters_;
+  hpp::rbprm::State workingState_;
+  bool checkStabilityGenerate_;
+  Q_State candidates_;
+  const core::PathConstPtr_t comPath_;
+  const double currentPathId_;
+  bool quasiStatic_;
+  bool testReachability_;
+  const bool maximiseContacts_;
+  const bool accept_unreachable_;
+  const bool tryQuasiStatic_;
+  const int reachabilityPointPerPhases_;
 };
 
-
-std::vector<hpp::pinocchio::CollisionObjectPtr_t> HPP_RBPRM_DLLAPI getAffObjectsForLimb(const std::string& limb,
-    const affMap_t& affordances, const std::map<std::string, std::vector<std::string> >& affFilters);
+std::vector<hpp::pinocchio::CollisionObjectPtr_t> HPP_RBPRM_DLLAPI
+getAffObjectsForLimb(const std::string& limb, const affMap_t& affordances,
+                     const std::map<std::string, std::vector<std::string> >& affFilters);
 
 /// Generates all potentially valid cases of valid contact maintenance
 /// given a previous configuration.
@@ -88,7 +79,8 @@ std::vector<hpp::pinocchio::CollisionObjectPtr_t> HPP_RBPRM_DLLAPI getAffObjects
 /// \param maxBrokenContacts max number of contacts that can be broken in the process
 /// \return a queue of contact states candidates for maintenance, ordered by number of contacts broken
 /// and priority in the list wrt the contact order
-Q_State maintain_contacts_combinatorial(const hpp::rbprm::State& currentState, const std::size_t maxBrokenContacts=1);
+Q_State maintain_contacts_combinatorial(const hpp::rbprm::State& currentState,
+                                        const std::size_t maxBrokenContacts = 1);
 
 /// Generates all potentially valid cases of valid contact creation by removing the top state of the priority
 /// stack
@@ -97,7 +89,8 @@ Q_State maintain_contacts_combinatorial(const hpp::rbprm::State& currentState, c
 /// \param maxCreatedContacts max number of contacts that can be created in the process
 /// \return a QUEUE of contact states candidates for maintenance, ordered by number of contacts broken
 /// and priority in the list wrt the contact order
-T_ContactState gen_contacts_combinatorial(const std::vector<std::string>& freeEffectors, const State& previous, const std::size_t maxCreatedContacts, const bool maximiseContacts=false);
+T_ContactState gen_contacts_combinatorial(const std::vector<std::string>& freeEffectors, const State& previous,
+                                          const std::size_t maxCreatedContacts, const bool maximiseContacts = false);
 
 /// Given a combinatorial of possible contacts, generate
 /// the first "valid" configuration, that is the first kinematic
@@ -111,7 +104,8 @@ projection::ProjectionReport maintain_contacts(ContactGenHelper& contactGenHelpe
 /// \param ContactGenHelper parametrization of the planner
 /// \param limb the limb to create a contact with
 /// \return the best candidate wrt the priority in the list and the contact order
-projection::ProjectionReport generate_contact(const ContactGenHelper& contactGenHelper, const std::string& limb, sampling::HeuristicParam &params,
+projection::ProjectionReport generate_contact(const ContactGenHelper& contactGenHelper, const std::string& limb,
+                                              sampling::HeuristicParam& params,
                                               const sampling::heuristic evaluate = 0);
 
 /// Given a combinatorial of possible contacts, generate
@@ -126,7 +120,7 @@ projection::ProjectionReport gen_contacts(ContactGenHelper& contactGenHelper);
 /// \param ContactGenHelper parametrization of the planner
 /// \return the best candidate wrt the priority in the list and the contact order
 projection::ProjectionReport repositionContacts(ContactGenHelper& contactGenHelper);
-    } // namespace projection
-  } // namespace rbprm
-} // namespace hpp
-#endif // HPP_RBPRM_CONTACT_GENERATION_HH
+}  // namespace contact
+}  // namespace rbprm
+}  // namespace hpp
+#endif  // HPP_RBPRM_CONTACT_GENERATION_HH

include/hpp/rbprm/contact_generation/kinematics_constraints.hh

@@ -6,40 +6,43 @@
 #include <hpp/pinocchio/configuration.hh>
 #include <hpp/rbprm/rbprm-state.hh>
 namespace hpp {
-  namespace rbprm {
+namespace rbprm {
 
-  HPP_PREDEF_CLASS(RbPrmFullBody);
-  class RbPrmFullBody;
-  typedef boost::shared_ptr <RbPrmFullBody> RbPrmFullBodyPtr_t;
+HPP_PREDEF_CLASS(RbPrmFullBody);
+class RbPrmFullBody;
+typedef boost::shared_ptr<RbPrmFullBody> RbPrmFullBodyPtr_t;
 
-  namespace reachability{
+namespace reachability {
 
-
-  /**
+/**
  * @brief loadConstraintsFromObj load the obj file and compute a list of normal and vertex position from it
  * @param fileName
  * @param minDistance if > 0 : add an additionnal plane of normal (0,0,1) and origin (0,0,minDistance)
  * @return
  */
 std::pair<MatrixX3, MatrixX3> loadConstraintsFromObj(const std::string& fileName, double minDistance = 0.);
 
-std::pair<MatrixX3, VectorX> computeAllKinematicsConstraints(const RbPrmFullBodyPtr_t& fullBody,const pinocchio::ConfigurationPtr_t& configuration);
+std::pair<MatrixX3, VectorX> computeAllKinematicsConstraints(const RbPrmFullBodyPtr_t& fullBody,
+                                                             const pinocchio::ConfigurationPtr_t& configuration);
 
-std::pair<MatrixX3, VectorX> computeKinematicsConstraintsForState(const RbPrmFullBodyPtr_t& fullBody, const State& state);
+std::pair<MatrixX3, VectorX> computeKinematicsConstraintsForState(const RbPrmFullBodyPtr_t& fullBody,
+                                                                  const State& state);
 
-std::pair<MatrixX3, VectorX> computeKinematicsConstraintsForLimb(const RbPrmFullBodyPtr_t& fullBody, const State& state,const std::string& limbName);
+std::pair<MatrixX3, VectorX> computeKinematicsConstraintsForLimb(const RbPrmFullBodyPtr_t& fullBody,
+                                                                 const State& state, const std::string& limbName);
 
-std::pair<MatrixX3, VectorX> getInequalitiesAtTransform(const std::pair<MatrixX3, MatrixX3>& NV, const hpp::pinocchio::Transform3f& transform);
+std::pair<MatrixX3, VectorX> getInequalitiesAtTransform(const std::pair<MatrixX3, MatrixX3>& NV,
+                                                        const hpp::pinocchio::Transform3f& transform);
 
 bool verifyKinematicConstraints(const std::pair<MatrixX3, VectorX>& Ab, const fcl::Vec3f& point);
 
-bool verifyKinematicConstraints(const std::pair<MatrixX3, MatrixX3>& NV, const hpp::pinocchio::Transform3f& transform, const fcl::Vec3f& point);
-
+bool verifyKinematicConstraints(const std::pair<MatrixX3, MatrixX3>& NV, const hpp::pinocchio::Transform3f& transform,
+                                const fcl::Vec3f& point);
 
-bool verifyKinematicConstraints(const RbPrmFullBodyPtr_t& fullbody,const State& state, fcl::Vec3f point);
+bool verifyKinematicConstraints(const RbPrmFullBodyPtr_t& fullbody, const State& state, fcl::Vec3f point);
 
-}
-}
-}
+}  // namespace reachability
+}  // namespace rbprm
+}  // namespace hpp
 
-#endif // KINEMATICS_CONSTRAINTS_HH
+#endif  // KINEMATICS_CONSTRAINTS_HH