add scene_monitor services to robossebler node

rasmt_moveit_config/config/rasmt_moveit.rviz

@@ -3,11 +3,9 @@ Panels:
     Help Height: 78
     Name: Displays
     Property Tree Widget:
-      Expanded:
+      Expanded: ~
-        - /Global Options1
-        - /Status1
       Splitter Ratio: 0.5
-    Tree Height: 389
+    Tree Height: 393
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -63,68 +61,6 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          rasmt_Base_Link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          rasmt_Dock_Link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          rasmt_Fork_1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          rasmt_Fork_2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          rasmt_Fork_3:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          rasmt_Grip_Body:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          rasmt_Grip_L:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          rasmt_Grip_R:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          rasmt_Link_1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          rasmt_Link_2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          rasmt_tool0:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          rasmt_tool_end_point:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: true
@@ -216,10 +152,6 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          rasmt_tool_end_point:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
           world:
             Alpha: 1
             Show Axes: false
@@ -323,10 +255,6 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          rasmt_tool_end_point:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
           world:
             Alpha: 1
             Show Axes: false
@@ -336,6 +264,58 @@ Visualization Manager:
         Show Robot Visual: true
       Value: true
       Velocity_Scaling_Factor: 1
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        rasmt_Base_Link:
+          Value: true
+        rasmt_Dock_Link:
+          Value: true
+        rasmt_Fork_1:
+          Value: true
+        rasmt_Fork_2:
+          Value: true
+        rasmt_Fork_3:
+          Value: true
+        rasmt_Grip_Body:
+          Value: true
+        rasmt_Grip_L:
+          Value: true
+        rasmt_Grip_R:
+          Value: true
+        rasmt_Link_1:
+          Value: true
+        rasmt_Link_2:
+          Value: true
+        rasmt_tool0:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        world:
+          rasmt_Base_Link:
+            rasmt_Fork_1:
+              rasmt_Link_1:
+                rasmt_Fork_2:
+                  rasmt_Link_2:
+                    rasmt_Fork_3:
+                      rasmt_Dock_Link:
+                        rasmt_Grip_Body:
+                          rasmt_Grip_L:
+                            {}
+                          rasmt_Grip_R:
+                            {}
+                        rasmt_tool0:
+                          {}
+      Update Interval: 0
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -409,7 +389,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000210000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000253000001880000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000053f0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f3000003a2fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000212000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002530000018a0000016900fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000053f000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:

rasmt_support/world/robossembler.world

@@ -69,6 +69,28 @@
         <kinematic>0</kinematic>
       </link>
     </model>
+    <model name="box">
+        <pose>0.5 0 0.1 0 0 0</pose>
+        <link name="link">
+            <collision name="collision">
+                <geometry>
+                <box>
+                    <size>0.15 0.15 0.05</size>
+                </box>
+                </geometry>
+            </collision>
+            <visual name="visual">
+                <geometry>
+                <box>
+                    <size>0.15 0.15 0.05</size>
+                </box>
+                </geometry>
+                <material>
+                <script>Gazebo/WoodPallet</script>
+                </material>
+            </visual>
+        </link>
+    </model>
     <gravity>0 0 -9.8</gravity>
     <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
     <atmosphere type='adiabatic'/>

robossembler/CMakeLists.txt

@@ -22,6 +22,7 @@ find_package(gazebo_msgs REQUIRED)
 find_package(robossembler_interfaces REQUIRED)
 find_package(behavior_tree REQUIRED)
 find_package(control_msgs REQUIRED)
+find_package(scene_monitor_interfaces REQUIRED)
 
 if (NOT CMAKE_CXX_STANDARD)
     set(CMAKE_CXX_STANDARD 17)
@@ -48,6 +49,7 @@ set(dependencies
     behavior_tree
     std_msgs
     control_msgs
+    scene_monitor_interfaces
 )
 
 include_directories(include)
@@ -64,6 +66,9 @@ list(APPEND plugin_libs robossembler_move_gripper_bt_action_client)
 add_library(robossembler_print_bt_node SHARED src/behavior_tree_nodes/Print.cpp)
 list(APPEND plugin_libs robossembler_print_bt_node)
 
+add_library(robossembler_get_grasp_poses_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/GetGraspPoses.cpp)
+list(APPEND plugin_libs robossembler_get_grasp_poses_bt_action_client)
+
 foreach(bt_plugin ${plugin_libs})
     ament_target_dependencies(${bt_plugin} ${dependencies})
     target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

robossembler/behavior_trees_xml/atomic_skills_xml/assemble.xml

@@ -1,22 +1,22 @@
 <?xml version="1.0"?>
-<root main_tree_to_execute="BehaviorTree">
+<root main_tree_to_execute="Assemble">
     <!-- ////////// -->
-    <BehaviorTree ID="BehaviorTree">
+    <BehaviorTree ID="Assemble">
         <Sequence>
-            <Action ID="Print" frame="${arg2}" server_name="/spawn_entity" server_timeout="10"/>
+            <Action ID="GetGraspPoses" frame="${arg0}" server_name="/scene_monitor/get_grasp_poses" server_timeout="10" cancel_timeout="5" />
-            <Action ID="GetEntityState" part_name="${arg2}" server_name="/get_entity_state" server_timeout="10"/>
+            <Action ID="MoveToPose" command="go_to" arm_group="${arg4}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
-            <Action ID="MoveToPose" command="go_to" arm_group="${arg0}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
             <Action ID="GripperCmd" command="open" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
-            <Action ID="MoveToPose" command="down" arm_group="${arg0}" server_name="/move_cartesian" server_timeout="10" cancel_timeout="5" />
+            <Action ID="MoveToPose" command="down" arm_group="${arg4}" server_name="/move_cartesian" server_timeout="10" cancel_timeout="5" />
             <Action ID="GripperCmd" command="close" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
-            <Action ID="MoveToPose" command="up" arm_group="${arg0}" server_name="/move_cartesian" server_timeout="10" cancel_timeout="5" />
+            <Action ID="MoveToPose" command="up" arm_group="${arg4}" server_name="/move_cartesian" server_timeout="10" cancel_timeout="5" />
             <Action ID="GripperCmd" command="open" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
         </Sequence>
     </BehaviorTree>
     <!-- ////////// -->
     <TreeNodesModel>
-        <Action ID="GetEntityState">
+        <Action ID="GetGraspPoses">
-            <input_port name="part_name"/>
+            <input_port name="frame"/>
+            <output_port name="grasp_pose"/>
         </Action>
         <Action ID="MoveToPose">
             <input_port name="arm_group"/>

robossembler/behavior_trees_xml/atomic_skills_xml/print.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="Print">
+    <!-- ////////// -->
+    <BehaviorTree ID="Print">
+        <Sequence>
+            <Action ID="Print" frame="${arg0}" printer="${arg1}" server_name="/scene_monitor/print_part" server_timeout="10" />
+        </Sequence>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="Print">
+            <input_port name="frame"/>
+            <input_port name="printer"/>
+        </Action>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>

robossembler/behavior_trees_xml/atomic_skills_xml/remove.xml

@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="Remove">
+    <!-- ////////// -->
+    <BehaviorTree ID="Remove">
+        <Sequence>
+            <Action ID="GetGraspPoses" 
+                part_id="${arg0}" 
+                grasp_pose="{grasp_pose}" 
+                pre_grasp_pose="{pre_grasp_pose}" 
+                gap_distance="{gap_distance}" 
+                
+                server_name="/scene_monitor/get_grasp_poses" 
+                server_timeout="10" />
+            <Action ID="MoveToPose"  
+                arm_group="${arg3}" 
+                pose="{pre_grasp_pose}"
+
+                server_name="/move_topose" 
+                server_timeout="10" 
+                cancel_timeout="5" />
+            <Action ID="GripperCmd" 
+                gap_distance="{gap_distance}" 
+                
+                server_name="/gripper_cmd" 
+                server_timeout="10" 
+                cancel_timeout="5" />
+
+        </Sequence>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        
+        <Action ID="GetGraspPoses">
+            <input_port name="part_id"/>
+
+            <output_port name="grasp_pose"/>
+            <output_port name="pre_grasp_pose"/>
+            <output_port name="gap_distance"/>
+        </Action>
+        
+        <Action ID="MoveToPose">
+            <input_port name="arm_group"/>
+            <input_port name="pose"/>
+        </Action>
+        
+        <Action ID="GripperCmd">
+            <input_port name="gap_distance"/>
+        </Action>
+    
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>

robossembler/config/params.yaml

@@ -1,24 +1,20 @@
-assemble_1:
+# assemble:
+#   ros__parameters:
+#     plugins:
+#       - robossembler_get_entity_state_bt_action_client
+#       - robossembler_move_topose_bt_action_client
+#       - robossembler_move_gripper_bt_action_client 
+#       - robossembler_get_grasp_poses_bt_action_client
+
+print_part:
   ros__parameters:
     plugins:
-      - robossembler_get_entity_state_bt_action_client
+      - robossembler_print_bt_node
+
+remove_part:
+  ros__parameters:
+    plugins:
+      - robossembler_get_grasp_poses_bt_action_client
       - robossembler_move_topose_bt_action_client
       - robossembler_move_gripper_bt_action_client
-      - robossembler_print_bt_node
-    frames: ["cube"]
-    materials: ["material1", material2]
-    printers: ["printer1", "printer2"]
-    printer_pos:
-      printer1: [1.0, 1.0, 0.0, 
-                 0.0, 0.0, 0.0,
-                 0.0]
-      printer2: [-1.0, -1.0, 0.0, 
-                 0.0, 0.0, 0.0,
-                 0.0]
-    pick_entity:
-        go_to: [0.40644, 0.0, 0.3274, 0.0, 1.0, 0.0, 0.009]
-        bring: [0.40644, 0.0, 0.2274, 0.0, 1.0, 0.0, 0.009]
-    gripp_cmd:
-        open: 0.06
-        close: 0.02
 

robossembler/include/robot_bt/behavior_tree_nodes/GetGraspPoses.hpp

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <behavior_tree/BtService.hpp>
+#include <scene_monitor_interfaces/srv/get_grasp_poses.hpp>
+
+#include <behaviortree_cpp_v3/bt_factory.h>
+#include <geometry_msgs/msg/pose.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+
+class GetGraspPoses : public BtService<scene_monitor_interfaces::srv::GetGraspPoses>
+{
+public:
+    GetGraspPoses(const std::string &xml_tag_name, 
+        const BT::NodeConfiguration &conf);
+
+    scene_monitor_interfaces::srv::GetGraspPoses::Request::SharedPtr populate_request() override;
+
+    BT::NodeStatus handle_response(scene_monitor_interfaces::srv::GetGraspPoses::Response::SharedPtr response) override;
+
+    static BT::PortsList providedPorts()
+    {
+        return providedBasicPorts({
+            BT::InputPort<std::string>("part_id"),
+            BT::OutputPort<geometry_msgs::msg::Pose>("grasp_pose"),
+            BT::OutputPort<geometry_msgs::msg::Pose>("pre_grasp_pose"),
+            BT::OutputPort<double>("gap_distance")
+        });
+    }
+
+private:
+    std::string frame_;
+    scene_monitor_interfaces::msg::GraspPose grasp_pose_;
+};

robossembler/include/robot_bt/behavior_tree_nodes/Print.hpp

@@ -3,33 +3,34 @@
 #include <string>
 #include <vector>
 
-#include <behaviortree_cpp_v3/bt_factory.h>
-#include <gazebo_msgs/srv/spawn_entity.hpp>
 #include <behavior_tree/BtService.hpp>
+#include <scene_monitor_interfaces/srv/print_part.hpp>
+
+#include <behaviortree_cpp_v3/bt_factory.h>
 #include <geometry_msgs/msg/pose.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 
-class Print : public BtService<gazebo_msgs::srv::SpawnEntity>
+class Print : public BtService<scene_monitor_interfaces::srv::PrintPart>
 {
 public:
     Print(const std::string &xml_tag_name, 
         const BT::NodeConfiguration &conf);
 
-    gazebo_msgs::srv::SpawnEntity::Request::SharedPtr populate_request();
+    scene_monitor_interfaces::srv::PrintPart::Request::SharedPtr populate_request() override;
 
-    BT::NodeStatus handle_response(gazebo_msgs::srv::SpawnEntity::Response::SharedPtr response);
+    BT::NodeStatus handle_response(scene_monitor_interfaces::srv::PrintPart::Response::SharedPtr response) override;
 
     static BT::PortsList providedPorts()
     {
         return providedBasicPorts({
-            BT::InputPort<std::string>("frame")
+            BT::InputPort<std::string>("frame"),
+            BT::InputPort<std::string>("printer")
         });
     }
 
 private:
-    std::map<std::string, geometry_msgs::msg::Pose> printer_coords_;
+
-    std::string material_;
     std::string printer_;
     std::string frame_;
 };

robossembler/launch/task_planner.launch.py

@@ -79,14 +79,14 @@ def generate_launch_description():
         'launch',
         'plansys2_bringup_launch_monolithic.py')),
         launch_arguments={
-          'model_file': pkg_dir + '/pddl/domain.pddl',
+          'model_file': pkg_dir + '/pddl/cat_domain.pddl',
           'namespace': namespace
         }.items())
-
+    """
-    assemble_1 = Node(
+    assemble = Node(
         package='plansys2_bt_actions',
         executable='bt_action_node',
-        name='assemble_1',
+        name='assemble',
         namespace=namespace,
         output='screen',
         parameters=[
@@ -95,12 +95,44 @@ def generate_launch_description():
           robot_description_semantic, 
           kinematics_yaml,
           {
-            'action_name': 'assemble-1',
+            'action_name': 'assemble',
-            'publisher_port': 1666,
+            # 'publisher_port': 1666,
-            'server_port': 1667,
+            # 'server_port': 1667,
-            'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml',
+            'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/assemble.xml',
           }
         ])
+    """
+    print_part = Node(
+        package='plansys2_bt_actions',
+        executable='bt_action_node',
+        name='print_part',
+        namespace=namespace,
+        output='screen',
+        parameters=[
+          pkg_dir + '/config/params.yaml',
+          {
+            'action_name': 'print',
+            'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/print.xml',
+          }
+        ])
+    
+    remove_part = Node(
+        package='plansys2_bt_actions',
+        executable='bt_action_node',
+        name='remove_part',
+        namespace=namespace,
+        output='screen',
+        parameters=[
+          pkg_dir + '/config/params.yaml',
+          robot_description, 
+          robot_description_semantic, 
+          kinematics_yaml,
+          {
+            'action_name': 'remove',
+            'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/remove.xml',
+          }
+        ])
+
     """gz_get_entity_state = Node(
         package='plansys2_bt_actions',
         executable='bt_action_node',
@@ -124,6 +156,8 @@ def generate_launch_description():
 
     # Declare the launch options
     ld.add_action(plansys2_cmd)
-    ld.add_action(assemble_1)
+    # ld.add_action(assemble)
+    ld.add_action(print_part)
+    ld.add_action(remove_part)
     
     return ld

robossembler/package.xml

@@ -30,6 +30,7 @@
  <depend>ament_index_cpp</depend>
  <depend>robossembler_interfaces</depend>
  <depend>behavior_tree</depend>
+  <depend>scene_monitor_interfaces</depend>
 
  <test_depend>ament_lint_common</test_depend>
  <test_depend>ament_lint_auto</test_depend>

robossembler/pddl/cat_domain.pddl

@@ -0,0 +1,44 @@
+;; 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/src/behavior_tree_nodes/Print.cpp

@@ -1,82 +1,50 @@
+#include "robot_bt/behavior_tree_nodes/Print.hpp"
+
+#include <scene_monitor_interfaces/srv/print_part.hpp>
+#include <rclcpp/qos.hpp>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
 #include <iostream>
 #include <filesystem>
 #include <fstream>
 #include <sstream>
 #include <thread>
 
-#include <rclcpp/qos.hpp>
-#include <gazebo_msgs/srv/spawn_entity.hpp>
-#include <ament_index_cpp/get_package_share_directory.hpp>
-#include "robot_bt/behavior_tree_nodes/Print.hpp"
 
-using SpawnEntitySrv = gazebo_msgs::srv::SpawnEntity;
+using PrintPartSrv = scene_monitor_interfaces::srv::PrintPart;
 
 Print::Print(const std::string &xml_tag_name,
                 const BT::NodeConfiguration &conf)
-:BtService<SpawnEntitySrv>(xml_tag_name, conf)
+:BtService<PrintPartSrv>(xml_tag_name, conf)
-{
+{}
-    geometry_msgs::msg::Pose a;
-    a.position.x = 0.8;
-    a.position.y = 0.8;
-    a.position.z = 0.0;
-    geometry_msgs::msg::Pose b;
-    a.position.x = 0.4;
-    a.position.y = 0.0;
-    a.position.z =  0.05;
-    printer_coords_.insert(std::make_pair("printerB", b));
-    printer_coords_.insert(std::make_pair("printerA", a));
-}
 
-SpawnEntitySrv::Request::SharedPtr Print::populate_request()
+PrintPartSrv::Request::SharedPtr Print::populate_request()
 {
-    std::string frame, printer, material;
+    std::string frame, printer;
     getInput<std::string>("frame", frame_);
+    RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< frame_ <<"]");
     
-    printer_ = "printerA";
+    getInput<std::string>("printer", printer_);
-    material_="material1";
+    RCLCPP_INFO_STREAM(_node->get_logger(), "Provided printer name ["<< printer_ <<"]");
 
-
+    auto request = std::make_shared<PrintPartSrv::Request>();
-    std::string package_share_directory = ament_index_cpp::get_package_share_directory("sdf_models");
+    request->frame_id = frame_;
-    std::filesystem::path sdf_path = package_share_directory + "/models/" + frame_ + "/model.sdf";
+    request->printer_id = printer_;
-    
-    RCLCPP_INFO(_node->get_logger(), "Start read frame (%s) frame_path (%s)", frame_.c_str(), sdf_path.c_str());
-
-    std::ifstream sdf_file(sdf_path);
-    std::stringstream buffer;
-    
-    if (sdf_file.is_open()) {       
-        buffer << sdf_file.rdbuf();         
-        sdf_file.close();
-    } else {
-        
-        RCLCPP_WARN(_node->get_logger(), "Failed while try to open file (%s)", sdf_path.c_str());
-        return 0;
-    }  
-
-    std::string xml = buffer.str();
-    
-    geometry_msgs::msg::Pose pose = printer_coords_[printer_];
-
-    auto request = std::make_shared<SpawnEntitySrv::Request>();
-    request->name = frame_;
-    request->initial_pose = pose;
-    request->xml = xml;
-    RCLCPP_INFO(_node->get_logger(), "Connecting to '/spawn_entity' service...");
     
     return request;
 }
 
-BT::NodeStatus Print::handle_response(SpawnEntitySrv::Response::SharedPtr response)
+BT::NodeStatus Print::handle_response(PrintPartSrv::Response::SharedPtr response)
 {
     RCLCPP_INFO(_node->get_logger(), "Handle Response call");
     
     if (!response->success)
     {
-        RCLCPP_WARN(_node->get_logger(), "Get failure response from service with msg: `%s`", response->status_message.c_str());
+        RCLCPP_WARN(_node->get_logger(), "Get failure response from service with msg: `%s`", response->message.c_str());
         return BT::NodeStatus::FAILURE;
     }
 
-    RCLCPP_INFO(_node->get_logger(), "Get success response status with msg: `%s`", response->status_message.c_str());
+    RCLCPP_INFO(_node->get_logger(), "Get success response status with msg: `%s`", response->message.c_str());
     return BT::NodeStatus::SUCCESS;
 }
 

robossembler/src/behavior_tree_nodes/atomic_skills/GetGraspPoses.cpp

@@ -0,0 +1,57 @@
+#include "robot_bt/behavior_tree_nodes/GetGraspPoses.hpp"
+
+#include <scene_monitor_interfaces/srv/get_grasp_poses.hpp>
+#include <rclcpp/qos.hpp>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
+#include <iostream>
+#include <filesystem>
+#include <fstream>
+#include <sstream>
+#include <thread>
+
+
+using GetGraspPosesSrv = scene_monitor_interfaces::srv::GetGraspPoses;
+
+GetGraspPoses::GetGraspPoses(const std::string &xml_tag_name,
+                const BT::NodeConfiguration &conf)
+:BtService<GetGraspPosesSrv>(xml_tag_name, conf)
+{}
+
+GetGraspPosesSrv::Request::SharedPtr GetGraspPoses::populate_request()
+{
+    std::string frame, printer;
+    getInput<std::string>("part_id", frame_);
+    RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< frame_ <<"]");
+    
+    auto request = std::make_shared<GetGraspPosesSrv::Request>();
+    request->frame_id = frame_;
+    
+    return request;
+}
+
+BT::NodeStatus GetGraspPoses::handle_response(GetGraspPosesSrv::Response::SharedPtr response)
+{
+    RCLCPP_INFO(_node->get_logger(), "Handle Response call");
+    
+    if (!response->success)
+    {
+        RCLCPP_WARN(_node->get_logger(), "Get failure response from service with msg: `%s`", response->message.c_str());
+        return BT::NodeStatus::FAILURE;
+    }
+
+    RCLCPP_INFO(_node->get_logger(), "Get success response status with msg: `%s`", response->message.c_str());
+
+    scene_monitor_interfaces::msg::GraspPose grasp_pose = response->grasp_poses[0];
+
+    setOutput<geometry_msgs::msg::Pose>("grasp_pose", grasp_pose.grasp_pose);
+    setOutput<geometry_msgs::msg::Pose>("pre_grasp_pose", grasp_pose.pre_grasp_pose);
+    setOutput<double>("gap_distance", grasp_pose.gap_distance);
+
+    return BT::NodeStatus::SUCCESS;
+}
+
+BT_REGISTER_NODES(factory)
+{
+    factory.registerNodeType<GetGraspPoses>("GetGraspPoses");
+}

robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp

@@ -13,12 +13,12 @@ class SendGripperCmd : public BtAction<GripperCmd>
     public:
         SendGripperCmd(const std::string & name, const BT::NodeConfiguration & config) 
         : BtAction<GripperCmd>(name, config) {
-            //gripper_gap = 0.02;
+            // //gripper_gap = 0.02;
-            gripper_point_a_ = 0.06;
+            // gripper_point_a_ = 0.06;
-            gripper_point_b_ = 0.05;
+            // gripper_point_b_ = 0.05;
 
-            gripper_cmd.insert(std::make_pair("open", gripper_point_a_));
+            // gripper_cmd.insert(std::make_pair("open", gripper_point_a_));
-            gripper_cmd.insert(std::make_pair("close", gripper_point_b_));
+            // gripper_cmd.insert(std::make_pair("close", gripper_point_b_));
 
         }
 
@@ -26,25 +26,29 @@ class SendGripperCmd : public BtAction<GripperCmd>
         {
 
             auto goal = GripperCmd::Goal();
-            getInput<std::string>("command", command);
+            // getInput<std::string>("command", command);
-            goal.value = gripper_cmd[command];
+            //goal.value = gripper_cmd[command];
             //goal.trajectory.points = gripper_cmd[command];
 
+            getInput<double>("gap_distance", distance_);
+            goal.value = distance_;
+
             return goal;
         }
 
         static PortsList providedPorts()
         {
             return providedBasicPorts({
-                    InputPort<std::string>("command")});
+                    InputPort<double>("gap_distance")});
         }
 
     private:
         std::vector<std::string> joint_names_ = {"rasmt_Slide_1", "rasmt_Slide_2"};
-        double gripper_point_a_{0.0};
+        // double gripper_point_a_{0.0};
-        double gripper_point_b_{0.0};
+        // double gripper_point_b_{0.0};
-        std::map<std::string, double> gripper_cmd;
+        // std::map<std::string, double> gripper_cmd;
-        std::string command;
+        // std::string command;
+        double distance_;
 
 }; // class MoveToJointState
 

robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp

@@ -1,9 +1,10 @@
 #include "robossembler_interfaces/action/moveit_send_pose.hpp"
+
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "behavior_tree/BtAction.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "moveit_msgs/action/move_group.h"
-#include "Eigen/Geometry"
+// #include "Eigen/Geometry"
 #include "rclcpp/parameter.hpp"
 
 using namespace BT;
@@ -15,33 +16,33 @@ class MoveToPose : public BtAction<MoveToPoseAction>
     public:
         MoveToPose(const std::string & name, const BT::NodeConfiguration & config) 
         : BtAction<MoveToPoseAction>(name, config) {
-            target_pose_a_.position.x = 0.40644;
+            // target_pose_a_.position.x = 0.519;//0.40644;
-            target_pose_a_.position.y = 0.0;
+            // target_pose_a_.position.y = 0.5;//0.0;
-            target_pose_a_.position.z = 0.3274;
+            // target_pose_a_.position.z = -0.021;//0.3274;
-            target_pose_a_.orientation.x = 0;
+            // target_pose_a_.orientation.x = 0.58;//0;
-            target_pose_a_.orientation.y = 1.0;
+            // target_pose_a_.orientation.y = -0.16;//1.0;
-            target_pose_a_.orientation.z = 0.0;
+            // target_pose_a_.orientation.z = 0.16;//0.0;
-            target_pose_a_.orientation.w = 0.009;
+            // target_pose_a_.orientation.w = 0.77;//0.009;
 
-            target_pose_b_.position.x = 0.0;
+            // target_pose_b_.position.x = 0.0;
-            target_pose_b_.position.y = 0.0;
+            // target_pose_b_.position.y = 0.0;
-            target_pose_b_.position.z = -0.12;
+            // target_pose_b_.position.z = -0.12;
-            target_pose_b_.orientation.x = 0;
+            // target_pose_b_.orientation.x = 0;
-            target_pose_b_.orientation.y = 0.0;
+            // target_pose_b_.orientation.y = 0.0;
-            target_pose_b_.orientation.z = 0.0;
+            // target_pose_b_.orientation.z = 0.0;
-            target_pose_b_.orientation.w = 0.0;
+            // target_pose_b_.orientation.w = 0.0;
 
-            target_pose_c_.position.x = 0.0;
+            // target_pose_c_.position.x = 0.0;
-            target_pose_c_.position.y = 0.0;
+            // target_pose_c_.position.y = 0.0;
-            target_pose_c_.position.z = 0.12;
+            // target_pose_c_.position.z = 0.12;
-            target_pose_c_.orientation.x = 0;
+            // target_pose_c_.orientation.x = 0;
-            target_pose_c_.orientation.y = 0.0;
+            // target_pose_c_.orientation.y = 0.0;
-            target_pose_c_.orientation.z = 0.0;
+            // target_pose_c_.orientation.z = 0.0;
-            target_pose_c_.orientation.w = 0.0;
+            // target_pose_c_.orientation.w = 0.0;
             
-            pick_and_place_command.insert(std::make_pair("go_to", target_pose_a_));
+            // pick_and_place_command.insert(std::make_pair("go_to", target_pose_a_));
-            pick_and_place_command.insert(std::make_pair("down", target_pose_b_));
+            // pick_and_place_command.insert(std::make_pair("down", target_pose_b_));
-            pick_and_place_command.insert(std::make_pair("up", target_pose_c_));
+            // pick_and_place_command.insert(std::make_pair("up", target_pose_c_));
 
 
         }
@@ -50,13 +51,17 @@ class MoveToPose : public BtAction<MoveToPoseAction>
         {
             getInput<std::string>("arm_group", robot_name_);
             //robot_name_ = getInput<std::string>("arm_group").value();
-            getInput<std::string>("command", command);
+            getInput<geometry_msgs::msg::Pose>("pose", pose_);
+
+            RCLCPP_INFO(_node->get_logger(), "GrasPose \npose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f", 
+                pose_.position.x, pose_.position.y, pose_.position.z,
+                pose_.orientation.x, pose_.orientation.y, pose_.orientation.z, pose_.orientation.w);
             RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
             auto goal = MoveToPoseAction::Goal();
             goal.robot_name = robot_name_;
             goal.end_effector_acceleration = 1.0;
             goal.end_effector_velocity = 1.0;
-            goal.target_pose = pick_and_place_command[command];
+            goal.target_pose = pose_;
             RCLCPP_INFO(_node->get_logger(), "Goal populated");
 
             return goal;
@@ -66,16 +71,16 @@ class MoveToPose : public BtAction<MoveToPoseAction>
         {
             return providedBasicPorts({
                     InputPort<std::string>("arm_group"),
-                    InputPort<std::string>("command")});
+                    InputPort<geometry_msgs::msg::Pose>("pose")});
         }
 
     private:
         geometry_msgs::msg::Pose target_pose_a_;
         geometry_msgs::msg::Pose target_pose_b_, target_pose_c_;
         std::string robot_name_;
-        std::map<std::string, geometry_msgs::msg::Pose> pick_and_place_command;
+        // std::map<std::string, geometry_msgs::msg::Pose> pick_and_place_command;
-        std::string command;
+        geometry_msgs::msg::Pose pose_;
-        std::string pick;
+        // std::string pick;
 
 }; // class MoveToPose
 

robossembler/src/move_controller_node.cpp

@@ -54,11 +54,19 @@ public:
 
     void init_knowledge()
     {
-        problem_expert_->addInstance(plansys2::Instance{"rasmt_arm_group", "robot"});
+        problem_expert_->addInstance(plansys2::Instance{"printer1", "printer"});
-        problem_expert_->addInstance(plansys2::Instance{"cube", "part"});
+        problem_expert_->addInstance(plansys2::Instance{"output_shaft_211118", "part"});
-        problem_expert_->addInstance(plansys2::Instance("rasmt_hand_arm_group", "gripper"));
+        problem_expert_->addInstance(plansys2::Instance{"rasmt_arm_group", "arm"});
+        problem_expert_->addInstance(plansys2::Instance{"workspace1", "workspace"});
 
-        problem_expert_->setGoal(plansys2::Goal("(and(enviroment_setup rasmt_arm_group rasmt_hand_arm_group cube))"));
+
+        // problem_expert_->addInstance(plansys2::Instance("rasmt_hand_arm_group", "gripper"));
+
+        problem_expert_->addPredicate(plansys2::Predicate{"(printer_ready printer1)"});
+        problem_expert_->addPredicate(plansys2::Predicate{"(arm_available rasmt_arm_group)"});
+
+        problem_expert_->setGoal(plansys2::Goal("(and(part_at output_shaft_211118 workspace1))"));
+        // problem_expert_->setGoal(plansys2::Goal("(and(enviroment_setup rasmt_arm_group rasmt_hand_arm_group cube))"));
 
     }
 

robossembler_scene_monitor/printer/include/printer_base.hpp

@@ -1,33 +0,0 @@
-#pragma once
-
-#include <string>
-#include <rclcpp/rclcpp.hpp>
-// #include <geometry_msgs/msg/pose.hpp>
-
-namespace printer
-{
-
-template <class Derived>
-class PrinterBase: public rclcpp::Node
-{
-
-    inline const Derived& derived() const& noexcept
-    {
-        return *static_cast<const Derived*>(this);
-    }
-
-public:
-    explicit PrinterBase(const rclcpp::NodeOptions& options);
-    virtual ~PrinterBase() = default;
-    // virtual void print(const std::string& frame_name, 
-    //                    const geometry_msgs::msg::Pose& placement_pose, 
-    //                    const std::string& package_name);
-    // virtual ~Printer();
-
-protected:
-    std::string _printer_name;
-    // geometry_msgs::msg::Pose _printer_pose;
-
-};
-
-} // namespace printer

robossembler_scene_monitor/printer/src/gz_printer_plugin.cpp

@@ -1,15 +0,0 @@
-#include "printer_base.hpp"
-
-namespace printer_plugins
-{
-
-class GZPrinter : public printer_base::Printer
-{
-public:
-    void init(const std::string &printer_name) override
-    {
-        this->_printer_name = printer_name;
-    }
-};
-
-}// namespace printer_plugins

robossembler_scene_monitor/scene_monitor/config/component_state_node.param.yaml

@@ -1,10 +1,11 @@
 component_state_monitor_node:
   ros__parameters:
     models-package: "sdf_models"
-    printers_names: ["printerA"]
+    global_frame: "world"
+    printers_names: ["printer1"]
     printers:
-      printerA:
+      printer1:
-        position: [0.5, 0.5, 0.0]
+        position: [0.5, 0.0, 0.12]
         orientation: [0.0, 0.0, 0.0, 0.0]
       # printerB:
       #   position: [2.0, 2.0, 2.0]

robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp

@@ -70,5 +70,7 @@ private:
     std::unique_ptr<tf2_ros::Buffer> _tf2_buffer;
     rclcpp::TimerBase::SharedPtr _timer;
 
+    std::string _global_frame;
+
 };
 }

robossembler_scene_monitor/scene_monitor/package.xml

@@ -18,7 +18,6 @@
   <!-- <depend>action_msgs</depend> -->
   <depend>scene_monitor_interfaces</depend>
   <depend>nlohmann_json</depend>
-  <depend>printer_plugins</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

robossembler_scene_monitor/scene_monitor/src/component.cpp

@@ -95,15 +95,22 @@ namespace component_state_monitor
             return; 
         }
 
-        _placement = construct_pose(input.at("placement"));
+        _placement = construct_pose(input.at("features").at("placements").at("placement"));
 
         auto grasp_poses = input.at("features").at("grasp-poses");
         for (auto it = grasp_poses.begin(); it != grasp_poses.end(); ++it)
         {
             scene_monitor_interfaces::msg::GraspPose grasp_pose;
             grasp_pose.label = it.key();
-            grasp_pose.placement_pose = construct_pose(it.value().at("placement"));
+            grasp_pose.grasp_pose = construct_pose(it.value().at("placement"));
-            grasp_pose.distance = it.value().at("distance");
+            
+            geometry_msgs::msg::Pose pre_pose;
+            pre_pose.position.z = 0.12;
+            //pre_pose.orientation = grasp_pose.grasp_pose.orientation;
+
+            grasp_pose.pre_grasp_pose = pre_pose;
+
+            grasp_pose.gap_distance = it.value().at("distance");
 
             _grasp_poses.insert({grasp_pose.label, grasp_pose});
         }

robossembler_scene_monitor/scene_monitor/src/component_pose_estimation.cpp

@@ -45,9 +45,9 @@ geometry_msgs::msg::Pose ComponentPoseEstimation::framePose(const std::string& f
     _pose = result->state.pose;
     
     RCLCPP_INFO(this->get_logger(), "Get success response status");
-    RCLCPP_INFO(this->get_logger(), "update frame (%s) with \npose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f", 
+    // RCLCPP_INFO(this->get_logger(), "update frame (%s) with \npose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f", 
-                frame_id.c_str(), _pose.position.x, _pose.position.y, _pose.position.z,
+    //             frame_id.c_str(), _pose.position.x, _pose.position.y, _pose.position.z,
-                _pose.orientation.x, _pose.orientation.y, _pose.orientation.z, _pose.orientation.w);
+    //             _pose.orientation.x, _pose.orientation.y, _pose.orientation.z, _pose.orientation.w);
 
     return _pose;
 }

robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp

@@ -13,6 +13,7 @@ namespace component_state_monitor
     ComponentStateMonitor::ComponentStateMonitor(): Node("component_state_monitor_node")
     {   
         this->declare_parameter<std::string>("models-package", "sdf_models");
+        this->declare_parameter<std::string>("global_frame", "world");
         this->declare_parameter<std::vector<std::string>>("printers_names", std::vector<std::string>{});
 
         RCLCPP_INFO(this->get_logger(), "Initialize [get_part] service");
@@ -43,7 +44,6 @@ namespace component_state_monitor
 
         initializeParameters();
         initializeComponents(_models_package);
-        // _viz_marker_pub = this->create_publisher<visualization_msgs::msg::MarkerArray>("scene_monitor_viz_markers", 10);
 
         _timer = this->create_wall_timer(
             20ms, std::bind(&ComponentStateMonitor::updateComponents, this)
@@ -56,11 +56,12 @@ namespace component_state_monitor
         {
             if (component.second.getCurrentState() == ComponentState::Printed)
             {
-                makeTransform(component.second, "world");
+                makeTransform(component.second, _global_frame);
                 auto grasp_poses = component.second.getGraspPoses();
                 for (const auto& grasp_pose: grasp_poses)
                 {
-                    makeTransform(grasp_pose.label, grasp_pose.placement_pose, component.first);
+                    makeTransform(grasp_pose.label, grasp_pose.grasp_pose, component.first);
+                    makeTransform(std::string("pre_" + grasp_pose.label), grasp_pose.pre_grasp_pose, grasp_pose.label);
                 }
             }
         }
@@ -116,6 +117,9 @@ namespace component_state_monitor
         this->get_parameter("models-package", _models_package);
         RCLCPP_INFO(this->get_logger(), "Initialize with models-package (%s)", _models_package.c_str());
 
+        this->get_parameter("global_frame", _global_frame);
+        RCLCPP_INFO(this->get_logger(), "Initialize with global_frame (%s)", _global_frame.c_str());
+        
         std::vector<std::string> printers;
         this->get_parameter("printers_names", printers);
         
@@ -146,7 +150,6 @@ namespace component_state_monitor
             tmp_pose.orientation.z = orientation.at(3);
 
             _loaded_printers.insert({printer, tmp_pose});
-            // makeTransform(printer, tmp_pose, "world");
         }
     }
 
@@ -184,7 +187,41 @@ namespace component_state_monitor
         if (_loaded_components.find(request->frame_id) != _loaded_components.end())
         {
             auto component = _loaded_components.at(request->frame_id);
-            response->grasp_poses = component.getGraspPoses();
+            auto grasp_poses = component.getGraspPoses();
+            std::vector<scene_monitor_interfaces::msg::GraspPose> result_poses;
+
+            for (auto pose: grasp_poses)
+            {
+                geometry_msgs::msg::TransformStamped transformStamped = _tf2_buffer->lookupTransform(
+                    _global_frame, pose.label, tf2::TimePointZero 
+                );
+
+                pose.grasp_pose.position.x = transformStamped.transform.translation.x;
+                pose.grasp_pose.position.y = transformStamped.transform.translation.y;
+                pose.grasp_pose.position.z = transformStamped.transform.translation.z;
+
+                pose.grasp_pose.orientation.x = transformStamped.transform.rotation.x;
+                pose.grasp_pose.orientation.y = transformStamped.transform.rotation.y;
+                pose.grasp_pose.orientation.z = transformStamped.transform.rotation.z;
+                pose.grasp_pose.orientation.w = transformStamped.transform.rotation.w;
+
+                geometry_msgs::msg::TransformStamped preTransformStamped = _tf2_buffer->lookupTransform(
+                    _global_frame, std::string("pre_"+pose.label), tf2::TimePointZero 
+                );
+
+                pose.pre_grasp_pose.position.x = preTransformStamped.transform.translation.x;
+                pose.pre_grasp_pose.position.y = preTransformStamped.transform.translation.y;
+                pose.pre_grasp_pose.position.z = preTransformStamped.transform.translation.z;
+
+                pose.pre_grasp_pose.orientation.x = preTransformStamped.transform.rotation.x;
+                pose.pre_grasp_pose.orientation.y = preTransformStamped.transform.rotation.y;
+                pose.pre_grasp_pose.orientation.z = preTransformStamped.transform.rotation.z;
+                pose.pre_grasp_pose.orientation.w = preTransformStamped.transform.rotation.w;
+
+                result_poses.push_back(pose);
+            }
+
+            response->grasp_poses = result_poses;
             response->success = true;
             response->message = "succesfully received a response from service [get_grasp_poses]";
         } else {
@@ -208,30 +245,33 @@ namespace component_state_monitor
             
             if (component.getCurrentState() == ComponentState::Printed)
             {
-                // std_msgs::msg::Header header;
-                // header.frame_id = request->frame_id;
-
-                // geometry_msgs::msg::PoseStamped target_pose;
-                // target_pose.header = header;
-                // target_pose.pose = component.getGraspPose(header.frame_id).placement_pose;
-
-                // std::string world = "world";
-                // geometry_msgs::msg::PoseStamped target_pose_req = 
-                //     _tf2_buffer->transform(target_pose, world);
                 geometry_msgs::msg::TransformStamped transformStamped = _tf2_buffer->lookupTransform(
-                    "world", request->grasp_pose_name, tf2::TimePointZero 
+                    _global_frame, request->grasp_pose_name, tf2::TimePointZero 
                 );
 
                 auto grasp_pose = component.getGraspPose(request->grasp_pose_name);
 
-                grasp_pose.placement_pose.position.x = transformStamped.transform.translation.x;
+                grasp_pose.grasp_pose.position.x = transformStamped.transform.translation.x;
-                grasp_pose.placement_pose.position.y = transformStamped.transform.translation.y;
+                grasp_pose.grasp_pose.position.y = transformStamped.transform.translation.y;
-                grasp_pose.placement_pose.position.z = transformStamped.transform.translation.z;
+                grasp_pose.grasp_pose.position.z = transformStamped.transform.translation.z;
 
-                grasp_pose.placement_pose.orientation.x = transformStamped.transform.rotation.x;
+                grasp_pose.grasp_pose.orientation.x = transformStamped.transform.rotation.x;
-                grasp_pose.placement_pose.orientation.y = transformStamped.transform.rotation.y;
+                grasp_pose.grasp_pose.orientation.y = transformStamped.transform.rotation.y;
-                grasp_pose.placement_pose.orientation.z = transformStamped.transform.rotation.z;
+                grasp_pose.grasp_pose.orientation.z = transformStamped.transform.rotation.z;
-                grasp_pose.placement_pose.orientation.w = transformStamped.transform.rotation.w;
+                grasp_pose.grasp_pose.orientation.w = transformStamped.transform.rotation.w;
+
+                geometry_msgs::msg::TransformStamped preTransformStamped = _tf2_buffer->lookupTransform(
+                    _global_frame, std::string("pre_"+grasp_pose.label), tf2::TimePointZero 
+                );
+
+                grasp_pose.pre_grasp_pose.position.x = preTransformStamped.transform.translation.x;
+                grasp_pose.pre_grasp_pose.position.y = preTransformStamped.transform.translation.y;
+                grasp_pose.pre_grasp_pose.position.z = preTransformStamped.transform.translation.z;
+
+                grasp_pose.pre_grasp_pose.orientation.x = preTransformStamped.transform.rotation.x;
+                grasp_pose.pre_grasp_pose.orientation.y = preTransformStamped.transform.rotation.y;
+                grasp_pose.pre_grasp_pose.orientation.z = preTransformStamped.transform.rotation.z;
+                grasp_pose.pre_grasp_pose.orientation.w = preTransformStamped.transform.rotation.w;
 
                 response->grasp_pose = grasp_pose;
                 response->success = true;

robossembler_scene_monitor/scene_monitor_interfaces/msg/GraspPose.msg

@@ -1,3 +1,4 @@
 string label
-geometry_msgs/Pose placement_pose # TODO: tf2
+geometry_msgs/Pose grasp_pose
-float64 distance
+geometry_msgs/Pose pre_grasp_pose
+float64 gap_distance

sdf_models/models/cube/frames.json

@@ -1,52 +0,0 @@
-{
-    "label": "cube",
-    "placement": {
-        "position": {
-            "x": 0.0,
-            "y": 0.0,
-            "z": 0.0
-        },
-        "orientation": {
-            "x": 0.0,
-            "y": 0.0,
-            "z": 0.0,
-            "w": 0.0
-        }
-    },
-    "features": {
-        "grasp-poses": {
-            "grasp-pose-1": {
-                "placement": {
-                    "position": {
-                        "x": 0.0,
-                        "y": 0.0,
-                        "z": 0.0
-                    },
-                    "orientation": {
-                        "x": 0.0,
-                        "y": 0.0,
-                        "z": 0.0,
-                        "w": 0.0
-                    }
-                },
-                "distance": 0.1
-            },
-            "grasp-pose-2": {
-                "placement": {
-                    "position": {
-                        "x": 0.0,
-                        "y": 0.0,
-                        "z": 0.0
-                    },
-                    "orientation": {
-                        "x": 0.0,
-                        "y": 0.0,
-                        "z": 0.0,
-                        "w": 0.0
-                    }
-                },
-                "distance": 0.1
-            }
-        }
-    }
-}

sdf_models/models/cube/model.config

@@ -1,14 +0,0 @@
-<?xml version="1.0"?>
-<model>
-  <name>cube</name>
-  <sdf version="1.4">model.sdf</sdf>
-
-  <author>
-    <name>Splinter1984</name>
-    <email>rom.andrianov1984@gmail.com</email>
-  </author>
-
-  <description>
-    A simple cube.
-  </description>
-</model>

sdf_models/models/cube/model.sdf

@@ -1,50 +0,0 @@
-<?xml version="1.0" ?>
-<sdf version="1.5">
-  <model name="cube">
-      <static>0</static>
-      <!--gravity>0</gravity-->
-    <pose>0 0 0 0 0 0</pose>
-    <link name="link">
-        <inertial>
-            <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 -0.000000</pose>
-            <mass>0.005709</mass>
-            <inertia>
-              <ixx>0.000001</ixx>
-              <ixy>-0.000000</ixy>
-              <ixz>-0.000000</ixz>
-              <iyy>0.000002</iyy>
-              <iyz>-0.000000</iyz>
-              <izz>0.000001</izz>
-            </inertia>
-          </inertial>
-      <collision name="collision">
-        <geometry>
-          <box>
-            <size>0.1 0.1 0.1</size>
-          </box>
-        </geometry>
-        <surface>
-            <friction>
-              <ode>
-                <mu>10000</mu>
-                <mu2>10000</mu2>
-              </ode>
-            </friction>
-          </surface>
-      </collision>
-      <visual name="visual">
-        <geometry>
-          <box>
-            <size>0.1 0.1 0.1</size>
-          </box>
-        </geometry>
-        <material>
-          <script>
-            <uri>file://media/materials/scripts/gazebo.material</uri>
-            <name>Gazebo/Grey</name>
-          </script>
-        </material>
-      </visual>
-    </link>
-  </model>
-</sdf>

sdf_models/models/output_shaft_211118/frames.json

@@ -1,30 +1,10 @@
 {
- "features": {
-  "grasp-poses": {
-   "Gripper_for_Part__Feature057": {
-    "placement": {
-     "position": {
-      "x": -7.2e-18,
-      "y": -0.0405,
-      "z": 3.9e-18
-     },
-     "orientation": {
-      "x": -0.1745956924081881,
-      "y": 0.1745956924081881,
-      "z": -0.969036990204714,
-      "w": 1.6022436365319073
-     }
-    },
-    "distance": 42.78237402995835
-   }
-  }
- },
  "label": "output_shaft_211118",
  "placement": {
   "position": {
-   "x": 0.0,
+   "x": -0.0,
-   "y": 0.0,
+   "y": -0.0,
-   "z": 0.0
+   "z": -0.0
   },
   "orientation": {
    "x": 0.0,
@@ -32,5 +12,40 @@
    "z": 1.0,
    "w": 0.0
   }
+ },
+ "features": {
+  "grasp-poses": {
+   "Gripper_for_Part__Feature057": {
+    "placement": {
+     "position": {
+      "x": -4e-19,
+      "y": -0.0815,
+      "z": 2.2099999999999998e-17
+     },
+     "orientation": {
+      "x": 0.7133549208900113,
+      "y": 0.4955425091967418,
+      "z": 0.4955425091967417,
+      "w": 4.38085841046605
+     }
+    },
+    "distance": 0.025
+   }
+  },
+  "placements": {
+   "placement": {
+    "position": {
+     "x": 0.0,
+     "y": 0.0,
+     "z": 0.0
+    },
+    "orientation": {
+     "x": 0.0,
+     "y": 0.0,
+     "z": 0.0,
+     "w": 0.0
+    }
+   }
+  }
  }
 }

sdf_models/models/output_shaft_211118/meshes/output_shaft_211118.dae

@@ -1,7 +1,7 @@
 <COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
   <asset>
-    <created>2022-03-09T21:43:30.158515</created>
+    <created>2022-03-28T00:04:33.211535</created>
-    <modified>2022-03-09T21:43:30.158515</modified>
+    <modified>2022-03-28T00:04:33.211535</modified>
     <up_axis>Z_UP</up_axis>
   </asset>
   <library_geometries>

sdf_models/models/pin_13d007/frames.json

@@ -1,36 +0,0 @@
-{
- "features": {
-  "grasp-poses": {
-   "Gripper_for_Part__Feature101": {
-    "placement": {
-     "position": {
-      "x": 0.016868083435990104,
-      "y": -0.032499999999999994,
-      "z": -0.006282337239951061
-     },
-     "orientation": {
-      "x": 0.365072162647178,
-      "y": 0.6583017226394415,
-      "z": -0.6583017226394415,
-      "w": 2.4415155878685884
-     }
-    },
-    "distance": 2.9902227596730917
-   }
-  }
- },
- "label": "pin_13d007",
- "placement": {
-  "position": {
-   "x": 0.0,
-   "y": 0.0,
-   "z": 0.0
-  },
-  "orientation": {
-   "x": 0.0,
-   "y": 0.0,
-   "z": 1.0,
-   "w": 0.0
-  }
- }
-}

sdf_models/models/pin_13d007/meshes/pin_13d007.dae

@@ -1,51 +0,0 @@
-<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
-  <asset>
-    <created>2022-03-03T17:24:28.233191</created>
-    <modified>2022-03-03T17:24:28.233191</modified>
-    <up_axis>Z_UP</up_axis>
-  </asset>
-  <library_geometries>
-    <geometry id="geometry0" name="pin_13d007">
-      <mesh>
-        <source id="cubeverts-array0">
-          <float_array count="156" id="cubeverts-array0-array">-8 18.64331 1.355045 5 18.30034 1.469625 5 18.64331 1.355045 -8 18.30034 1.469625 -8 18.9489 1.161714 5 18.9489 1.161714 -8 19.19935 0.9008695 5 19.19935 0.9008695 5 19.38009 0.5876692 -8 19.38009 0.5876692 -8 19.48062 0.2403158 5 19.48062 0.2403158 5 19.49511 -0.121004 -8 19.49511 -0.121004 -8 19.42271 -0.4752914 5 19.42271 -0.4752914 -8 19.26762 -0.8019566 5 19.26762 -0.8019566 -8 19.03887 -1.082015 5 19.03887 -1.082015 -8 18.74974 -1.299191 5 18.74974 -1.299191 5 18.41703 -1.440862 -8 18.41703 -1.440862 -8 18.06009 -1.498796 5 18.06009 -1.498796 -8 17.69966 -1.469625 5 17.69966 -1.469625 -8 17.35669 -1.355045 5 17.35669 -1.355045 -8 17.0511 -1.161714 5 17.0511 -1.161714 5 16.80065 -0.9008695 -8 16.80065 -0.9008695 5 16.61991 -0.5876692 -8 16.61991 -0.5876692 5 16.51938 -0.2403158 -8 16.51938 -0.2403158 -8 16.50489 0.121004 5 16.50489 0.121004 5 16.57729 0.4752914 -8 16.57729 0.4752914 5 16.73238 0.8019566 -8 16.73238 0.8019566 -8 16.96113 1.082015 5 16.96113 1.082015 -8 17.25026 1.299191 5 17.25026 1.299191 -8 17.58297 1.440862 5 17.58297 1.440862 5 17.93991 1.498796 -8 17.93991 1.498796</float_array>
-          <technique_common>
-            <accessor count="52" source="#cubeverts-array0-array" stride="3">
-              <param type="float" name="X"/>
-              <param type="float" name="Y"/>
-              <param type="float" name="Z"/>
-            </accessor>
-          </technique_common>
-        </source>
-        <source id="cubenormals-array0">
-          <float_array count="300" id="cubenormals-array0-array">-1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15</float_array>
-          <technique_common>
-            <accessor count="100" source="#cubenormals-array0-array" stride="3">
-              <param type="float" name="X"/>
-              <param type="float" name="Y"/>
-              <param type="float" name="Z"/>
-            </accessor>
-          </technique_common>
-        </source>
-        <vertices id="cubeverts-array0-vertices">
-          <input semantic="POSITION" source="#cubeverts-array0"/>
-        </vertices>
-        <triangles count="100" material="materialref">
-          <input offset="0" semantic="VERTEX" source="#cubeverts-array0-vertices"/>
-          <input offset="1" semantic="NORMAL" source="#cubenormals-array0"/>
-          <p>0 0 1 0 2 0 0 1 3 1 1 1 4 2 2 2 5 2 4 3 0 3 2 3 6 4 7 4 8 4 6 5 5 5 7 5 6 6 4 6 5 6 9 7 6 7 8 7 10 8 11 8 12 8 10 9 8 9 11 9 10 10 9 10 8 10 13 11 10 11 12 11 14 12 12 12 15 12 14 13 13 13 12 13 16 14 15 14 17 14 16 15 14 15 15 15 18 16 17 16 19 16 18 17 16 17 17 17 20 18 21 18 22 18 20 19 19 19 21 19 20 20 18 20 19 20 23 21 20 21 22 21 24 22 22 22 25 22 24 23 23 23 22 23 26 24 25 24 27 24 26 25 24 25 25 25 28 26 26 26 27 26 28 27 27 27 29 27 30 28 31 28 32 28 30 29 29 29 31 29 30 30 28 30 29 30 33 31 32 31 34 31 33 32 30 32 32 32 35 33 34 33 36 33 35 34 33 34 34 34 37 35 35 35 36 35 38 36 39 36 40 36 38 37 36 37 39 37 38 38 37 38 36 38 41 39 40 39 42 39 41 40 38 40 40 40 43 41 41 41 42 41 44 42 42 42 45 42 44 43 43 43 42 43 46 44 45 44 47 44 46 45 44 45 45 45 48 46 49 46 50 46 48 47 47 47 49 47 48 48 46 48 47 48 51 49 48 49 50 49 3 50 50 50 1 50 3 51 51 51 50 51 2 52 1 52 50 52 2 53 50 53 49 53 34 54 39 54 36 54 31 55 34 55 32 55 11 56 8 56 7 56 11 57 7 57 5 57 11 58 5 58 2 58 25 59 29 59 27 59 19 60 17 60 15 60 19 61 15 61 12 61 21 62 49 62 47 62 21 63 47 63 45 63 21 64 45 64 42 64 21 65 42 65 40 65 21 66 40 66 39 66 21 67 31 67 29 67 21 68 25 68 22 68 21 69 12 69 11 69 21 70 39 70 34 70 21 71 19 71 12 71 21 72 2 72 49 72 21 73 34 73 31 73 21 74 11 74 2 74 21 75 29 75 25 75 3 76 0 76 51 76 41 77 37 77 38 77 6 78 9 78 4 78 4 79 9 79 0 79 37 80 28 80 35 80 35 81 28 81 33 81 33 82 28 82 30 82 41 83 28 83 37 83 10 84 13 84 9 84 51 85 13 85 48 85 0 86 13 86 51 86 9 87 13 87 0 87 28 88 24 88 26 88 14 89 16 89 13 89 24 90 20 90 23 90 18 91 20 91 16 91 48 92 20 92 46 92 46 93 20 93 44 93 44 94 20 94 43 94 43 95 20 95 41 95 13 96 20 96 48 96 41 97 20 97 28 97 28 98 20 98 24 98 16 99 20 99 13 99</p>
-        </triangles>
-      </mesh>
-    </geometry>
-  </library_geometries>
-  <library_visual_scenes>
-    <visual_scene id="scene">
-      <node id="node0" name="node0">
-        <instance_geometry url="#geometry0"/>
-      </node>
-    </visual_scene>
-  </library_visual_scenes>
-  <scene>
-    <instance_visual_scene url="#scene"/>
-  </scene>
-</COLLADA>

sdf_models/models/pin_13d007/model.config

@@ -1,11 +0,0 @@
-<?xml version="1.0" ?>
-<model>
-  <name>pin_13d007</name>
-  <version>Version</version>
-  <sdf version="1.5">model.sdf</sdf>
-  <author>
-    <name>Author</name>
-    <email>Email</email>
-  </author>
-  <description>Comment</description>
-</model>

sdf_models/models/pin_13d007/model.sdf

@@ -1,39 +0,0 @@
-<?xml version="1.0" ?>
-<sdf version="1.5">
-  <model name="pin_13d007">
-    <pose>0.008000 -0.016505 0.001499 0.000000 0.000000 0.000000</pose>
-    <static>false</static>
-    <self_collide>false</self_collide>
-    <link name="pin_13d007">
-      <pose>-0.001500 0.018000 0.000000 -1.570796 1.369209 -1.570796</pose>
-      <inertial>
-        <pose>0.000000 0.000000 0.000000 -1.570796 1.369209 -1.570796</pose>
-        <mass>0.000092</mass>
-        <inertia>
-          <ixx>0.000000</ixx>
-          <ixy>-0.000000</ixy>
-          <ixz>-0.000000</ixz>
-          <iyy>0.000000</iyy>
-          <iyz>0.000000</iyz>
-          <izz>0.000000</izz>
-        </inertia>
-      </inertial>
-      <visual name="pin_13d007_visual">
-        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
-        <geometry>
-          <mesh>
-            <uri>model://pin_13d007\meshes\pin_13d007.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-      <collision name="pin_13d007_collision">
-        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
-        <geometry>
-          <mesh>
-            <uri>model://pin_13d007\meshes\pin_13d007.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-    </link>
-  </model>
-</sdf>

sdf_models/models/pin_13d008/frames.json

@@ -1,36 +0,0 @@
-{
- "features": {
-  "grasp-poses": {
-   "Gripper_for_Part__Feature102": {
-    "placement": {
-     "position": {
-      "x": 0.013874705362934565,
-      "y": -0.032499999999999994,
-      "z": 0.011467020148747559
-     },
-     "orientation": {
-      "x": 0.365072162647178,
-      "y": 0.6583017226394415,
-      "z": -0.6583017226394415,
-      "w": 2.4415155878685884
-     }
-    },
-    "distance": 2.9902227596730926
-   }
-  }
- },
- "label": "pin_13d008",
- "placement": {
-  "position": {
-   "x": 0.0,
-   "y": 0.0,
-   "z": 0.0
-  },
-  "orientation": {
-   "x": 0.0,
-   "y": 0.0,
-   "z": 1.0,
-   "w": 0.0
-  }
- }
-}

sdf_models/models/pin_13d008/meshes/pin_13d008.dae

@@ -1,51 +0,0 @@
-<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
-  <asset>
-    <created>2022-03-03T17:24:28.081730</created>
-    <modified>2022-03-03T17:24:28.081730</modified>
-    <up_axis>Z_UP</up_axis>
-  </asset>
-  <library_geometries>
-    <geometry id="geometry0" name="pin_13d008">
-      <mesh>
-        <source id="cubeverts-array0">
-          <float_array count="156" id="cubeverts-array0-array">-8 9.643314 16.9435 5 9.300337 17.05808 5 9.643314 16.9435 -8 9.300337 17.05808 -8 9.948904 16.75017 5 9.948904 16.75017 -8 10.19935 16.48933 5 10.19935 16.48933 5 10.38009 16.17613 -8 10.38009 16.17613 -8 10.48062 15.82877 5 10.48062 15.82877 5 10.49511 15.46745 -8 10.49511 15.46745 -8 10.42271 15.11317 5 10.42271 15.11317 -8 10.26762 14.7865 5 10.26762 14.7865 -8 10.03887 14.50644 5 10.03887 14.50644 -8 9.749736 14.28927 5 9.749736 14.28927 5 9.417033 14.1476 -8 9.417033 14.1476 -8 9.060094 14.08966 5 9.060094 14.08966 -8 8.699663 14.11883 5 8.699663 14.11883 -8 8.356686 14.23341 5 8.356686 14.23341 -8 8.051096 14.42674 5 8.051096 14.42674 5 7.800653 14.68759 -8 7.800653 14.68759 5 7.619911 15.00079 -8 7.619911 15.00079 5 7.519376 15.34814 -8 7.519376 15.34814 -8 7.504889 15.70946 5 7.504889 15.70946 5 7.577292 16.06375 -8 7.577292 16.06375 5 7.732378 16.39041 -8 7.732378 16.39041 -8 7.961134 16.67047 5 7.961134 16.67047 -8 8.250264 16.88765 5 8.250264 16.88765 -8 8.582967 17.02932 5 8.582967 17.02932 5 8.939906 17.08725 -8 8.939906 17.08725</float_array>
-          <technique_common>
-            <accessor count="52" source="#cubeverts-array0-array" stride="3">
-              <param type="float" name="X"/>
-              <param type="float" name="Y"/>
-              <param type="float" name="Z"/>
-            </accessor>
-          </technique_common>
-        </source>
-        <source id="cubenormals-array0">
-          <float_array count="300" id="cubenormals-array0-array">-1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15</float_array>
-          <technique_common>
-            <accessor count="100" source="#cubenormals-array0-array" stride="3">
-              <param type="float" name="X"/>
-              <param type="float" name="Y"/>
-              <param type="float" name="Z"/>
-            </accessor>
-          </technique_common>
-        </source>
-        <vertices id="cubeverts-array0-vertices">
-          <input semantic="POSITION" source="#cubeverts-array0"/>
-        </vertices>
-        <triangles count="100" material="materialref">
-          <input offset="0" semantic="VERTEX" source="#cubeverts-array0-vertices"/>
-          <input offset="1" semantic="NORMAL" source="#cubenormals-array0"/>
-          <p>0 0 1 0 2 0 0 1 3 1 1 1 4 2 2 2 5 2 4 3 0 3 2 3 6 4 7 4 8 4 6 5 5 5 7 5 6 6 4 6 5 6 9 7 6 7 8 7 10 8 11 8 12 8 10 9 8 9 11 9 10 10 9 10 8 10 13 11 10 11 12 11 14 12 12 12 15 12 14 13 13 13 12 13 16 14 15 14 17 14 16 15 14 15 15 15 18 16 17 16 19 16 18 17 16 17 17 17 20 18 21 18 22 18 20 19 19 19 21 19 20 20 18 20 19 20 23 21 20 21 22 21 24 22 22 22 25 22 24 23 23 23 22 23 26 24 25 24 27 24 26 25 24 25 25 25 28 26 26 26 27 26 28 27 27 27 29 27 30 28 31 28 32 28 30 29 29 29 31 29 30 30 28 30 29 30 33 31 32 31 34 31 33 32 30 32 32 32 35 33 34 33 36 33 35 34 33 34 34 34 37 35 35 35 36 35 38 36 39 36 40 36 38 37 36 37 39 37 38 38 37 38 36 38 41 39 40 39 42 39 41 40 38 40 40 40 43 41 41 41 42 41 44 42 42 42 45 42 44 43 43 43 42 43 46 44 45 44 47 44 46 45 44 45 45 45 48 46 49 46 50 46 48 47 47 47 49 47 48 48 46 48 47 48 51 49 48 49 50 49 3 50 50 50 1 50 3 51 51 51 50 51 2 52 1 52 50 52 2 53 50 53 49 53 32 54 42 54 40 54 32 55 40 55 39 55 32 56 39 56 36 56 32 57 36 57 34 57 29 58 47 58 45 58 29 59 45 59 42 59 29 60 32 60 31 60 29 61 42 61 32 61 25 62 29 62 27 62 17 63 15 63 12 63 17 64 12 64 11 64 21 65 49 65 47 65 21 66 25 66 22 66 21 67 19 67 17 67 21 68 11 68 8 68 21 69 8 69 7 69 21 70 7 70 5 70 21 71 5 71 2 71 21 72 2 72 49 72 21 73 47 73 29 73 21 74 29 74 25 74 21 75 17 75 11 75 3 76 0 76 51 76 51 77 0 77 48 77 43 78 33 78 41 78 41 79 33 79 38 79 38 80 33 80 37 80 37 81 33 81 35 81 46 82 28 82 44 82 44 83 28 83 43 83 33 84 28 84 30 84 43 85 28 85 33 85 28 86 24 86 26 86 14 87 16 87 13 87 13 88 16 88 10 88 24 89 20 89 23 89 18 90 20 90 16 90 10 91 20 91 9 91 9 92 20 92 6 92 6 93 20 93 4 93 4 94 20 94 0 94 48 95 20 95 46 95 0 96 20 96 48 96 16 97 20 97 10 97 46 98 20 98 28 98 28 99 20 99 24 99</p>
-        </triangles>
-      </mesh>
-    </geometry>
-  </library_geometries>
-  <library_visual_scenes>
-    <visual_scene id="scene">
-      <node id="node0" name="node0">
-        <instance_geometry url="#geometry0"/>
-      </node>
-    </visual_scene>
-  </library_visual_scenes>
-  <scene>
-    <instance_visual_scene url="#scene"/>
-  </scene>
-</COLLADA>

sdf_models/models/pin_13d008/model.config

@@ -1,11 +0,0 @@
-<?xml version="1.0" ?>
-<model>
-  <name>pin_13d008</name>
-  <version>Version</version>
-  <sdf version="1.5">model.sdf</sdf>
-  <author>
-    <name>Author</name>
-    <email>Email</email>
-  </author>
-  <description>Comment</description>
-</model>

sdf_models/models/pin_13d008/model.sdf

@@ -1,39 +0,0 @@
-<?xml version="1.0" ?>
-<sdf version="1.5">
-  <model name="pin_13d008">
-    <pose>0.008000 -0.007505 -0.014090 0.000000 0.000000 0.000000</pose>
-    <static>false</static>
-    <self_collide>false</self_collide>
-    <link name="pin_13d008">
-      <pose>-0.001500 0.009000 0.015588 -1.570796 1.369209 -1.570796</pose>
-      <inertial>
-        <pose>0.000000 0.000000 0.000000 -1.570796 1.369209 -1.570796</pose>
-        <mass>0.000092</mass>
-        <inertia>
-          <ixx>0.000000</ixx>
-          <ixy>-0.000000</ixy>
-          <ixz>-0.000000</ixz>
-          <iyy>0.000000</iyy>
-          <iyz>0.000000</iyz>
-          <izz>0.000000</izz>
-        </inertia>
-      </inertial>
-      <visual name="pin_13d008_visual">
-        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
-        <geometry>
-          <mesh>
-            <uri>model://pin_13d008\meshes\pin_13d008.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-      <collision name="pin_13d008_collision">
-        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
-        <geometry>
-          <mesh>
-            <uri>model://pin_13d008\meshes\pin_13d008.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-    </link>
-  </model>
-</sdf>

sdf_models/models/stator_3507/frames.json

@@ -1,52 +0,0 @@
-{
-    "label": "stator_3507",
-    "placement": {
-        "position": {
-            "x": 0.0,
-            "y": 0.0,
-            "z": 0.0
-        },
-        "orientation": {
-            "x": 0.0,
-            "y": 0.0,
-            "z": 0.0,
-            "w": 0.0
-        }
-    },
-    "features": {
-        "grasp-poses": {
-            "grasp-pose-1": {
-                "placement": {
-                    "position": {
-                        "x": 0.0,
-                        "y": 0.0,
-                        "z": 0.0
-                    },
-                    "orientation": {
-                        "x": 0.0,
-                        "y": 0.0,
-                        "z": 0.0,
-                        "w": 0.0
-                    }
-                },
-                "distance": 0.1
-            },
-            "grasp-pose-2": {
-                "placement": {
-                    "position": {
-                        "x": 0.0,
-                        "y": 0.0,
-                        "z": 0.0
-                    },
-                    "orientation": {
-                        "x": 0.0,
-                        "y": 0.0,
-                        "z": 0.0,
-                        "w": 0.0
-                    }
-                },
-                "distance": 0.1
-            }
-        }
-    }
-}

sdf_models/models/stator_3507/model.config

@@ -1,11 +0,0 @@
-<?xml version="1.0" ?>
-<model>
-  <name>stator_3507</name>
-  <version>Version</version>
-  <sdf version="1.5">model.sdf</sdf>
-  <author>
-    <name>Author</name>
-    <email>Email</email>
-  </author>
-  <description>Comment</description>
-</model>

sdf_models/models/stator_3507/model.sdf

@@ -1,39 +0,0 @@
-<?xml version="1.0" ?>
-<sdf version="1.5">
-  <model name="stator_3507">
-    <pose>0.015815 -0.019409 0.063983 0.000000 0.000000 0.000000</pose>
-    <static>false</static>
-    <self_collide>false</self_collide>
-    <link name="stator_3507">
-      <pose>0.008103 0.021435 -0.039983 0.000000 -0.000000 -0.000000</pose>
-      <inertial>
-        <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 -0.000000</pose>
-        <mass>0.005709</mass>
-        <inertia>
-          <ixx>0.000001</ixx>
-          <ixy>-0.000000</ixy>
-          <ixz>-0.000000</ixz>
-          <iyy>0.000002</iyy>
-          <iyz>-0.000000</iyz>
-          <izz>0.000001</izz>
-        </inertia>
-      </inertial>
-      <visual name="stator_3507_visual">
-        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
-        <geometry>
-          <mesh>
-            <uri>model://stator_3507\meshes\stator_3507.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-      <collision name="stator_3507_collision">
-        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
-        <geometry>
-          <mesh>
-            <uri>model://stator_3507\meshes\stator_3507.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-    </link>
-  </model>
-</sdf>

sdf_models/models/stator_3508/frames.json

@@ -1,52 +0,0 @@
-{
-    "label": "stator_3508",
-    "placement": {
-        "position": {
-            "x": 1.0,
-            "y": 1.0,
-            "z": 1.0
-        },
-        "orientation": {
-            "x": 1.0,
-            "y": 1.0,
-            "z": 1.0,
-            "w": 1.0
-        }
-    },
-    "features": {
-        "grasp-poses": {
-            "grasp-pose-1": {
-                "placement": {
-                    "position": {
-                        "x": 0.1,
-                        "y": 0.1,
-                        "z": 0.1
-                    },
-                    "orientation": {
-                        "x": 0.1,
-                        "y": 0.1,
-                        "z": 0.1,
-                        "w": 0.1
-                    }
-                },
-                "distance": 0.1
-            },
-            "grasp-pose-2": {
-                "placement": {
-                    "position": {
-                        "x": 0.0,
-                        "y": 0.0,
-                        "z": 0.0
-                    },
-                    "orientation": {
-                        "x": 0.0,
-                        "y": 0.0,
-                        "z": 0.0,
-                        "w": 0.0
-                    }
-                },
-                "distance": 0.1
-            }
-        }
-    }
-}

sdf_models/models/stator_3508/model.config

@@ -1,11 +0,0 @@
-<?xml version="1.0" ?>
-<model>
-  <name>stator_3507</name>
-  <version>Version</version>
-  <sdf version="1.5">model.sdf</sdf>
-  <author>
-    <name>Author</name>
-    <email>Email</email>
-  </author>
-  <description>Comment</description>
-</model>

sdf_models/models/stator_3508/model.sdf

@@ -1,39 +0,0 @@
-<?xml version="1.0" ?>
-<sdf version="1.5">
-  <model name="stator_3507">
-    <pose>0.015815 -0.019409 0.063983 0.000000 0.000000 0.000000</pose>
-    <static>false</static>
-    <self_collide>false</self_collide>
-    <link name="stator_3507">
-      <pose>0.008103 0.021435 -0.039983 0.000000 -0.000000 -0.000000</pose>
-      <inertial>
-        <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 -0.000000</pose>
-        <mass>0.005709</mass>
-        <inertia>
-          <ixx>0.000001</ixx>
-          <ixy>-0.000000</ixy>
-          <ixz>-0.000000</ixz>
-          <iyy>0.000002</iyy>
-          <iyz>-0.000000</iyz>
-          <izz>0.000001</izz>
-        </inertia>
-      </inertial>
-      <visual name="stator_3507_visual">
-        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
-        <geometry>
-          <mesh>
-            <uri>model://stator_3507\meshes\stator_3507.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-      <collision name="stator_3507_collision">
-        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
-        <geometry>
-          <mesh>
-            <uri>model://stator_3507\meshes\stator_3507.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-    </link>
-  </model>
-</sdf>