environment support (default hardcoded to true) && bugfix update

ADVRHumanoids · Mar 21, 2024 · 0fe7815 · 0fe7815
1 parent f5da833
commit 0fe7815
Show file tree

Hide file tree

Showing 2 changed files with 51 additions and 16 deletions.
diff --git a/include/OpenSoT/constraints/velocity/CollisionAvoidance.h b/include/OpenSoT/constraints/velocity/CollisionAvoidance.h
@@ -45,6 +45,8 @@ class CollisionAvoidance: public Constraint<Eigen::MatrixXd, Eigen::VectorXd>
 
     typedef std::shared_ptr<CollisionAvoidance> Ptr;
     typedef std::pair<std::string, std::string> LinksPair;
+    typedef XBot::Collision::CollisionModel::WitnessPointVector WitnessPointVector;
+    typedef XBot::Collision::CollisionModel::LinkPairVector LinkPairVector;
 
     /**
      * @brief SelfCollisionAvoidance
@@ -111,6 +113,12 @@ class CollisionAvoidance: public Constraint<Eigen::MatrixXd, Eigen::VectorXd>
      */
     bool updateCollisionList(std::set<std::pair<std::string, std::string>> collisionList);
 
+    /**
+     * @brief updateEnvironment must be called after a collision has been added or
+     * removed from the envronment
+     */
+    void updateEnvironment();
+
     // TODO: waiting for world collision support !
     //
     // /**
@@ -189,6 +197,11 @@ class CollisionAvoidance: public Constraint<Eigen::MatrixXd, Eigen::VectorXd>
 
 protected:
 
+    /**
+     * @brief _include_env
+     */
+    bool _include_env;
+
     /**
      * @brief _bound_scaling
      */

diff --git a/src/constraints/velocity/CollisionAvoidance.cpp b/src/constraints/velocity/CollisionAvoidance.cpp
@@ -33,6 +33,9 @@ CollisionAvoidance::CollisionAvoidance(
     _bound_scaling(1.0),
     _max_pairs(max_pairs)
 {
+    // enable collisions vs env
+    _include_env = true;
+
     // construct custom model for collisions
     _collision_model =
         ModelInterface::getModel(collision_urdf ? collision_urdf : robot.getUrdf(),
@@ -47,9 +50,11 @@ CollisionAvoidance::CollisionAvoidance(
     if(_max_pairs < 0)
     {
         // note: threshold = inf
-        _max_pairs = _dist_calc->getNumCollisionPairs();
+        _max_pairs = _dist_calc->getNumCollisionPairs(_include_env);
     }
-    _distance_J.setZero(_dist_calc->getNumCollisionPairs(), getXSize());
+
+    _distance_J.setZero(_dist_calc->getNumCollisionPairs(_include_env), getXSize());
+    _distances.setZero(_distance_J.rows());
 
     // initialize matrices
     _Aineq.setZero(_max_pairs, getXSize());
@@ -94,31 +99,43 @@ void CollisionAvoidance::update(const Eigen::VectorXd &x)
     _bUpperBound.setConstant(_max_pairs, std::numeric_limits<double>::max());
 
     // compute distances
-    _dist_calc->computeDistance(_distances, _detection_threshold);
+    _dist_calc->computeDistance(_distances, _include_env, _detection_threshold);
 
     // compute jacobians
-    _dist_calc->getDistanceJacobian(_distance_J);
+    _distance_J.setZero(_dist_calc->getNumCollisionPairs(_include_env), _distance_J.cols());
+    _dist_calc->getDistanceJacobian(_distance_J, _include_env);
 
     // populate Aineq and bUpperBound
+    int row_idx = 0;
     for(int i : _dist_calc->getOrderedCollisionPairIndices())
     {
-        if(i < _max_pairs)
+        if(row_idx >= _max_pairs)
         {
-            _Aineq.row(i) = -_distance_J.row(i);
-            _bUpperBound(i) = _bound_scaling*(_distances(i) - _distance_threshold);
-
-            // to avoid infeasibilities, cap upper bound to zero
-            // (i.e. don't change current distance if in collision)
-            if(_bUpperBound(i) < 0.0)
-            {
-                _bUpperBound(i) = 0.0;
-            }
-        }
-        else
             break;
+        }
+
+        // skip if above detection threshold
+        if(_detection_threshold > 0 && _distances(i) > _detection_threshold)
+        {
+            continue;
+        }
+
+        _Aineq.row(row_idx) = -_distance_J.row(i);
+        _bUpperBound(row_idx) = _bound_scaling*(_distances(i) - _distance_threshold);
 
+        // to avoid infeasibilities, cap upper bound to zero
+        // (i.e. don't change current distance if in collision)
+        if(_bUpperBound(row_idx) < 0.0)
+        {
+            _bUpperBound(row_idx) = 0.0;
+        }
+
+        row_idx++;
     }
 
+    // save number of active constraints
+    _num_active_pairs = row_idx;
+
 }
 
 bool CollisionAvoidance::setCollisionList(std::set<std::pair<std::string, std::string>> collisionList)
@@ -165,6 +182,11 @@ bool CollisionAvoidance::updateCollisionList(std::set<std::pair<std::string, std
     }
 }
 
+void CollisionAvoidance::updateEnvironment()
+{
+    _lpv = _dist_calc->getCollisionPairs(_include_env);
+}
+
 // bool CollisionAvoidance::setWorldCollisions(const moveit_msgs::PlanningSceneWorld &wc)
 // {
 //     return _dist_calc->setWorldCollisions(wc);