Merge pull request #79 from ARC-OPT/add_minor_functions

Add some minor functionality

src/core/RobotModelConfig.hpp

@@ -13,6 +13,9 @@ class ActiveContact{
     }
     ActiveContact(int active, double mu) : active(active), mu(mu){
 
+    }
+    ActiveContact(int active, double mu, double wx, double wy) : active(active), mu(mu), wx(wx), wy(wy){
+
     }
     int active; /** In contact*/
     double mu;  /** Friction coeffcient*/

src/core/Scene.cpp

@@ -168,6 +168,13 @@ void Scene::sortTaskConfig(const std::vector<TaskConfig>& config, std::vector< s
     }
 }
 
+uint Scene::getNTaskVariables(const std::vector<TaskConfig> &config){
+    uint n_task_variables = 0;
+    for(const TaskConfig& cfg : config)
+        n_task_variables += cfg.nVariables();
+    return n_task_variables;
+}
+
 std::vector<int> Scene::getNTaskVariablesPerPrio(const std::vector<TaskConfig> &config){
 
     std::vector< std::vector<TaskConfig> > sorted_config;

src/core/Scene.hpp

@@ -106,6 +106,11 @@ class Scene{
      */
     static void sortTaskConfig(const std::vector<TaskConfig>& config, std::vector< std::vector<TaskConfig> >& sorted_config);
 
+    /**
+     * @brief Return number of tasks per priority, given the task config
+     */
+    static uint getNTaskVariables(const std::vector<TaskConfig>& config);
+
     /**
      * @brief Return number of tasks per priority, given the task config
      */

src/core/TaskConfig.hpp

@@ -67,7 +67,7 @@ class TaskConfig {
     /** Unique identifier of the constraint. Must not be empty*/
     std::string name;
 
-    /** Constraint type, can be one of 'jnt' (joint space) or 'cart' (Cartesian) */
+    /** Task type, can be one of 'jnt' (joint space) or 'cart' (Cartesian) */
     TaskType type;
 
     /** Priority of this task. Must be >= 0! 0 corresponds to the highest priority. */