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:
|
protected:
|
||||||
// apply stored modifications to scene
|
// apply stored modifications to scene
|
||||||
InterfaceState apply(const InterfaceState& from, bool invert);
|
std::pair<InterfaceState, SubTrajectory> apply(const InterfaceState& from, bool invert);
|
||||||
void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::CollisionObject& object);
|
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,
|
void attachObjects(planning_scene::PlanningScene& scene, const std::pair<std::string, std::pair<Names, bool>>& pair,
|
||||||
bool invert);
|
bool invert);
|
||||||
void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert);
|
void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert);
|
||||||
|
|||||||
@ -25,24 +25,28 @@ class TestModifyPlanningScene(unittest.TestCase):
|
|||||||
def setUp(self):
|
def setUp(self):
|
||||||
super(TestModifyPlanningScene, self).setUp()
|
super(TestModifyPlanningScene, self).setUp()
|
||||||
self.psi = PlanningSceneInterface(synchronous=True)
|
self.psi = PlanningSceneInterface(synchronous=True)
|
||||||
|
self.make_box = self.psi._PlanningSceneInterface__make_box
|
||||||
# insert a box to collide with
|
# 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.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
|
# colliding motion
|
||||||
move = stages.MoveRelative("move", core.JointInterpolationPlanner())
|
move = stages.MoveRelative("move", core.JointInterpolationPlanner())
|
||||||
move.group = self.PLANNING_GROUP
|
move.group = self.PLANNING_GROUP
|
||||||
move.setDirection({"joint_1": 0.3})
|
move.setDirection({"joint_1": 0.3})
|
||||||
|
self.task.insert(move, position)
|
||||||
self.task = task = core.Task()
|
|
||||||
task.add(stages.CurrentState("current"), move)
|
|
||||||
|
|
||||||
def test_collision(self):
|
def test_collision(self):
|
||||||
|
self.insertMove()
|
||||||
self.assertFalse(self.task.plan())
|
self.assertFalse(self.task.plan())
|
||||||
|
|
||||||
def test_allow_collision_list(self):
|
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)
|
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())
|
self.assertTrue(self.task.plan())
|
||||||
|
|
||||||
def test_allow_collision_all(self):
|
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])
|
self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])
|
||||||
# attach box to end effector
|
# attach box to end effector
|
||||||
self.psi.attach_box("link_6", "box")
|
self.psi.attach_box("link_6", "box")
|
||||||
mps = stages.ModifyPlanningScene("mps")
|
self.insertMove()
|
||||||
self.assertFalse(self.task.plan())
|
self.assertFalse(self.task.plan())
|
||||||
|
|
||||||
# allow all collisions for attached "box" object
|
# allow all collisions for attached "box" object
|
||||||
|
mps = stages.ModifyPlanningScene("allow all collisions for box")
|
||||||
mps.allowCollisions("box", True)
|
mps.allowCollisions("box", True)
|
||||||
self.task.insert(mps, 1)
|
self.task.insert(mps, 1)
|
||||||
self.assertTrue(self.task.plan())
|
self.assertTrue(self.task.plan())
|
||||||
@ -61,6 +66,46 @@ class TestModifyPlanningScene(unittest.TestCase):
|
|||||||
self.psi.remove_attached_object("link_6", "box")
|
self.psi.remove_attached_object("link_6", "box")
|
||||||
self.psi.remove_world_object("block")
|
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__":
|
if __name__ == "__main__":
|
||||||
roscpp_initialize("test_mtc")
|
roscpp_initialize("test_mtc")
|
||||||
|
|||||||
@ -84,11 +84,13 @@ void ModifyPlanningScene::allowCollisions(const std::string& first, const moveit
|
|||||||
}
|
}
|
||||||
|
|
||||||
void ModifyPlanningScene::computeForward(const InterfaceState& from) {
|
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) {
|
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,
|
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)
|
// invert indicates, whether to detach instead of attach (and vice versa)
|
||||||
// as well as to forbid instead of allow collision (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();
|
planning_scene::PlanningScenePtr scene = from.scene()->diff();
|
||||||
InterfaceState result(scene);
|
InterfaceState state(scene);
|
||||||
// add/remove objects
|
SubTrajectory traj;
|
||||||
for (const auto& collision_object : collision_objects_)
|
try {
|
||||||
processCollisionObject(*scene, collision_object);
|
// add/remove objects
|
||||||
|
for (auto& collision_object : collision_objects_)
|
||||||
|
processCollisionObject(*scene, collision_object, invert);
|
||||||
|
|
||||||
// attach/detach objects
|
// attach/detach objects
|
||||||
for (const auto& pair : attach_objects_)
|
for (const auto& pair : attach_objects_)
|
||||||
attachObjects(*scene, pair, invert);
|
attachObjects(*scene, pair, invert);
|
||||||
|
|
||||||
// allow/forbid collisions
|
// allow/forbid collisions
|
||||||
for (const auto& pairs : collision_matrix_edits_)
|
for (const auto& pairs : collision_matrix_edits_)
|
||||||
allowCollisions(*scene, pairs, invert);
|
allowCollisions(*scene, pairs, invert);
|
||||||
|
|
||||||
if (callback_)
|
if (callback_)
|
||||||
callback_(scene, properties());
|
callback_(scene, properties());
|
||||||
|
} catch (const std::exception& e) {
|
||||||
return result;
|
traj.markAsFailure(e.what());
|
||||||
|
}
|
||||||
|
return std::make_pair(state, traj);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& scene,
|
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);
|
scene.processCollisionObjectMsg(object);
|
||||||
|
object.operation = op;
|
||||||
}
|
}
|
||||||
} // namespace stages
|
} // namespace stages
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user