Skip to content
Snippets Groups Projects
Commit 7f54471a authored by Raphael Grimm's avatar Raphael Grimm
Browse files

Improve self collision check

parent a307ca27
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+ 69
− 36
View file @ 7f54471a
......@@ -35,6 +35,9 @@ namespace armarx
void SelfCollisionChecker::_preOnInitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//get the robot
selfCollisionAvoidanceRobot = _module<RobotData>().cloneRobot(true);
//get pairs to check
{
const std::string colModelsString = getProperty<std::string>("SelfCollisionCheck_ModelGroupsToCheck").getValue();
std::vector<std::string> groups;
......@@ -42,11 +45,13 @@ namespace armarx
{
groups = armarx::Split(colModelsString, ";", true, true);
}
ARMARX_DEBUG << "Processing groups for self collision checking:";
for (std::string& group : groups)
{
ARMARX_DEBUG << "---- group: " << group;
// Removing parentheses
boost::trim_if(group, boost::is_any_of(" \t{}"));
std::set<std::string> nodes;
std::set<std::set<std::string>> setsOfNode;
{
auto splittedRaw = armarx::Split(group, ",", true, true);
if (splittedRaw.size() < 2)
......@@ -58,6 +63,7 @@ namespace armarx
boost::trim_if(subentry, boost::is_any_of(" \t{}"));
if (selfCollisionAvoidanceRobot->hasRobotNodeSet(subentry))
{
std::set<std::string> nodes;
//the entry is a set
for (const auto& node : selfCollisionAvoidanceRobot->getRobotNodeSet(subentry)->getAllRobotNodes())
{
......@@ -69,7 +75,9 @@ namespace armarx
continue;
}
nodes.emplace(node->getName());
ARMARX_DEBUG << "-------- from set: " << subentry << ", node: " << node->getName();
}
setsOfNode.emplace(std::move(nodes));
}
else if (selfCollisionAvoidanceRobot->hasRobotNode(subentry))
{
......@@ -81,12 +89,14 @@ namespace armarx
<< selfCollisionAvoidanceRobot->getRobotNode(subentry)->getName() << "'";
continue;
}
nodes.emplace(subentry);
setsOfNode.emplace(std::set<std::string>{subentry});
ARMARX_DEBUG << "-------- node: " << subentry;
}
else if (subentry == "FLOOR")
{
//the entry is the floor
nodes.emplace(subentry);
setsOfNode.emplace(std::set<std::string>{subentry});
ARMARX_DEBUG << "-------- floor: " << subentry;
}
else
{
......@@ -98,18 +108,45 @@ namespace armarx
}
}
auto addCombinationOfSetsToCollisionCheck = [this](const std::set<std::string>& a, const std::set<std::string>& b)
{
for(const auto& nodeA : a)
{
for(const auto& nodeB : b)
{
if(nodeA == nodeB)
{
continue;
}
if(nodeA < nodeB)
{
ARMARX_DEBUG << "------------ " << nodeA << " " << nodeB;
namePairsToCheck.emplace(nodeA, nodeB);
}
else
{
ARMARX_DEBUG << "------------ " << nodeB << " " << nodeA;
namePairsToCheck.emplace(nodeB, nodeA);
}
}
}
};
for (auto it1 = nodes.begin(); it1 != nodes.end(); ++it1)
ARMARX_DEBUG << "-------- adding pairs to check:";
for (auto setAIt = setsOfNode.begin(); setAIt != setsOfNode.end(); ++setAIt)
{
auto it2 = it1;
++it2;
for (; it2 != nodes.end(); ++it2)
auto setBIt = setAIt;
++setBIt;
for (; setBIt != setsOfNode.end(); ++setBIt)
{
namePairsToCheck.emplace(*it1, *it2);
addCombinationOfSetsToCollisionCheck(*setAIt, *setBIt);
}
}
ARMARX_DEBUG << "-------- group: " << group << "...DONE!\n";
}
ARMARX_DEBUG << "Processing groups for self collision checking...DONE!";
}
setSelfCollisionAvoidanceFrequency(getProperty<float>("SelfCollisionCheck_Frequency").getValue());
setSelfCollisionAvoidanceDistance(getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
......@@ -139,7 +176,8 @@ namespace armarx
//set floor
{
floor.reset(new VirtualRobot::SceneObjectSet("FLOORSET"));
VirtualRobot::ObstaclePtr boxOb = VirtualRobot::Obstacle::createBox(200000, 200000, std::min(0.001f, minSelfDistance / 2));
static constexpr float floorSize = 1e16;
VirtualRobot::ObstaclePtr boxOb = VirtualRobot::Obstacle::createBox(floorSize, floorSize, std::min(0.001f, minSelfDistance / 2));
boxOb->setGlobalPose(Eigen::Matrix4f::Identity());
boxOb->setName("FLOOR");
floor->addSceneObject(boxOb);
......@@ -167,6 +205,7 @@ namespace armarx
nodePairsToCheck.emplace_back(first, second);
}
ARMARX_CHECK_EQUAL(nodePairsToCheck.size(), nodePairsToCheck.size());
}
void SelfCollisionChecker::setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current&)
......@@ -221,6 +260,8 @@ namespace armarx
void SelfCollisionChecker::selfCollisionAvoidanceTask()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_INFO << "entered selfCollisionAvoidanceTask";
ARMARX_ON_SCOPE_EXIT { ARMARX_INFO << "leaving selfCollisionAvoidanceTask"; };
while (true)
{
const auto startT = std::chrono::high_resolution_clock::now();
......@@ -230,11 +271,10 @@ namespace armarx
return;
}
const auto freq = checkFrequency.load();
if (
_module<ControlThread>().getEmergencyStopState() ||
freq == 0
)
const bool inEmergencyStop = _module<ControlThread>().getEmergencyStopState() == eEmergencyStopActive;
if (inEmergencyStop || freq == 0)
{
ARMARX_INFO << deactivateSpam() << "skipping " << VAROUT(freq) << " " << VAROUT(inEmergencyStop);
//currently wait
std::this_thread::sleep_for(std::chrono::microseconds {1000});
continue;
......@@ -245,32 +285,25 @@ namespace armarx
//update robot
_module<ControlThreadDataBuffer>().updateVirtualRobot(selfCollisionAvoidanceRobot, selfCollisionAvoidanceRobotNodes);
//check pose
float absSum = 0;
for (auto& node : selfCollisionAvoidanceRobotNodes)
{
absSum += std::abs(node->getJointValue());
}
// completely zero -> skip
if (absSum >= 0.00001f)
{
ARMARX_INFO << deactivateSpam() << "All joint values are zero - skipping collision check for now";
}
else
bool collision = false;
for (std::size_t idx = 0; idx < nodePairsToCheck.size(); ++idx)
{
//check (not in zero pose)
for (const auto& pair : nodePairsToCheck)
const auto& pair = nodePairsToCheck.at(idx);
if (selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(pair.first, pair.second))
{
if (selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(pair.first, pair.second))
{
ARMARX_WARNING << "COLLISION: '" << pair.first->getName() << "' and '" << pair.second->getName() << "'";
_module<Units>().getEmergencyStopMaster()->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
// since at least one of the NJoint-Controllers is going to cause a collision, we just kick them all.
_module<ControlThreadDataBuffer>().setActivateControllersRequest({});
continue;
}
collision = true;
lastCollisionPairIndex = idx;
ARMARX_WARNING << "COLLISION: '" << pair.first->getName() << "' and '" << pair.second->getName() << "'";
_module<Units>().getEmergencyStopMaster()->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
// since at least one of the NJoint-Controllers is going to cause a collision, we just kick them all.
_module<ControlThreadDataBuffer>().setActivateControllersRequest({});
break;
}
}
if (!collision)
{
ARMARX_INFO << deactivateSpam() << "no collision found between the " << nodePairsToCheck.size() << " pairs";
}
}
//sleep remaining
std::this_thread::sleep_until(startT + std::chrono::microseconds {static_cast<int64_t>(1000000 / freq)});
......
source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
+ 2
− 0
View file @ 7f54471a
......@@ -154,6 +154,8 @@ namespace armarx
std::vector<VirtualRobot::RobotNodePtr> selfCollisionAvoidanceRobotNodes;
/// @brief A Collision object for the floor
VirtualRobot::SceneObjectSetPtr floor;
/// @brief The pair of nodes causing the last detected collision (as index in \ref nodePairsToCheck or -1)
std::atomic_size_t lastCollisionPairIndex{std::numeric_limits<std::size_t>::max()};
};
}
}
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