From 4ec5fd0ccdb97076f892e15856a15a3850085c79 Mon Sep 17 00:00:00 2001
From: Roman Andrianov <rom.andrianov1984@gmail.com>
Date: Thu, 7 Apr 2022 15:17:46 +0000
Subject: [PATCH] Fix meshes position

---
 .gitignore                                    |  2 +
 .../urdf/tools/rasmt_hand_macro.xacro         | 16 ++---
 robossembler/launch/task_planner.launch.py    |  2 +-
 robossembler/pddl/cat_domain.pddl             | 44 --------------
 robossembler/pddl/domain.pddl                 | 60 +++++--------------
 robossembler/pddl/problem.pddl                | 25 +++-----
 robossembler_servers/src/gripper_cmd.cpp      | 13 ++--
 sdf_models/models/box1/frames.json            |  2 +-
 8 files changed, 43 insertions(+), 121 deletions(-)
 delete mode 100644 robossembler/pddl/cat_domain.pddl

diff --git a/.gitignore b/.gitignore
index 8408aba..afdd69f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,4 @@
 ref
 *.blend1
+*.pyc
+*.vscode
\ No newline at end of file
diff --git a/rasmt_support/urdf/tools/rasmt_hand_macro.xacro b/rasmt_support/urdf/tools/rasmt_hand_macro.xacro
index 9156081..a76580d 100644
--- a/rasmt_support/urdf/tools/rasmt_hand_macro.xacro
+++ b/rasmt_support/urdf/tools/rasmt_hand_macro.xacro
@@ -79,7 +79,7 @@
     </inertial>
     <visual>
       <origin
-        xyz="-0.02 0 0"
+        xyz="0 0 0"
         rpy="0 0 0" />
       <geometry>
         <mesh
@@ -93,7 +93,7 @@
     </visual>
     <collision>
       <origin
-        xyz="-0.02 0 0"
+        xyz="0 0 0"
         rpy="0 0 0" />
       <geometry>
         <mesh
@@ -106,14 +106,14 @@
     name="${prefix}_Slide_1"
     type="prismatic">
     <origin
-      xyz="0 0 -0.09305"
+      xyz="0 -0.02 -0.09305"
       rpy="3.1416 0 -1.5708" />
     <parent
       link="${prefix}_Grip_Body" />
     <child
       link="${prefix}_Grip_L" />
     <axis
-      xyz="1 0 0" />
+      xyz="-1 0 0" />
     <limit
       effort="20"
       lower="0.0"
@@ -139,7 +139,7 @@
     </inertial>
     <visual>
       <origin
-        xyz="-0.02 0 0"
+        xyz="0 0 0"
         rpy="0 0 0" />
       <geometry>
         <mesh
@@ -153,7 +153,7 @@
     </visual>
     <collision>
       <origin
-        xyz="-0.02 0 0"
+        xyz="0 0 0"
         rpy="0 0 0" />
       <geometry>
         <mesh
@@ -166,14 +166,14 @@
     name="${prefix}_Slide_2"
     type="prismatic">
     <origin
-      xyz="0 0 -0.09305"
+      xyz="0 0.02 -0.09305"
       rpy="3.1416 0 1.5708" />
     <parent
       link="${prefix}_Grip_Body" />
     <child
       link="${prefix}_Grip_R" />
     <axis
-      xyz="1 0 0" />
+      xyz="-1 0 0" />
     <limit
       effort="20"
       lower="0.0"
diff --git a/robossembler/launch/task_planner.launch.py b/robossembler/launch/task_planner.launch.py
index cbaf10f..0ec4edc 100644
--- a/robossembler/launch/task_planner.launch.py
+++ b/robossembler/launch/task_planner.launch.py
@@ -79,7 +79,7 @@ def generate_launch_description():
         'launch',
         'plansys2_bringup_launch_monolithic.py')),
         launch_arguments={
-          'model_file': pkg_dir + '/pddl/cat_domain.pddl',
+          'model_file': pkg_dir + '/pddl/domain.pddl',
           'namespace': namespace
         }.items())
     """
diff --git a/robossembler/pddl/cat_domain.pddl b/robossembler/pddl/cat_domain.pddl
deleted file mode 100644
index 6417ac8..0000000
--- a/robossembler/pddl/cat_domain.pddl
+++ /dev/null
@@ -1,44 +0,0 @@
-;; Modified domain taken from
-;; "Knowledge transfer in robot manipulation tasks" by Jacob O. Huckaby 2014 
-(define (domain robossembler)
-    (:requirements :strips :typing :adl :fluents :durative-actions)
-    (:types
-        printer workspace - zone
-        part
-        arm
-    )
-
-    (:predicates
-        (part_at ?p - part ?z - zone)
-        (printer_ready ?pr - printer)
-        (arm_available ?a - arm)
-    )
-
-    (:functions)
-
-    (:durative-action print
-        :parameters (?p - part ?pr - printer)
-        :duration ( = ?duration 10)
-        :condition (and
-            (at start(printer_ready ?pr))
-        )
-        :effect (and
-            (at end(part_at ?p ?pr))
-        )
-    )
-
-    (:durative-action remove
-        :parameters (?p - part ?pr - printer ?z - zone ?a - arm)
-        :duration (= ?duration 1)
-        :condition (and 
-            (at start (part_at ?p ?pr))
-            (at start (arm_available ?a))
-        )
-        :effect (and 
-            (at start (not (arm_available ?a)))
-            (at end (part_at ?p ?z))
-            (at end (arm_available ?a))
-            (at end (printer_ready ?pr))
-        )
-    )
-);; end Domain ;;;;;;;;;;;;;;;;;;;;;;;;
\ No newline at end of file
diff --git a/robossembler/pddl/domain.pddl b/robossembler/pddl/domain.pddl
index e954010..6417ac8 100644
--- a/robossembler/pddl/domain.pddl
+++ b/robossembler/pddl/domain.pddl
@@ -3,33 +3,27 @@
 (define (domain robossembler)
     (:requirements :strips :typing :adl :fluents :durative-actions)
     (:types
-        printer workspace - zone ; workspace for assembly operations and storage
-        arm ; robotic manipulator arm
-        ; grip weld scan - tool ; manipulator tool for grip, weld, solder and other operations
-        part ; solid part, that cannot be disassembled
-        assembly ; assembly - can be dissassembled
+        printer workspace - zone
+        part
+        arm
     )
 
     (:predicates
-        (arm-available ?a - arm)
-        (part-at ?p - part ?z - zone)
-        (printer-ready ?p - printer)
-        (part-of ?part - part ?whole - assembly)
-        (assembly-order ?prev ?next - assembly)
-        (assembled ?whole - assembly ?z - zone)
+        (part_at ?p - part ?z - zone)
+        (printer_ready ?pr - printer)
+        (arm_available ?a - arm)
     )
 
     (:functions)
 
     (:durative-action print
         :parameters (?p - part ?pr - printer)
-        :duration (= ?duration 10)
+        :duration ( = ?duration 10)
         :condition (and
-            (at start (printer-ready ?pr))  
+            (at start(printer_ready ?pr))
         )
         :effect (and
-            (at start (not (printer-ready ?pr)))
-            (at end (part-at ?p ?pr))
+            (at end(part_at ?p ?pr))
         )
     )
 
@@ -37,36 +31,14 @@
         :parameters (?p - part ?pr - printer ?z - zone ?a - arm)
         :duration (= ?duration 1)
         :condition (and 
-            (at start (and
-                (part-at ?p ?pr)
-                (arm-available ?a)
-            ))
+            (at start (part_at ?p ?pr))
+            (at start (arm_available ?a))
         )
         :effect (and 
-            (at start (not (arm-available ?a)))
-            (at end (and 
-                (part-at ?p ?z)
-                (arm-available ?a)
-                (printer-ready ?pr)
-            ))
+            (at start (not (arm_available ?a)))
+            (at end (part_at ?p ?z))
+            (at end (arm_available ?a))
+            (at end (printer_ready ?pr))
         )
     )
-
-    (:durative-action assemble
-        :parameters (?p - part ?prev ?next - assembly ?w - workspace ?arm - arm)
-        :duration (= ?duration 5)
-        :condition (and
-            (at start (assembled ?prev ?w))
-            (at start (part-at ?p ?w))
-            (at start (part-of ?p ?next))
-            (at start (arm-available ?arm))
-            (at start (assembly-order ?prev ?next))
-        )
-        :effect (and
-            (at start (not (arm-available ?arm)))
-            (at end (not (part-at ?p ?w)))
-            (at end (arm-available ?arm))
-            (at end (assembled ?next ?w))
-        )
-    )
-)
\ No newline at end of file
+);; end Domain ;;;;;;;;;;;;;;;;;;;;;;;;
\ No newline at end of file
diff --git a/robossembler/pddl/problem.pddl b/robossembler/pddl/problem.pddl
index 38bdc5e..fed4491 100644
--- a/robossembler/pddl/problem.pddl
+++ b/robossembler/pddl/problem.pddl
@@ -1,29 +1,18 @@
 (define (problem p1)
 (:domain robossembler)
 (:objects
-	; information from Scene
-	rasmt - arm
+	rasmt_arm_group - arm
 	printer1 printer2 - printer
-	workspace1 - workspace
-	; information from CAD
-	part1 part2 - part
-	subasm0 subasm1 subasm2 - assembly
+	workspace1 workspace2 - workspace
+	box1 - part
 )
 (:init
-	; information from Scene
-	(printer-ready printer1)
-	(printer-ready printer2)
-	(arm-available rasmt)
-	; information from CAD
-	(assembled subasm0 workspace1)
-	(part-of part1 subasm1)
-	(part-of part2 subasm2)
-	(assembly-order subasm0 subasm1)
-	(assembly-order subasm1 subasm2)
+	(printer_ready printer1)
+	(printer_ready printer2)
+	(arm_available rasmt_arm_group)
 )
 (:goal (and
-	; information from CAD
-	(assembled subasm2 workspace1)
+	(part_at box1 workspace1)
 	)
 )
 )
diff --git a/robossembler_servers/src/gripper_cmd.cpp b/robossembler_servers/src/gripper_cmd.cpp
index e8dbf85..49a1511 100644
--- a/robossembler_servers/src/gripper_cmd.cpp
+++ b/robossembler_servers/src/gripper_cmd.cpp
@@ -89,12 +89,15 @@ private:
         auto finger_state_change = goal->value - finger_state;
         auto frame_name = goal->frame;
         RCLCPP_ERROR(this->get_logger(), "Finger change [%.2f]", finger_state_change);
-        if(finger_state_change<0.0)
+        if (!frame_name.empty())
         {
-            send_attach_goal("rasmt", frame_name, "rasmt_Dock_Link", "link"); // TODO: read parameters from other sources
-        } else if (finger_state_change>0.0)
-        {
-            send_detach_goal("rasmt", frame_name, "rasmt_Dock_Link", "link");
+            if(finger_state_change<0.0)
+            {
+                send_attach_goal("rasmt", frame_name, "rasmt_Dock_Link", "link"); // TODO: read parameters from other sources
+            } else if (finger_state_change>0.0)
+            {
+                send_detach_goal("rasmt", frame_name, "rasmt_Dock_Link", "link");
+            }
         }
         // Send commands to gripper
         std_msgs::msg::Float64MultiArray commands;
diff --git a/sdf_models/models/box1/frames.json b/sdf_models/models/box1/frames.json
index b968d55..492eb98 100644
--- a/sdf_models/models/box1/frames.json
+++ b/sdf_models/models/box1/frames.json
@@ -9,7 +9,7 @@
      "position": {
       "x": 0.0,
       "y": 0.0,
-      "z": 0.129
+      "z": 0.149
      },
      "orientation": {
       "x": 0.0,