mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
1daef934ee
commit
bd400de184
@ -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);
|
||||
|
||||
@ -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")
|
||||
|
||||
@ -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,12 +123,14 @@ 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);
|
||||
InterfaceState state(scene);
|
||||
SubTrajectory traj;
|
||||
try {
|
||||
// add/remove objects
|
||||
for (const auto& collision_object : collision_objects_)
|
||||
processCollisionObject(*scene, collision_object);
|
||||
for (auto& collision_object : collision_objects_)
|
||||
processCollisionObject(*scene, collision_object, invert);
|
||||
|
||||
// attach/detach objects
|
||||
for (const auto& pair : attach_objects_)
|
||||
@ -138,13 +142,24 @@ InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool inver
|
||||
|
||||
if (callback_)
|
||||
callback_(scene, properties());
|
||||
|
||||
return result;
|
||||
} 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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user