Skip to content

Commit

Permalink
[API CHANGE] CHanged the API fo the collisiona avoidance contraint. A…
Browse files Browse the repository at this point in the history 
…dded test for environment collision
  • Loading branch information
@EnricoMingo
EnricoMingo committed Mar 26, 2024
1 parent f5a8a01 commit 51f8c32
Show file tree
Hide file tree
Showing 5 changed files with 136 additions and 183 deletions.
57 changes: 15 additions & 42 deletions include/OpenSoT/constraints/velocity/CollisionAvoidance.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,61 +94,34 @@ class CollisionAvoidance: public Constraint<Eigen::MatrixXd, Eigen::VectorXd>
void update();


/**
* @brief setCollisionList permits to set a list of collision pairs (links).
* @note when using set CollisionList, the internal list of collision is reset. This entails the resizing of
* internal constraint matrices. For this reason, this method SHOULD NOT be called once the constraint has been
* added into the solver but only BEFORE!
* @param collisionList set of link pairs names
* @return false if links are not founded in the model
*/
bool setCollisionList(std::set<std::pair<std::string, std::string>> collisionList);
void setMaxPairs(const unsigned int max_pairs);

void setCollisionList(std::set<std::pair<std::string, std::string>> collisionList);


/**
* @brief updateCollisionList permits to update the collision list with different collision pairs
* @note the parameter collisionList in input can not be larger than the maximum number of link pairs (_max_pairs),
* this is to avoid resize of constraint matrix
* @param collisionList set of link pairs names
* @return false if collisionList.size() > _max_pairs or if links are not founded in the model
*/
bool updateCollisionList(std::set<std::pair<std::string, std::string>> collisionList);

/**
* @brief collisionModelUpdated must be called after a collision has been added or
* removed from the model
*/
void collisionModelUpdated();

// TODO: waiting for world collision support !
//
// /**
// * @brief add/remove world collision objects according to the given planning
// * scene world
// * @return true if all requests (additions, deletions) could be performs
// * succesfully, false on (partial) insuccess
// */
// bool setWorldCollisions(const moveit_msgs::PlanningSceneWorld& wc);

// /**
// * @brief add single collision to the world
// * @param id is the unique collision id
// * @param fcl_obj is the fcl collision object (geometry + transform)
// * @return true if input is valid
// */
// bool addWorldCollision(const std::string& id,
// std::shared_ptr<fcl::CollisionObjectd> fcl_obj);
bool addCollisionShape(const std::string& name,
const std::string& link,
const XBot::Collision::Shape::Variant& shape,
const Eigen::Affine3d& link_T_shape,
const std::vector<std::string>& disabled_collisions = {});

// /**
// * @brief remove world collision with given id
// */
// bool removeWorldCollision(const std::string& id);

// /**
// * @brief change the transform w.r.t. the world for the given
// * world collision
// */
// bool moveWorldCollision(const std::string& id,
// Eigen::Affine3d new_pose);
/**
* @brief change the transform w.r.t. the world for the given
* world collision
*/
bool moveCollisionShape(const std::string& id, const Eigen::Affine3d& new_pose);

/**
* @brief setBoundScaling sets bound scaling for the capsule constraint
Expand All @@ -162,7 +135,7 @@ class CollisionAvoidance: public Constraint<Eigen::MatrixXd, Eigen::VectorXd>
* @brief setLinksVsEnvironment
* @param links
*/
void setLinksVsEnvironment(const std::list<std::string>& links);
void setLinksVsEnvironment(const std::set<std::string>& links);

/**
* @brief getter for the internal collision model
Expand Down
85 changes: 27 additions & 58 deletions src/constraints/velocity/CollisionAvoidance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,12 +97,16 @@ void CollisionAvoidance::update()
// reset constraint
_Aineq.setZero(_max_pairs, getXSize());
_bUpperBound.setConstant(_max_pairs, std::numeric_limits<double>::max());
_bLowerBound.setConstant(_max_pairs, std::numeric_limits<double>::lowest());


_distance_J.setZero(_dist_calc->getNumCollisionPairs(_include_env), _distance_J.cols());
_distances.setZero(_distance_J.rows());

// compute distances
_dist_calc->computeDistance(_distances, _include_env, _detection_threshold);

// compute jacobians
_distance_J.setZero(_dist_calc->getNumCollisionPairs(_include_env), _distance_J.cols());
_dist_calc->getDistanceJacobian(_distance_J, _include_env);

// populate Aineq and bUpperBound
Expand Down Expand Up @@ -138,90 +142,55 @@ void CollisionAvoidance::update()

}

bool CollisionAvoidance::setCollisionList(std::set<std::pair<std::string, std::string>> collisionList)
bool CollisionAvoidance::addCollisionShape(const std::string &name,
const std::string &link,
const XBot::Collision::Shape::Variant &shape,
const Eigen::Affine3d &link_T_shape,
const std::vector<std::string> &disabled_collisions)
{
try
{
_dist_calc->setLinkPairs(collisionList);
_max_pairs = collisionList.size();
// initialize matrices
_Aineq.setZero(_max_pairs, getXSize());
_bUpperBound.setZero(_max_pairs);
_bLowerBound.setConstant(_max_pairs, std::numeric_limits<double>::lowest());
_distance_J.setZero(_max_pairs, _robot.getNv());

return true;
}
catch(std::out_of_range& e)
{
Logger::error(e.what());
return false;
}
return _dist_calc->addCollisionShape(name, link, shape, link_T_shape, disabled_collisions);
}

bool CollisionAvoidance::updateCollisionList(std::set<std::pair<std::string, std::string>> collisionList)
bool CollisionAvoidance::moveCollisionShape(const std::string& id, const Eigen::Affine3d& new_pose)
{
if(collisionList.size() > _max_pairs)
{
Logger::error("collisionList.size() > _max_pairs when updating collision list");
return false;
}
return _dist_calc->moveCollisionShape(id, new_pose);
}

try
{
_dist_calc->setLinkPairs(collisionList);
_Aineq.setZero();
_bUpperBound.setZero();
_distance_J.setZero(collisionList.size(), _robot.getNv());
return true;
}
catch(std::out_of_range& e)
{
Logger::error(e.what());
return false;
}
void CollisionAvoidance::setMaxPairs(const unsigned int max_pairs)
{
_max_pairs = max_pairs;
update();
}

void CollisionAvoidance::setCollisionList(std::set<std::pair<std::string, std::string>> collisionList)
{
_dist_calc->setLinkPairs(collisionList);
}

void CollisionAvoidance::collisionModelUpdated()
{
_lpv = _dist_calc->getCollisionPairs(_include_env);
}

// bool CollisionAvoidance::setWorldCollisions(const moveit_msgs::PlanningSceneWorld &wc)
// {
// return _dist_calc->setWorldCollisions(wc);
// }

// void CollisionAvoidance::setLinksVsEnvironment(const std::list<std::string>& links)
// {
// _dist_calc->setLinksVsEnvironment(links);
// }

// bool CollisionAvoidance::addWorldCollision(const std::string &id,
// std::shared_ptr<fcl::CollisionObjectd> fcl_obj)
// {
// return _dist_calc->addWorldCollision(id, fcl_obj);
// }


// bool CollisionAvoidance::removeWorldCollision(const std::string &id)
// {
// return _dist_calc->removeWorldCollision(id);
// }

// bool CollisionAvoidance::moveWorldCollision(const std::string &id,
// Eigen::Affine3d new_pose)
// {
// return _dist_calc->moveWorldCollision(id, new_pose);
// }


void CollisionAvoidance::setBoundScaling(const double boundScaling)
{
_bound_scaling = boundScaling;
}

void CollisionAvoidance::setLinksVsEnvironment(const std::list<std::string> &links)
void CollisionAvoidance::setLinksVsEnvironment(const std::set<std::string> &links)
{
_dist_calc->setLinksVsEnvironment(std::set(links.begin(), links.end()));
_dist_calc->setLinksVsEnvironment(links);
}

Collision::CollisionModel &CollisionAvoidance::getCollisionModel()
Expand Down
8 changes: 4 additions & 4 deletions tests/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -212,10 +212,10 @@ if(${FCL_FOUND})
add_dependencies(testCollisionAvoidanceVelocityConstraint OpenSoT)
add_test(NAME OpenSoT_constraints_velocity_CollisionAvoidance COMMAND testCollisionAvoidanceVelocityConstraint)

# ADD_EXECUTABLE(testCollisionAvoidanceEnvironmentVelocityConstraint constraints/velocity/TestCollisionAvoidanceEnvironment.cpp)
# TARGET_LINK_LIBRARIES(testCollisionAvoidanceEnvironmentVelocityConstraint ${TestLibs} -lccd)
# add_dependencies(testCollisionAvoidanceEnvironmentVelocityConstraint OpenSoT)
# add_test(NAME OpenSoT_constraints_velocity_CollisionAvoidanceEnvironment COMMAND testCollisionAvoidanceEnvironmentVelocityConstraint)
ADD_EXECUTABLE(testCollisionAvoidanceEnvironmentVelocityConstraint constraints/velocity/TestCollisionAvoidanceEnvironment.cpp)
TARGET_LINK_LIBRARIES(testCollisionAvoidanceEnvironmentVelocityConstraint ${TestLibs} -lccd)
add_dependencies(testCollisionAvoidanceEnvironmentVelocityConstraint OpenSoT)
add_test(NAME OpenSoT_constraints_velocity_CollisionAvoidanceEnvironment COMMAND testCollisionAvoidanceEnvironmentVelocityConstraint)
endif()

ADD_EXECUTABLE(testCartesianUtils utils/cartesian_utils_test.cpp)
Expand Down
5 changes: 4 additions & 1 deletion tests/constraints/velocity/TestCollisionAvoidance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -489,6 +489,7 @@ TEST_F(testSelfCollisionAvoidanceConstraint, testCartesianTaskWithSC){
std::set<std::pair<std::string,std::string> > whiteList;
whiteList.insert(std::pair<std::string, std::string>(linkA,linkB));
this->sc_constraint->setCollisionList(whiteList);
this->sc_constraint->setMaxPairs(whiteList.size());
std::cout << "xxx Whitelist of size " << whiteList.size() << " set. Constraint automatically updated" << std::endl;

std::list<OpenSoT::tasks::velocity::Cartesian::TaskPtr> cartesianTasks;
Expand Down Expand Up @@ -724,6 +725,7 @@ TEST_F(testSelfCollisionAvoidanceConstraint, testMultipleCapsulePairsSC){
whiteList.insert(std::pair<std::string,std::string>(linkA,linkB));
whiteList.insert(std::pair<std::string,std::string>(linkC,linkD));
this->sc_constraint->setCollisionList(whiteList);
this->sc_constraint->setMaxPairs(whiteList.size());
std::cout << "xxx Whitelist of size " << whiteList.size() << " set. Constraint automatically updated" << std::endl;

std::list<OpenSoT::tasks::velocity::Cartesian::TaskPtr> cartesianTasks;
Expand Down Expand Up @@ -945,6 +947,7 @@ TEST_F(testSelfCollisionAvoidanceConstraint, testChangeWhitelistOnline){
whiteList.insert(std::pair<std::string,std::string>(linkA,linkB));
whiteList.insert(std::pair<std::string,std::string>(linkC,linkD));
this->sc_constraint->setCollisionList(whiteList);
this->sc_constraint->setMaxPairs(whiteList.size());
std::cout << "xxx Whitelist of size " << whiteList.size() << " set. Constraint automatically updated" << std::endl;

std::list<OpenSoT::tasks::velocity::Cartesian::TaskPtr> cartesianTasks;
Expand Down Expand Up @@ -1089,7 +1092,7 @@ TEST_F(testSelfCollisionAvoidanceConstraint, testChangeWhitelistOnline){
std::cout << "xxx Setting whitelist" << std::endl;
std::set<std::pair<std::string,std::string> > whiteList2;
whiteList2.insert(std::pair<std::string,std::string>(linkA,linkB));
EXPECT_TRUE(this->sc_constraint->updateCollisionList(whiteList2));
this->sc_constraint->setCollisionList(whiteList2);
std::cout << "xxx Whitelist of size " << whiteList.size() << " set. Constraint automatically updated" << std::endl;

dq.setZero();
Expand Down
Loading

0 comments on commit 51f8c32

Please sign in to comment.