From 704bab29200f14b621e5a40f090ebe3a2a82cb76 Mon Sep 17 00:00:00 2001
From: Arturo Laurenzi <arturo.laurenzi@iit.it>
Date: Tue, 19 Mar 2024 17:53:45 +0100
Subject: [PATCH] missing model sync && using new order joint indices api

---
 .../constraints/velocity/CollisionAvoidance.h |  2 ++
 .../velocity/CollisionAvoidance.cpp           | 20 +++++++++----------
 2 files changed, 12 insertions(+), 10 deletions(-)

diff --git a/include/OpenSoT/constraints/velocity/CollisionAvoidance.h b/include/OpenSoT/constraints/velocity/CollisionAvoidance.h
index a469092d..4432f825 100644
--- a/include/OpenSoT/constraints/velocity/CollisionAvoidance.h
+++ b/include/OpenSoT/constraints/velocity/CollisionAvoidance.h
@@ -194,6 +194,8 @@ class CollisionAvoidance: public Constraint<Eigen::MatrixXd, Eigen::VectorXd>
      */
     const XBot::ModelInterface& _robot;
 
+    XBot::ModelInterface::Ptr _collision_model;
+
     /**
      * @brief _dist_calc
      */
diff --git a/src/constraints/velocity/CollisionAvoidance.cpp b/src/constraints/velocity/CollisionAvoidance.cpp
index abffa519..f1dc7584 100644
--- a/src/constraints/velocity/CollisionAvoidance.cpp
+++ b/src/constraints/velocity/CollisionAvoidance.cpp
@@ -36,10 +36,13 @@ CollisionAvoidance::CollisionAvoidance(
     _max_pairs(max_pairs)
 {
     // construct custom model for collisions
-    ModelInterface::Ptr collision_model = ModelInterface::getModel(collision_urdf, collision_srdf, robot.getType());
+    _collision_model =
+        ModelInterface::getModel(collision_urdf ? collision_urdf : robot.getUrdf(),
+                                 collision_srdf ? collision_srdf : robot.getSrdf(),
+                                 robot.getType());
 
     // construct link distance computation util
-    _dist_calc = std::make_unique<Collision::CollisionModel>(collision_model);
+    _dist_calc = std::make_unique<Collision::CollisionModel>(_collision_model);
 
     // if max pairs not specified, set it as number of total
     // link pairs
@@ -78,7 +81,11 @@ void CollisionAvoidance::setDetectionThreshold(const double detection_threshold)
 
 void CollisionAvoidance::update(const Eigen::VectorXd &x)
 {
+    // update collision model
+    _collision_model->syncFrom(_robot, ControlMode::POSITION);
+    _dist_calc->update();
 
+    // reset constraint
     _Aineq.setZero(_max_pairs, getXSize());
     _bUpperBound.setConstant(_max_pairs, std::numeric_limits<double>::max());
 
@@ -88,16 +95,9 @@ void CollisionAvoidance::update(const Eigen::VectorXd &x)
     // compute jacobians
     _dist_calc->getDistanceJacobian(_distance_J);
 
-    // order indices in ascending distance
-    std::multimap<double, int> dist_to_idx;
-    for(int i = 0; i < _distances.size(); i++)
-    {
-        dist_to_idx.insert({_distances[i], i});
-    }
-
     // populate Aineq and bUpperBound
     int row_idx = 0;
-    for(auto& [d, i] : dist_to_idx)
+    for(int i : _dist_calc->getOrderedCollisionPairIndices())
     {
         _Aineq.row(row_idx) = _distance_J.row(i);
         _bUpperBound(row_idx) = _bound_scaling*(_distances(i) - _distance_threshold);