Merge pull request #1039 from tue-robotics/fix/example_grasping2

Feature(example grasping): update to working state

robot_smach_states/examples/manipulation/example_grasping.py

@@ -13,6 +13,9 @@
 # Robot Smach States
 import robot_smach_states.util.designators as ds
 from robot_smach_states.manipulation import Grab
+from robot_skills.util.shape import RightPrism
+from robot_skills.util.entity import Entity
+import PyKDL as kdl
 
 
 if __name__ == "__main__":
@@ -32,10 +35,11 @@
     entity_id = "test_item"
     pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0), kdl.Vector(args.x, args.y, args.z)),
                         frame_id="/map")
-
     robot.ed.update_entity(id=entity_id, frame_stamped=pose)
+    shape = RightPrism([kdl.Vector(0, 0, 0), kdl.Vector(0, 0.05, 0), kdl.Vector(0.05, 0.05, 0), kdl.Vector(0.05, 0, 0)], -0.1, 0.1)
+    item = Entity(entity_id, "test_type", pose.frame_id, pose.frame, shape, None, None, None)
 
-    item = ds.EdEntityDesignator(robot, id=entity_id)
+    item = ds.Designator(item)
 
     arm = ds.UnoccupiedArmDesignator(robot, arm_properties={"required_trajectories": ["prepare_grasp"],
                                                             "required_goals": ["carrying_pose"],