Fix add/remove object in backward operation

- addObject() will actually remove the object from scene
- removeObject() is not supported (we would need to know which object to add)
This commit is contained in:
Robert Haschke 2023-05-16 01:15:26 +02:00
parent 1daef934ee
commit bd400de184
3 changed files with 86 additions and 26 deletions

View File

@ -149,8 +149,8 @@ protected:
protected:
// apply stored modifications to scene
InterfaceState apply(const InterfaceState& from, bool invert);
void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::CollisionObject& object);
std::pair<InterfaceState, SubTrajectory> apply(const InterfaceState& from, bool invert);
void processCollisionObject(planning_scene::PlanningScene& scene, moveit_msgs::CollisionObject& object, bool invert);
void attachObjects(planning_scene::PlanningScene& scene, const std::pair<std::string, std::pair<Names, bool>>& pair,
bool invert);
void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert);

View File

@ -25,24 +25,28 @@ class TestModifyPlanningScene(unittest.TestCase):
def setUp(self):
super(TestModifyPlanningScene, self).setUp()
self.psi = PlanningSceneInterface(synchronous=True)
self.make_box = self.psi._PlanningSceneInterface__make_box
# insert a box to collide with
self.psi.add_box("box", make_pose(0.8, 0.25, 1.25), [0.2, 0.2, 0.2])
self.task = task = core.Task()
task.add(stages.CurrentState("current"))
def insertMove(self, position=-1):
# colliding motion
move = stages.MoveRelative("move", core.JointInterpolationPlanner())
move.group = self.PLANNING_GROUP
move.setDirection({"joint_1": 0.3})
self.task = task = core.Task()
task.add(stages.CurrentState("current"), move)
self.task.insert(move, position)
def test_collision(self):
self.insertMove()
self.assertFalse(self.task.plan())
def test_allow_collision_list(self):
mps = stages.ModifyPlanningScene("mps")
mps = stages.ModifyPlanningScene("allow specific collisions for box")
mps.allowCollisions("box", ["link_4", "link_5", "link_6"], True)
self.task.insert(mps, 1)
self.task.add(mps)
self.insertMove()
self.assertTrue(self.task.plan())
def test_allow_collision_all(self):
@ -50,10 +54,11 @@ class TestModifyPlanningScene(unittest.TestCase):
self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])
# attach box to end effector
self.psi.attach_box("link_6", "box")
mps = stages.ModifyPlanningScene("mps")
self.insertMove()
self.assertFalse(self.task.plan())
# allow all collisions for attached "box" object
mps = stages.ModifyPlanningScene("allow all collisions for box")
mps.allowCollisions("box", True)
self.task.insert(mps, 1)
self.assertTrue(self.task.plan())
@ -61,6 +66,46 @@ class TestModifyPlanningScene(unittest.TestCase):
self.psi.remove_attached_object("link_6", "box")
self.psi.remove_world_object("block")
def test_fw_add_object(self):
mps = stages.ModifyPlanningScene("addObject(block)")
mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]))
self.task.add(mps)
self.insertMove()
self.assertFalse(self.task.plan())
def test_fw_remove_object(self):
mps = stages.ModifyPlanningScene("removeObject(box)")
mps.removeObject("box")
self.task.insert(mps, 1)
self.assertTrue(self.task.plan())
s = self.task.solutions[0].toMsg()
self.assertEqual(s.sub_trajectory[1].scene_diff.world.collision_objects[0].id, "box")
def test_bw_add_object(self):
# add object to move_group's planning scene
self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])
# backward operation will actually remove the object
mps = stages.ModifyPlanningScene("addObject(block) backwards")
mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]))
self.task.insert(mps, 0)
self.insertMove(0)
self.assertTrue(self.task.plan())
self.psi.remove_world_object("block") # restore original state
s = self.task.solutions[0].toMsg()
for ps in [s.start_scene] + [s.sub_trajectory[i].scene_diff for i in [0, 1]]:
objects = [o.id for o in ps.world.collision_objects]
self.assertTrue(objects == ["block", "box"])
def test_bw_remove_object(self):
mps = stages.ModifyPlanningScene("removeObject(box) backwards")
mps.removeObject("box")
self.task.insert(mps, 0)
self.insertMove(0)
self.assertFalse(self.task.plan())
if __name__ == "__main__":
roscpp_initialize("test_mtc")

View File

@ -84,11 +84,13 @@ void ModifyPlanningScene::allowCollisions(const std::string& first, const moveit
}
void ModifyPlanningScene::computeForward(const InterfaceState& from) {
sendForward(from, apply(from, false), SubTrajectory());
auto result = apply(from, false);
sendForward(from, std::move(result.first), std::move(result.second));
}
void ModifyPlanningScene::computeBackward(const InterfaceState& to) {
sendBackward(apply(to, true), to, SubTrajectory());
auto result = apply(to, true);
sendBackward(std::move(result.first), to, std::move(result.second));
}
void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene& scene,
@ -121,30 +123,43 @@ void ModifyPlanningScene::allowCollisions(planning_scene::PlanningScene& scene,
// invert indicates, whether to detach instead of attach (and vice versa)
// as well as to forbid instead of allow collision (and vice versa)
InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool invert) {
std::pair<InterfaceState, SubTrajectory> ModifyPlanningScene::apply(const InterfaceState& from, bool invert) {
planning_scene::PlanningScenePtr scene = from.scene()->diff();
InterfaceState result(scene);
// add/remove objects
for (const auto& collision_object : collision_objects_)
processCollisionObject(*scene, collision_object);
InterfaceState state(scene);
SubTrajectory traj;
try {
// add/remove objects
for (auto& collision_object : collision_objects_)
processCollisionObject(*scene, collision_object, invert);
// attach/detach objects
for (const auto& pair : attach_objects_)
attachObjects(*scene, pair, invert);
// attach/detach objects
for (const auto& pair : attach_objects_)
attachObjects(*scene, pair, invert);
// allow/forbid collisions
for (const auto& pairs : collision_matrix_edits_)
allowCollisions(*scene, pairs, invert);
// allow/forbid collisions
for (const auto& pairs : collision_matrix_edits_)
allowCollisions(*scene, pairs, invert);
if (callback_)
callback_(scene, properties());
return result;
if (callback_)
callback_(scene, properties());
} catch (const std::exception& e) {
traj.markAsFailure(e.what());
}
return std::make_pair(state, traj);
}
void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& scene,
const moveit_msgs::CollisionObject& object) {
moveit_msgs::CollisionObject& object, bool invert) {
auto op = object.operation;
if (invert) {
if (op == moveit_msgs::CollisionObject::ADD)
op = moveit_msgs::CollisionObject::REMOVE;
else if (op == moveit_msgs::CollisionObject::REMOVE)
throw std::runtime_error("cannot apply removeObject() backwards");
}
scene.processCollisionObjectMsg(object);
object.operation = op;
}
} // namespace stages
} // namespace task_constructor