Feature/moveit2z probe attack (#483)

* renaming to moveit2z and samples

* missing files

* fix

* mising

* fixing broken build

* missing renaming

* fixing conflicts

* fixing format

* testing moveit2z with panda

* fixing precommit

* minor fix

* demo launch

* last refinements

* docker fix

smacc2_dev_tools/docker/Dockerfile

@@ -30,7 +30,8 @@ WORKDIR "/home/ros2_ws"
 RUN vcs import  src --skip-existing --input src/SMACC2/.github/SMACC2-not-released.$ROS_DISTRO.repos
 RUN ls src
 
+RUN apt update
 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 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,11 +47,19 @@ 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);
   };