Fix meshes position

.gitignore

@@ -1,2 +1,4 @@
 ref
 *.blend1
+*.pyc
+*.vscode

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"

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())
     """

robossembler/pddl/cat_domain.pddl

@@ -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 ;;;;;;;;;;;;;;;;;;;;;;;;

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
+        printer workspace - zone
-        arm ; robotic manipulator arm
+        part
-        ; grip weld scan - tool ; manipulator tool for grip, weld, solder and other operations
+        arm
-        part ; solid part, that cannot be disassembled
-        assembly ; assembly - can be dissassembled
     )
 
     (:predicates
-        (arm-available ?a - arm)
+        (part_at ?p - part ?z - zone)
-        (part-at ?p - part ?z - zone)
+        (printer_ready ?pr - printer)
-        (printer-ready ?p - printer)
+        (arm_available ?a - arm)
-        (part-of ?part - part ?whole - assembly)
-        (assembly-order ?prev ?next - assembly)
-        (assembled ?whole - assembly ?z - zone)
     )
 
     (: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
+            (at start (part_at ?p ?pr))
-                (part-at ?p ?pr)
+            (at start (arm_available ?a))
-                (arm-available ?a)
-            ))
         )
         :effect (and 
-            (at start (not (arm-available ?a)))
+            (at start (not (arm_available ?a)))
-            (at end (and 
+            (at end (part_at ?p ?z))
-                (part-at ?p ?z)
+            (at end (arm_available ?a))
-                (arm-available ?a)
+            (at end (printer_ready ?pr))
-                (printer-ready ?pr)
-            ))
         )
     )
-
+);; end Domain ;;;;;;;;;;;;;;;;;;;;;;;;
-    (: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))
-        )
-    )
-)

robossembler/pddl/problem.pddl

@@ -1,29 +1,18 @@
 (define (problem p1)
 (:domain robossembler)
 (:objects
-	; information from Scene
+	rasmt_arm_group - arm
-	rasmt - arm
 	printer1 printer2 - printer
-	workspace1 - workspace
+	workspace1 workspace2 - workspace
-	; information from CAD
+	box1 - part
-	part1 part2 - part
-	subasm0 subasm1 subasm2 - assembly
 )
 (:init
-	; information from Scene
+	(printer_ready printer1)
-	(printer-ready printer1)
+	(printer_ready printer2)
-	(printer-ready printer2)
+	(arm_available rasmt_arm_group)
-	(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)
 )
 (:goal (and
-	; information from CAD
+	(part_at box1 workspace1)
-	(assembled subasm2 workspace1)
 	)
 )
 )

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
+            if(finger_state_change<0.0)
-        } else if (finger_state_change>0.0)
+            {
-        {
+                send_attach_goal("rasmt", frame_name, "rasmt_Dock_Link", "link"); // TODO: read parameters from other sources
-            send_detach_goal("rasmt", frame_name, "rasmt_Dock_Link", "link");
+            } 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;

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,