Commit 39bda32e authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add ProblemSolver::addObstacle from a Device

parent 2f100c3e
......@@ -419,6 +419,15 @@ namespace hpp {
/// \name Obstacles
/// \{
/// Add collision object of device as obstacles to the list.
/// \param device the Device to be added.
/// \param collision whether collision checking should be performed
/// for this object.
/// \param distance whether distance computation should be performed
/// for this object.
virtual void addObstacle (const DevicePtr_t& device, bool collision,
bool distance);
/// Add obstacle to the list.
/// \param inObject a new object.
/// \param collision whether collision checking should be performed
......
......@@ -865,6 +865,23 @@ namespace hpp {
}
}
void ProblemSolver::addObstacle (const DevicePtr_t& device,
bool collision, bool distance)
{
device->computeForwardKinematics();
device->updateGeometryPlacements();
const std::string& prefix = device->name();
// Detach objects from joints
pinocchio::DeviceObjectVector& objects = device->objectVector();
for (pinocchio::DeviceObjectVector::iterator itObj = objects.begin();
itObj != objects.end(); ++itObj) {
addObstacle (prefix + (*itObj)->name (),
*(*itObj)->fcl (),
collision, distance);
}
}
void ProblemSolver::addObstacle (const CollisionObjectPtr_t& object,
bool collision, bool distance)
{
......
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