testing moveit2z with panda

smacc2_dev_tools/docker/Dockerfile

@@ -23,14 +23,24 @@ RUN apt-get update && apt-get install -q -y --no-install-recommends \
 WORKDIR "/home/ros2_ws/src"
 
 RUN echo "copying current code version to docker image:"
-ADD . /home/ros2_ws/src/SMACC2
+
+# # -------- moveit ----------
+# RUN git clone https://github.com/ros-planning/moveit2_tutorials.git -b humble src/moveit2_tutorials
+# RUN vcs import < src/moveit2_tutorials/moveit2_tutorials.repos
+
+# RUN apt-get update
+# RUN rosdep install --from-paths src --ignore-src -r -y
+RUN apt-get update && apt-get install -q -y --no-install-recommends xterm
+
 WORKDIR "/home/ros2_ws"
+#ADD . /home/ros2_ws/src/SMACC2
+RUN git clone https://github.com/robosoft-ai/SMACC2.git -b humble src/SMACC2
 
 # install dependencies and build
 RUN vcs import  src --skip-existing --input src/SMACC2/.github/SMACC2-not-released.$ROS_DISTRO.repos
 RUN ls src
-
 RUN rosdep install --from-paths src --ignore-src -r -y
-RUN apt-get update && apt-get install -q -y --no-install-recommends xterm
 
-RUN bash -c "source /opt/ros/$ROS_DISTRO/setup.sh && colcon build --merge-install --packages-skip moveit_servo"
+RUN git clone https://github.com/ros2/ros_testing.git src/ros_testing
+
+RUN bash -c "source /opt/ros/$ROS_DISTRO/setup.sh && colcon build --merge-install --parallel-workers 1"

smacc2_dev_tools/docker/run_docker_bash_humble.sh

@@ -4,4 +4,4 @@ xhost +
 DIR="$(dirname "$(realpath "$0")")"
 ROOT_DIR=`realpath $DIR/../..`
 
-sudo nvidia-docker  run -it -e DISPLAY  -e QT_X11_NO_MITSHM=1 -v $ROOT_DIR:/home/ros2_ws/src/SMACC2  -v /tmp/.X11-unix:/tmp/.X11-unix smacc2:humble /bin/bash
+sudo nvidia-docker  run -it -e DISPLAY  -e QT_X11_NO_MITSHM=1 -v $ROOT_DIR:/home/ros2_ws/src/SMACC2  -v /dev/dri:/dev/dri -v /tmp/.X11-unix:/tmp/.X11-unix smacc2:humble /bin/bash

smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/orthogonals/or_arm.hpp

@@ -32,7 +32,7 @@ class OrArm : public smacc2::Orthogonal<OrArm>
 public:
   void onInitialize() override
   {
-    auto move_group_client = this->createClient<cl_moveit2z::ClMoveit2z>("ur_manipulator"); //ur_manipulator
+    auto move_group_client = this->createClient<cl_moveit2z::ClMoveit2z>("panda_arm"); //ur_manipulator
     move_group_client->createComponent<cl_moveit2z::CpTrajectoryHistory>();
     auto graspingComponent = move_group_client->createComponent<cl_moveit2z::CpGraspingComponent>();
 

smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_end_effector.hpp

@@ -47,16 +47,16 @@ struct StMoveEndEffector : smacc2::SmaccState<StMoveEndEffector, SmPandaMoveit2z
   static void staticConfigure()
   {
     geometry_msgs::msg::PoseStamped target_pose;
-    target_pose.header.frame_id = "world";
-    target_pose.pose.position.x = 0.137;
-    target_pose.pose.position.y = -0.630;
+    target_pose.header.frame_id = "panda_link0";
+    target_pose.pose.position.x = 0.4;
+    target_pose.pose.position.y = 0;
     target_pose.pose.position.z = 0.467;
 
-    target_pose.pose.orientation.x = 0.498;
-    target_pose.pose.orientation.y = 0.470;
-    target_pose.pose.orientation.z = 0.715;
+    target_pose.pose.orientation.x = 0;
+    target_pose.pose.orientation.y = 0;
+    target_pose.pose.orientation.z = 0;
 
-    target_pose.pose.orientation.w = 0.141;
+    target_pose.pose.orientation.w = 1;
 
     //target_pose.pose.position.z = 0.579;
 
@@ -71,7 +71,7 @@ struct StMoveEndEffector : smacc2::SmaccState<StMoveEndEffector, SmPandaMoveit2z
     // target_pose.pose.orientation.z = 0;
     // target_pose.pose.orientation.w = -0.4480736 ;
 
-    configure_orthogonal<OrArm, CbMoveEndEffector>(target_pose, "tool0");
+    configure_orthogonal<OrArm, CbMoveEndEffector>(target_pose, "panda_link8");
   }
 
   void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StMoveEndEffector"); }

smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints.hpp

@@ -38,6 +38,7 @@ struct StMoveJoints : smacc2::SmaccState<StMoveJoints, SmPandaMoveit2zCbInventor
   // TRANSITION TABLE
   typedef boost::mpl::list<
     Transition<EvCbSuccess<CbMoveJoints, OrArm>, StMoveEndEffector, SUCCESS>
+    ,Transition<EvCbFailure<CbMoveJoints, OrArm>, StMoveJoints, ABORT>
 
     >
     reactions;
@@ -46,12 +47,20 @@ struct StMoveJoints : smacc2::SmaccState<StMoveJoints, SmPandaMoveit2zCbInventor
   static void staticConfigure()
   {
     std::map<std::string, double> jointValues{
-      {"shoulder_lift_joint", 0.0},
-      {"shoulder_pan_joint", 0.0},
-      {"wrist_1_joint", M_PI / 4},
-      {"wrist_2_joint", 0.0},
-      {"wrist_3_joint", 0.0}};
+      {"panda_joint1", 0.0},
+      {"panda_joint2", 0.0},
+      {"panda_joint3", 0.0},
+      {"panda_joint4", -M_PI/2},
+      {"panda_joint5", 0.0},
+      {"panda_joint6", M_PI/2},
+      {"panda_joint7", 0.0}
+      };
 
+    // panda_joint6:    
+    // panda_joint7:
+    // panda_finger_joint1:
+    // panda_finger_joint2:
+
     configure_orthogonal<OrArm, CbMoveJoints>(jointValues);
   };
 

smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/launch/demo.launch.py

@@ -0,0 +1,145 @@
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.actions import ExecuteProcess
+from ament_index_python.packages import get_package_share_directory
+from moveit_configs_utils import MoveItConfigsBuilder
+
+
+def generate_launch_description():
+
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "rviz_config",
+            default_value="panda_moveit_config_demo.rviz",
+            description="RViz configuration file",
+        )
+    )
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )
+
+
+def launch_setup(context, *args, **kwargs):
+
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(file_path="config/panda.urdf.xacro")
+        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
+        .planning_scene_monitor(
+            publish_robot_description=True, publish_robot_description_semantic=True
+        )
+        .planning_pipelines(
+            pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"]
+        )
+        .to_moveit_configs()
+    )
+
+    # Start the actual move_group node/action server
+    run_move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[moveit_config.to_dict()],
+        prefix="xterm -hold -e",
+    )
+
+    rviz_base = LaunchConfiguration("rviz_config")
+    rviz_config = PathJoinSubstitution(
+        [FindPackageShare("sm_panda_moveit2z_cb_inventory"), "launch", rviz_base]
+    )
+
+    # RViz
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config],
+        parameters=[
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+            moveit_config.planning_pipelines,
+            moveit_config.joint_limits,
+        ],
+    )
+
+    # Static TF
+    static_tf = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        output="log",
+        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
+    )
+
+    # Publish TF
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="both",
+        parameters=[moveit_config.robot_description],
+    )
+
+    # ros2_control using FakeSystem as hardware
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("moveit_resources_panda_moveit_config"),
+        "config",
+        "ros2_controllers.yaml",
+    )
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[moveit_config.robot_description, ros2_controllers_path],
+        output="both",
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "joint_state_broadcaster",
+            "--controller-manager-timeout",
+            "300",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
+    arm_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["panda_arm_controller", "-c", "/controller_manager"],
+    )
+
+    smacc_state_machine_spawner = Node(package= "sm_panda_moveit2z_cb_inventory",
+                                       executable="sm_panda_moveit2z_cb_inventory_node",
+                                       prefix="xterm -hold -e",
+                                        output="screen")
+
+    hand_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["panda_hand_controller", "-c", "/controller_manager"],
+    )
+    nodes_to_start = [
+        smacc_state_machine_spawner,
+        rviz_node,
+        static_tf,
+        robot_state_publisher,
+        run_move_group_node,
+        ros2_control_node,
+        joint_state_broadcaster_spawner,
+        arm_controller_spawner,
+        hand_controller_spawner,
+    ]
+
+    return nodes_to_start