Merge pull request #10 from sunava/main

[actions-designator] mixing, cutting demos and actions meanwhile fixi…

demos/pycram_cutting_demo/cutting-demo.py

@@ -0,0 +1,64 @@
+from pycram.process_module import simulated_robot, with_simulated_robot
+from pycram.designators.action_designator import *
+from pycram.enums import Arms
+from pycram.designators.object_designator import *
+from pycram.designators.object_designator import BelieveObject
+import pycram.helper as helper
+from pycram.resolver.action.cutting import CuttingActionSPARQL
+
+world = BulletWorld()
+world.set_gravity([0, 0, -9.8])
+robot = Object("pr2", "robot", "../../resources/" + robot_description.name + ".urdf")
+robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve()
+kitchen = Object("kitchen", "environment", "kitchen.urdf")
+robot.set_joint_state(robot_description.torso_joint, 0.24)
+kitchen_desig = ObjectDesignatorDescription(names=["kitchen"])
+spawning_poses = {
+    # 'bigknife': Pose([-0.95, 1.2, 1.3], [1, -1, 1, -1]),
+    'bigknife': Pose([0.9, 0.6, 0.8], [0, 0, 0, -1]),
+    # 'bread': Pose([-0.85, 0.9, 0.90], [0, 0, -1, 1])
+    'bread': Pose([-0.85, 0.9, 0.90], [0, 0, -1, -1]),
+    'board': Pose([-0.85, 0.9, 0.85], [0, 0, -1, -1]),
+    'cocumber': Pose([-0.85, 0.9, 0.87], [0, 0, -1, -1])
+}
+bigknife = Object("bigknife", "bigknife", "big-knife.stl", spawning_poses["bigknife"])
+cocumber = Object("cocumber", "cocumber", "cocumber.stl", spawning_poses["cocumber"])
+board = Object("board", "board", "board.stl", spawning_poses["board"])
+cocumber.set_color([0, 1, 0.04, 1])
+board.set_color([0.4, 0.2, 0.06, 1])
+bigknife_BO = BelieveObject(names=["bigknife"])
+bread_BO = BelieveObject(names=["bread"])
+cocumber_BO = BelieveObject(names=["cocumber"])
+
+
+with simulated_robot:
+    ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+    MoveTorsoAction([0.33]).resolve().perform()
+    grasp = robot_description.grasps.get_orientation_for_grasp("top")
+    arm = "left"
+    pickup_pose_knife = CostmapLocation(target=bigknife_BO.resolve(), reachable_for=robot_desig).resolve()
+    pickup_arm = pickup_pose_knife.reachable_arms[0]
+    NavigateAction(target_locations=[pickup_pose_knife.pose]).resolve().perform()
+    PickUpAction(object_designator_description=bigknife_BO,
+                 arms=["left"],
+                 grasps=["top"]).resolve().perform()
+
+    ParkArmsAction([Arms.BOTH]).resolve().perform()
+    original_quaternion = (0, 0, 0, 1)
+    rotation_axis = (0, 0, 1)
+    rotation_quaternion = helper.axis_angle_to_quaternion(rotation_axis, 180)
+    resulting_quaternion = helper.multiply_quaternions(original_quaternion, rotation_quaternion)
+    nav_pose = Pose([-0.3, 0.9, 0.0], resulting_quaternion)
+    NavigateAction(target_locations=[nav_pose]).resolve().perform()
+    LookAtAction(targets=[cocumber_BO.resolve().pose]).resolve().perform()
+
+    detected_bread_desig = DetectAction(cocumber_BO).resolve().perform()
+
+    CuttingAction(object_designator_description=cocumber_BO,
+                        arms=["left"],
+                        grasps=["top"],).resolve().perform()
+
+    # CuttingActionSPARQL(object_designator_description=bread_BO,
+    #              arms=["left"],
+    #              grasps=["top"]).resolve().perform()

demos/pycram_cutting_demo/queries/food_has_part.sparql

@@ -0,0 +1,13 @@
+# Query for assessing whether a specific food object has a specific part
+PREFIX owl: <http://www.w3.org/2002/07/owl#>
+PREFIX cut: <http://www.ease-crc.org/ont/food_cutting#>
+PREFIX rdf: <http://www.w3.org/1999/02/22-rdf-syntax-ns#>
+PREFIX rdfs: <http://www.w3.org/2000/01/rdf-schema#>
+PREFIX foodon: <http://purl.obolibrary.org/obo/>
+ASK {
+	foodon:${food} rdfs:subClassOf* ?parts_node.
+	?parts_node owl:onProperty cut:hasPart.
+	?parts_node owl:someValuesFrom ?val_node.
+	?val_node owl:intersectionOf ?inter_node.
+	?inter_node rdf:first cut:${part}.
+}

demos/pycram_cutting_demo/queries/get_cutting_position.sparql

@@ -0,0 +1,11 @@
+# Query for extracting the cutting position for the specific cutting verb
+PREFIX owl: <http://www.w3.org/2002/07/owl#>
+PREFIX cut: <http://www.ease-crc.org/ont/food_cutting#>
+PREFIX soma: <http://www.ease-crc.org/ont/SOMA.owl#>
+PREFIX rdfs: <http://www.w3.org/2000/01/rdf-schema#>
+SELECT * WHERE {
+	${verb} rdfs:subClassOf* ?pos_node.
+	?pos_node owl:onProperty cut:affordsPosition.
+	?pos_node owl:someValuesFrom ?pos.
+	BIND(REPLACE(STR(?pos), "^.*[#/]", "") AS ?res).
+} 

demos/pycram_cutting_demo/queries/get_cutting_tool.sparql

@@ -0,0 +1,22 @@
+# Query for extracting the tool for cutting the specific food object
+PREFIX owl: <http://www.w3.org/2002/07/owl#>
+PREFIX soma: <http://www.ease-crc.org/ont/SOMA.owl#>
+PREFIX rdf: <http://www.w3.org/1999/02/22-rdf-syntax-ns#>
+PREFIX rdfs: <http://www.w3.org/2000/01/rdf-schema#>
+PREFIX foodon: <http://purl.obolibrary.org/obo/>
+PREFIX sit_aware: <http://www.ease-crc.org/ont/situation_awareness#>
+SELECT ?res WHERE {
+	foodon:${food} rdfs:subClassOf* ?peel_dis.
+	?peel_dis owl:onProperty soma:hasDisposition.
+	?peel_dis owl:someValuesFrom ?peel_dis_vals.
+	?peel_dis_vals owl:intersectionOf ?afford_vals.
+	?afford_vals rdf:first sit_aware:Cuttability.
+	?afford_vals rdf:rest ?task_trigger.
+	?task_trigger rdf:rest ?trigger.
+	?trigger rdf:first ?trigger_wo_nil.
+	?trigger_wo_nil owl:onProperty soma:affordsTrigger.
+	?trigger_wo_nil owl:allValuesFrom ?trigger_tool.
+	?trigger_tool owl:allValuesFrom ?tool.
+	?tool rdfs:subClassOf soma:CuttingTool.
+	BIND(REPLACE(STR(?tool), "^.*[#/]", "") AS ?res).
+}

demos/pycram_cutting_demo/queries/get_peeling_tool.sparql

@@ -0,0 +1,22 @@
+# Query for extracting the tool for peeling the specific food object
+PREFIX owl: <http://www.w3.org/2002/07/owl#>
+PREFIX cut: <http://www.ease-crc.org/ont/food_cutting#>
+PREFIX soma: <http://www.ease-crc.org/ont/SOMA.owl#>
+PREFIX rdf: <http://www.w3.org/1999/02/22-rdf-syntax-ns#>
+PREFIX rdfs: <http://www.w3.org/2000/01/rdf-schema#>
+PREFIX foodon: <http://purl.obolibrary.org/obo/>
+SELECT ?res WHERE {
+	foodon:${food} rdfs:subClassOf* ?peel_dis.
+	?peel_dis owl:onProperty soma:hasDisposition.
+	?peel_dis owl:someValuesFrom ?peel_dis_vals.
+	?peel_dis_vals owl:intersectionOf ?afford_vals.
+	?afford_vals rdf:first cut:Peelability.
+	?afford_vals rdf:rest ?task_trigger.
+	?task_trigger rdf:rest ?trigger.
+	?trigger rdf:first ?trigger_wo_nil.
+	?trigger_wo_nil owl:onProperty soma:affordsTrigger.
+	?trigger_wo_nil owl:allValuesFrom ?trigger_tool.
+	?trigger_tool owl:allValuesFrom ?tool.
+	?tool rdfs:subClassOf cut:PeelingTool.
+	BIND(REPLACE(STR(?tool), "^.*[#/]", "") AS ?res).
+}

demos/pycram_cutting_demo/queries/get_prior_task.sparql

@@ -0,0 +1,11 @@
+# Query for extracting the prior task for the specific cutting verb
+PREFIX owl: <http://www.w3.org/2002/07/owl#>
+PREFIX cut: <http://www.ease-crc.org/ont/food_cutting#>
+PREFIX soma: <http://www.ease-crc.org/ont/SOMA.owl#>
+PREFIX rdfs: <http://www.w3.org/2000/01/rdf-schema#>
+SELECT ?res WHERE {
+	${verb} rdfs:subClassOf* ?sub.
+	?sub owl:onProperty cut:requiresPriorTask .
+	?sub owl:someValuesFrom ?priortask.
+	BIND(REPLACE(STR(?priortask), "^.*[#/]", "") AS ?res).
+}

demos/pycram_cutting_demo/queries/get_repetitions.sparql

@@ -0,0 +1,25 @@
+# Query for extracting the amount of repetitions necessary for executing the specific cutting verb
+PREFIX owl: <http://www.w3.org/2002/07/owl#>
+PREFIX cut: <http://www.ease-crc.org/ont/food_cutting#>
+PREFIX soma: <http://www.ease-crc.org/ont/SOMA.owl#>
+PREFIX rdfs: <http://www.w3.org/2000/01/rdf-schema#>
+
+SELECT ?res WHERE {
+	{
+		${verb} rdfs:subClassOf* ?rep_node.
+		?rep_node owl:onProperty cut:repetitions.
+		FILTER EXISTS {
+			?rep_node owl:hasValue ?val.
+			}
+		BIND("exactly 1" AS ?res)
+	}
+	UNION
+	{
+		${verb} rdfs:subClassOf* ?rep_node.
+		?rep_node owl:onProperty cut:repetitions.
+		FILTER EXISTS {
+			?rep_node owl:minQualifiedCardinality ?val.
+		}
+		BIND("at least 1" AS ?res)
+	}
+}

demos/pycram_cutting_demo/queries/get_target.sparql

@@ -0,0 +1,30 @@
+# Query the target for the specific verb. This is either the food itself or a specific shape/part like Halve or Slice
+PREFIX owl: <http://www.w3.org/2002/07/owl#>
+PREFIX soma: <http://www.ease-crc.org/ont/SOMA.owl#>
+PREFIX cut: <http://www.ease-crc.org/ont/food_cutting#>
+PREFIX rdf: <http://www.w3.org/1999/02/22-rdf-syntax-ns#>
+PREFIX rdfs: <http://www.w3.org/2000/01/rdf-schema#>
+SELECT ?res WHERE {
+	{
+		${verb} rdfs:subClassOf* ?inter_node.
+		?inter_node owl:intersectionOf ?in_res_node.
+		?in_res_node rdf:first ?input_node.
+		?input_node owl:onProperty cut:hasInputObject.
+		?input_node owl:someValuesFrom ?target.
+		FILTER NOT EXISTS {
+			?target owl:unionOf ?union_node.
+		}
+		BIND(REPLACE(STR(?target), "^.*[#/]", "") AS ?res).
+	} 
+	UNION
+	{
+		${verb} rdfs:subClassOf* ?inter_node.
+		?inter_node owl:intersectionOf ?in_res_node.
+		?in_res_node rdf:first ?input_node.
+		?input_node owl:onProperty cut:hasInputObject.
+		?input_node owl:someValuesFrom ?targets_node.
+		?targets_node owl:unionOf ?union_node.
+		?union_node rdf:first ?target.
+		BIND(REPLACE(STR(?target), "^.*[#/]", "") AS ?res).
+	}
+} 

demos/pycram_mixing_demo/mixing-demo.py

@@ -0,0 +1,59 @@
+from pycram.process_module import simulated_robot, with_simulated_robot
+from pycram.designators.action_designator import *
+from pycram.enums import Arms
+from pycram.designators.object_designator import *
+from pycram.designators.object_designator import BelieveObject
+import pycram.helper as helper
+
+
+world = BulletWorld()
+world.set_gravity([0, 0, -9.8])
+# plane = Object("floor", "environment", "plane.urdf", world=world)
+robot = Object("pr2", "robot", "../../resources/" + robot_description.name + ".urdf")
+robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve()
+kitchen = Object("kitchen", "environment", "kitchen.urdf")
+robot.set_joint_state(robot_description.torso_joint, 0.24)
+kitchen_desig = ObjectDesignatorDescription(names=["kitchen"])
+
+spawning_poses = {
+    'whisk': Pose([0.9, 0.6, 0.8], [0, 0, 0, -1]),
+    'big-bowl': Pose([-0.85, 0.9, 1], [0, 0, -1, -1])
+}
+whisk = Object("whisk", "whisk", "whisk.stl", spawning_poses["whisk"])
+big_bowl = Object("big-bowl", "big-bowl", "big-bowl.stl", spawning_poses["big-bowl"])
+whisk_BO = BelieveObject(names=["whisk"])
+big_bowl_BO = BelieveObject(names=["big-bowl"])
+
+
+with simulated_robot:
+    ParkArmsAction([Arms.BOTH]).resolve().perform()
+    MoveTorsoAction([0.33]).resolve().perform()
+    grasp = robot_description.grasps.get_orientation_for_grasp("top")
+    arm = "left"
+    #
+    pickup_pose_knife = CostmapLocation(target=whisk_BO.resolve(), reachable_for=robot_desig).resolve()
+    pickup_arm = pickup_pose_knife.reachable_arms[0]
+
+    NavigateAction(target_locations=[pickup_pose_knife.pose]).resolve().perform()
+
+    PickUpAction(object_designator_description=whisk_BO,
+                 arms=["left"],
+                 grasps=["top"]).resolve().perform()
+
+    ParkArmsAction([Arms.BOTH]).resolve().perform()
+    original_quaternion = (0, 0, 0, 1)
+    rotation_axis = (0, 0, 1)
+
+    rotation_quaternion = helper.axis_angle_to_quaternion(rotation_axis, 180)
+    resulting_quaternion = helper.multiply_quaternions(original_quaternion, rotation_quaternion)
+
+    nav_pose = Pose([-0.3, 0.9, 0.0], resulting_quaternion)
+
+    # NavigateAction(target_locations=[pickup_pose_knife.pose]).resolve().perform()
+    NavigateAction(target_locations=[nav_pose]).resolve().perform()
+    LookAtAction(targets=[big_bowl_BO.resolve().pose]).resolve().perform()
+
+    MixingAction(object_designator_description=big_bowl_BO,
+                 object_tool_designator_description=big_bowl_BO,
+                 arms=["left"],
+                 grasps=["top"]).resolve().perform()

src/pycram/bullet_world.py

@@ -1277,6 +1277,23 @@ def get_AABB(self, link_name: Optional[str] = None) -> Tuple[List[float], List[f
         else:
             return p.getAABB(self.id, physicsClientId=self.world.client_id)
 
+    def get_object_dimensions(self, link_name: Optional[str] = None) -> Tuple[float, float, float]:
+        """
+        Return the dimensions of the object.
+        :param link_name: The Optional name of a link of this object.
+        :return: The dimensions of the object, as a Tuple with float values.
+        """
+        # Retrieve AABB based on link_name presence
+        if link_name:
+            aabb = p.getAABB(self.id, self.links[link_name], self.world.client_id)
+        else:
+            aabb = p.getAABB(self.id, physicsClientId=self.world.client_id)
+
+        # Calculate dimensions
+        dimensions = [np.absolute(aabb[0][i] - aabb[1][i]) for i in range(3)]
+
+        return dimensions
+
     def get_base_origin(self, link_name: Optional[str] = None) -> Pose:
         """
         Returns the origin of the base/bottom of an object/link

src/pycram/designator.py

@@ -714,29 +714,10 @@ def special_knowledge_adjustment_pose(self, grasp: str, pose: Pose) -> Pose:
                     pose_in_object.pose.position.x += value[0]
                     pose_in_object.pose.position.y += value[1]
                     pose_in_object.pose.position.z += value[2]
-                    rospy.loginfo("Adjusted target pose based on special knowledge for grasp: ", grasp)
+                    rospy.loginfo("Adjusted target pose based on special knowledge for grasp: " + grasp)
                     return pose_in_object
             return pose
 
-        # def special_knowledge(self, grasp, pose):
-        #     """
-        #     Returns t special knowledge for "grasp front".
-        #     """
-        #
-        #     special_knowledge = []  # Initialize as an empty list
-        #     if self.type in SPECIAL_KNOWLEDGE:
-        #         special_knowledge = SPECIAL_KNOWLEDGE[self.type]
-        #
-        #     for key, value in special_knowledge:
-        #         if key == grasp:
-        #             # Adjust target pose based on special knowledge
-        #             pose.pose.position.x += value[0]
-        #             pose.pose.position.y += value[1]
-        #             pose.pose.position.z += value[2]
-        #             print("Adjusted target pose based on special knowledge for grasp: ", grasp)
-        #             return pose
-        #     return pose
-
     def __init__(self, names: Optional[List[str]] = None, types: Optional[List[str]] = None,
                  resolver: Optional[Callable] = None):
         """