Commit a9c900b3 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Allow to set security margin per body-body pair.

parent 2a5c0014
Pipeline #13166 failed with stage
in 1 minute and 6 seconds
......@@ -163,6 +163,11 @@ namespace hpp {
/// \copydoc ObstacleUserInterface::setSecurityMargins
virtual void setSecurityMargins(const matrix_t& securityMatrix);
/// \copydoc ObstacleUserInterface::setSecurityMarginBetweenBodies
virtual void setSecurityMarginBetweenBodies(const std::string& body_a,
const std::string& body_b,
const value_type& margin);
/// \name Delegate
/// \{
......
......@@ -56,12 +56,24 @@ namespace hpp {
///
/// \param relMotion square symmetric matrix of RelativeMotionType of size numberDof x numberDof
virtual void filterCollisionPairs (const RelativeMotion::matrix_type& relMotion) = 0;
/// Set different security margins for collision pairs
///
/// This function works joint-wise. If you need a finer control, use
/// \ref setSecurityMarginBetweenBodies
///
/// This method enables users to choose different security margins
/// for each pair of robot body or each pair robot body - obstacle.
/// for each pair of robot joint or each pair robot joint - obstacle.
/// \sa hpp::fcl::CollisionRequest::security_margin.
virtual void setSecurityMargins(const matrix_t& securityMatrix) = 0;
/// Set security margin for collision pair between the two bodies.
///
/// \sa hpp::fcl::CollisionRequest::security_margin.
virtual void setSecurityMarginBetweenBodies(const std::string& body_a,
const std::string& body_b,
const value_type& margin) = 0;
}; // class ObstacleUserInterface
/// Vector of validation class instances.
......@@ -129,11 +141,7 @@ namespace hpp {
}
}
/// Set different security margins for collision pairs
///
/// This method enables users to choose different security margins
/// for each pair of robot body or each pair robot body - obstacle.
/// \sa hpp::fcl::CollisionRequest::security_margin.
/// \copydoc ObstacleUserInterface::setSecurityMargins
void setSecurityMargins(const matrix_t& securityMatrix)
{
for (std::size_t i = 0; i < validations_.size(); ++i) {
......@@ -143,6 +151,19 @@ namespace hpp {
}
}
/// \copydoc ObstacleUserInterface::setSecurityMarginBetweenBodies
void setSecurityMarginBetweenBodies(const std::string& body_a,
const std::string& body_b,
const value_type& margin)
{
for (std::size_t i = 0; i < validations_.size(); ++i) {
shared_ptr<ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]);
if (oui)
oui->setSecurityMarginBetweenBodies (body_a, body_b, margin);
}
}
// Clear the vector of config validations
void clear ()
{
......@@ -234,6 +255,12 @@ namespace hpp {
/// \copydoc ObstacleUserInterface::setSecurityMargins
virtual void setSecurityMargins(const matrix_t& securityMatrix);
/// \copydoc ObstacleUserInterface::setSecurityMarginBetweenBodies
virtual void setSecurityMarginBetweenBodies(const std::string& body_a,
const std::string& body_b,
const value_type& margin);
protected:
/// Constructor of body pair collision
ObstacleUser (DevicePtr_t robot)
......
......@@ -344,6 +344,37 @@ namespace hpp {
bodyPairCollisionPool_.clear();
}
void ContinuousValidation::setSecurityMarginBetweenBodies(
const std::string& body_a, const std::string& body_b,
const value_type& margin)
{
// Loop over collision pairs and remove disabled ones.
bool found = true;
for (IntervalValidations_t::iterator _colPair
(intervalValidations_.begin());
_colPair != intervalValidations_.end(); ++_colPair)
{
BodyPairCollisionPtr_t bpc(HPP_DYNAMIC_PTR_CAST(BodyPairCollision,
*_colPair));
if (!bpc) continue;
const CollisionPairs_t& prs (bpc->pairs());
CollisionRequests_t& requests (bpc->requests());
for (std::size_t i = 0; i < prs.size(); ++i) {
const CollisionPair& pair (prs[i]);
if ( (pair.first->name() == body_a && pair.second->name() == body_b)
||(pair.first->name() == body_b && pair.second->name() == body_a))
{
requests[i].security_margin = margin;
found = true;
}
}
}
if (!found)
throw std::invalid_argument("Could not find a collision pair between "
"body " + body_a + " and " + body_b);
bodyPairCollisionPool_.clear();
}
template <>
void ContinuousValidation::add<ContinuousValidation::AddObstacle>
(const AddObstacle& delegate)
......
......@@ -189,6 +189,29 @@ namespace hpp {
}
}
void ObstacleUser::setSecurityMarginBetweenBodies(const std::string& body_a,
const std::string& body_b, const value_type& margin)
{
auto setMargin = [&body_a, &body_b, &margin](const CollisionPairs_t& pairs,
CollisionRequests_t& requests) {
for (std::size_t i = 0; i < pairs.size(); ++i) {
const CollisionPair_t& pair = pairs[i];
if ( (pair.first->name() == body_a && pair.second->name() == body_b)
||(pair.first->name() == body_b && pair.second->name() == body_a))
{
requests[i].security_margin = margin;
return true;
}
}
return false;
};
if ( setMargin(cPairs_, cRequests_)
&& setMargin(pPairs_, pRequests_)
&& setMargin(dPairs_, dRequests_)) return;
throw std::invalid_argument("Could not find a collision pair between body"
" " + body_a + " and " + body_b);
}
void ObstacleUser::addRobotCollisionPairs ()
{
const pinocchio::GeomModel& model = robot_->geomModel();
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment