From 4d8eadd9abf144363cf7f5a9d9c547682123cfb7 Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Mon, 26 Jun 2023 13:33:43 +0800
Subject: [PATCH 001/147] Add rectifyForLink7().

---
 engineer_middleware/include/engineer_middleware/motion.h | 5 ++++-
 engineer_middleware/include/engineer_middleware/points.h | 8 ++++++++
 2 files changed, 12 insertions(+), 1 deletion(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 76b279f..ffe4add 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -238,6 +238,7 @@ class SpaceEeMotion : public EndEffectorMotion
     : EndEffectorMotion(motion, interface, tf), tf_(tf)
   {
     radius_ = xmlRpcGetDouble(motion, "radius", 0.1);
+    link7_length_ = xmlRpcGetDouble(motion, "link7_length", 0.);
     if (motion.hasMember("basics_length"))
     {
       ROS_ASSERT(motion["basics_length"].getType() == XmlRpc::XmlRpcValue::TypeArray);
@@ -290,6 +291,8 @@ class SpaceEeMotion : public EndEffectorMotion
           double roll, pitch, yaw;
           quatToRPY(exchange2base_.transform.rotation, roll, pitch, yaw);
           points_.rectifyForRPY(pitch, yaw, k_x_, k_theta_, k_beta_);
+          if (link7_length_)
+            points_.rectifyForLink7(pitch_temp, link7_length_);
         }
         catch (tf2::TransformException& ex)
         {
@@ -327,7 +330,7 @@ class SpaceEeMotion : public EndEffectorMotion
   geometry_msgs::TransformStamped exchange2base_;
   int max_planning_times_{};
   double radius_, point_resolution_, x_length_, y_length_, z_length_, k_theta_, k_beta_, k_x_, tolerance_position_,
-      tolerance_orientation_;
+      tolerance_orientation_, link7_length_;
 };
 
 class JointMotion : public MoveitMotionBase
diff --git a/engineer_middleware/include/engineer_middleware/points.h b/engineer_middleware/include/engineer_middleware/points.h
index c0375a7..71a0e0d 100644
--- a/engineer_middleware/include/engineer_middleware/points.h
+++ b/engineer_middleware/include/engineer_middleware/points.h
@@ -92,6 +92,14 @@ class Points
       points_final_[i].z -= rectify.z;
     }
   }
+  void rectifyForLink7(double theta, double link7_length)
+  {
+    for (int i = 0; i < (int)points_final_.size(); ++i)
+    {
+      points_final_[i].x -= link7_length * pow(sin(theta), 2) / (tan(M_PI_2 - theta / 2));
+      points_final_[i].z = link7_length * sin(theta) * cos(theta) / (tan(M_PI_2 - theta / 2));
+    }
+  }
   void generateBasicsPoints(double center_x, double center_y, double center_z, double x_length, double y_length,
                             double z_length, double point_resolution)
   {

From 0b206b07e7be8d241366441e8960719c2c7fa2d0 Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Mon, 26 Jun 2023 14:16:35 +0800
Subject: [PATCH 002/147] Add joint7 replace joint4.

---
 .../include/engineer_middleware/motion.h      | 27 ++++++++++++-------
 1 file changed, 18 insertions(+), 9 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index ffe4add..7479000 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -276,21 +276,30 @@ class SpaceEeMotion : public EndEffectorMotion
     int move_times = (int)points_.getPoints().size();
     for (int i = 0; i < move_times && i < max_planning_times_; ++i)
     {
-      geometry_msgs::PoseStamped final_target;
+      tf2::Quaternion original_quat_msg;
       target_.pose.position.x = points_.getPoints()[i].x;
       target_.pose.position.y = points_.getPoints()[i].y;
       target_.pose.position.z = points_.getPoints()[i].z;
+      target_.pose.orientation = original_quat_msg;
       if (!target_.header.frame_id.empty())
       {
         try
         {
-          tf2::doTransform(target_.pose, final_target.pose,
-                           tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0)));
-          final_target.header.frame_id = interface_.getPlanningFrame();
+          double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
           exchange2base_ = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
-          double roll, pitch, yaw;
-          quatToRPY(exchange2base_.transform.rotation, roll, pitch, yaw);
-          points_.rectifyForRPY(pitch, yaw, k_x_, k_theta_, k_beta_);
+          quatToRPY(exchange2base_.transform.rotation, roll_temp, pitch_temp, yaw_temp);
+          quatToRPY(target_.pose.orientation, roll, pitch, yaw);
+          pitch -= pitch_temp;
+          tf2::Quaternion tmp_tf_quaternion;
+          tmp_tf_quaternion.setRPY(roll, pitch, yaw);
+          geometry_msgs::Quaternion quat_tf = tf2::toMsg(tmp_tf_quaternion);
+          target_.pose.orientation = quat_tf;
+          quatToRPY(target_.pose.orientation, roll, pitch, yaw);
+
+          tf2::doTransform(target_.pose, final_target_.pose,
+                           tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0)));
+          final_target_.header.frame_id = interface_.getPlanningFrame();
+          points_.rectifyForRPY(pitch_temp, yaw_temp, k_x_, k_theta_, k_beta_);
           if (link7_length_)
             points_.rectifyForLink7(pitch_temp, link7_length_);
         }
@@ -300,7 +309,7 @@ class SpaceEeMotion : public EndEffectorMotion
           return false;
         }
       }
-      interface_.setPoseTarget(final_target);
+      interface_.setPoseTarget(final_target_);
       moveit::planning_interface::MoveGroupInterface::Plan plan;
       msg_.data = interface_.plan(plan).val;
       if (msg_.data == 1)
@@ -326,7 +335,7 @@ class SpaceEeMotion : public EndEffectorMotion
                 tolerance_orientation_);
   }
   tf2_ros::Buffer& tf_;
-  geometry_msgs::PoseStamped target_;
+  geometry_msgs::PoseStamped target_, final_target_;
   geometry_msgs::TransformStamped exchange2base_;
   int max_planning_times_{};
   double radius_, point_resolution_, x_length_, y_length_, z_length_, k_theta_, k_beta_, k_x_, tolerance_position_,

From a4f4ee22da459dc6490b0d4b7ece56d7d45752f0 Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Mon, 26 Jun 2023 14:19:19 +0800
Subject: [PATCH 003/147] Fix target_.orientation.

---
 engineer_middleware/include/engineer_middleware/motion.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 7479000..a961e1f 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -280,7 +280,6 @@ class SpaceEeMotion : public EndEffectorMotion
       target_.pose.position.x = points_.getPoints()[i].x;
       target_.pose.position.y = points_.getPoints()[i].y;
       target_.pose.position.z = points_.getPoints()[i].z;
-      target_.pose.orientation = original_quat_msg;
       if (!target_.header.frame_id.empty())
       {
         try

From eedd08814c67af4d72f188eb06e32d438aa368df Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Mon, 26 Jun 2023 14:20:18 +0800
Subject: [PATCH 004/147] Delete useless.

---
 engineer_middleware/include/engineer_middleware/motion.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index a961e1f..21d45c6 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -276,7 +276,6 @@ class SpaceEeMotion : public EndEffectorMotion
     int move_times = (int)points_.getPoints().size();
     for (int i = 0; i < move_times && i < max_planning_times_; ++i)
     {
-      tf2::Quaternion original_quat_msg;
       target_.pose.position.x = points_.getPoints()[i].x;
       target_.pose.position.y = points_.getPoints()[i].y;
       target_.pose.position.z = points_.getPoints()[i].z;

From ac85f2702b23b4bc0d74ff58a7dbc65de3ada26c Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Mon, 26 Jun 2023 14:30:39 +0800
Subject: [PATCH 005/147] Fix error.

---
 .../include/engineer_middleware/motion.h            | 13 ++++++-------
 .../include/engineer_middleware/points.h            |  1 -
 2 files changed, 6 insertions(+), 8 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 21d45c6..540e77b 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -276,9 +276,6 @@ class SpaceEeMotion : public EndEffectorMotion
     int move_times = (int)points_.getPoints().size();
     for (int i = 0; i < move_times && i < max_planning_times_; ++i)
     {
-      target_.pose.position.x = points_.getPoints()[i].x;
-      target_.pose.position.y = points_.getPoints()[i].y;
-      target_.pose.position.z = points_.getPoints()[i].z;
       if (!target_.header.frame_id.empty())
       {
         try
@@ -293,13 +290,15 @@ class SpaceEeMotion : public EndEffectorMotion
           geometry_msgs::Quaternion quat_tf = tf2::toMsg(tmp_tf_quaternion);
           target_.pose.orientation = quat_tf;
           quatToRPY(target_.pose.orientation, roll, pitch, yaw);
-
-          tf2::doTransform(target_.pose, final_target_.pose,
-                           tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0)));
-          final_target_.header.frame_id = interface_.getPlanningFrame();
           points_.rectifyForRPY(pitch_temp, yaw_temp, k_x_, k_theta_, k_beta_);
           if (link7_length_)
             points_.rectifyForLink7(pitch_temp, link7_length_);
+          target_.pose.position.x = points_.getPoints()[i].x;
+          target_.pose.position.y = points_.getPoints()[i].y;
+          target_.pose.position.z = points_.getPoints()[i].z;
+          tf2::doTransform(target_.pose, final_target_.pose,
+                           tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0)));
+          final_target_.header.frame_id = interface_.getPlanningFrame();
         }
         catch (tf2::TransformException& ex)
         {
diff --git a/engineer_middleware/include/engineer_middleware/points.h b/engineer_middleware/include/engineer_middleware/points.h
index 71a0e0d..c0b69eb 100644
--- a/engineer_middleware/include/engineer_middleware/points.h
+++ b/engineer_middleware/include/engineer_middleware/points.h
@@ -84,7 +84,6 @@ class Points
     rectify.x = abs(points_final_[0].x) * sin(theta) * k_x;
     rectify.y = abs(points_final_[0].x) * sin(beta) * k_beta;
     rectify.z = abs(points_final_[0].x) * sin(theta) * k_theta;
-    ROS_INFO_STREAM(rectify);
     for (int i = 0; i < (int)points_final_.size(); ++i)
     {
       points_final_[i].x += rectify.x;

From 74a402928ad1dded9b032fa9c7c2f506c18ce9bd Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Mon, 26 Jun 2023 14:48:35 +0800
Subject: [PATCH 006/147] Add joint7 tf.

---
 .../include/engineer_middleware/motion.h      | 24 +++++++++++++++----
 .../include/engineer_middleware/step.h        |  2 +-
 2 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 540e77b..5d62e73 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -485,18 +485,30 @@ class StoneNumMotion : public PublishMotion<std_msgs::String>
 class JointPositionMotion : public PublishMotion<std_msgs::Float64>
 {
 public:
-  JointPositionMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface)
-    : PublishMotion<std_msgs::Float64>(motion, interface)
+  JointPositionMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface, tf2_ros::Buffer& tf)
+    : PublishMotion<std_msgs::Float64>(motion, interface), tf_(tf)
   {
-    ROS_ASSERT(motion.hasMember("target"));
-    ROS_ASSERT(motion.hasMember("delay"));
+    original_tf_ = std::string(motion["original_tf"]);
+    reference_tf_ = std::string(motion["reference_tf"]);
+    direction_ = std::string(motion["direction"]);
     target_ = xmlRpcGetDouble(motion, "target", 0.0);
     delay_ = xmlRpcGetDouble(motion, "delay", 0.0);
   }
   bool move() override
   {
+    double roll, pitch, yaw;
+    geometry_msgs::TransformStamped tf;
+    tf = tf_.lookupTransform(reference_tf_, original_tf_, ros::Time(0));
+    quatToRPY(tf.transform.rotation, roll, pitch, yaw);
     start_time_ = ros::Time::now();
-    msg_.data = target_;
+    if (direction_ == "roll")
+      msg_.data = roll;
+    else if (direction_ == "pitch")
+      msg_.data = pitch;
+    else if (direction_ == "yaw")
+      msg_.data = yaw;
+    else
+      msg_.data = target_;
     return PublishMotion::move();
   }
   bool isFinish() override
@@ -507,6 +519,8 @@ class JointPositionMotion : public PublishMotion<std_msgs::Float64>
 private:
   double target_, delay_;
   ros::Time start_time_;
+  tf2_ros::Buffer& tf_;
+  std::string original_tf_, reference_tf_, direction_;
 };
 
 class GimbalMotion : public PublishMotion<rm_msgs::GimbalCmd>
diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h
index 9c9ddbd..b8d7ef3 100644
--- a/engineer_middleware/include/engineer_middleware/step.h
+++ b/engineer_middleware/include/engineer_middleware/step.h
@@ -69,7 +69,7 @@ class Step
     if (step.hasMember("hand"))
       hand_motion_ = new HandMotion(step["hand"], hand_pub);
     if (step.hasMember("end_effector"))
-      end_effector_motion_ = new JointPositionMotion(step["end_effector"], end_effector_pub);
+      end_effector_motion_ = new JointPositionMotion(step["end_effector"], end_effector_pub, tf);
     if (step.hasMember("stone_num"))
       stone_num_motion_ = new StoneNumMotion(step["stone_num"], stone_num_pub);
     if (step.hasMember("gimbal"))

From 463ac05d3414b9e58ff1fe6a7cb26bc36b220f63 Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Tue, 27 Jun 2023 00:26:47 +0800
Subject: [PATCH 007/147] Fix error in pr.

---
 .../include/engineer_middleware/motion.h             | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 5d62e73..677237f 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -280,19 +280,19 @@ class SpaceEeMotion : public EndEffectorMotion
       {
         try
         {
-          double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
+          double roll, pitch, yaw, roll_exchange2base, pitch_exchange2base, yaw__exchange2base;
           exchange2base_ = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
-          quatToRPY(exchange2base_.transform.rotation, roll_temp, pitch_temp, yaw_temp);
+          quatToRPY(exchange2base_.transform.rotation, roll_exchange2base, pitch_exchange2base, yaw__exchange2base);
           quatToRPY(target_.pose.orientation, roll, pitch, yaw);
-          pitch -= pitch_temp;
+          pitch -= pitch_exchange2base;
           tf2::Quaternion tmp_tf_quaternion;
           tmp_tf_quaternion.setRPY(roll, pitch, yaw);
           geometry_msgs::Quaternion quat_tf = tf2::toMsg(tmp_tf_quaternion);
           target_.pose.orientation = quat_tf;
           quatToRPY(target_.pose.orientation, roll, pitch, yaw);
-          points_.rectifyForRPY(pitch_temp, yaw_temp, k_x_, k_theta_, k_beta_);
-          if (link7_length_)
-            points_.rectifyForLink7(pitch_temp, link7_length_);
+          points_.rectifyForRPY(pitch_exchange2base, yaw__exchange2base, k_x_, k_theta_, k_beta_);
+          if (link7_length_ > 0)
+            points_.rectifyForLink7(pitch_exchange2base, link7_length_);
           target_.pose.position.x = points_.getPoints()[i].x;
           target_.pose.position.y = points_.getPoints()[i].y;
           target_.pose.position.z = points_.getPoints()[i].z;

From 03bb92a0c71b4a57f326d3cdbd077517b0be7112 Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Sun, 9 Jul 2023 11:30:11 +0800
Subject: [PATCH 008/147] 7-9 11:30.

---
 engineer.urdf | 969 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 969 insertions(+)
 create mode 100644 engineer.urdf

diff --git a/engineer.urdf b/engineer.urdf
new file mode 100644
index 0000000..909a7d5
--- /dev/null
+++ b/engineer.urdf
@@ -0,0 +1,969 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from ../rm_description/urdf/engineer/engineer.urdf.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="engineer">
+  <link name="base_link">
+    <inertial>
+      <mass value="10.34269"/>
+      <origin xyz="-0.003691 -0.001885 0.024404"/>
+      <inertia ixx="1.952E-01" ixy="-1.504E-03" ixz="5.871E-03" iyy="3.011E-01" iyz="-1.077E-03" izz="4.074E-01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/base_link.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0 0 0.0525"/>
+      <geometry>
+        <box size="0.59 0.59 0.105"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0 0 0.165"/>
+      <geometry>
+        <box size="0.59 0.171 0.12"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0 0.15 0.3675"/>
+      <geometry>
+        <box size="0.24 0.034 0.445"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0 -0.15 0.3675"/>
+      <geometry>
+        <box size="0.24 0.034 0.445"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0 0.187 0.46625"/>
+      <geometry>
+        <box size="0.39 0.034 0.6425"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0 -0.187 0.46625"/>
+      <geometry>
+        <box size="0.39 0.034 0.6425"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="drag_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/drag.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.124977"/>
+      <origin xyz="0. 1.32489 -0.00403"/>
+      <inertia ixx="3.004e-3" ixy="-0.045e-9" ixz="-2.0566e-8" iyy="9.642549e-6" iyz="1.05812e-7" izz="3.003e-3"/>
+    </inertial>
+  </link>
+  <joint name="drag_joint" type="revolute">
+    <origin rpy="0 0 0" xyz="0. 0. 0."/>
+    <limit effort="8.1" lower="-1e10" upper="1e10" velocity="9.96"/>
+    <parent link="base_link"/>
+    <child link="drag_link"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+  <transmission name="trans_drag_joint">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="drag_joint_motor">
+      <mechanicalReduction>51.0</mechanicalReduction>
+    </actuator>
+    <joint name="drag_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <offset>0.2</offset>
+    </joint>
+  </transmission>
+  <link name="left_front_pivot">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/common/swerve_pivot.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.114131"/>
+      <origin rpy="0 0 0" xyz="-0.00005 0.017079 -0.041536"/>
+      <inertia ixx="1.335e-4" ixy="26.001e-9" ixz="65.031e-9" iyy="1.372e-4" iyz="20051.742e-9" izz="66866.081e-9"/>
+    </inertial>
+  </link>
+  <joint name="left_front_pivot_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.219 0.2 0.06795"/>
+    <parent link="base_link"/>
+    <child link="left_front_pivot"/>
+    <dynamics damping="0.0" friction="0.1"/>
+    <limit effort="1.2" lower="-1e10" upper="1e10" velocity="50"/>
+  </joint>
+  <link name="left_front_wheel">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/common/swerve_wheel.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.267401"/>
+      <origin rpy="0 0 0" xyz="-0.000004 0.000125 0.000001"/>
+      <inertia ixx="3.585e-4" ixy="0" ixz="13.459e-9" iyy="6.846e-4" iyz="0" izz="3.584e-4"/>
+    </inertial>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.055"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_front_wheel_joint" type="revolute">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="0 0 -0.06635"/>
+    <parent link="left_front_pivot"/>
+    <child link="left_front_wheel"/>
+    <limit effort="3" lower="-1e10" upper="1e10" velocity="76"/>
+    <dynamics damping="0.0" friction="0.05"/>
+  </joint>
+  <gazebo reference="left_front_wheel">
+    <mu1 value="1.0"/>
+    <mu2 value="1.0"/>
+    <kp value="1000000.0"/>
+    <kd value="100.0"/>
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <link name="right_front_pivot">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/common/swerve_pivot.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.114131"/>
+      <origin rpy="0 0 0" xyz="-0.00005 0.017079 -0.041536"/>
+      <inertia ixx="1.335e-4" ixy="26.001e-9" ixz="65.031e-9" iyy="1.372e-4" iyz="20051.742e-9" izz="66866.081e-9"/>
+    </inertial>
+  </link>
+  <joint name="right_front_pivot_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.219 -0.2 0.06795"/>
+    <parent link="base_link"/>
+    <child link="right_front_pivot"/>
+    <dynamics damping="0.0" friction="0.1"/>
+    <limit effort="1.2" lower="-1e10" upper="1e10" velocity="50"/>
+  </joint>
+  <link name="right_front_wheel">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/common/swerve_wheel.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.267401"/>
+      <origin rpy="0 0 0" xyz="-0.000004 0.000125 0.000001"/>
+      <inertia ixx="3.585e-4" ixy="0" ixz="13.459e-9" iyy="6.846e-4" iyz="0" izz="3.584e-4"/>
+    </inertial>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.055"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_front_wheel_joint" type="revolute">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="0 0 -0.06635"/>
+    <parent link="right_front_pivot"/>
+    <child link="right_front_wheel"/>
+    <limit effort="3" lower="-1e10" upper="1e10" velocity="76"/>
+    <dynamics damping="0.0" friction="0.05"/>
+  </joint>
+  <gazebo reference="right_front_wheel">
+    <mu1 value="1.0"/>
+    <mu2 value="1.0"/>
+    <kp value="1000000.0"/>
+    <kd value="100.0"/>
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <link name="left_back_pivot">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/common/swerve_pivot.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.114131"/>
+      <origin rpy="0 0 0" xyz="-0.00005 0.017079 -0.041536"/>
+      <inertia ixx="1.335e-4" ixy="26.001e-9" ixz="65.031e-9" iyy="1.372e-4" iyz="20051.742e-9" izz="66866.081e-9"/>
+    </inertial>
+  </link>
+  <joint name="left_back_pivot_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="-0.219 0.2 0.06795"/>
+    <parent link="base_link"/>
+    <child link="left_back_pivot"/>
+    <dynamics damping="0.0" friction="0.1"/>
+    <limit effort="1.2" lower="-1e10" upper="1e10" velocity="50"/>
+  </joint>
+  <link name="left_back_wheel">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/common/swerve_wheel.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.267401"/>
+      <origin rpy="0 0 0" xyz="-0.000004 0.000125 0.000001"/>
+      <inertia ixx="3.585e-4" ixy="0" ixz="13.459e-9" iyy="6.846e-4" iyz="0" izz="3.584e-4"/>
+    </inertial>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.055"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_back_wheel_joint" type="revolute">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="0 0 -0.06635"/>
+    <parent link="left_back_pivot"/>
+    <child link="left_back_wheel"/>
+    <limit effort="3" lower="-1e10" upper="1e10" velocity="76"/>
+    <dynamics damping="0.0" friction="0.05"/>
+  </joint>
+  <gazebo reference="left_back_wheel">
+    <mu1 value="1.0"/>
+    <mu2 value="1.0"/>
+    <kp value="1000000.0"/>
+    <kd value="100.0"/>
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <link name="right_back_pivot">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/common/swerve_pivot.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.114131"/>
+      <origin rpy="0 0 0" xyz="-0.00005 0.017079 -0.041536"/>
+      <inertia ixx="1.335e-4" ixy="26.001e-9" ixz="65.031e-9" iyy="1.372e-4" iyz="20051.742e-9" izz="66866.081e-9"/>
+    </inertial>
+  </link>
+  <joint name="right_back_pivot_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="-0.219 -0.2 0.06795"/>
+    <parent link="base_link"/>
+    <child link="right_back_pivot"/>
+    <dynamics damping="0.0" friction="0.1"/>
+    <limit effort="1.2" lower="-1e10" upper="1e10" velocity="50"/>
+  </joint>
+  <link name="right_back_wheel">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/common/swerve_wheel.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.267401"/>
+      <origin rpy="0 0 0" xyz="-0.000004 0.000125 0.000001"/>
+      <inertia ixx="3.585e-4" ixy="0" ixz="13.459e-9" iyy="6.846e-4" iyz="0" izz="3.584e-4"/>
+    </inertial>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.055"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_back_wheel_joint" type="revolute">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="0 0 -0.06635"/>
+    <parent link="right_back_pivot"/>
+    <child link="right_back_wheel"/>
+    <limit effort="3" lower="-1e10" upper="1e10" velocity="76"/>
+    <dynamics damping="0.0" friction="0.05"/>
+  </joint>
+  <gazebo reference="right_back_wheel">
+    <mu1 value="1.0"/>
+    <mu2 value="1.0"/>
+    <kp value="1000000.0"/>
+    <kd value="100.0"/>
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <transmission name="left_front_pivot_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="left_front_pivot_motor">
+      <mechanicalReduction>-1</mechanicalReduction>
+    </actuator>
+    <joint name="left_front_pivot_joint">
+      <offset>2.092605</offset>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="left_front_wheel_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="left_front_wheel_motor">
+      <mechanicalReduction>19</mechanicalReduction>
+    </actuator>
+    <joint name="left_front_wheel_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="right_front_pivot_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="right_front_pivot_motor">
+      <mechanicalReduction>-1</mechanicalReduction>
+    </actuator>
+    <joint name="right_front_pivot_joint">
+      <offset>4.973006</offset>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="right_front_wheel_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="right_front_wheel_motor">
+      <mechanicalReduction>19</mechanicalReduction>
+    </actuator>
+    <joint name="right_front_wheel_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="left_back_pivot_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="left_back_pivot_motor">
+      <mechanicalReduction>-1</mechanicalReduction>
+    </actuator>
+    <joint name="left_back_pivot_joint">
+      <offset>5.26066</offset>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="left_back_wheel_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="left_back_wheel_motor">
+      <mechanicalReduction>19</mechanicalReduction>
+    </actuator>
+    <joint name="left_back_wheel_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="right_back_pivot_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="right_back_pivot_motor">
+      <mechanicalReduction>-1</mechanicalReduction>
+    </actuator>
+    <joint name="right_back_pivot_joint">
+      <offset>-2.074195</offset>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="right_back_wheel_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="right_back_wheel_motor">
+      <mechanicalReduction>19</mechanicalReduction>
+    </actuator>
+    <joint name="right_back_wheel_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <!--  link  -->
+  <link name="link1">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/link1.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="1.191644"/>
+      <origin xyz="0. 0. 0."/>
+      <inertia ixx="5.559e-1" ixy="-7.326e-3" ixz="2.084e-2" iyy="3.889e-1" iyz="4.594e-3" izz="5.871e-1"/>
+    </inertial>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0 0.25 0.028"/>
+      <geometry>
+        <box size="0.334 0.15 0.056"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0 -0.25 0.028"/>
+      <geometry>
+        <box size="0.334 0.15 0.056"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0.217 0.182 0.04"/>
+      <geometry>
+        <box size="0.07 0.097 0.1"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0.217 -0.182 0.04"/>
+      <geometry>
+        <box size="0.07 0.097 0.1"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link2">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/link2.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="2.997641"/>
+      <origin xyz="0. 0. 0."/>
+      <inertia ixx="1.438e-1" ixy="-7.086e-3" ixz="-5.178e-3" iyy="9.638e-2" iyz="-2.120e-3" izz="2.365e-1"/>
+    </inertial>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0.0075 -0.215 0.0125"/>
+      <geometry>
+        <box size="0.565 0.07 0.025"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0.0075 0.215 0.0125"/>
+      <geometry>
+        <box size="0.565 0.07 0.025"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0.249 0. 0.01"/>
+      <geometry>
+        <box size="0.046 0.43 0.026"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link3">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/link3.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="1.144584"/>
+      <origin xyz="0. -0. 0."/>
+      <inertia ixx="3.307e-2" ixy="-1.813e-3" ixz="2.5936e-5" iyy="8.059e-4" iyz="1.760e-4" izz="3.340e-2"/>
+    </inertial>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0. 0. 0."/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/link3.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link4">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0."/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/link4.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.07901"/>
+      <origin xyz="0. 0. 0. "/>
+      <inertia ixx="1.2e-4" ixy="1.90146e-7" ixz="-1.654e-6" iyy="3.797e-5" iyz="-8.45708e-7" izz="1.13e-4"/>
+    </inertial>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0. 0.0045 -0.0165"/>
+      <geometry>
+        <box size="0.04 0.257 0.07"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link5">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/link5.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.178017"/>
+      <origin xyz="0. 0. 0."/>
+      <inertia ixx="2.936e-3" ixy="2.366e-4" ixz="-5.559e-4" iyy="2.88e-3" iyz="6.198e-4" izz="6.447e-4"/>
+    </inertial>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="-0.025 0.0 -0.1275"/>
+      <geometry>
+        <box size="0.032 0.032 0.18"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link6">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0."/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/link6.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.242891"/>
+      <origin xyz="0. 0. 0."/>
+      <inertia ixx="1.225e-2" ixy="3.486e-4" ixz="-1.804e-3" iyy="1.224e-2" iyz="2.204e-3" izz="1.033e-3"/>
+    </inertial>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0. 0. 0."/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/link6.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0.07 0. -0.01"/>
+      <geometry>
+        <box size="0.03 0.03 0.015"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link7">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/link7.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.124977"/>
+      <origin xyz="0. 1.32489 -0.00403"/>
+      <inertia ixx="3.004e-3" ixy="-0.045e-9" ixz="-2.0566e-8" iyy="9.642549e-6" iyz="1.05812e-7" izz="3.003e-3"/>
+    </inertial>
+    <collision>
+      <origin rpy="0. 0. 0." xyz="0. 0. 0."/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/link7.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="tools_link"/>
+  <link name="fake_link5"/>
+  <joint name="joint1" type="prismatic">
+    <parent link="base_link"/>
+    <child link="link1"/>
+    <origin rpy="0 0 0" xyz="0 0 0.21"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="136" lower="-0.1" upper="0.48" velocity="2.84"/>
+    <dynamics damping="0.0" friction="0.1"/>
+  </joint>
+  <joint name="joint2" type="prismatic">
+    <parent link="link1"/>
+    <child link="link2"/>
+    <origin rpy="0 0 0" xyz="0 0. 0.056"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="90.25" lower="-0.08" upper="0.268" velocity="2.77"/>
+    <dynamics damping="0.0" friction="0.1"/>
+  </joint>
+  <joint name="joint3" type="prismatic">
+    <parent link="link2"/>
+    <child link="link3"/>
+    <origin rpy="0 0 0" xyz="0.2595 0. 0.046"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="135.8" lower="-0.3" upper="0.3" velocity="1.84"/>
+    <dynamics damping="0.0" friction="0.1"/>
+  </joint>
+  <joint name="joint4" type="revolute">
+    <parent link="link3"/>
+    <child link="link4"/>
+    <origin rpy="0 0 0" xyz="0. 0. 0.073"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="11.72" lower="-1.578" upper="0.1" velocity="3.09"/>
+    <dynamics damping="0.0" friction="0.1"/>
+  </joint>
+  <joint name="joint5" type="revolute">
+    <parent link="link4"/>
+    <child link="link5"/>
+    <origin rpy="0 0 0" xyz="0.07 0. 0."/>
+    <axis xyz="1 0 0"/>
+    <limit effort="5" lower="-1.6" upper="1.61" velocity="50"/>
+    <safety_controller k_position="80." k_velocity="1.2" soft_lower_limit="-1.5990000000000002" soft_upper_limit="1.6090000000000002"/>
+    <dynamics damping="0.0" friction="0.1"/>
+  </joint>
+  <joint name="fake_joint5" type="fixed">
+    <parent link="link5"/>
+    <child link="fake_link5"/>
+    <origin rpy="0 1.57 0" xyz="0. 0. 0."/>
+  </joint>
+  <joint name="joint6" type="revolute">
+    <parent link="link5"/>
+    <child link="link6"/>
+    <origin rpy="0 0 0" xyz="-0.07 0 -0.2165"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="-1.66" upper="3.8" velocity="50"/>
+    <safety_controller k_position="28." k_velocity="0.8" soft_lower_limit="-1.659" soft_upper_limit="3.5999999999999996"/>
+    <dynamics damping="0.0" friction="0.1"/>
+  </joint>
+  <joint name="joint7" type="revolute">
+    <origin rpy="0 0 0" xyz="0.037 -0.0425 -0.028"/>
+    <limit effort="1.56" lower="0.0" upper="1.62" velocity="19"/>
+    <parent link="link6"/>
+    <child link="link7"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+  <joint name="tools_joint" type="fixed">
+    <parent link="link7"/>
+    <child link="tools_link"/>
+    <origin rpy="0 -1.57 0" xyz="0.165 0.04 0.1"/>
+  </joint>
+  <!-- transmission -->
+  <transmission name="trans_joint1">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="joint1_motor">
+      <mechanicalReduction>-519.8</mechanicalReduction>
+    </actuator>
+    <joint name="joint1">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <offset>0.45604</offset>
+    </joint>
+  </transmission>
+  <transmission name="trans_joint2">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="joint2_motor">
+      <mechanicalReduction>-346.54</mechanicalReduction>
+    </actuator>
+    <joint name="joint2">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <offset>0.264</offset>
+    </joint>
+  </transmission>
+  <transmission name="trans_joint3">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="joint3_motor">
+      <mechanicalReduction>-519.8</mechanicalReduction>
+    </actuator>
+    <joint name="joint3">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <offset>-0.297076</offset>
+    </joint>
+  </transmission>
+  <transmission name="trans_joint4">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="joint4_motor">
+      <mechanicalReduction>50</mechanicalReduction>
+    </actuator>
+    <joint name="joint4">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <offset>-1.622</offset>
+    </joint>
+  </transmission>
+  <transmission name="trans_joint5">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="joint5_motor">
+      <mechanicalReduction>19.2032</mechanicalReduction>
+    </actuator>
+    <joint name="joint5">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <offset>1.608</offset>
+    </joint>
+  </transmission>
+  <transmission name="trans_joint6">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="joint6_motor">
+      <mechanicalReduction>-19.2032</mechanicalReduction>
+    </actuator>
+    <joint name="joint6">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <offset>-1.6642</offset>
+    </joint>
+  </transmission>
+  <transmission name="trans_joint7">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="joint7_motor">
+      <mechanicalReduction>1.56</mechanicalReduction>
+    </actuator>
+    <joint name="joint7">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <offset>0.</offset>
+    </joint>
+  </transmission>
+  <link name="pitch_behind_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/pitch_behind.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.124977"/>
+      <origin xyz="0. 1.32489 -0.00403"/>
+      <inertia ixx="3.004e-3" ixy="-0.045e-9" ixz="-2.0566e-8" iyy="9.642549e-6" iyz="1.05812e-7" izz="3.003e-3"/>
+    </inertial>
+  </link>
+  <link name="pitch_front_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/pitch_front.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.124977"/>
+      <origin xyz="0. 1.32489 -0.00403"/>
+      <inertia ixx="3.004e-3" ixy="-0.045e-9" ixz="-2.0566e-8" iyy="9.642549e-6" iyz="1.05812e-7" izz="3.003e-3"/>
+    </inertial>
+  </link>
+  <link name="roll_left_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/roll_left.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.155395"/>
+      <origin xyz="-0.002548 -0.001141 -1.5623"/>
+      <inertia ixx="5.377e-3" ixy="-1.0076e-5" ixz="-1.385968e-6" iyy="5.395e-3" iyz="-6.20542e-7" izz="3.7128e-5"/>
+    </inertial>
+  </link>
+  <link name="roll_right_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/roll_right.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.155395"/>
+      <origin xyz="-0.002548 -0.001141 -1.5623"/>
+      <inertia ixx="5.377e-3" ixy="-1.0076e-5" ixz="-1.385968e-6" iyy="5.395e-3" iyz="-6.20542e-7" izz="3.7128e-5"/>
+    </inertial>
+  </link>
+  <joint name="pitch_behind_joint" type="prismatic">
+    <origin rpy="0 0 0" xyz="0. 0.143 0.26"/>
+    <limit effort="1.0" lower="-1e10" upper="1e10" velocity="30"/>
+    <parent link="base_link"/>
+    <child link="pitch_behind_link"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+  <joint name="pitch_front_joint" type="prismatic">
+    <origin rpy="0 0 0" xyz="0. 0.143 0.48"/>
+    <limit effort="1.0" lower="-1e10" upper="1e10" velocity="30"/>
+    <parent link="base_link"/>
+    <child link="pitch_front_link"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+  <joint name="roll_left_joint" type="prismatic">
+    <origin rpy="0 0 0" xyz="0.14 0.115 0.6"/>
+    <limit effort="1.0" lower="-1e10" upper="1e10" velocity="30"/>
+    <parent link="base_link"/>
+    <child link="roll_left_link"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <joint name="roll_right_joint" type="prismatic">
+    <origin rpy="0 0 0" xyz="0.14 -0.115 0.6"/>
+    <limit effort="1.0" lower="-1e10" upper="1e10" velocity="30"/>
+    <parent link="base_link"/>
+    <child link="roll_right_link"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <transmission name="trans_pitch_behind_joint">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="pitch_behind_joint_motor">
+      <mechanicalReduction>-1</mechanicalReduction>
+    </actuator>
+    <joint name="pitch_behind_joint">
+      <offset>0.</offset>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="trans_pitch_front_joint">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="pitch_front_joint_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="pitch_front_joint">
+      <offset>0.</offset>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="trans_roll_left_joint">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="roll_left_joint_motor">
+      <mechanicalReduction>-1</mechanicalReduction>
+    </actuator>
+    <joint name="roll_left_joint">
+      <offset>0.</offset>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="trans_roll_right_joint">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="roll_right_joint_motor">
+      <mechanicalReduction>-1</mechanicalReduction>
+    </actuator>
+    <joint name="roll_right_joint">
+      <offset>0.</offset>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <link name="gimbal_base">
+    <inertial>
+      <mass value="0.163485"/>
+      <origin xyz="0.000652 0.002177 0.009113"/>
+      <inertia ixx="7.4761869e-5" ixy="-1.177056e-6" ixz="-2.348598e-6" iyy="7.440176e-5" iyz="6.29861e-6" izz="6.3915e-5"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/gimbal_base.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+  </link>
+  <link name="yaw">
+    <inertial>
+      <mass value="0.074489"/>
+      <origin xyz="0.000003 -0.001925 0.013798"/>
+      <inertia ixx="1.672e-4" ixy="-0.42e-9" ixz="1.5288e-8" iyy="5.1826669e-5" iyz="-5.33194e-7" izz="1.439e-4"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/yaw.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+  </link>
+  <link name="pitch">
+    <inertial>
+      <mass value="0.28963"/>
+      <origin xyz="0.001291 0.005233 0.001395"/>
+      <inertia ixx="2.976e-4" ixy="5.294138e-6" ixz="-5.514329e-6" iyy="1.889e-4" iyz="3.085924e-6" izz="4.132e-4"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/engineer/pitch.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="gimbal_base_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.208 0.015 0.084"/>
+    <parent link="link1"/>
+    <child link="gimbal_base"/>
+  </joint>
+  <joint name="yaw_joint" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0.03"/>
+    <limit effort="0.15" lower="-10000000000.0" upper="10000000000.0" velocity="900"/>
+    <parent link="gimbal_base"/>
+    <child link="yaw"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <joint name="pitch_joint" type="revolute">
+    <origin rpy="0 0 0" xyz="0.  0. 0.091 "/>
+    <limit effort="0.15" lower="-10000000000.0" upper="10000000000.0" velocity="900"/>
+    <parent link="yaw"/>
+    <child link="pitch"/>
+    <axis xyz="0 1 0 "/>
+  </joint>
+  <transmission name="trans_yaw_joint">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="yaw_joint_motor">
+      <mechanicalReduction>-1</mechanicalReduction>
+    </actuator>
+    <joint name="yaw_joint">
+      <offset>1.66</offset>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="trans_pitch_joint">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="pitch_joint_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="pitch_joint">
+      <offset>-0.46</offset>
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <joint name="camera_link_joint" type="fixed">
+    <axis xyz="0 1 0"/>
+    <origin rpy="3.141592653589793 0 -0.02" xyz="-0.036 -0.012 0.0135"/>
+    <parent link="pitch"/>
+    <child link="camera_link"/>
+  </joint>
+  <link name="camera_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/common/camera.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <mass value="0.17671"/>
+      <origin xyz="0.022 0.0 0.0"/>
+      <inertia ixx="2.129e-5" ixy="7.329e-8" ixz="-1.11e-7" iyy="2.198e-4" iyz="-8.04e-7" izz="2.197e-4"/>
+    </inertial>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rm_description/meshes/common/camera.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="camera_optical_frame_joint" type="fixed">
+    <origin rpy="-1.5786 -0.025 -1.570" xyz="0.043098346349538025 -0.015 0.027"/>
+    <parent link="pitch"/>
+    <child link="camera_optical_frame"/>
+  </joint>
+  <link name="camera_optical_frame">
+    <inertial>
+      <mass value="0.001"/>
+      <origin xyz="0.0 0.0 0.0"/>
+      <inertia ixx="2.129e-9" ixy="7.329e-9" ixz="-1.11e-9" iyy="2.198e-9" iyz="-8.04e-9" izz="2.197e-9"/>
+    </inertial>
+  </link>
+  <gazebo reference="camera_link">
+    <sensor name="camera" type="camera">
+      <update_rate>100.0</update_rate>
+      <camera name="head">
+        <horizontal_fov>1.3962634</horizontal_fov>
+        <image>
+          <width>1920</width>
+          <height>1080</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.05</near>
+          <far>5</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>0.0</updateRate>
+        <cameraName>galaxy_camera</cameraName>
+        <imageTopicName>image_raw</imageTopicName>
+        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+        <frameName>camera_optical_frame</frameName>
+        <hackBaseline>0.07</hackBaseline>
+        <distortionK1>0.0</distortionK1>
+        <distortionK2>0.0</distortionK2>
+        <distortionK3>0.0</distortionK3>
+        <distortionT1>0.0</distortionT1>
+        <distortionT2>0.0</distortionT2>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <gazebo>
+    <plugin filename="librm_robot_hw_sim.so" name="rm_ros_control">
+      <robotNamespace>/</robotNamespace>
+      <robotSimType>rm_gazebo/RmRobotHWSim</robotSimType>
+    </plugin>
+  </gazebo>
+</robot>

From 3c11d5f135015a6490a183d491c7d1d3ddfe882e Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Sun, 9 Jul 2023 19:57:26 +0800
Subject: [PATCH 009/147] 7-9 20:00.

---
 engineer_arm_ikfast_plugin/src/engineer_arm_ikfast_solver.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/engineer_arm_ikfast_plugin/src/engineer_arm_ikfast_solver.cpp b/engineer_arm_ikfast_plugin/src/engineer_arm_ikfast_solver.cpp
index 15e54a8..69372fc 100644
--- a/engineer_arm_ikfast_plugin/src/engineer_arm_ikfast_solver.cpp
+++ b/engineer_arm_ikfast_plugin/src/engineer_arm_ikfast_solver.cpp
@@ -12,7 +12,7 @@
 /// See the License for the specific language governing permissions and
 /// limitations under the License.
 ///
-/// ikfast version 0x10000049 generated on 2023-05-09 11:58:03.208674
+/// ikfast version 0x10000049 generated on 2023-06-26 07:05:12.012575
 /// To compile with gcc:
 ///     gcc -lstdc++ ik.cpp
 /// To compile without any main function as a shared object (might need -llapack):

From df9d6c5f9ccc30832e2cba1cd5c3d29b9f0f9e0e Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Mon, 10 Jul 2023 15:26:07 +0800
Subject: [PATCH 010/147] 7-10 15:30.

---
 engineer_arm_config/config/servo.yaml  |   2 +-
 engineer_arm_config/launch/moveit.rviz | 307 +++++++++++--------------
 2 files changed, 131 insertions(+), 178 deletions(-)

diff --git a/engineer_arm_config/config/servo.yaml b/engineer_arm_config/config/servo.yaml
index 6070cc1..a987de8 100644
--- a/engineer_arm_config/config/servo.yaml
+++ b/engineer_arm_config/config/servo.yaml
@@ -54,7 +54,7 @@ status_topic: status # Publish status to this topic
 command_out_topic: /controllers/arm_trajectory_controller/command # Publish outgoing commands here
 
 ## Collision checking for the entire robot body
-check_collisions: true # Check collisions?
+check_collisions: false # Check collisions?
 collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often.
 # Two collision check algorithms are available:
 # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
diff --git a/engineer_arm_config/launch/moveit.rviz b/engineer_arm_config/launch/moveit.rviz
index b85f19a..47b9890 100644
--- a/engineer_arm_config/launch/moveit.rviz
+++ b/engineer_arm_config/launch/moveit.rviz
@@ -5,8 +5,10 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /MotionPlanning1
+        - /TF1
+        - /TF1/Frames1
       Splitter Ratio: 0.5
-    Tree Height: 215
+    Tree Height: 613
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -41,7 +43,7 @@ Visualization Manager:
       Value: true
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
-      Enabled: true
+      Enabled: false
       Move Group Namespace: ""
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: false
@@ -70,91 +72,6 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_pivot:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_pivot:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link3:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link4:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link5:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link6:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          mimic_link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          mimic_link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_pivot:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_pivot:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
         Loop Animation: true
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
@@ -201,96 +118,132 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_pivot:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_pivot:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link3:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link4:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link5:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link6:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          mimic_link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          mimic_link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_pivot:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_pivot:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
         Robot Alpha: 0.5
         Show Robot Collision: false
         Show Robot Visual: true
-      Value: true
+      Value: false
       Velocity_Scaling_Factor: 0.1
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base_link:
+          Value: true
+        camera_link:
+          Value: true
+        camera_optical_frame:
+          Value: false
+        drag_link:
+          Value: true
+        exchanger:
+          Value: true
+        fake_link5:
+          Value: false
+        gimbal_base:
+          Value: false
+        left_back_pivot:
+          Value: false
+        left_back_wheel:
+          Value: false
+        left_front_pivot:
+          Value: false
+        left_front_wheel:
+          Value: false
+        link1:
+          Value: false
+        link2:
+          Value: false
+        link3:
+          Value: false
+        link4:
+          Value: false
+        link5:
+          Value: false
+        link6:
+          Value: true
+        link7:
+          Value: true
+        map:
+          Value: false
+        odom:
+          Value: false
+        pitch:
+          Value: true
+        pitch_behind_link:
+          Value: false
+        pitch_front_link:
+          Value: false
+        right_back_pivot:
+          Value: false
+        right_back_wheel:
+          Value: false
+        right_front_pivot:
+          Value: false
+        right_front_wheel:
+          Value: false
+        roll_left_link:
+          Value: false
+        roll_right_link:
+          Value: false
+        tools_link:
+          Value: true
+        yaw:
+          Value: false
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          exchanger:
+            {}
+          odom:
+            base_link:
+              drag_link:
+                {}
+              left_back_pivot:
+                left_back_wheel:
+                  {}
+              left_front_pivot:
+                left_front_wheel:
+                  {}
+              link1:
+                gimbal_base:
+                  yaw:
+                    pitch:
+                      camera_link:
+                        {}
+                      camera_optical_frame:
+                        {}
+                link2:
+                  link3:
+                    link4:
+                      link5:
+                        fake_link5:
+                          {}
+                        link6:
+                          link7:
+                            tools_link:
+                              {}
+              pitch_behind_link:
+                {}
+              pitch_front_link:
+                {}
+              right_back_pivot:
+                right_back_wheel:
+                  {}
+              right_front_pivot:
+                right_front_wheel:
+                  {}
+              roll_left_link:
+                {}
+              roll_right_link:
+                {}
+      Update Interval: 0
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -307,7 +260,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2.8098559379577637
+      Distance: 3.086756467819214
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -323,9 +276,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.20999997854232788
+      Pitch: 0.29000014066696167
       Target Frame: base_link
-      Yaw: 6.099963665008545
+      Yaw: 3.673126697540283
     Saved: ~
 Window Geometry:
   Displays:
@@ -339,7 +292,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000168000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001ab000001880000017d00ffffff00000312000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d000002f6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001ab000001880000017d00ffffff00000312000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
   Width: 1291

From 7a28ceef22acc8c3859144fa21c0fb25997cc456 Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Mon, 10 Jul 2023 17:29:53 +0800
Subject: [PATCH 011/147] 7-10 17:30.

---
 engineer_arm_config/launch/moveit.rviz     | 18 ++++++-------
 engineer_middleware/config/steps_list.yaml | 30 ++++++++++++----------
 2 files changed, 26 insertions(+), 22 deletions(-)

diff --git a/engineer_arm_config/launch/moveit.rviz b/engineer_arm_config/launch/moveit.rviz
index 47b9890..8acb7dc 100644
--- a/engineer_arm_config/launch/moveit.rviz
+++ b/engineer_arm_config/launch/moveit.rviz
@@ -131,11 +131,11 @@ Visualization Manager:
         base_link:
           Value: true
         camera_link:
-          Value: true
-        camera_optical_frame:
           Value: false
-        drag_link:
+        camera_optical_frame:
           Value: true
+        drag_link:
+          Value: false
         exchanger:
           Value: true
         fake_link5:
@@ -169,7 +169,7 @@ Visualization Manager:
         odom:
           Value: false
         pitch:
-          Value: true
+          Value: false
         pitch_behind_link:
           Value: false
         pitch_front_link:
@@ -260,7 +260,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 3.086756467819214
+      Distance: 3.905416488647461
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -276,9 +276,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.29000014066696167
+      Pitch: 0.2750001847743988
       Target Frame: base_link
-      Yaw: 3.673126697540283
+      Yaw: 4.083136081695557
     Saved: ~
 Window Geometry:
   Displays:
@@ -296,5 +296,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1291
-  X: 454
-  Y: 27
+  X: -240
+  Y: 154
diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 2325c4b..63d7f2d 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -139,6 +139,8 @@ common:
                          0.011
     drag_car_position: &JOINT5_DRAG_CAR_POSITION
                          0.4977
+    home_position: &JOINT5_HOME_POSITION
+                      -1.53
   joint6:
     up_position: &JOINT6_UP_POSITION
                    0.0127
@@ -180,6 +182,11 @@ common:
     down_position_no_delay: &JOINT7_DOWN_NO_DELAY_POSITION
       target: 1.67
       delay: 0.05
+    auto_exchange_position: &JOINT7_AUTO_EXCHANGE_POSITION
+      original_tf: link4
+      reference_tf: base_link
+      direction: pitch
+      delay: 0.05
   gripper:
     open_gripper: &OPEN_GRIPPER
       state: false
@@ -196,37 +203,37 @@ common:
         change: "+1"
   arm:
     home_zero_stone: &HOME_ZERO_STONE
-      joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_HOLD_STONE_POSITION ]
+      joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_REVERSAL_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_HOLD_STONE_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
     home_zero_stone_and_no_stone_in_hand: &HOME_ZERO_STONE_WITH_NO_STONE_IN_HAND
-      joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_LITTLE_BACK_POSITION ]
+      joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_HOME_POSITION, *JOINT6_LITTLE_BACK_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
     home_one_stone: &HOME_ONE_STONE
-      joints: [ *JOINT1_ONE_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_HAS_STONE_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_HOLD_STONE_POSITION ]
+      joints: [ *JOINT1_ONE_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_HAS_STONE_POSITION, *JOINT5_HOME_POSITION, *JOINT6_HOLD_STONE_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
     home_two_stone: &HOME_TWO_STONE
-      joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_HAS_STONE_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_HOLD_STONE_POSITION ]
+      joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_HAS_STONE_POSITION, *JOINT5_HOME_POSITION, *JOINT6_HOLD_STONE_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
     home_three_stone: &HOME_THREE_STONE
-      joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_HAS_STONE_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_HOLD_STONE_POSITION ]
+      joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_HAS_STONE_POSITION, *JOINT5_HOME_POSITION, *JOINT6_HOLD_STONE_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
     pick_wait: &PICK_WAIT
-      joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_HOLD_STONE_POSITION ]
+      joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_HOME_POSITION, *JOINT6_HOLD_STONE_POSITION ]
       common:
         <<: *QUICKLY
       tolerance:
@@ -371,6 +378,10 @@ steps_list:
     - step: "joint7 down"
       end_effector:
         <<: *JOINT7_DOWN_NO_DELAY_POSITION
+  JOINT7_AUTO_EXCHANGE:
+    - step: "joint7 auto exchange"
+      end_effector:
+        <<: *JOINT7_AUTO_EXCHANGE_POSITION
   ################### GRIPPER ####################
   OPEN_GRIPPER:
     - step: "open gripper"
@@ -438,13 +449,6 @@ steps_list:
     - step: "joint7 up"
       end_effector:
         <<: *JOINT7_UP_NO_DELAY_POSITION
-    - step: "avoid collision"
-      arm:
-        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_HOLD_STONE_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
     - step: "home with no stone"
       arm:
         <<: *HOME_ZERO_STONE

From 150be814d67a7e0c21452654e62b5e73dbed286c Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Thu, 13 Jul 2023 22:51:23 +0800
Subject: [PATCH 012/147] Useful version 7-13 22:50.

---
 engineer_arm_config/launch/moveit.rviz     |  297 ++-
 engineer_middleware/config/steps_list.yaml | 2392 ++++----------------
 2 files changed, 741 insertions(+), 1948 deletions(-)

diff --git a/engineer_arm_config/launch/moveit.rviz b/engineer_arm_config/launch/moveit.rviz
index 8acb7dc..9943155 100644
--- a/engineer_arm_config/launch/moveit.rviz
+++ b/engineer_arm_config/launch/moveit.rviz
@@ -5,10 +5,11 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /MotionPlanning1
+        - /MotionPlanning1/Planning Metrics1
         - /TF1
         - /TF1/Frames1
       Splitter Ratio: 0.5
-    Tree Height: 613
+    Tree Height: 357
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -43,7 +44,7 @@ Visualization Manager:
       Value: true
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
-      Enabled: false
+      Enabled: true
       Move Group Namespace: ""
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: false
@@ -72,11 +73,119 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-        Loop Animation: true
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          camera_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          camera_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          fake_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          gimbal_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          pitch:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          pitch_behind_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          pitch_front_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          right_back_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          roll_left_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          roll_right_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          tools_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          yaw:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Loop Animation: false
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
         Show Robot Collision: false
-        Show Robot Visual: true
+        Show Robot Visual: false
         Show Trail: false
         State Display Time: 0.05 s
         Trail Step Size: 1
@@ -118,10 +227,118 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          camera_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          camera_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          fake_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          gimbal_base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          pitch:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          pitch_behind_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          pitch_front_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          right_back_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          roll_left_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          roll_right_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          tools_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          yaw:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
         Robot Alpha: 0.5
         Show Robot Collision: false
         Show Robot Visual: true
-      Value: false
+      Value: true
       Velocity_Scaling_Factor: 0.1
     - Class: rviz/TF
       Enabled: true
@@ -134,52 +351,42 @@ Visualization Manager:
           Value: false
         camera_optical_frame:
           Value: true
-        drag_link:
-          Value: false
         exchanger:
           Value: true
         fake_link5:
           Value: false
         gimbal_base:
           Value: false
-        left_back_pivot:
-          Value: false
         left_back_wheel:
           Value: false
-        left_front_pivot:
-          Value: false
         left_front_wheel:
           Value: false
         link1:
-          Value: false
+          Value: true
         link2:
-          Value: false
+          Value: true
         link3:
-          Value: false
+          Value: true
         link4:
-          Value: false
+          Value: true
         link5:
-          Value: false
+          Value: true
         link6:
           Value: true
         link7:
           Value: true
         map:
-          Value: false
+          Value: true
         odom:
-          Value: false
+          Value: true
         pitch:
           Value: false
         pitch_behind_link:
           Value: false
         pitch_front_link:
           Value: false
-        right_back_pivot:
-          Value: false
         right_back_wheel:
           Value: false
-        right_front_pivot:
-          Value: false
         right_front_wheel:
           Value: false
         roll_left_link:
@@ -202,14 +409,10 @@ Visualization Manager:
             {}
           odom:
             base_link:
-              drag_link:
+              left_back_wheel:
+                {}
+              left_front_wheel:
                 {}
-              left_back_pivot:
-                left_back_wheel:
-                  {}
-              left_front_pivot:
-                left_front_wheel:
-                  {}
               link1:
                 gimbal_base:
                   yaw:
@@ -232,12 +435,10 @@ Visualization Manager:
                 {}
               pitch_front_link:
                 {}
-              right_back_pivot:
-                right_back_wheel:
-                  {}
-              right_front_pivot:
-                right_front_wheel:
-                  {}
+              right_back_wheel:
+                {}
+              right_front_wheel:
+                {}
               roll_left_link:
                 {}
               roll_right_link:
@@ -260,7 +461,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 3.905416488647461
+      Distance: 4.128435134887695
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -268,33 +469,33 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: -0.10000000149011612
-        Y: 0.25
-        Z: 0.30000001192092896
+        X: -0.17658083140850067
+        Y: 0.4902980625629425
+        Z: 0.38593053817749023
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.2750001847743988
+      Pitch: 0.3947972059249878
       Target Frame: base_link
-      Yaw: 4.083136081695557
+      Yaw: 1.5382111072540283
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: false
-  Height: 848
+    collapsed: true
+  Height: 1016
   Help:
     collapsed: false
-  Hide Left Dock: false
+  Hide Left Dock: true
   Hide Right Dock: false
   MotionPlanning:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d000002f6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001ab000001880000017d00ffffff00000312000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f30000039efc0200000007fb000000100044006900730070006c006100790073000000003d0000039e000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001ab000002300000017d00ffffff000007320000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
-  Width: 1291
-  X: -240
-  Y: 154
+  Width: 1842
+  X: 78
+  Y: 27
diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 63d7f2d..1834730 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -9,10 +9,6 @@ common:
       accel: 0.1
       timeout: 15.
     normally: &NORMALLY
-      speed: 0.4
-      accel: 0.3
-      timeout: 8.
-    joint6_normally: &JOINT6_NORMALLY
       speed: 0.4
       accel: 0.3
       timeout: 2.
@@ -20,6 +16,10 @@ common:
       speed: 0.8
       accel: 0.8
       timeout: 7.
+    joint6_normally: &JOINT6_NORMALLY
+      speed: 0.4
+      accel: 0.3
+      timeout: 2.
 
   tolerance:
     small_tolerance: &SMALL_TOLERANCE
@@ -31,280 +31,268 @@ common:
     max_tolerance: &MAX_TOLERANCE
       tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 ]
 
-  joint1:
-    down_position: &JOINT1_DOWN_POSITION
-                     -0.029600
-    zero_stone_position: &JOINT1_ZERO_STONE_POSITION
-                           0.08
-    one_stone_position: &JOINT1_ONE_STONE_POSITION
-                          0.125
-    take_one_stone_position: &JOINT1_TAKE_ONE_STONE_POSITION
-                               0.134
-    stone_storage_high_position: &STONE_STORAGE_HIGH_POSITION
-                                   0.245
-    take_two_stone_position: &JOINT1_TAKE_TWO_STONE_POSITION
-                               0.327
-    take_three_stone_position: &JOINT1_TAKE_THREE_STONE_POSITION
-                                 0.327
-    get_stone_position: &JOINT1_GET_STONE_POSITION
-                          0.286600
-    get_stone_low_position: &JOINT1_GET_STONE_LOW_POSITION
-                              0.246600
-    two_stone_position: &JOINT1_TWO_STONE_POSITION
-                          0.315
-    three_stone_position: &JOINT1_THREE_STONE_POSITION
-                            0.4192
-    up_position: &JOINT1_UP_POSITION
-                   0.43
-    ready_to_get_stone: &JOINT1_READY_TO_GET_STONE_POSITION
-                          0.438
-    get_ground_stone: &JOINT1_GET_GROUND_STONE_POSITION
-                        0.170
-    go_get_ground_stone: &JOINT1_GO_GET_GROUND_STONE
-                           0.11
-    parallel_get_island_stone: &JOINT1_PARALLEL_GET_ISLAND_STONE_POSITION
-                                 0.10
-    parallel_up_island_stone: &JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION
-                                0.245
 
+  joint1:
+    mechanical:
+      down_position: &JOINT1_DOWN_POSITION
+                       -0.09600
+      up_position: &JOINT1_UP_POSITION
+                     0.43
+    home:
+      zero_stone_position: &JOINT1_ZERO_STONE_POSITION
+                             0.08
+      one_stone_position: &JOINT1_ONE_STONE_POSITION
+                            0.08
+      two_stone_position: &JOINT1_TWO_STONE_POSITION
+                            0.153000
+      three_stone_position: &JOINT1_THREE_STONE_POSITION
+                              0.4192
+    get_ore:
+      ready_get_ore: &JOINT1_READY_TO_GET_ORE_POSITION
+                       0.40
+      get_ore: &JOINT1_GET_ORE_POSITION
+                 0.310500
+    exchange_position: &JOINT1_EXCHANGE_POSITION
+                         0.43
 
   joint2:
-    back_position: &JOINT2_BACK_POSITION
-                     0.002
-    recently_position: &JOINT2_RECENTLY_POSITION
-                         0.002
-    reversal_position: &JOINT2_REVERSAL_POSITION
-                         0.113
-    avoid_line_collision_position: &JOINT2_AVOID_LINE_COLLISION_POSITION
-                                     0.093
-    sky_position: &JOINT2_SKY_POSITION
-                    0.260
-    furthest_position: &JOINT2_FURTHEST_POSITION
-                         0.260
-    ground_stone_position: &JOINT2_GROUND_STONE_POSITION
-                             0.056
-    parallel_get_island_stone: &JOINT2_PARALLEL_GET_ISLAND_STONE_POSITION
-                                 0.12
-    parallel_back_island_stone: &JOINT2_PARALLEL_BACK_ISLAND_STONE_POSITION
-                                  0.06
-    store_stone_position: &JOINT2_STORE_STONE_POSITION
-                            0.002
-    take_stone_position: &JOINT2_TAKE_STONE_POSITION
-                           -0.003
+    mechanical:
+      back_position: &JOINT2_BACK_POSITION
+                       0.05
+      furthest_position: &JOINT2_FURTHEST_POSITION
+                           0.40045
+    store:
+      access_store_position: &JOINT2_ACCESS_STORE_POSITION
+                               0.0001
+      ready_to_access_stone_position: &JOINT2_READY_TO_ACCESS_STONE_POSITION
+                                        0.025
+    exchange_position: &JOINT2_EXCHANGE_POSITION
+                         0.05
+    get_ore_position: &JOINT2_GET_ORE_POSITION
+                        0.032590
+
   joint3:
-    lf_take_position: &JOINT3_LF_TAKE_POSITION
-                        0.265600
-    lf_take_right_position: &JOINT3_LF_TAKE_RIGHT_POSITION
-                              0.2012
-    rt_take_position: &JOINT3_RT_TAKE_POSITION
-                        -0.249
-    rt_take_right_position: &JOINT3_RT_TAKE_RIGHT_POSITION
-                              -0.249
-    mid_position: &JOINT3_MID_POSITION
-                    -0.009
-    mid_take_right_position: &JOINT3_MID_TAKE_RIGHT_POSITION
-                               -0.056150
-    sky_store_position: &JOINT3_SKY_STORE_POSITION
-                          -0.292
-    ground_store_stone_position: &GROUND_JOINT3_STORE_STONE_POSITION
-                                   -0.246
-    take_store_position: &JOINT3_TAKE_STORE_POSITION
-                           -0.233
-    store_stone_position: &JOINT3_STORE_STONE_POSITION
-                            -0.2582
-    drag_car_position: &JOINT3_DRAG_CAR_POSITION
-                         -0.193102
-    parallel_store_stone: &JOINT3_PARALLEL_STORE_STONE_POSITION
-                            -0.299
+    mechanical:
+      left_position: &JOINT3_LF_POSITION
+                       0.25
+      mid_position: &JOINT3_MID_POSITION
+                      0.0001
+      right_position: &JOINT3_RT_POSITION
+                        -0.275
+    get_ore:
+      lf_get_ore_position: &JOINT3_LF_GET_ORE_POSITION
+                             0.275100
+      mid_get_ore_position: &JOINT3_MID_GET_ORE_POSITION
+                              0.018900
+      rt_get_ore_position: &JOINT3_RT_GET_ORE_POSITION
+                             -0.237300
+    store_and_take:
+      store_stone_position: &JOINT3_ACCESS_STONE_POSITION
+                              0.195800
+
+
 
   joint4:
-    up_position: &JOINT4_UP_POSITION
-                   -1.578000
-    ready_position: &JOINT4_READY_POSITION
-                      -1.578000
-    down_position: &JOINT4_DOWN_POSITION
-                     -0.002
-    has_stone_position: &JOINT4_HAS_STONE_POSITION
-                          -1.578000
-    drag_car_position: &JOINT4_DRAG_CAR_POSITION
-                         -0.244
+    mechanical:
+      up_position: &JOINT4_UP_POSITION
+                     0.1
+      down_position: &JOINT4_DOWN_POSITION
+                       1.07
+
+
   joint5:
-    left_position: &JOINT5_LEFT_POSITION
-                     1.527
-    left_more_position: &JOINT5_LEFT_MORE_POSITION
-                          1.527
-    mid_position: &JOINT5_MID_POSITION
-                    0.027
-    reversal_position: &JOINT5_REVERSAL_POSITION
-                         0.011
-    drag_car_position: &JOINT5_DRAG_CAR_POSITION
-                         0.4977
-    home_position: &JOINT5_HOME_POSITION
-                      -1.53
+    mechanical:
+      right_position: &JOINT5_RT_POSITION
+                        -1.5
+      mid_position: &JOINT5_MID_POSITION
+                      0.01
+      left_position: &JOINT5_LF_POSITION
+                       1.25
+
+
   joint6:
     up_position: &JOINT6_UP_POSITION
-                   0.0127
+                   0.051740
     down_position: &JOINT6_DOWN_POSITION
-                     3.1
+                     3.409060
+    access_stone: &JOINT6_ACCESS_STONE_POSITION
+                    1.663420
     little_back_position: &JOINT6_LITTLE_BACK_POSITION
-                            1.21
-    back_position: &JOINT6_BACK_POSITION
-                     1.548990
-    hold_stone: &JOINT6_HOLD_STONE_POSITION
-                  -0.2567
-    reversal: &JOINT6_REVERSAL_POSITION
-                1.0
-    drag_car_position: &JOINT6_DRAG_CAR_POSITION
-                         1.631
+                            0.887580
 
-  end_effector:
+  joint7:
     use_for_delay: &DELAY_0_1
       target: 0.008
       delay: 0.1
     up_position: &JOINT7_UP_POSITION
-      target: 0.008
-      delay: 0.6
+      target: 0.
+      delay:  0.1
+    up_position_no_delay: &JOINT7_UP_POSITION_NO_DELAY
+      target: 0.
+      delay:  0.
+    mid_more_position: &JOINT7_MID_MORE_POSITION
+      target: 0.25
+      delay: 0.1
+    mid_more_position_no_delay: &JOINT7_MID_MORE_POSITION_NO_DELAY
+      target: 0.25
+      delay: 0.
+    mid_position: &JOINT7_MID_POSITION
+      target: 0.5
+      delay: 0.1
+    mid_position_no_delay: &JOINT7_MID_POSITION_NO_DELAY
+      target: 0.5
+      delay: 0.
+    mid_less_position: &JOINT7_MID_LESS_POSITION
+      target: 0.75
+      delay: 0.1
+    mid_less_position_no_delay: &JOINT7_MID_LESS_POSITION_NO_DELAY
+      target: 0.75
+      delay: 0.
     down_position: &JOINT7_DOWN_POSITION
-      target: 1.67
-      delay: 0.6
-    up_position_no_delay: &JOINT7_UP_NO_DELAY_POSITION
-      target: -0.1
+      target: 1
+      delay: 0.1
+    down_position_no_delay: &JOINT7_DOWN_POSITION_NO_DELAY
+      target: 1
       delay: 0.
-    mid_position_no_delay: &JOINT7_MID_NO_DELAY_POSITION
-      target: 0.7
-      delay: 0.05
-    mid_more_position_no_delay: &JOINT7_MID_MORE_NO_DELAY_POSITION
-      target: 1.5
-      delay: 0.05
-    mid_less_position_no_delay: &JOINT7_MID_LESS_NO_DELAY_POSITION
-      target: 1.3
-      delay: 0.05
-    down_position_no_delay: &JOINT7_DOWN_NO_DELAY_POSITION
-      target: 1.67
-      delay: 0.05
     auto_exchange_position: &JOINT7_AUTO_EXCHANGE_POSITION
       original_tf: link4
       reference_tf: base_link
       direction: pitch
       delay: 0.05
+
+
+
   gripper:
     open_gripper: &OPEN_GRIPPER
       state: false
     close_gripper: &CLOSE_GRIPPER
       state: true
 
-  TEST_MIN_1:
-    - step: "change stone"
-      stone_num:
-        change: "-1"
-  TEST_PLUS_1:
-    - step: "change stone"
-      stone_num:
-        change: "+1"
+  gimbal:
+    back_pos: &BACK_POS
+      frame: gimbal_base
+      position: [ -2 ,-0.25, 0 ]
+    side_pos: &SIDE_POS
+      frame: gimbal_base
+      position: [ 0.03 ,3, -0.02 ]
+    island_pos: &ISLAND_POS
+      frame: gimbal_base
+      position: [ 2, 0.03, -0.8 ]
+    ground_pos: &GROUND_POS
+      frame: gimbal_base
+      position: [ 2, 0.03, -0.8 ]
+    reversal_pos: &REVERSAL_POS
+      frame: gimbal_base
+      position: [ 2, 0.015, -0.25 ]
+    exchange_pos: &EXCHANGE_POS
+      frame: gimbal_base
+      position: [ 0, -3, -2.3 ]
+
   arm:
-    home_zero_stone: &HOME_ZERO_STONE
-      joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_REVERSAL_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_HOLD_STONE_POSITION ]
+    home_zero_stone: &HOME
+      joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
-    home_zero_stone_and_no_stone_in_hand: &HOME_ZERO_STONE_WITH_NO_STONE_IN_HAND
-      joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_HOME_POSITION, *JOINT6_LITTLE_BACK_POSITION ]
-      common:
-        <<: *NORMALLY
-      tolerance:
-        <<: *NORMAL_TOLERANCE
-    home_one_stone: &HOME_ONE_STONE
-      joints: [ *JOINT1_ONE_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_HAS_STONE_POSITION, *JOINT5_HOME_POSITION, *JOINT6_HOLD_STONE_POSITION ]
+
+    get_ore:
+      ready_to_get_ore: &READY_TO_GET_ORE
+        joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+      get_ore: &GET_ORE
+        joints: [ *JOINT1_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    access_store:
+      ready_to_access_store: &READY_TO_ACCESS_STORE
+        joints: [ "VARIABLE", *JOINT2_READY_TO_ACCESS_STONE_POSITION, *JOINT3_ACCESS_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+      access_store: &ACCESS_STORE
+        joints: [ "VARIABLE", *JOINT2_ACCESS_STORE_POSITION, *JOINT3_ACCESS_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, "VARIABLE" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    #      ready_to_get_ore_mid: &READY_TO_GET_ORE_MID
+    #        joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, *JOINT3_MID_GET_ORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
+    #        common:
+    #          <<: *NORMALLY
+    #        tolerance:
+    #          <<: *NORMAL_TOLERANCE
+    #      get_ore_mid: &GET_ORE_MID
+    #        joints: [ *JOINT1_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, *JOINT3_MID_GET_ORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
+    #        common:
+    #          <<: *NORMALLY
+    #        tolerance:
+    #          <<: *NORMAL_TOLERANCE
+    #      ready_to_get_ore_rt: &READY_TO_GET_ORE_RT
+    #        joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, *JOINT3_RT_GET_ORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
+    #        common:
+    #          <<: *NORMALLY
+    #        tolerance:
+    #          <<: *NORMAL_TOLERANCE
+    #      get_ore_rt: &GET_ORE_RT
+    #        joints: [ *JOINT1_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, *JOINT3_RT_GET_ORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
+    #        common:
+    #          <<: *NORMALLY
+    #        tolerance:
+    #          <<: *NORMAL_TOLERANCE
+
+    ready_to_store_stone_after_get_ore: &READY_TO_STORE_STONE_AFTER_GET_ORE
+      joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_ACCESS_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
-    home_two_stone: &HOME_TWO_STONE
-      joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_HAS_STONE_POSITION, *JOINT5_HOME_POSITION, *JOINT6_HOLD_STONE_POSITION ]
+
+    store_stone_after_get_ore: &STORE_STONE_AFTER_GET_ORE
+      joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_ACCESS_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_ACCESS_STONE_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
-    home_three_stone: &HOME_THREE_STONE
-      joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_HAS_STONE_POSITION, *JOINT5_HOME_POSITION, *JOINT6_HOLD_STONE_POSITION ]
+
+    exchange_position: &EXCHANGE_POSITION
+      joints: [ *JOINT1_EXCHANGE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_LF_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
       common:
         <<: *NORMALLY
-      tolerance:
-        <<: *NORMAL_TOLERANCE
-    pick_wait: &PICK_WAIT
-      joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_HOME_POSITION, *JOINT6_HOLD_STONE_POSITION ]
-      common:
-        <<: *QUICKLY
       tolerance:
         <<: *BIGGER_TOLERANCE
 
-  gimbal:
-    sky_gimbal: &SKY_POS
-      frame: gimbal_base
-      position: [ 2, 0., 0.5 ]
-    back_gimbal: &BACK_POS
-      frame: gimbal_base
-      position: [ -2 ,-0.25, 0. ]
-    side_gimbal: &SIDE_POS
-      frame: gimbal_base
-      position: [ 0.03 ,3, -0.02 ]
-    island_gimbal: &ISLAND_POS
-      frame: gimbal_base
-      position: [ 2 ,0.03, 0. ]
-    ground_pos: &GROUND_POS
-      frame: gimbal_base
-      position: [ 2 ,0.03, -0.8 ]
-    reversal_pos: &REVERSAL_POS
-      frame: gimbal_base
-      position: [ 2,0.015, -0.25 ]
-    exchange_pos: &EXCHANGE_POS
-      frame: gimbal_base
-      position: [ 0.,-3, -2.3 ]
+
+
 
 
   reversal:
     velocity_stop: &VELOCITY_STOP
       mode: "VELOCITY"
       values: [ 0.,0.,0.,0.,0.,0. ]
-    velocity_z_out: &VELOCITY_Z_OUT
-      mode: "VELOCITY"
-      values: [ 0.,0.,3.,0.,0.,0. ]
-    velocity_z_out_store: &VELOCITY_Z_OUT_STORE
-      mode: "VELOCITY"
-      values: [ 0.,0.,0.2,0.,0.,0. ]
     velocity_z_in: &VELOCITY_Z_IN
       mode: "VELOCITY"
       values: [ 0.,0.,-1.2,0.,-0.01,0. ]
     velocity_z_in_quickly: &VELOCITY_Z_IN_QUICKLY
       mode: "VELOCITY"
       values: [ 0.,0.,-3.0,0.,-0.01,0. ]
-    velocity_z_pitch_in: &VELOCITY_PITCH_Z_IN
-      mode: "VELOCITY"
-      values: [ 0.,0.,0.,0.,-1.0,0. ]
-    velocity_z_in_slowly: &VELOCITY_Z_IN_SLOWLY
-      mode: "VELOCITY"
-      values: [ 0.,0.,-0.5,0.,-0.01,0. ]
-    velocity_z_in_delay: &VELOCITY_Z_IN_DELAY
-      mode: "VELOCITY"
-      values: [ 0.,0.,-1.,0.,0.,0. ]
-      delay: 0.5
     position_z_out: &POSITION_Z_OUT
       mode: "POSITION"
       values: [ 0.,0.,17.,0.,0.,0. ]
       delay: 0.4
-    position_pitch_out: &POSITION_PITCH_OUT
-      mode: "POSITION"
-      values: [ 0.,0.,10.,0.,-1.,0. ]
-    position_pitch_reversal_pi_2: &POSITION_PITCH_REVERSAL_PI_2
-      mode: "POSITION"
-      values: [ 0.,0.,0.,0.,21.,0. ]
-      delay: 0.6
-    position_pitch_reversal_pi: &POSITION_PITCH_REVERSAL_PI
-      mode: "POSITION"
-      values: [ 0.,0.,0.,0.,42.,0. ]
-      delay: 0.8
+    velocity_z_out: &VELOCITY_Z_OUT
+      mode: "VELOCITY"
+      values: [ 0.,0.,3.,0.,0.,0. ]
 
+  chassis_move:
     chassis_backward_7: &CHASSIS_BACKWARD_7
       frame: base_link
       position: [ -0.07, 0. ]
@@ -333,1824 +321,428 @@ common:
       chassis_tolerance_angular_: 0.2
 
 
-
-
 steps_list:
-  ################### COMMON STEP####################
-  DELETE_SCENE:
-    - step: "delete_scene"
-  ################### GIMBAL ####################
-  SKY_GIMBAL:
-    - step: "sky island gimbal"
-      gimbal:
-        <<: *SKY_POS
-  ISLAND_GIMBAL:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
-  BACK_GIMBAL:
-    - step: "back gimbal"
-      gimbal:
-        <<: *BACK_POS
-  SIDE_GIMBAL:
-    - step: "side gimbal"
-      gimbal:
-        <<: *SIDE_POS
-  GROUND_GIMBAL:
-    - step: "ground gimbal"
-      gimbal:
-        <<: *GROUND_POS
-  REVERSAL_GIMBAL:
-    - step: "reversal gimbal"
-      gimbal:
-        <<: *REVERSAL_POS
-  EXCHANGE_GIMBAL:
-    - step: "exchange gimbal"
-      gimbal:
-        <<: *EXCHANGE_POS
-
-  ################### JOINT7 ####################
-  JOINT7_UP:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
-  JOINT7_DOWN:
-    - step: "joint7 down"
-      end_effector:
-        <<: *JOINT7_DOWN_NO_DELAY_POSITION
-  JOINT7_AUTO_EXCHANGE:
-    - step: "joint7 auto exchange"
-      end_effector:
-        <<: *JOINT7_AUTO_EXCHANGE_POSITION
-  ################### GRIPPER ####################
-  OPEN_GRIPPER:
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-
-  CLOSE_GRIPPER:
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-  ################### CHASSIS ####################
-  CHASSIS_FORWARD:
-    - step: "forward"
-      chassis:
-        frame: base_link
-        position: [ 0.3, 0. ]
-        yaw: 0.0
-        chassis_tolerance_position_: 0.025
-        chassis_tolerance_angular_: 0.1
-
-  CHASSIS_BACKWARD:
-    - step: "backward"
-      chassis:
-        frame: base_link
-        position: [ -0.7, 0. ]
-        yaw: 0.0
-        chassis_tolerance_position_: 0.04
-        chassis_tolerance_angular_: 0.2
-
-  CHASSIS_LEFT:
-    - step: "left"
-      chassis:
-        frame: base_link
-        position: [ 0., 0.5 ]
-        yaw: 0.0
-        chassis_tolerance_position_: 0.1
-        chassis_tolerance_angular_: 0.01
-
-  CHASSIS_RIGHT:
-    - step: "right"
-      chassis:
-        frame: base_link
-        position: [ 0., -0.5 ]
-        yaw: 0.0
-        chassis_tolerance_position_: 0.1
-        chassis_tolerance_angular_: 0.01
-
-  CHASSIS_ROTATE:
-    - step: "rotate"
-      chassis:
-        frame: base_link
-        position: [ 0., 0. ]
-        yaw: 3.14
-        chassis_tolerance_position_: 1.
-        chassis_tolerance_angular_: 0.01
-
-  ################### CHASSIS ####################
-
-  ################### HOME ####################
+  ###############   HOME   ###############
 
   HOME_ZERO_STONE:
-    - step: "gimbal side"
-      gimbal:
-        <<: *SIDE_POS
     - step: "joint7 up"
       end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
+        <<: *JOINT7_UP_POSITION
     - step: "home with no stone"
       arm:
-        <<: *HOME_ZERO_STONE
+        <<: *HOME
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+
   HOME_ONE_STONE:
-    - step: "gimbal side"
+    - step: "gimbal look side"
       gimbal:
         <<: *SIDE_POS
     - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
+      joint7:
+        <<: *JOINT7_UP_POSITION
     - step: "home with one stone"
       arm:
-        <<: *HOME_ONE_STONE
+        <<: *HOME
+        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ]
+
   HOME_TWO_STONE:
-    - step: "gimbal side"
+    - step: "gimbal look side"
       gimbal:
         <<: *SIDE_POS
     - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
+      joint7:
+        <<: *JOINT7_UP_POSITION
     - step: "home with two stone"
       arm:
-        <<: *HOME_TWO_STONE
+        <<: *HOME
+        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+
   HOME_THREE_STONE:
-    - step: "gimbal side"
+    - step: "gimbal look side"
       gimbal:
         <<: *SIDE_POS
     - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
+      joint7:
+        <<: *JOINT7_UP_POSITION
     - step: "home with three stone"
       arm:
-        <<: *HOME_THREE_STONE
+        <<: *HOME
+        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, 0 ]
 
-  ################### DRAG ####################
-  ENGINEER_DRAG_CAR:
-    - step: "gimbal ready"
-      gimbal:
-        <<: *GROUND_POS
-  ENGINEER_DRAG_CAR0:
-    - step: "gimbal ready"
-      gimbal:
-        <<: *BACK_POS
-    - step: "ready to drag car"
-      arm:
-        <<: *HOME_ZERO_STONE
+      ###################   TAKE   ####################
 
+  #############  SMALL_ISLAND_TOP  ################
 
-  ################### GROUND ####################
-  GROUND_STONE0:
-    - step: "gimbal ready"
+  ########### LF  ############
+
+  SMALL_ISLAND_LF:
+    - step: "joint7 up"
+      joint7:
+        <<: *JOINT7_UP_POSITION_NO_DELAY
+    - step: "island gimbal"
       gimbal:
-        <<: *GROUND_POS
-    - step: "ready to get ground stone"
+        <<: *ISLAND_POS
+    - step: "ready_to_get_ore"
       arm:
-        joints: [ *JOINT1_GET_GROUND_STONE_POSITION, *JOINT2_GROUND_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_DOWN_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_DOWN_POSITION
-  GROUND_STONE00:
-    - step: "change gripper state"
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, 0 ]
+    - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
-    - step: "get ground stone"
-      arm:
-        joints: [ *JOINT1_GO_GET_GROUND_STONE, *JOINT2_GROUND_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_DOWN_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "up ground stone"
+    - step: "reversal move"
+      reversal:
+        <<: *VELOCITY_Z_IN
+  SMALL_ISLAND_LF0:
+    - step: "move down to get ore"
       arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_GROUND_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_DOWN_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-  GROUND_STONE000:
-    - step: "move joint1 and 3"
+        <<: *GET_ORE
+        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, 0 ]
+    - step: "move up ore"
       arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_GROUND_STONE_POSITION, *GROUND_JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
-    - step: "move joint2"
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, 0 ]
+    - step: "ready to store stone"
       arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *GROUND_JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-  GROUND_STONE0000:
-    - step: "move joint6"
+        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
+    - step: "store stone"
       arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *GROUND_JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
+        <<: *STORE_STONE_AFTER_GET_ORE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "avoid collision"
-      arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_REVERSAL_POSITION, *GROUND_JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-  GROUND_STONE00000:
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_BACK_POSITION, *GROUND_JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL OUT"
+    - step: "reverse joint6"
+      joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_READY_TO_ACCESS_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
+      common:
+        <<: *NORMALLY
+      tolerance:
+        <<: *NORMAL_TOLERANCE
+    - step: "stop reversal"
       reversal:
-        <<: *POSITION_Z_OUT
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_BACK_POSITION, *GROUND_JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_HOLD_STONE_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-  ################### EXCHANGE ####################
-  EXCHANGE_WAIT:
+        <<: *VELOCITY_STOP
+
+  ########### MID ############
+
+  SMALL_ISLAND_MID:
+    - step: "joint7 up"
+      joint7:
+        <<: *JOINT7_UP_POSITION_NO_DELAY
     - step: "island gimbal"
       gimbal:
         <<: *ISLAND_POS
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_MID_NO_DELAY_POSITION
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_MID_LESS_NO_DELAY_POSITION
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_MID_MORE_NO_DELAY_POSITION
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_DOWN_NO_DELAY_POSITION
-    - step: "move to exchange wait state"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_REVERSAL_POSITION, *JOINT3_LF_TAKE_RIGHT_POSITION , *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-  ###################    SKY_BIG_ISLAND    ####################
-  SKY_BIG_ISLAND0:
-    - step: "arm wait state"
+    - step: "ready_to_get_ore"
       arm:
-        <<: *PICK_WAIT
-  SKY_BIG_ISLAND00:
-    - step: "change gripper state"
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0 ]
+    - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
-    - step: "move to wait stone down"
+    - step: "reversal move"
+      reversal:
+        <<: *VELOCITY_Z_IN
+  SMALL_ISLAND_MID0:
+    - step: "move down to get ore"
       arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_SKY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-  SKY_BIG_ISLAND000:
-    - step: "move joint1 and 3"
+        <<: *GET_ORE
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0 ]
+    - step: "move up ore"
       arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_SKY_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN_SLOWLY
-    - step: "move joint6"
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0 ]
+    - step: "ready to store stone"
       arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_SKY_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
+        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
+    - step: "store stone"
+      arm:
+        <<: *STORE_STONE_AFTER_GET_ORE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "home with no stone"
-      arm:
-        <<: *HOME_ONE_STONE
-    - step: "REVERSAL STOP"
+    - step: "reverse joint6"
+      joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_READY_TO_ACCESS_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
+      common:
+        <<: *NORMALLY
+      tolerance:
+        <<: *NORMAL_TOLERANCE
+    - step: "stop reversal"
       reversal:
         <<: *VELOCITY_STOP
-    - step: "change stone"
-      stone_num:
-        change: "+1"
-    ###################    BIG_ISLAND    ####################
-    ###################    MID    ################
-  BIG_ISLAND0:
-    - step: "arm wait"
+
+  ########### RT  ############
+
+  SMALL_ISLAND_RT:
+    - step: "joint7 up"
+      joint7:
+        <<: *JOINT7_UP_POSITION_NO_DELAY
+    - step: "island gimbal"
+      gimbal:
+        <<: *ISLAND_POS
+    - step: "ready_to_get_ore"
       arm:
-        <<: *PICK_WAIT
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, 0 ]
     - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
-    - step: "joint7 down"
-      end_effector:
-        <<: *JOINT7_DOWN_NO_DELAY_POSITION
-    - step: "move to exchange wait state"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_REVERSAL_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-  BIG_ISLAND00:
-    - step: "joint7 up"
+    - step: "reversal move"
+      reversal:
+        <<: *VELOCITY_Z_IN
+  SMALL_ISLAND_RT0:
+    - step: "move down to get ore"
       arm:
-        joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION
-    - step: "move joint6"
+        <<: *GET_ORE
+        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, 0 ]
+    - step: "move up ore"
       arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_REVERSAL_POSITION, *JOINT3_SKY_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_HOLD_STONE_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "move joint6"
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, 0 ]
+    - step: "ready to store stone"
       arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_SKY_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "move joint6"
+        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
+    - step: "store stone"
       arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_SKY_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
+        <<: *STORE_STONE_AFTER_GET_ORE
+        variable: [ *JOINT1_READY_TO_GET_ORE_POSITION, 0, 0, 0, 0, 0 ]
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "home with no stone"
-      arm:
-        <<: *HOME_THREE_STONE
-    - step: "change stone"
-      stone_num:
-        change: "+1"
-  ###################    SMALL_ISLAND    ####################
-  LF_SMALL_ISLAND0:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
+    - step: "reverse joint6"
+      joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_READY_TO_ACCESS_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
+      common:
+        <<: *NORMALLY
+      tolerance:
+        <<: *NORMAL_TOLERANCE
+    - step: "stop reversal"
+      reversal:
+        <<: *VELOCITY_STOP
+
+      ###################   TAKE   ####################
+
+  ########### TAKE ###########
+  TAKE_WHEN_ONE_STONE:
     - step: "island gimbal"
       gimbal:
         <<: *ISLAND_POS
-    - step: "arm wait"
+    - step: "joint7 up"
+      joint7:
+        <<: *JOINT7_UP_POSITION_NO_DELAY
+    - step: "move down to get stone"
       arm:
-        <<: *PICK_WAIT
-  LF_SMALL_ISLAND00:
-    - step: "add scenes"
-      scene_name: MID_SMALL_ISLAND
-    - step: "arm ready to get stone"
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "REVERSAL OUT"
+      reversal:
+        <<: *POSITION_Z_OUT
+    - step: "move up with stone and ready to get stone"
       arm:
-        joints: [ *JOINT1_READY_TO_GET_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "change gripper state"
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
-    - step: "move arm down to gain stone"
+    - step: "move joint2 to get stone"
       arm:
-        joints: [ *JOINT1_GET_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
+        <<: *ACCESS_STORE
+        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    - step: "REVERSAL Z OUT"
       reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move arm up"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "move arm right"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_LF_TAKE_RIGHT_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "reversal stone"
+        <<: *VELOCITY_Z_OUT
+    - step: "move joint6"
       arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_LF_TAKE_RIGHT_POSITION, *JOINT4_UP_POSITION, *JOINT5_REVERSAL_POSITION, *JOINT6_REVERSAL_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "REVERSAL Z IN "
+        <<: *ACCESS_STORE
+        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_UP_POSITION ]
+    - step: "REVERSAL OUT"
       reversal:
-        <<: *VELOCITY_Z_OUT
-    - step: "arm wait"
+        <<: *VELOCITY_STOP
+    - step: "change stone"
+      stone_num:
+        change: "-1"
+    - step: "move to exchange wait state"
       arm:
-        <<: *PICK_WAIT
+        <<: *EXCHANGE_POSITION
 
-  NEW_LF_SMALL_ISLAND:
-    - step: "arm get  second stone"
+  TAKE_WHEN_TWO_STONE:
+    - step: "island gimbal"
+      gimbal:
+        <<: *ISLAND_POS
+    - step: "joint7 up"
+      joint7:
+        <<: *JOINT7_UP_POSITION_NO_DELAY
+    - step: "move down to get stone"
       arm:
-        joints: [ *JOINT1_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_DOWN_NO_DELAY_POSITION
-    - step: "change gripper state"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "REVERSAL Z IN "
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "REVERSAL OUT"
       reversal:
-        <<: *VELOCITY_Z_IN
-  NEW_LF_SMALL_ISLAND0:
-    - step: "arm get  second stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "move arm up"
+        <<: *POSITION_Z_OUT
+    - step: "move up with stone and ready to get stone"
       arm:
-        joints: [ *JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_BACK_ISLAND_STONE_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_UP_POSITION
-    - step: "move joint1 and 3"
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "move joint2 to get stone"
       arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
+        <<: *ACCESS_STORE
+        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    - step: "REVERSAL Z OUT"
       reversal:
-        <<: *VELOCITY_Z_IN_SLOWLY
+        <<: *VELOCITY_Z_OUT
     - step: "move joint6"
       arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "REVERSAL Z IN "
+        <<: *ACCESS_STORE
+        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_UP_POSITION ]
+    - step: "REVERSAL OUT"
       reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move joint6"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
+        <<: *VELOCITY_STOP
     - step: "change stone"
       stone_num:
-        change: "+1"
-    - step: "arm wait"
+        change: "-1"
+    - step: "move to exchange wait state"
       arm:
-        <<: *HOME_TWO_STONE
+        <<: *EXCHANGE_POSITION
 
-
-
-    ###################    MID    ################
-  SMALL_ISLAND:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
-    - step: "arm wait"
-      arm:
-        <<: *HOME_ZERO_STONE_WITH_NO_STONE_IN_HAND
-  SMALL_ISLAND0:
-    - step: "chassis back"
-      chassis:
-        <<: *CHASSIS_BACKWARD_7
+  TAKE_WHEN_THREE_STONE:
     - step: "island gimbal"
       gimbal:
         <<: *ISLAND_POS
-    - step: "arm wait"
+    - step: "joint7 up"
+      joint7:
+        <<: *JOINT7_UP_POSITION_NO_DELAY
+    - step: "move down to get stone"
       arm:
-        <<: *PICK_WAIT
-    - step: "add scenes"
-      scene_name: MID_SMALL_ISLAND
-    - step: "arm ready to get stone"
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "REVERSAL OUT"
+      reversal:
+        <<: *POSITION_Z_OUT
+    - step: "move up with stone and ready to get stone"
       arm:
-        joints: [ *JOINT1_READY_TO_GET_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "delete_scene"
-  SMALL_ISLAND000:
-    - step: "change gripper state"
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
-    - step: "arm get stone"
-      arm:
-        joints: [ *JOINT1_GET_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "arm up stone"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "arm right stone"
+    - step: "move joint2 to get stone"
       arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_TAKE_RIGHT_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "reversal stone"
+        <<: *ACCESS_STORE
+        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    - step: "REVERSAL Z OUT"
+      reversal:
+        <<: *VELOCITY_Z_OUT
+    - step: "move joint6"
       arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_TAKE_RIGHT_POSITION, *JOINT4_UP_POSITION, *JOINT5_REVERSAL_POSITION, *JOINT6_REVERSAL_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "arm back"
+        <<: *ACCESS_STORE
+        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_UP_POSITION ]
+    - step: "REVERSAL OUT"
+      reversal:
+        <<: *VELOCITY_STOP
+    - step: "change stone"
+      stone_num:
+        change: "-1"
+    - step: "move to exchange wait state"
       arm:
-        <<: *PICK_WAIT
-
+        <<: *EXCHANGE_POSITION
 
-  NEW_SMALL_ISLAND:
+  ########### STORE ############
+  STORE_WHEN_ZERO_STONE:
     - step: "island gimbal"
       gimbal:
         <<: *ISLAND_POS
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_DOWN_NO_DELAY_POSITION
-    - step: "arm ready to get stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *OPEN_GRIPPER
-
-  NEW_SMALL_ISLAND0:
-    - step: "arm up stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "arm back stone"
+    - step: "prepare"
       arm:
-        joints: [ *JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_BACK_ISLAND_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_UP_POSITION
-    - step: "move joint1 and 3"
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "ready to store"
       arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
+        <<: *ACCESS_STORE
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, *JOINT6_UP_POSITION ]
+    - step: "reversal z in"
       reversal:
-        <<: *VELOCITY_Z_IN_SLOWLY
-    - step: "move joint6"
+        <<: *VELOCITY_Z_IN
+    - step: "store"
       arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "change gripper state"
+        <<: *ACCESS_STORE
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "REVERSAL Z IN "
+    - step: "reversal z stop"
       reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move joint6"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "change stone"
-      stone_num:
-        change: "+1"
-    - step: "home"
+        <<: *VELOCITY_STOP
+    - step: "move out arm"
       arm:
-        <<: *HOME_ONE_STONE
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
 
-    ###################    RT    ################
-  RT_SMALL_ISLAND0:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
+  STORE_WHEN_ONE_STONE:
     - step: "island gimbal"
       gimbal:
         <<: *ISLAND_POS
-    - step: "arm wait"
-      arm:
-        <<: *PICK_WAIT
-  RT_SMALL_ISLAND00:
-    - step: "add scenes"
-      scene_name: MID_SMALL_ISLAND
-    - step: "arm ready"
+    - step: "prepare"
       arm:
-        joints: [ *JOINT1_READY_TO_GET_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "arm get stone"
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "ready to store"
       arm:
-        joints: [ *JOINT1_GET_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
+        <<: *ACCESS_STORE
+        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_UP_POSITION ]
+    - step: "reversal z in"
       reversal:
         <<: *VELOCITY_Z_IN
-    - step: "arm up stone"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "arm up stone"
+    - step: "store"
       arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_RT_TAKE_RIGHT_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "reversal stone"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_RT_TAKE_RIGHT_POSITION, *JOINT4_UP_POSITION, *JOINT5_REVERSAL_POSITION, *JOINT6_REVERSAL_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "REVERSAL Z IN "
+        <<: *ACCESS_STORE
+        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "reversal z stop"
       reversal:
-        <<: *VELOCITY_Z_OUT
-    - step: "arm back"
+        <<: *VELOCITY_STOP
+    - step: "move out arm"
       arm:
-        <<: *PICK_WAIT
-
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ]
 
-  NEW_RT_SMALL_ISLAND:
-    - step: "arm ready"
+  STORE_WHEN_TWO_STONE:
+    - step: "island gimbal"
+      gimbal:
+        <<: *ISLAND_POS
+    - step: "prepare"
       arm:
-        joints: [ *JOINT1_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_DOWN_NO_DELAY_POSITION
-    - step: "change gripper state"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "REVERSAL Z IN "
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "ready to store"
+      arm:
+        <<: *ACCESS_STORE
+        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, *JOINT6_UP_POSITION ]
+    - step: "reversal z in"
       reversal:
         <<: *VELOCITY_Z_IN
-  NEW_RT_SMALL_ISLAND0:
-    - step: "arm up stone"
+    - step: "store"
       arm:
-        joints: [ *JOINT1_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "arm up stone"
+        <<: *ACCESS_STORE
+        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "reversal z stop"
+      reversal:
+        <<: *VELOCITY_STOP
+    - step: "move out arm"
       arm:
-        joints: [ *JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_BACK_ISLAND_STONE_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "joint7"
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+
+  EXCHANGE_WAIT:
+    - step: "joint7 up"
       end_effector:
         <<: *JOINT7_UP_POSITION
-    - step: "move joint1 and 3"
-      arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN_SLOWLY
-    - step: "move joint6"
+    - step: "exchange wait"
       arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
+        joints: [ *JOINT1_ONE_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_LF_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
         common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "change stone"
-      stone_num:
-        change: "+1"
-    - step: "arm wait"
-      arm:
-        <<: *HOME_THREE_STONE
-      ###################    SMALL_ISLAND    ####################
-      ###################    STORE    ####################
-  STORE_WHEN_ZERO_STONE0:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "move joint1 and 3"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move joint6"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "avoid collision"
-      arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_AVOID_LINE_COLLISION_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_HOLD_STONE_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "move joint6"
-      arm:
-        <<: *HOME_ONE_STONE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "change stone"
-      stone_num:
-        change: "+1"
-
-  STORE_WHEN_ONE_STONE0:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "move joint1 and 3"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move joint6"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "avoid collision"
-      arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_AVOID_LINE_COLLISION_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_HOLD_STONE_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "home two stone"
-      arm:
-        <<: *HOME_TWO_STONE
-    - step: "change stone"
-      stone_num:
-        change: "+1"
-
-  STORE_WHEN_TWO_STONE0:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "move joint1 and 3"
-      arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "avoid collision"
-      arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_AVOID_LINE_COLLISION_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "home two stone"
-      arm:
-        <<: *HOME_THREE_STONE
-    - step: "REVERSAL STOP "
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "change stone"
-      stone_num:
-        change: "+1"
-
-      ###################   TAKE   ####################
-  TAKE_WHEN_ONE_STONE0:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
-    - step: "avoid collision"
-      arm:
-        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_AVOID_LINE_COLLISION_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "move joint1 and 3 and 6"
-      arm:
-        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_REVERSAL_POSITION, *JOINT3_TAKE_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *MAX_TOLERANCE
-    - step: "REVERSAL OUT"
-      reversal:
-        <<: *POSITION_Z_OUT
-    - step: "move joint1 and 3 and 6"
-      arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_REVERSAL_POSITION, *JOINT3_TAKE_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "move joint2 to get stone"
-      arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_TAKE_STONE_POSITION, *JOINT3_TAKE_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_MORE_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "REVERSAL Z OUT"
-      reversal:
-        <<: *VELOCITY_Z_OUT
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_TAKE_TWO_STONE_POSITION, *JOINT2_TAKE_STONE_POSITION, *JOINT3_TAKE_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_HOLD_STONE_POSITION ]
-        common:
-          <<: *JOINT6_NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL OUT"
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "change stone"
-      stone_num:
-        change: "-1"
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_MID_NO_DELAY_POSITION
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_MID_LESS_NO_DELAY_POSITION
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_MID_MORE_NO_DELAY_POSITION
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_DOWN_NO_DELAY_POSITION
-    - step: "move to exchange wait state"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_REVERSAL_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-
-  TAKE_WHEN_TWO_STONE0:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
-    - step: "avoid collision"
-      arm:
-        joints: [ *JOINT1_TAKE_ONE_STONE_POSITION, *JOINT2_AVOID_LINE_COLLISION_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "move joint1 and 3 and 6"
-      arm:
-        joints: [ *JOINT1_TAKE_TWO_STONE_POSITION, *JOINT2_REVERSAL_POSITION, *JOINT3_TAKE_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *MAX_TOLERANCE
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "move joint2 to get stone"
-      arm:
-        joints: [ *JOINT1_TAKE_TWO_STONE_POSITION, *JOINT2_TAKE_STONE_POSITION, *JOINT3_TAKE_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_MORE_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "DELAY 0.1"
-      end_effector:
-        <<: *DELAY_0_1
-    - step: "REVERSAL OUT"
-      reversal:
-        <<: *VELOCITY_Z_OUT
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_TAKE_TWO_STONE_POSITION, *JOINT2_TAKE_STONE_POSITION, *JOINT3_TAKE_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_HOLD_STONE_POSITION ]
-        common:
-          <<: *JOINT6_NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL OUT"
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "change stone"
-      stone_num:
-        change: "-1"
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_MID_NO_DELAY_POSITION
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_MID_LESS_NO_DELAY_POSITION
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_MID_MORE_NO_DELAY_POSITION
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_DOWN_NO_DELAY_POSITION
-    - step: "move to exchange wait state"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_REVERSAL_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-
-  TAKE_WHEN_THREE_STONE0:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
-    - step: "avoid collision"
-      arm:
-        joints: [ *JOINT1_TAKE_THREE_STONE_POSITION, *JOINT2_AVOID_LINE_COLLISION_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "move joint1 "
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_REVERSAL_POSITION, *JOINT3_TAKE_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "move joint2 to get stone"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_BACK_POSITION, *JOINT3_TAKE_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "DELAY 0.1"
-      end_effector:
-        <<: *DELAY_0_1
-    - step: "move joint2 to get stone"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_TAKE_STONE_POSITION, *JOINT3_TAKE_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "REVERSAL OUT"
-      reversal:
-        <<: *VELOCITY_Z_OUT
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_TAKE_STONE_POSITION, *JOINT3_TAKE_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_HOLD_STONE_POSITION ]
-        common:
-          <<: *JOINT6_NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL OUT"
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "change stone"
-      stone_num:
-        change: "-1"
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_MID_NO_DELAY_POSITION
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_MID_LESS_NO_DELAY_POSITION
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_MID_MORE_NO_DELAY_POSITION
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_DOWN_NO_DELAY_POSITION
-    - step: "move to exchange wait state"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_REVERSAL_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-
-  ADD:
-    - step: "add scenes"
-      scene_name: MID_SMALL_ISLAND
-
-  #####################################JOINT7######################################
-  NEW_THREE_STONE_SMALL_ISLAND:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_DOWN_NO_DELAY_POSITION
-    - step: "arm ready to get stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *OPEN_GRIPPER
-
-  NEW_THREE_STONE_SMALL_ISLAND0:
-    - step: "move arm up"
-      arm:
-        joints: [ *JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_BACK_ISLAND_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "arm back stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_UP_POSITION
-    - step: "move joint1 and 3"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN_SLOWLY
-    - step: "move joint6"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move joint6"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "change stone"
-      stone_num:
-        change: "+1"
-    - step: "arm ready to get  second stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_DOWN_POSITION
-    - step: "change gripper state"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "arm get  second stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "move arm up"
-      arm:
-        joints: [ *JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_BACK_ISLAND_STONE_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "arm back stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_UP_POSITION
-    - step: "move joint1 and 3"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN_SLOWLY
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "change stone"
-      stone_num:
-        change: "+1"
-    - step: "arm ready to get last stone"
-      arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_DOWN_POSITION
-    - step: "change gripper state"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "arm ready get stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "arm get stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move arm up"
-      arm:
-        joints: [ *JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_BACK_ISLAND_STONE_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "arm back stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_UP_POSITION
-    - step: "move joint1 and 3"
-      arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN_SLOWLY
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "change stone"
-      stone_num:
-        change: "+1"
-    - step: "change gripper state"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "home three stone"
-      arm:
-        <<: *HOME_THREE_STONE
-    - step: "REVERSAL OUT"
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "change stone"
-      stone_num:
-        change: "+3"
-
-
-
-
-
-  TWO_STONE_SMALL_ISLAND:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_DOWN_NO_DELAY_POSITION
-    - step: "arm ready to get stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *OPEN_GRIPPER
-
-  TWO_STONE_SMALL_ISLAND0:
-    - step: "move arm up"
-      arm:
-        joints: [ *JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_BACK_ISLAND_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "arm back stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_UP_POSITION
-    - step: "move joint1 and 3"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN_SLOWLY
-    - step: "move joint6"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move joint6"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "change stone"
-      stone_num:
-        change: "+1"
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN_SLOWLY
-    - step: "arm ready to get  second stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_DOWN_POSITION
-    - step: "change gripper state"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "arm get  second stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_GET_ISLAND_STONE_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-  TWO_STONE_SMALL_ISLAND00:
-    - step: "move arm up"
-      arm:
-        joints: [ *JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION, *JOINT2_PARALLEL_BACK_ISLAND_STONE_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN_QUICKLY
-    - step: "arm back stone"
-      arm:
-        joints: [ *JOINT1_PARALLEL_UP_ISLAND_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "joint7"
-      end_effector:
-        <<: *JOINT7_UP_POSITION
-    - step: "move joint1 and 3"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN_SLOWLY
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_PARALLEL_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "change stone"
-      stone_num:
-        change: "+1"
-    - step: "move home"
-      arm:
-        joints: [ *JOINT1_ONE_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_STOP
-  TWO_STONE_SMALL_ISLAND000:
-    - step: "CHASSIS 180 "
-      chassis:
-        <<: *CHASSIS_180
-    - step: "GIMBAL HOME"
-      gimbal:
-        <<: *SIDE_POS
-  THREE_STONE_SMALL_ISLAND:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_NO_DELAY_POSITION
-    - step: "arm wait"
-      arm:
-        <<: *HOME_ZERO_STONE_WITH_NO_STONE_IN_HAND
-  THREE_STONE_SMALL_ISLAND0:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "chassis back"
-      chassis:
-        <<: *CHASSIS_BACKWARD_7
-    - step: "arm wait"
-      arm:
-        <<: *PICK_WAIT
-    - step: "arm ready to get stone"
-      arm:
-        joints: [ *JOINT1_READY_TO_GET_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-  THREE_STONE_SMALL_ISLAND00:
-    - step: "add scenes"
-      scene_name: MID_SMALL_ISLAND
-    - step: "change gripper state"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "arm get  first stone"
-      arm:
-        joints: [ *JOINT1_GET_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "arm up stone"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "right stone"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_TAKE_RIGHT_POSITION, *JOINT4_UP_POSITION,*JOINT5_REVERSAL_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "reversal stone"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_MID_TAKE_RIGHT_POSITION, *JOINT4_UP_POSITION,*JOINT5_REVERSAL_POSITION, *JOINT6_REVERSAL_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "move joint1 and 3"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_BACK_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move joint6"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_BACK_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "arm ready to get  second stone"
-      arm:
-        joints: [ *JOINT1_READY_TO_GET_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "REVERSAL STOP "
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "move arm down to gain stone"
-      arm:
-        joints: [ *JOINT1_GET_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "move arm up"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_LF_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "right stone"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_LF_TAKE_RIGHT_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "reversal stone"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_LF_TAKE_RIGHT_POSITION, *JOINT4_UP_POSITION, *JOINT5_REVERSAL_POSITION, *JOINT6_REVERSAL_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "move joint1 and 3"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_BACK_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move joint6"
-      arm:
-        joints: [ *STONE_STORAGE_HIGH_POSITION, *JOINT2_BACK_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_HOLD_STONE_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "arm ready"
-      arm:
-        joints: [ *JOINT1_READY_TO_GET_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "REVERSAL STOP "
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "arm get stone"
-      arm:
-        joints: [ *JOINT1_GET_STONE_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "arm up stone"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_RT_TAKE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "reversal stone"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_RECENTLY_POSITION, *JOINT3_RT_TAKE_RIGHT_POSITION, *JOINT4_UP_POSITION, *JOINT5_REVERSAL_POSITION, *JOINT6_REVERSAL_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL stop "
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "move joint1 and 3"
-      arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "REVERSAL Z IN "
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "move joint6"
-      arm:
-        joints: [ *JOINT1_THREE_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_LEFT_POSITION, *JOINT6_BACK_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-    - step: "change gripper state"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "home three stone"
-      arm:
-        <<: *HOME_THREE_STONE
-    - step: "REVERSAL Z IN with delay "
-      reversal:
-        <<: *VELOCITY_Z_IN_DELAY
-  THREE_STONE_SMALL_ISLAND000:
-    - step: "home three stone"
-      arm:
-        <<: *HOME_THREE_STONE
-    - step: "change gripper state"
-      gripper:
-        <<: *CLOSE_GRIPPER
-
-    #  TEST MULTI DOF CONTROLLER
-  PITCH_PI:
-    - step: "TEST"
-      reversal:
-        <<: *POSITION_PITCH_REVERSAL_PI
-  PITCH_PI_2:
-    - step: "TEST"
-      reversal:
-        <<: *POSITION_PITCH_REVERSAL_PI_2
-
-  EXCHANGE_AUTO:
-    - step: "auto exchange"
-      arm:
-        frame: exchanger
-        position: [ -0.5, 0., 0. ]
-        rpy: [ 0., -1.57, 0. ]
-        rpy_rectify: [ 0.01,0.03,0.03 ]
-        tolerance_position: 0.2
-        tolerance_orientation: 0.2
-        spacial_shape: SPHERE
-        radius: 0.3
-        point_resolution: 0.90
-        max_planning_times: 100
-        common:
-          <<: *SLOWLY_LOW_TIMEOUT
-  JOINT_TWO_BACK:
-    - step: "JOINT_TWO_BACK"
-      arm:
-        joints: [ "KEEP", *JOINT2_BACK_POSITION, "KEEP", "KEEP", "KEEP", "KEEP" ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-
-  JOINT_TWO_AND_THREE_BACK:
-    - step: "JOINT_TWO_AND_THREE_BACK"
-      arm:
-        joints: [ "KEEP", *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, "KEEP", "KEEP", "KEEP" ]
-        common:
-          <<: *SLOWLY_LOW_TIMEOUT
-        tolerance:
-          <<: *BIGGER_TOLERANCE

From 564d7318360a124d6b1e8b201b26a1d671614c1d Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Fri, 14 Jul 2023 18:11:43 +0800
Subject: [PATCH 013/147] Small change.

---
 engineer_middleware/config/steps_list.yaml | 16 +++++++++++++---
 1 file changed, 13 insertions(+), 3 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 1834730..a02fa5d 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -58,7 +58,7 @@ common:
   joint2:
     mechanical:
       back_position: &JOINT2_BACK_POSITION
-                       0.05
+                       0.0001
       furthest_position: &JOINT2_FURTHEST_POSITION
                            0.40045
     store:
@@ -95,7 +95,7 @@ common:
   joint4:
     mechanical:
       up_position: &JOINT4_UP_POSITION
-                     0.1
+                     0.000001
       down_position: &JOINT4_DOWN_POSITION
                        1.07
 
@@ -322,6 +322,16 @@ common:
 
 
 steps_list:
+  ###############   GRIPPER   ###############
+  CLOSE_GRIPPER:
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
+
+  OPEN_GRIPPER:
+    - step: "open  gripper"
+      gripper:
+        <<: *OPEN_GRIPPER
   ###############   HOME   ###############
 
   HOME_ZERO_STONE:
@@ -741,7 +751,7 @@ steps_list:
         <<: *JOINT7_UP_POSITION
     - step: "exchange wait"
       arm:
-        joints: [ *JOINT1_ONE_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_LF_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
+        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_LF_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:

From a899c2567cafd59ad3eea491bb0872d0b00b75f4 Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Sat, 15 Jul 2023 08:09:31 +0800
Subject: [PATCH 014/147] Add auto_exchange.h like motion.h.

---
 engineer_middleware/CMakeLists.txt            |   2 +-
 .../engineer_middleware/auto_exchange.h       | 115 ++++++++++++++++++
 2 files changed, 116 insertions(+), 1 deletion(-)
 create mode 100644 engineer_middleware/include/engineer_middleware/auto_exchange.h

diff --git a/engineer_middleware/CMakeLists.txt b/engineer_middleware/CMakeLists.txt
index d0e5c73..4c94c69 100644
--- a/engineer_middleware/CMakeLists.txt
+++ b/engineer_middleware/CMakeLists.txt
@@ -69,7 +69,7 @@ include_directories(
 
 add_executable(${PROJECT_NAME}
         src/middleware.cpp
-        src/engineer_middleware.cpp)
+        src/engineer_middleware.cpp include/engineer_middleware/auto_exchange.h include/engineer_middleware/auto_exchange.h)
 
 add_dependencies(${PROJECT_NAME}
         ${catkin_EXPORTED_TARGETS}
diff --git a/engineer_middleware/include/engineer_middleware/auto_exchange.h b/engineer_middleware/include/engineer_middleware/auto_exchange.h
new file mode 100644
index 0000000..03bcaf8
--- /dev/null
+++ b/engineer_middleware/include/engineer_middleware/auto_exchange.h
@@ -0,0 +1,115 @@
+//
+// Created by lsy on 23-7-15.
+//
+#pragma once
+#include <rm_common/ros_utilities.h>
+
+namespace auto_exchange
+{
+enum FindProcess
+{
+  SWING,
+  FOUND,
+  LOCKED
+};
+
+enum AdjustProcess
+{
+  COMPUTER,
+  MOVE_CHASSIS
+};
+
+enum ServoMoveProcess
+{
+  YZ,
+  ROLL_YAW,
+  XYZ,
+  PITCH,
+  PUSH,
+  DONE
+};
+
+enum MotionMoveProcess
+{
+  SPHERE,
+  LINE,
+  POINT,
+  ACHIEVE
+};
+
+enum ExchangeProcess
+{
+  FIND,
+  PRE_ADJUST,
+  MOVE,
+  POST_ADJUST,
+  FINISH
+};
+
+class JointInfo
+{
+public:
+  JointInfo(XmlRpc::XmlRpcValue& joint)
+  {
+  }
+  bool judgeJointPosition()
+  {
+    return (abs(current_position - offset - min_position) <= near_tolerance_ ||
+            abs(max_position + offset - current_position) <= near_tolerance_) ?
+               true :
+               false;
+  }
+
+private:
+  int move_direct;
+  double offset, max_position, min_position, current_position, max_vel, near_tolerance_;
+};
+
+class ProgressBase
+{
+public:
+  ProgressBase(XmlRpc::XmlRpcValue& progress)
+  {
+    time_out_ = xmlRpcGetDouble(progress["common"], "timeout", 1e10);
+  }
+  virtual void nextProcess();
+  virtual bool isFinish() = 0;
+  bool checkTimeout(ros::Duration period)
+  {
+    if (period.toSec() > time_out_)
+    {
+      ROS_ERROR("Step timeout,it should be finish in %f seconds", time_out_);
+      return false;
+    }
+    return true;
+  }
+
+protected:
+  double time_out_{};
+};
+
+class Find
+{
+};
+
+class ProAdjust
+{
+};
+
+class ServoMove
+{
+};
+
+class MotionMove
+{
+};
+
+class PostAdjust
+{
+};
+
+class Finish
+{
+};
+
+}  // namespace auto_exchange

From 0151e4e0e435f0d2c8ff26056e1392f84b2bbd89 Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Sat, 15 Jul 2023 08:40:42 +0800
Subject: [PATCH 015/147] Complete JointInfo class.

---
 .../engineer_middleware/auto_exchange.h       | 20 +++++++++++++++----
 1 file changed, 16 insertions(+), 4 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/auto_exchange.h b/engineer_middleware/include/engineer_middleware/auto_exchange.h
index 03bcaf8..df69e70 100644
--- a/engineer_middleware/include/engineer_middleware/auto_exchange.h
+++ b/engineer_middleware/include/engineer_middleware/auto_exchange.h
@@ -51,18 +51,30 @@ class JointInfo
 public:
   JointInfo(XmlRpc::XmlRpcValue& joint)
   {
+    //      joint1:
+    //        offset: 0.01
+    //        range: [0.,1.]
+    //        near_tolerance: 0.03
+    offset_ = xmlRpcGetDouble(joint, "offset", 0.);
+    near_tolerance_ = xmlRpcGetDouble(joint, "near_tolerance", 0.05);
+    ROS_ASSERT(joint["range"].getType() == XmlRpc::XmlRpcValue::TypeArray);
+    min_position_ = xmlRpcGetDouble(joint["range"], 0);
+    max_position_ = xmlRpcGetDouble(joint["range"], 1);
   }
   bool judgeJointPosition()
   {
-    return (abs(current_position - offset - min_position) <= near_tolerance_ ||
-            abs(max_position + offset - current_position) <= near_tolerance_) ?
+    return (abs(current_position_ - offset_ - min_position_) <= near_tolerance_ ||
+            abs(max_position_ + offset_ - current_position_) <= near_tolerance_) ?
                true :
                false;
   }
+  int judgeMoveDirect()
+  {
+    return ((current_position_ - offset_) >= ((max_position_ - min_position_) / 2)) ? 1 : -1;
+  }
 
 private:
-  int move_direct;
-  double offset, max_position, min_position, current_position, max_vel, near_tolerance_;
+  double offset_, max_position_, min_position_, current_position_, max_vel_, near_tolerance_;
 };
 
 class ProgressBase

From 958318652cf1a62e5986bfcb9d11b792045cf1ac Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Sat, 15 Jul 2023 09:28:25 +0800
Subject: [PATCH 016/147] Decide to introduce commander into auto_exchange.h.

---
 .../engineer_middleware/auto_exchange.h       | 69 ++++++++++++++++---
 1 file changed, 61 insertions(+), 8 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/auto_exchange.h b/engineer_middleware/include/engineer_middleware/auto_exchange.h
index df69e70..b48ca10 100644
--- a/engineer_middleware/include/engineer_middleware/auto_exchange.h
+++ b/engineer_middleware/include/engineer_middleware/auto_exchange.h
@@ -3,6 +3,7 @@
 //
 #pragma once
 #include <rm_common/ros_utilities.h>
+#include <rm_common/decision/command_sender.h>
 
 namespace auto_exchange
 {
@@ -54,8 +55,10 @@ class JointInfo
     //      joint1:
     //        offset: 0.01
     //        range: [0.,1.]
+    //        max_vel: 0.03
     //        near_tolerance: 0.03
     offset_ = xmlRpcGetDouble(joint, "offset", 0.);
+    max_vel_ = xmlRpcGetDouble(joint, "max_vel", 1.0);
     near_tolerance_ = xmlRpcGetDouble(joint, "near_tolerance", 0.05);
     ROS_ASSERT(joint["range"].getType() == XmlRpc::XmlRpcValue::TypeArray);
     min_position_ = xmlRpcGetDouble(joint["range"], 0);
@@ -63,10 +66,8 @@ class JointInfo
   }
   bool judgeJointPosition()
   {
-    return (abs(current_position_ - offset_ - min_position_) <= near_tolerance_ ||
-            abs(max_position_ + offset_ - current_position_) <= near_tolerance_) ?
-               true :
-               false;
+    return abs(current_position_ - offset_ - min_position_) <= near_tolerance_ ||
+           abs(max_position_ + offset_ - current_position_) <= near_tolerance_;
   }
   int judgeMoveDirect()
   {
@@ -82,10 +83,14 @@ class ProgressBase
 public:
   ProgressBase(XmlRpc::XmlRpcValue& progress)
   {
-    time_out_ = xmlRpcGetDouble(progress["common"], "timeout", 1e10);
+    // progress:
+    //        timeout: 3.
+    time_out_ = xmlRpcGetDouble(progress, "timeout", 1e10);
   }
-  virtual void nextProcess();
-  virtual bool isFinish() = 0;
+  virtual void nextProcess(bool flag);
+  virtual void manageProcess(bool flag);
+  virtual void run();
+  virtual void printProcess();
   bool checkTimeout(ros::Duration period)
   {
     if (period.toSec() > time_out_)
@@ -97,11 +102,59 @@ class ProgressBase
   }
 
 protected:
+  int process_;
+  bool is_finish_{ false };
   double time_out_{};
+  ros::Time start_time_;
 };
 
-class Find
+class Find : public ProgressBase
 {
+public:
+  Find(XmlRpc::XmlRpcValue& find) : ProgressBase(find)
+  {
+    //        find:
+    //          pitch:
+    //        offset: 0.01
+    //        range: [0.,1.]
+    //        max_vel: 0.03
+    //        near_tolerance: 0.03
+    //          confirm_lock_time
+    process_ = SWING;
+    ROS_ASSERT(find["yaw"].getType() == XmlRpc::XmlRpcValue::TypeStruct);
+    ROS_ASSERT(find["yaw"].getType() == XmlRpc::XmlRpcValue::TypeStruct);
+    yaw_ = new JointInfo(find["yaw"]);
+    pitch_ = new JointInfo(find["pitch_"]);
+    confirm_lock_time_ = xmlRpcGetDouble(find, "confirm_lock_time", 10);
+  }
+  void nextProcess(bool flag) override
+  {
+    process_++;
+    printProcess();
+  }
+  void manageProcess(bool flag) override
+  {
+    if (process_ != LOCKED)
+      nextProcess(flag);
+    else
+      is_finish_ = true;
+  }
+  void printProcess() override
+  {
+    if (process_ == SWING)
+      ROS_INFO_STREAM("SWING");
+    else if (process_ == FOUND)
+      ROS_INFO_STREAM("FOUND");
+    else if (process_ == LOCKED)
+      ROS_INFO_STREAM("LOCKED");
+  }
+  void run() override
+  {
+  }
+
+private:
+  JointInfo *yaw_{}, *pitch_{};
+  double search_angle_{}, confirm_lock_time_{};
 };
 
 class ProAdjust

From 5af7c2a346992b20c5f7a68f5f69ceb77e94d7a8 Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Sat, 15 Jul 2023 12:28:38 +0800
Subject: [PATCH 017/147] Add ServoMove.

---
 .../engineer_middleware/auto_exchange.h       | 197 +++++++++++++++++-
 1 file changed, 189 insertions(+), 8 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/auto_exchange.h b/engineer_middleware/include/engineer_middleware/auto_exchange.h
index b48ca10..70037e8 100644
--- a/engineer_middleware/include/engineer_middleware/auto_exchange.h
+++ b/engineer_middleware/include/engineer_middleware/auto_exchange.h
@@ -4,6 +4,9 @@
 #pragma once
 #include <rm_common/ros_utilities.h>
 #include <rm_common/decision/command_sender.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/buffer.h>
+#include <rm_common/ori_tool.h>
 
 namespace auto_exchange
 {
@@ -81,14 +84,15 @@ class JointInfo
 class ProgressBase
 {
 public:
-  ProgressBase(XmlRpc::XmlRpcValue& progress)
+  ProgressBase(XmlRpc::XmlRpcValue& progress, tf2_ros::Buffer& tf_buffer) : tf_buffer_(tf_buffer)
   {
     // progress:
     //        timeout: 3.
     time_out_ = xmlRpcGetDouble(progress, "timeout", 1e10);
   }
-  virtual void nextProcess(bool flag);
-  virtual void manageProcess(bool flag);
+  virtual void init();
+  virtual void nextProcess();
+  virtual void manageProcess();
   virtual void run();
   virtual void printProcess();
   bool checkTimeout(ros::Duration period)
@@ -102,6 +106,7 @@ class ProgressBase
   }
 
 protected:
+  tf2_ros::Buffer& tf_buffer_;
   int process_;
   bool is_finish_{ false };
   double time_out_{};
@@ -111,7 +116,7 @@ class ProgressBase
 class Find : public ProgressBase
 {
 public:
-  Find(XmlRpc::XmlRpcValue& find) : ProgressBase(find)
+  Find(XmlRpc::XmlRpcValue& find, tf2_ros::Buffer& tf_buffer) : ProgressBase(find, tf_buffer)
   {
     //        find:
     //          pitch:
@@ -127,15 +132,20 @@ class Find : public ProgressBase
     pitch_ = new JointInfo(find["pitch_"]);
     confirm_lock_time_ = xmlRpcGetDouble(find, "confirm_lock_time", 10);
   }
-  void nextProcess(bool flag) override
+  void init() override
+  {
+    is_finish_ = false;
+    process_ = { SWING };
+  }
+  void nextProcess() override
   {
     process_++;
     printProcess();
   }
-  void manageProcess(bool flag) override
+  void manageProcess() override
   {
     if (process_ != LOCKED)
-      nextProcess(flag);
+      nextProcess();
     else
       is_finish_ = true;
   }
@@ -161,8 +171,179 @@ class ProAdjust
 {
 };
 
-class ServoMove
+class ServoMove : public ProgressBase
 {
+public:
+  ServoMove(XmlRpc::XmlRpcValue& servo_move, ros::NodeHandle& nh, tf2_ros::Buffer& tf_buffer)
+    : ProgressBase(servo_move, tf_buffer)
+  {
+    //      servo_move:
+    //        xyz_offset: [ 0.08, 0., -0.04]
+    //        link7_length: 0.1
+    //        servo_p: [ 6., 6., 7., 3., 2., 1. ]
+    //        servo_error_tolerance: [ 0.01, 0.01, 0.01, 0.01, 0.01, 0.01 ]
+    ROS_ASSERT(servo_move["xyz_offset"].getType() == XmlRpc::XmlRpcValue::TypeArray);
+    ROS_ASSERT(servo_move["servo_p"].getType() == XmlRpc::XmlRpcValue::TypeArray);
+    ROS_ASSERT(servo_move["servo_error_tolerance"].getType() == XmlRpc::XmlRpcValue::TypeArray);
+    ROS_ASSERT(servo_move["link7_length"].getType() == XmlRpc::XmlRpcValue::TypeDouble);
+    process_ = { YZ };
+    enter_servo_move_.data = false;
+    xyz_offset_.resize(3, 0.);
+    servo_p_.resize(6, 0.);
+    servo_errors_.resize(6, 0.);
+    servo_scales_.resize(6, 0.);
+    servo_error_tolerance_.resize(6, 0.01);
+    for (int i = 0; i < (int)xyz_offset_.size(); ++i)
+      xyz_offset_[i] = servo_move["xyz_offset"][i];
+    for (int i = 0; i < (int)servo_p_.size(); ++i)
+    {
+      servo_p_[i] = servo_move["servo_p"][i];
+      servo_error_tolerance_[i] = servo_move["servo_error_tolerance"][i];
+    }
+    exchanger_tf_update_pub_ = nh.advertise<std_msgs::Bool>("/is_update_exchanger", 1);
+  }
+  void init() override
+  {
+    is_finish_ = false;
+    process_ = { YZ };
+    enter_servo_move_.data = false;
+    initComputerValue();
+  }
+
+  void nextProcess() override
+  {
+    process_++;
+    printProcess();
+  }
+  void manageProcess() override
+  {
+    if (process_ != DONE)
+      nextProcess();
+    else
+      is_finish_ = true;
+  }
+  void printProcess() override
+  {
+    if (process_ == YZ)
+      ROS_INFO_STREAM("YZ");
+    else if (process_ == ROLL_YAW)
+      ROS_INFO_STREAM("ROLL_YAW");
+    else if (process_ == XYZ)
+      ROS_INFO_STREAM("XYZ");
+    else if (process_ == PITCH)
+      ROS_INFO_STREAM("PITCH");
+    else if (process_ == PUSH)
+      ROS_INFO_STREAM("PUSH");
+  }
+  void run() override
+  {
+    enter_servo_move_.data = true;
+    exchanger_tf_update_pub_.publish(enter_servo_move_);
+    if (!is_finish_)
+    {
+      computeServoMoveScale();
+      manageProcess();
+    }
+  }
+
+private:
+  void initComputerValue()
+  {
+    for (int i = 0; i < (int)servo_scales_.size(); ++i)
+    {
+      servo_errors_[i] = 0;
+      servo_scales_[i] = 0;
+    }
+  }
+  void computeServoMoveError()
+  {
+    double roll, pitch, yaw;
+    std::vector<double> errors;
+    geometry_msgs::TransformStamped tools2exchanger;
+    try
+    {
+      tools2exchanger = tf_buffer_.lookupTransform("tools_link", "exchanger", ros::Time(0));
+      quatToRPY(tools2exchanger.transform.rotation, roll, pitch, yaw);
+    }
+    catch (tf2::TransformException& ex)
+    {
+      ROS_WARN("%s", ex.what());
+    }
+    initComputerValue();
+    servo_errors_[0] = (process_ == PUSH ? (tools2exchanger.transform.translation.x) :
+                                           (tools2exchanger.transform.translation.x - xyz_offset_[0]));
+    servo_errors_[1] = tools2exchanger.transform.translation.y - xyz_offset_[1] + 0.06 * sin(roll + M_PI_2);
+    servo_errors_[2] = tools2exchanger.transform.translation.z - xyz_offset_[2];
+    servo_errors_[3] = roll;
+    servo_errors_[4] = pitch;
+    servo_errors_[5] = yaw;
+  }
+  void computeServoMoveScale(rm_common::JointPositionBinaryCommandSender* joint7_command_sender)
+  {
+    computeServoMoveError();
+    switch (process_)
+    {
+      case YZ:
+      {
+        for (int i = 1; i < 3; ++i)
+        {
+          servo_scales_[i] = servo_errors_[i] * servo_p_[i];
+        }
+      }
+      break;
+      case ROLL_YAW:
+      {
+        servo_scales_[3] = servo_errors_[3] * servo_p_[3];
+        servo_scales_[5] = servo_errors_[5] * servo_p_[5];
+      }
+      break;
+      case XYZ:
+      {
+        for (int i = 0; i < 3; ++i)
+          servo_scales_[i] = servo_errors_[i] * servo_p_[i];
+      }
+      break;
+      case PITCH:
+      {
+        joint7_command_sender->getMsg()->data = servo_errors_[4];
+      }
+      break;
+      case PUSH:
+      {
+        servo_scales_[0] = servo_errors_[0] * servo_p_[0];
+      }
+      break;
+    }
+  }
+  void manageServoMoveProcess()
+  {
+    int move_joint_num = 0, arrived_joint_num = 0;
+    for (int i = 0; i < (int)servo_scales_.size(); ++i)
+    {
+      if (servo_scales_[i] != 0)
+      {
+        move_joint_num++;
+        if (abs(servo_errors_[i]) <= servo_error_tolerance_[i])
+          arrived_joint_num++;
+      }
+    }
+    if (arrived_joint_num == move_joint_num)
+      nextProcess();
+    //        if (checkTimeout())
+    //        {
+    //            switch ()
+    //            {
+    //            }
+    //            nextProcess();
+    //            ROS_INFO_STREAM("TIME OUT");
+    //        }
+    //        else if (arrived_joint_num == move_joint_num)
+    //            nextProcess();
+  }
+  std_msgs::Bool enter_servo_move_{};
+  double link7_length_{};
+  ros::Publisher exchanger_tf_update_pub_;
+  std::vector<double> xyz_offset_{}, servo_p_{}, servo_errors_{}, servo_scales_{}, servo_error_tolerance_{};
 };
 
 class MotionMove

From 12b8d2c04c3971f8c652fd832406ba856faf8bda Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Sat, 15 Jul 2023 12:31:16 +0800
Subject: [PATCH 018/147] Fix Error.

---
 .../include/engineer_middleware/auto_exchange.h        | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/auto_exchange.h b/engineer_middleware/include/engineer_middleware/auto_exchange.h
index 70037e8..f118be0 100644
--- a/engineer_middleware/include/engineer_middleware/auto_exchange.h
+++ b/engineer_middleware/include/engineer_middleware/auto_exchange.h
@@ -278,7 +278,7 @@ class ServoMove : public ProgressBase
     servo_errors_[4] = pitch;
     servo_errors_[5] = yaw;
   }
-  void computeServoMoveScale(rm_common::JointPositionBinaryCommandSender* joint7_command_sender)
+  void computeServoMoveScale()
   {
     computeServoMoveError();
     switch (process_)
@@ -305,7 +305,7 @@ class ServoMove : public ProgressBase
       break;
       case PITCH:
       {
-        joint7_command_sender->getMsg()->data = servo_errors_[4];
+        joint7_msg_ = servo_errors_[4];
       }
       break;
       case PUSH:
@@ -340,8 +340,12 @@ class ServoMove : public ProgressBase
     //        else if (arrived_joint_num == move_joint_num)
     //            nextProcess();
   }
+  double getJoint7Msg()
+  {
+    return joint7_msg_;
+  }
   std_msgs::Bool enter_servo_move_{};
-  double link7_length_{};
+  double link7_length_{}, joint7_msg_{};
   ros::Publisher exchanger_tf_update_pub_;
   std::vector<double> xyz_offset_{}, servo_p_{}, servo_errors_{}, servo_scales_{}, servo_error_tolerance_{};
 };

From 5ed8a430c6767780c1a1a877121f67d33f143f55 Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Sat, 12 Aug 2023 08:35:38 +0800
Subject: [PATCH 019/147] test

---
 engineer_middleware/config/steps_list.yaml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index a02fa5d..bfa0dd7 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -164,9 +164,9 @@ common:
 
   gripper:
     open_gripper: &OPEN_GRIPPER
-      state: false
-    close_gripper: &CLOSE_GRIPPER
       state: true
+    close_gripper: &CLOSE_GRIPPER
+      state: false
 
   gimbal:
     back_pos: &BACK_POS

From beb074d1135fbcb36826ea3b2aa113490e82da4f Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Sat, 12 Aug 2023 08:48:40 +0800
Subject: [PATCH 020/147] record.

---
 engineer_middleware/config/steps_list.yaml    | 1284 +++++++++++++----
 .../engineer_middleware/auto_exchange.h       | 1038 ++++++++++---
 .../include/engineer_middleware/motion.h      |  149 +-
 .../include/engineer_middleware/step.h        |    2 +
 4 files changed, 1926 insertions(+), 547 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index a02fa5d..8033466 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -11,11 +11,19 @@ common:
     normally: &NORMALLY
       speed: 0.4
       accel: 0.3
-      timeout: 2.
+      timeout: 3
+    normally_high_timeout: &NORMALLY_HIGH_TIMEOUT
+      speed: 0.4
+      accel: 0.3
+      timeout: 4
     quickly: &QUICKLY
       speed: 0.8
       accel: 0.8
       timeout: 7.
+    little_quickly: &LITTLE_QUICKLY
+      speed: 0.6
+      accel: 0.6
+      timeout: 1.
     joint6_normally: &JOINT6_NORMALLY
       speed: 0.4
       accel: 0.3
@@ -35,101 +43,123 @@ common:
   joint1:
     mechanical:
       down_position: &JOINT1_DOWN_POSITION
-                       -0.09600
+                       0.
       up_position: &JOINT1_UP_POSITION
                      0.43
     home:
       zero_stone_position: &JOINT1_ZERO_STONE_POSITION
-                             0.08
+                             0.001
       one_stone_position: &JOINT1_ONE_STONE_POSITION
                             0.08
       two_stone_position: &JOINT1_TWO_STONE_POSITION
-                            0.153000
+                            0.145700
       three_stone_position: &JOINT1_THREE_STONE_POSITION
-                              0.4192
+                              0.35
     get_ore:
       ready_get_ore: &JOINT1_READY_TO_GET_ORE_POSITION
-                       0.40
+                       0.44
       get_ore: &JOINT1_GET_ORE_POSITION
-                 0.310500
+                 0.265
+      store_ore_position: &JOINT1_STORE_ORE_POSITION
+                            0.25
     exchange_position: &JOINT1_EXCHANGE_POSITION
-                         0.43
+                         0.4
+    ground_stone:
+      ready_to_get_ground_stone: &JOINT1_READY_TO_GET_GROUND_STONE
+                                   0.15184
+      get_ground_stone: &JOINT1_GET_GROUND_STONE
+                          0.02545
 
   joint2:
     mechanical:
       back_position: &JOINT2_BACK_POSITION
                        0.0001
       furthest_position: &JOINT2_FURTHEST_POSITION
-                           0.40045
+                           0.270000
     store:
       access_store_position: &JOINT2_ACCESS_STORE_POSITION
-                               0.0001
+                               0.00001
       ready_to_access_stone_position: &JOINT2_READY_TO_ACCESS_STONE_POSITION
-                                        0.025
+                                        0.06
     exchange_position: &JOINT2_EXCHANGE_POSITION
                          0.05
     get_ore_position: &JOINT2_GET_ORE_POSITION
-                        0.032590
+                        0.002
 
   joint3:
     mechanical:
       left_position: &JOINT3_LF_POSITION
-                       0.25
+                       0.28
       mid_position: &JOINT3_MID_POSITION
                       0.0001
       right_position: &JOINT3_RT_POSITION
                         -0.275
     get_ore:
       lf_get_ore_position: &JOINT3_LF_GET_ORE_POSITION
-                             0.275100
+                             0.2560
       mid_get_ore_position: &JOINT3_MID_GET_ORE_POSITION
-                              0.018900
+                              0.00
       rt_get_ore_position: &JOINT3_RT_GET_ORE_POSITION
-                             -0.237300
-    store_and_take:
-      store_stone_position: &JOINT3_ACCESS_STONE_POSITION
-                              0.195800
-
+                             -0.266600
+      lf_avoid_other_ore: &JOINT3_LF_AVOID_ORE
+                            0.3
+      mid_avoid_other_ore: &JOINT3_MID_AVOID_ORE
+                             0.1
+      rt_avoid_other_ore: &JOINT3_RT_AVOID_ORE
+                            -0.2
+      sky_store_position: &JOINT3_SKY_STORE_POSITION
+                            0.2515
+      gold_ore_position: &JOINT3_GOLD_ORE_POSITION
+                           0.24564
 
+    store_and_take:
+      store_stone_position: &JOINT3_STORE_STONE_POSITION
+                              0.187890
+      take_stone_position: &JOINT3_TAKE_STONE_POSITION
+                             0.255230
 
   joint4:
     mechanical:
       up_position: &JOINT4_UP_POSITION
-                     0.000001
+                     0.0001
+      mid_position: &JOINT4_MID_POSITION
+                      0.7
       down_position: &JOINT4_DOWN_POSITION
-                       1.07
-
+                       1.55
 
   joint5:
     mechanical:
       right_position: &JOINT5_RT_POSITION
-                        -1.5
+                        -1.54
       mid_position: &JOINT5_MID_POSITION
                       0.01
       left_position: &JOINT5_LF_POSITION
                        1.25
 
-
   joint6:
     up_position: &JOINT6_UP_POSITION
-                   0.051740
+                   0.0001
     down_position: &JOINT6_DOWN_POSITION
-                     3.409060
+                     3.1415926
     access_stone: &JOINT6_ACCESS_STONE_POSITION
-                    1.663420
+                    1.562120
     little_back_position: &JOINT6_LITTLE_BACK_POSITION
-                            0.887580
+                            2.167400
+    place_on_reverse: &JOINT6_PLACE_STONE_POSITION
+                        1.047
+    block_dart_position: &JOINT6_BLOCK_DART_POSITION
+                           0.5236
 
   joint7:
     use_for_delay: &DELAY_0_1
-      target: 0.008
+      target: 0.001
       delay: 0.1
     up_position: &JOINT7_UP_POSITION
-      target: 0.
-      delay:  0.1
+      target: 0.001
+      delay: 0.1
     up_position_no_delay: &JOINT7_UP_POSITION_NO_DELAY
-      target: 0.
-      delay:  0.
+      target: 0.001
+      delay: 0.
     mid_more_position: &JOINT7_MID_MORE_POSITION
       target: 0.25
       delay: 0.1
@@ -149,10 +179,10 @@ common:
       target: 0.75
       delay: 0.
     down_position: &JOINT7_DOWN_POSITION
-      target: 1
+      target: 1.0
       delay: 0.1
     down_position_no_delay: &JOINT7_DOWN_POSITION_NO_DELAY
-      target: 1
+      target: 1.575
       delay: 0.
     auto_exchange_position: &JOINT7_AUTO_EXCHANGE_POSITION
       original_tf: link4
@@ -160,8 +190,6 @@ common:
       direction: pitch
       delay: 0.05
 
-
-
   gripper:
     open_gripper: &OPEN_GRIPPER
       state: false
@@ -169,239 +197,259 @@ common:
       state: true
 
   gimbal:
-    back_pos: &BACK_POS
-      frame: gimbal_base
-      position: [ -2 ,-0.25, 0 ]
     side_pos: &SIDE_POS
       frame: gimbal_base
       position: [ 0.03 ,3, -0.02 ]
     island_pos: &ISLAND_POS
       frame: gimbal_base
-      position: [ 2, 0.03, -0.8 ]
+      position: [ 2, 0.001, 0.001 ]
     ground_pos: &GROUND_POS
       frame: gimbal_base
-      position: [ 2, 0.03, -0.8 ]
-    reversal_pos: &REVERSAL_POS
-      frame: gimbal_base
-      position: [ 2, 0.015, -0.25 ]
+      position: [ 2, 0.001, -0.8 ]
     exchange_pos: &EXCHANGE_POS
       frame: gimbal_base
       position: [ 0, -3, -2.3 ]
+    drag_pos1: &DRAG_POSE1
+      frame: gimbal_base
+      position: [ 0.001, 2, -0.8 ]
+    drag_pos2: &DRAG_POSE2
+      frame: gimbal_base
+      position: [ 0.001, -2, 0.001 ]
 
   arm:
-    home_zero_stone: &HOME
-      joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
+    reverse_joint6_up: &REVERSE_JOINT6_UP
+      joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_UP_POSITION ]
+      common:
+        <<: *QUICKLY
+      tolerance:
+        <<: *NORMAL_TOLERANCE
+
+    reverse_joint6_down: &REVERSE_JOINT6_DOWN
+      joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_DOWN_POSITION ]
+      common:
+        <<: *QUICKLY
+      tolerance:
+        <<: *NORMAL_TOLERANCE
+
+    reverse_ore_up: &REVERSE_ORE_UP
+      joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_UP_POSITION ]
+      common:
+        <<: *NORMALLY
+      tolerance:
+        <<: *NORMAL_TOLERANCE
+
+    take_back_ore_after_catch_drop: &TAKE_BACK_ORE
+      joints: [ "KEEP", *JOINT2_READY_TO_ACCESS_STONE_POSITION, "KEEP", "KEEP", "KEEP", "KEEP" ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
 
+    home: &HOME
+      joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
+      common:
+        <<: *QUICKLY
+      tolerance:
+        <<: *SMALL_TOLERANCE
+
+
     get_ore:
       ready_to_get_ore: &READY_TO_GET_ORE
-        joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
+        joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, "VARIABLE" ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
       get_ore: &GET_ORE
         joints: [ *JOINT1_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *SMALL_TOLERANCE
+      catch_drop: &CATCH_DROP
+        joints: [ *JOINT1_UP_POSITION, *JOINT2_FURTHEST_POSITION, *JOINT3_MID_GET_ORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *SMALL_TOLERANCE
+      ready_to_get_gold_ore: &READY_TO_GET_GOLD_ORE
+        joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_FURTHEST_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, "VARIABLE" ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+      get_gold_ore: &GET_GOLD_ORE
+        joints: [ *JOINT1_GET_ORE_POSITION, *JOINT2_FURTHEST_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *SMALL_TOLERANCE
+
+      ready_to_get_ground_stone: &READY_TO_GET_GROUND_STONE
+        joints: [ *JOINT1_READY_TO_GET_GROUND_STONE, *JOINT2_READY_TO_ACCESS_STONE_POSITION, *JOINT3_MID_POSITION, "VARIABLE", *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+      get_ground_stone: &GET_GROUND_STONE
+        joints: [ *JOINT1_GET_GROUND_STONE, *JOINT2_READY_TO_ACCESS_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_DOWN_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+      ready_to_store_stone_after_get_ore: &READY_TO_STORE_STONE_AFTER_GET_ORE
+        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+      ready_to_store_stone_after_catch_drop: &READY_TO_STORE_STONE_AFTER_CATCH_DROP
+        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_SKY_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+      store_stone_after_get_ore: &STORE_STONE_AFTER_GET_ORE
+        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_ACCESS_STONE_POSITION ]
+        common:
+          <<: *NORMALLY_HIGH_TIMEOUT
+        tolerance:
+          <<: *MAX_TOLERANCE
+      ready_to_store_gold_stone_after_get_ore: &READY_TO_STORE_GOLD_STONE_AFTER_GET_ORE
+        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_GOLD_ORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+      store_gold_stone_after_get_ore: &STORE_GOLD_STONE_AFTER_GET_ORE
+        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_GOLD_ORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_ACCESS_STONE_POSITION ]
+        common:
+          <<: *NORMALLY_HIGH_TIMEOUT
+        tolerance:
+          <<: *MAX_TOLERANCE
+      store_stone_after_catch_drop: &STORE_STONE_AFTER_CATCH_DROP
+        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_SKY_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_ACCESS_STONE_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *MAX_TOLERANCE
     access_store:
       ready_to_access_store: &READY_TO_ACCESS_STORE
-        joints: [ "VARIABLE", *JOINT2_READY_TO_ACCESS_STONE_POSITION, *JOINT3_ACCESS_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
+        joints: [ "VARIABLE", *JOINT2_READY_TO_ACCESS_STONE_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, "VARIABLE" ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
       access_store: &ACCESS_STORE
-        joints: [ "VARIABLE", *JOINT2_ACCESS_STORE_POSITION, *JOINT3_ACCESS_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, "VARIABLE" ]
+        joints: [ "VARIABLE", *JOINT2_ACCESS_STORE_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, "VARIABLE" ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-    #      ready_to_get_ore_mid: &READY_TO_GET_ORE_MID
-    #        joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, *JOINT3_MID_GET_ORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-    #        common:
-    #          <<: *NORMALLY
-    #        tolerance:
-    #          <<: *NORMAL_TOLERANCE
-    #      get_ore_mid: &GET_ORE_MID
-    #        joints: [ *JOINT1_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, *JOINT3_MID_GET_ORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-    #        common:
-    #          <<: *NORMALLY
-    #        tolerance:
-    #          <<: *NORMAL_TOLERANCE
-    #      ready_to_get_ore_rt: &READY_TO_GET_ORE_RT
-    #        joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, *JOINT3_RT_GET_ORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-    #        common:
-    #          <<: *NORMALLY
-    #        tolerance:
-    #          <<: *NORMAL_TOLERANCE
-    #      get_ore_rt: &GET_ORE_RT
-    #        joints: [ *JOINT1_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, *JOINT3_RT_GET_ORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-    #        common:
-    #          <<: *NORMALLY
-    #        tolerance:
-    #          <<: *NORMAL_TOLERANCE
-
-    ready_to_store_stone_after_get_ore: &READY_TO_STORE_STONE_AFTER_GET_ORE
-      joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_ACCESS_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
-      common:
-        <<: *NORMALLY
-      tolerance:
-        <<: *NORMAL_TOLERANCE
-
-    store_stone_after_get_ore: &STORE_STONE_AFTER_GET_ORE
-      joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_ACCESS_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_ACCESS_STONE_POSITION ]
-      common:
-        <<: *NORMALLY
-      tolerance:
-        <<: *NORMAL_TOLERANCE
-
     exchange_position: &EXCHANGE_POSITION
-      joints: [ *JOINT1_EXCHANGE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_LF_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
+      joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_LF_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *BIGGER_TOLERANCE
 
-
-
-
-
   reversal:
     velocity_stop: &VELOCITY_STOP
       mode: "VELOCITY"
       values: [ 0.,0.,0.,0.,0.,0. ]
+    velocity_z_out: &VELOCITY_Z_OUT
+      mode: "VELOCITY"
+      values: [ 0.,0.,3.,0.,0.,0. ]
+    velocity_z_out_store: &VELOCITY_Z_OUT_STORE
+      mode: "VELOCITY"
+      values: [ 0.,0.,0.2,0.,0.,0. ]
     velocity_z_in: &VELOCITY_Z_IN
       mode: "VELOCITY"
       values: [ 0.,0.,-1.2,0.,-0.01,0. ]
     velocity_z_in_quickly: &VELOCITY_Z_IN_QUICKLY
       mode: "VELOCITY"
       values: [ 0.,0.,-3.0,0.,-0.01,0. ]
+    velocity_z_in_quickly_with_delay: &VELOCITY_Z_IN_QUICKLY_WITH_DELAY
+      mode: "VELOCITY"
+      values: [ 0.,0.,-3.0,0.,-0.01,0. ]
+      delay: 0.3
+    velocity_z_pitch_in: &VELOCITY_PITCH_Z_IN
+      mode: "VELOCITY"
+      values: [ 0.,0.,0.,0.,-5.0,0. ]
+      delay: 0.85
+    velocity_z_pitch_in_90: &VELOCITY_PITCH_Z_IN_90
+      mode: "VELOCITY"
+      values: [ 0.,0.,0.,0.,-5.0,0. ]
+      delay: 0.5
+    velocity_z_in_slowly: &VELOCITY_Z_IN_SLOWLY
+      mode: "VELOCITY"
+      values: [ 0.,0.,-0.5,0.,-0.01,0. ]
+    velocity_z_in_delay: &VELOCITY_Z_IN_DELAY
+      mode: "VELOCITY"
+      values: [ 0.,0.,-1.,0.,0.,0. ]
+      delay: 1.1
     position_z_out: &POSITION_Z_OUT
       mode: "POSITION"
-      values: [ 0.,0.,17.,0.,0.,0. ]
-      delay: 0.4
-    velocity_z_out: &VELOCITY_Z_OUT
-      mode: "VELOCITY"
+      values: [ 0.,0.,7.,0.,0.,0. ]
+      delay: 0.2
+    position_z_out_little: &POSITION_Z_OUT_LITTLE
+      mode: "POSITION"
       values: [ 0.,0.,3.,0.,0.,0. ]
+      delay: 0.4
+    position_z_out_little2: &POSITION_Z_OUT_LITTLE2
+      mode: "POSITION"
+      values: [ 0.,0.,4.,0.,0.,0. ]
+      delay: 0.4
+    position_pitch_out: &POSITION_PITCH_OUT
+      mode: "POSITION"
+      values: [ 0.,0.,10.,0.,-1.,0. ]
+    position_pitch_reversal_pi_2: &POSITION_PITCH_REVERSAL_PI_2
+      mode: "POSITION"
+      values: [ 0.,0.,0.,0.,10.,0. ]
+      delay: 0.6
+    position_pitch_reversal_pi: &POSITION_PITCH_REVERSAL_PI
+      mode: "POSITION"
+      values: [ 0.,0.,0.,0.,-100,0. ]
+      delay: 1
 
   chassis_move:
-    chassis_backward_7: &CHASSIS_BACKWARD_7
-      frame: base_link
-      position: [ -0.07, 0. ]
-      yaw: 0.0
-      chassis_tolerance_position_: 0.04
-      chassis_tolerance_angular_: 0.2
-
     chassis_backward_30: &CHASSIS_BACKWARD_25
       frame: base_link
-      position: [ -0.28,0. ]
+      position: [ -0.35,0. ]
       yaw: 0.0
-      chassis_tolerance_position_: 0.04
-      chassis_tolerance_angular_: 0.2
-
-    chassis_right_90: &CHASSIS_RIGHT_90
-      frame: base_link
-      position: [ 0., 0. ]
-      yaw: -1.57
-      chassis_tolerance_position_: 0.1
-      chassis_tolerance_angular_: 0.1
+      chassis_tolerance_position: 0.1
+      chassis_tolerance_angular: 0.3
+      common:
+        timeout: 2.
     chassis_180: &CHASSIS_180
       frame: base_link
       position: [ 0., 0. ]
       yaw: 2.80
-      chassis_tolerance_position_: 0.1
-      chassis_tolerance_angular_: 0.2
+      chassis_tolerance_position: 0.1
+      chassis_tolerance_angular: 0.2
+      common:
+        timeout: 2.
 
 
 steps_list:
-  ###############   GRIPPER   ###############
-  CLOSE_GRIPPER:
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-
-  OPEN_GRIPPER:
-    - step: "open  gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-  ###############   HOME   ###############
-
-  HOME_ZERO_STONE:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION
-    - step: "home with no stone"
-      arm:
-        <<: *HOME
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
-
-  HOME_ONE_STONE:
-    - step: "gimbal look side"
-      gimbal:
-        <<: *SIDE_POS
-    - step: "joint7 up"
-      joint7:
-        <<: *JOINT7_UP_POSITION
-    - step: "home with one stone"
-      arm:
-        <<: *HOME
-        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ]
-
-  HOME_TWO_STONE:
-    - step: "gimbal look side"
-      gimbal:
-        <<: *SIDE_POS
-    - step: "joint7 up"
-      joint7:
-        <<: *JOINT7_UP_POSITION
-    - step: "home with two stone"
-      arm:
-        <<: *HOME
-        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
-
-  HOME_THREE_STONE:
-    - step: "gimbal look side"
-      gimbal:
-        <<: *SIDE_POS
-    - step: "joint7 up"
-      joint7:
-        <<: *JOINT7_UP_POSITION
-    - step: "home with three stone"
-      arm:
-        <<: *HOME
-        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, 0 ]
-
-      ###################   TAKE   ####################
-
-  #############  SMALL_ISLAND_TOP  ################
-
+  #############  SMALL_ISLAND  ################
   ########### LF  ############
-
   SMALL_ISLAND_LF:
     - step: "joint7 up"
-      joint7:
+      end_effector:
         <<: *JOINT7_UP_POSITION_NO_DELAY
     - step: "island gimbal"
       gimbal:
-        <<: *ISLAND_POS
+        <<: *GROUND_POS
     - step: "ready_to_get_ore"
       arm:
         <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, 0 ]
+        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
     - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
-    - step: "reversal move"
-      reversal:
-        <<: *VELOCITY_Z_IN
   SMALL_ISLAND_LF0:
     - step: "move down to get ore"
       arm:
@@ -410,45 +458,50 @@ steps_list:
     - step: "move up ore"
       arm:
         <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, 0 ]
+        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+    - step: "reverse ore"
+      arm:
+        <<: *REVERSE_ORE_UP
+    - step: "reversal move"
+      reversal:
+        <<: *VELOCITY_Z_IN_QUICKLY_WITH_DELAY
     - step: "ready to store stone"
       arm:
         <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
     - step: "store stone"
       arm:
         <<: *STORE_STONE_AFTER_GET_ORE
+    - step: "change stone"
+      stone_num:
+        change: "WHITE"
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
+    - step: "move joint6 up"
+      arm:
+        <<: *REVERSE_JOINT6_UP
     - step: "reverse joint6"
-      joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_READY_TO_ACCESS_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
-      common:
-        <<: *NORMALLY
-      tolerance:
-        <<: *NORMAL_TOLERANCE
-    - step: "stop reversal"
+      arm:
+        <<: *HOME
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "reversal stop"
       reversal:
         <<: *VELOCITY_STOP
-
   ########### MID ############
-
   SMALL_ISLAND_MID:
     - step: "joint7 up"
-      joint7:
+      end_effector:
         <<: *JOINT7_UP_POSITION_NO_DELAY
     - step: "island gimbal"
       gimbal:
-        <<: *ISLAND_POS
+        <<: *GROUND_POS
     - step: "ready_to_get_ore"
       arm:
         <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0 ]
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
     - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
-    - step: "reversal move"
-      reversal:
-        <<: *VELOCITY_Z_IN
   SMALL_ISLAND_MID0:
     - step: "move down to get ore"
       arm:
@@ -457,7 +510,13 @@ steps_list:
     - step: "move up ore"
       arm:
         <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0 ]
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+    - step: "reverse ore"
+      arm:
+        <<: *REVERSE_ORE_UP
+    - step: "reversal move"
+      reversal:
+        <<: *VELOCITY_Z_IN_QUICKLY_WITH_DELAY
     - step: "ready to store stone"
       arm:
         <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
@@ -467,35 +526,47 @@ steps_list:
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
+    - step: "move joint6 up"
+      arm:
+        <<: *REVERSE_JOINT6_UP
+    - step: "change stone"
+      stone_num:
+        change: "WHITE"
     - step: "reverse joint6"
-      joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_READY_TO_ACCESS_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
-      common:
-        <<: *NORMALLY
-      tolerance:
-        <<: *NORMAL_TOLERANCE
-    - step: "stop reversal"
+      arm:
+        <<: *HOME
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "reversal stop"
       reversal:
         <<: *VELOCITY_STOP
-
-  ########### RT  ############
-
+    - step: "chassis backward"
+      chassis:
+        <<: *CHASSIS_BACKWARD_25
+    - step: "home one stone"
+      arm:
+        <<: *HOME
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "gimbal side"
+      gimbal:
+        <<: *SIDE_POS
+    - step: "chassis 180"
+      chassis:
+        <<: *CHASSIS_180
+    ########### RT  ############
   SMALL_ISLAND_RT:
     - step: "joint7 up"
-      joint7:
+      end_effector:
         <<: *JOINT7_UP_POSITION_NO_DELAY
     - step: "island gimbal"
       gimbal:
-        <<: *ISLAND_POS
+        <<: *GROUND_POS
     - step: "ready_to_get_ore"
       arm:
         <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, 0 ]
+        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
     - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
-    - step: "reversal move"
-      reversal:
-        <<: *VELOCITY_Z_IN
   SMALL_ISLAND_RT0:
     - step: "move down to get ore"
       arm:
@@ -504,104 +575,289 @@ steps_list:
     - step: "move up ore"
       arm:
         <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, 0 ]
+        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+    - step: "reverse ore"
+      arm:
+        <<: *REVERSE_ORE_UP
+    - step: "reversal move"
+      reversal:
+        <<: *VELOCITY_Z_IN_QUICKLY_WITH_DELAY
     - step: "ready to store stone"
       arm:
         <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
     - step: "store stone"
       arm:
         <<: *STORE_STONE_AFTER_GET_ORE
-        variable: [ *JOINT1_READY_TO_GET_ORE_POSITION, 0, 0, 0, 0, 0 ]
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
+    - step: "change stone"
+      stone_num:
+        change: "WHITE"
+    - step: "move joint6 up"
+      arm:
+        <<: *REVERSE_JOINT6_UP
     - step: "reverse joint6"
-      joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_READY_TO_ACCESS_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
-      common:
-        <<: *NORMALLY
-      tolerance:
-        <<: *NORMAL_TOLERANCE
-    - step: "stop reversal"
+      arm:
+        <<: *HOME
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "reversal stop"
       reversal:
         <<: *VELOCITY_STOP
 
       ###################   TAKE   ####################
-
-  ########### TAKE ###########
-  TAKE_WHEN_ONE_STONE:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
+  ########## GET DUAL ORE #######
+  SMALL_ISLAND_TWO_ORE_L:
     - step: "joint7 up"
-      joint7:
+      end_effector:
         <<: *JOINT7_UP_POSITION_NO_DELAY
-    - step: "move down to get stone"
-      arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
-    - step: "REVERSAL OUT"
-      reversal:
-        <<: *POSITION_Z_OUT
-    - step: "move up with stone and ready to get stone"
+    - step: "island gimbal"
+      gimbal:
+        <<: *GROUND_POS
+    - step: "ready_to_get_mid_ore"
       arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
     - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
-    - step: "move joint2 to get stone"
-      arm:
-        <<: *ACCESS_STORE
-        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
-    - step: "REVERSAL Z OUT"
-      reversal:
-        <<: *VELOCITY_Z_OUT
-    - step: "move joint6"
+  SMALL_ISLAND_TWO_ORE_L0:
+    ##### GET MID ORE
+    - step: "move down to get mid ore"
       arm:
-        <<: *ACCESS_STORE
-        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_UP_POSITION ]
-    - step: "REVERSAL OUT"
+        <<: *GET_ORE
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0 ]
+    - step: "move up ore"
+      arm:
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+    - step: "reverse ore"
+      arm:
+        <<: *REVERSE_ORE_UP
+    - step: "reversal move"
+      reversal:
+        <<: *VELOCITY_Z_IN_QUICKLY
+    - step: "ready to store stone"
+      arm:
+        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
+    - step: "store stone"
+      arm:
+        <<: *STORE_STONE_AFTER_GET_ORE
+    - step: "change stone"
+      stone_num:
+        change: "WHITE"
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "move joint6 up"
+      arm:
+        <<: *REVERSE_JOINT6_UP
+  SMALL_ISLAND_TWO_ORE_L00:
+    - step: "ready_to_get_left_ore"
+      arm:
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+    - step: "reversal stop"
       reversal:
         <<: *VELOCITY_STOP
+    ###### GET LEFT ORE
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "move down to get ore"
+      arm:
+        <<: *GET_ORE
+        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, 0 ]
+    - step: "move up ore"
+      arm:
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+    - step: "reverse ore"
+      arm:
+        <<: *REVERSE_ORE_UP
+    - step: "reversal move"
+      reversal:
+        <<: *VELOCITY_Z_IN_QUICKLY
+    - step: "ready to store stone"
+      arm:
+        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
+    - step: "store stone"
+      arm:
+        <<: *STORE_STONE_AFTER_GET_ORE
     - step: "change stone"
       stone_num:
-        change: "-1"
-    - step: "move to exchange wait state"
+        change: "WHITE"
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "move joint6 up"
       arm:
-        <<: *EXCHANGE_POSITION
+        <<: *REVERSE_JOINT6_UP
+    - step: "chassis backward"
+      chassis:
+        <<: *CHASSIS_BACKWARD_25
+    - step: "reversal stop"
+      reversal:
+        <<: *VELOCITY_STOP
+    - step: "home two stone"
+      arm:
+        <<: *HOME
+        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "gimbal side"
+      gimbal:
+        <<: *SIDE_POS
+    - step: "chassis 180"
+      chassis:
+        <<: *CHASSIS_180
 
-  TAKE_WHEN_TWO_STONE:
+  SMALL_ISLAND_TWO_ORE_R:
+    - step: "joint7 up"
+      end_effector:
+        <<: *JOINT7_UP_POSITION_NO_DELAY
+    - step: "island gimbal"
+      gimbal:
+        <<: *GROUND_POS
+    - step: "ready_to_get_mid_ore"
+      arm:
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_GRIPPER
+  SMALL_ISLAND_TWO_ORE_R0:
+    ##### GET RIGHT ORE
+    - step: "move down to get right ore"
+      arm:
+        <<: *GET_ORE
+        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, 0 ]
+    - step: "move up ore"
+      arm:
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+    - step: "reverse ore"
+      arm:
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
+    - step: "reversal move"
+      reversal:
+        <<: *VELOCITY_Z_IN_QUICKLY
+    - step: "ready to store stone"
+      arm:
+        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
+    - step: "store stone"
+      arm:
+        <<: *STORE_STONE_AFTER_GET_ORE
+    - step: "change stone"
+      stone_num:
+        change: "WHITE"
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
+  SMALL_ISLAND_TWO_ORE_R00:
+    - step: "move joint6 up"
+      arm:
+        <<: *REVERSE_JOINT6_UP
+    - step: "move up"
+      arm:
+        <<: *HOME
+        variable: [ *JOINT1_READY_TO_GET_ORE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "ready_to_get_mid_ore"
+      arm:
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+    - step: "reversal stop"
+      reversal:
+        <<: *VELOCITY_STOP
+    ###### GET MID ORE
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "move down to get ore"
+      arm:
+        <<: *GET_ORE
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0 ]
+    - step: "move up ore"
+      arm:
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+    - step: "reverse ore"
+      arm:
+        <<: *READY_TO_GET_ORE
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
+    - step: "reversal move"
+      reversal:
+        <<: *VELOCITY_Z_IN_QUICKLY
+    - step: "ready to store stone"
+      arm:
+        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
+    - step: "store stone"
+      arm:
+        <<: *STORE_STONE_AFTER_GET_ORE
+    - step: "change stone"
+      stone_num:
+        change: "WHITE"
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "move joint6 up"
+      arm:
+        <<: *REVERSE_JOINT6_UP
+    - step: "reverse joint6"
+      arm:
+        <<: *HOME
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "reversal stop"
+      reversal:
+        <<: *VELOCITY_STOP
+  ########### TAKE STORED STONE ###########
+  ##### AUTO REVERSE ####
+  TAKE_WHEN_ONE_STONE_AUTO_REVERSE:
     - step: "island gimbal"
       gimbal:
         <<: *ISLAND_POS
     - step: "joint7 up"
-      joint7:
+      end_effector:
         <<: *JOINT7_UP_POSITION_NO_DELAY
     - step: "move down to get stone"
       arm:
         <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ]
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
     - step: "REVERSAL OUT"
       reversal:
         <<: *POSITION_Z_OUT
     - step: "move up with stone and ready to get stone"
       arm:
         <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+  #    - step: "reversal pitch pi"
+  #      reversal:
+  #        <<: *VELOCITY_PITCH_Z_IN
+  #    - step: "stop reversal"
+  #      reversal:
+  #        <<: *VELOCITY_STOP
+  TAKE_WHEN_ONE_STONE_AUTO_REVERSE0:
+    - step: "move up with stone and ready to get stone"
+      arm:
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
     - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
     - step: "move joint2 to get stone"
       arm:
         <<: *ACCESS_STORE
-        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    - step: "wait gripper get stone steady"
+      reversal:
+        <<: *VELOCITY_STOP
+        delay: 0.3
     - step: "REVERSAL Z OUT"
       reversal:
         <<: *VELOCITY_Z_OUT
-    - step: "move joint6"
+    - step: "reverse joint6"
       arm:
         <<: *ACCESS_STORE
-        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_UP_POSITION ]
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
     - step: "REVERSAL OUT"
       reversal:
         <<: *VELOCITY_STOP
@@ -612,39 +868,97 @@ steps_list:
       arm:
         <<: *EXCHANGE_POSITION
 
-  TAKE_WHEN_THREE_STONE:
+  TAKE_WHEN_TWO_STONE_AUTO_REVERSE:
     - step: "island gimbal"
       gimbal:
         <<: *ISLAND_POS
     - step: "joint7 up"
-      joint7:
+      end_effector:
         <<: *JOINT7_UP_POSITION_NO_DELAY
     - step: "move down to get stone"
       arm:
         <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+        variable: [ *JOINT1_TWO_STONE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    - step: "reversal out little"
+      reversal:
+        <<: *POSITION_Z_OUT_LITTLE
+    - step: "move up with stone and ready to get stone"
+      arm:
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    #    - step: "reversal pitch pi"
+    #      reversal:
+    #        <<: *VELOCITY_PITCH_Z_IN
+    #    - step: "stop reversal"
+    #      reversal:
+    #        <<: *VELOCITY_STOP
+    - step: "move up with stone and ready to get stone"
+      arm:
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+  TAKE_WHEN_TWO_STONE_AUTO_REVERSE0:
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "move joint2 to get stone"
+      arm:
+        <<: *ACCESS_STORE
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    - step: "wait gripper get stone steady"
+      reversal:
+        <<: *VELOCITY_STOP
+        delay: 0.3
+    - step: "REVERSAL OUT"
+      reversal:
+        <<: *VELOCITY_Z_OUT
+    - step: "move joint6"
+      arm:
+        <<: *ACCESS_STORE
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
+    - step: "change stone"
+      stone_num:
+        change: "-1"
+    - step: "reversal stop"
+      reversal:
+        <<: *VELOCITY_STOP
+    - step: "move to exchange wait state"
+      arm:
+        <<: *EXCHANGE_POSITION
+  ##### NO REVERSE ####
+  TAKE_WHEN_ONE_STONE_NO_REVERSE:
+    - step: "island gimbal"
+      gimbal:
+        <<: *ISLAND_POS
+    - step: "joint7 up"
+      end_effector:
+        <<: *JOINT7_UP_POSITION_NO_DELAY
+    - step: "move down to get stone"
+      arm:
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
     - step: "REVERSAL OUT"
       reversal:
         <<: *POSITION_Z_OUT
     - step: "move up with stone and ready to get stone"
       arm:
         <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+  TAKE_WHEN_ONE_STONE_NO_REVERSE0:
     - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
     - step: "move joint2 to get stone"
       arm:
         <<: *ACCESS_STORE
-        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
     - step: "REVERSAL Z OUT"
       reversal:
         <<: *VELOCITY_Z_OUT
-    - step: "move joint6"
+    - step: "reverse joint6"
       arm:
         <<: *ACCESS_STORE
-        variable: [ *JOINT1_THREE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_UP_POSITION ]
-    - step: "REVERSAL OUT"
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
+    - step: "REVERSAL STOP"
       reversal:
         <<: *VELOCITY_STOP
     - step: "change stone"
@@ -654,105 +968,463 @@ steps_list:
       arm:
         <<: *EXCHANGE_POSITION
 
-  ########### STORE ############
-  STORE_WHEN_ZERO_STONE:
+  TAKE_WHEN_TWO_STONE_NO_REVERSE:
     - step: "island gimbal"
       gimbal:
         <<: *ISLAND_POS
-    - step: "prepare"
+    - step: "joint7 up"
+      end_effector:
+        <<: *JOINT7_UP_POSITION_NO_DELAY
+    - step: "move down to get stone"
       arm:
         <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
-    - step: "ready to store"
+        variable: [ *JOINT1_TWO_STONE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    - step: "move up with stone and ready to get stone"
+      arm:
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+  TAKE_WHEN_TWO_STONE_NO_REVERSE0:
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "move joint2 to get stone"
       arm:
         <<: *ACCESS_STORE
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, *JOINT6_UP_POSITION ]
-    - step: "reversal z in"
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+    - step: "REVERSAL OUT"
       reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "store"
+        <<: *VELOCITY_Z_OUT
+    - step: "move joint6"
       arm:
         <<: *ACCESS_STORE
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
+    - step: "change stone"
+      stone_num:
+        change: "-1"
+    - step: "reversal stop"
+      reversal:
+        <<: *VELOCITY_STOP
+    - step: "move to exchange wait state"
+      arm:
+        <<: *EXCHANGE_POSITION
+
+  ##### BIG ISLAND ########
+  BIG_ISLAND:
+    - step: "joint7 up"
+      end_effector:
+        <<: *JOINT7_UP_POSITION_NO_DELAY
+    - step: "island gimbal"
+      gimbal:
+        <<: *GROUND_POS
+    - step: "move up to avoid collision"
+      arm:
+        <<: *HOME
+        variable: [ *JOINT1_READY_TO_GET_ORE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "ready_to_get_mid_ore"
+      arm:
+        <<: *READY_TO_GET_GOLD_ORE
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_GRIPPER
+  BIG_ISLAND0:
+    ##### GET MID ORE
+    - step: "move down to get mid ore"
+      arm:
+        <<: *GET_GOLD_ORE
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0 ]
+    - step: "move up ore"
+      arm:
+        <<: *READY_TO_GET_GOLD_ORE
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+    - step: "reverse ore"
+      arm:
+        <<: *REVERSE_ORE_UP
+    - step: "reversal move"
+      reversal:
+        <<: *VELOCITY_Z_IN_QUICKLY
+    - step: "ready to store stone"
+      arm:
+        <<: *READY_TO_STORE_GOLD_STONE_AFTER_GET_ORE
+    - step: "store stone"
+      arm:
+        <<: *STORE_GOLD_STONE_AFTER_GET_ORE
+    - step: "change stone"
+      stone_num:
+        change: "WHITE"
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "reversal z stop"
+    - step: "move joint6 up"
+      arm:
+        <<: *REVERSE_JOINT6_UP
+
+
+  CATCH_DROP:
+    - step: "joint7 up"
+      end_effector:
+        <<: *JOINT7_UP_POSITION_NO_DELAY
+    - step: "gimbal island pos"
+      gimbal:
+        <<: *ISLAND_POS
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "ready to catch drop"
+      arm:
+        <<: *CATCH_DROP
+  CATCH_DROP0:
+    - step: "take back ore"
+      arm:
+        <<: *TAKE_BACK_ORE
+    - step: "ready to store stone"
+      arm:
+        <<: *READY_TO_STORE_STONE_AFTER_CATCH_DROP
+    - step: "reversal in"
+      reversal:
+        <<: *VELOCITY_Z_IN_QUICKLY
+    - step: "place stone on reversal"
+      arm:
+        <<: *STORE_STONE_AFTER_CATCH_DROP
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "change stone"
+      stone_num:
+        change: "YELLOW"
+    - step: "move joint6 up"
+      arm:
+        <<: *REVERSE_JOINT6_UP
+    - step: "stop reversal"
       reversal:
         <<: *VELOCITY_STOP
-    - step: "move out arm"
+
+  GROUND_STONE:
+    - step: "ground gimbal"
+      gimbal:
+        <<: *GROUND_POS
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "reverse joint7"
+      end_effector:
+        <<: *JOINT7_DOWN_POSITION_NO_DELAY
+    - step: "ready to get ground stone"
       arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+        <<: *READY_TO_GET_GROUND_STONE
+        variable: [ 0, 0, 0, *JOINT4_DOWN_POSITION, 0, 0 ]
+  GROUND_STONE0:
+    - step: "move down to get ore"
+      arm:
+        <<: *GET_GROUND_STONE
+    - step: "move up ore"
+      arm:
+        <<: *READY_TO_GET_GROUND_STONE
+        variable: [ 0, 0, 0, *JOINT4_DOWN_POSITION, 0, 0 ]
+  GROUND_STONE00:
+    - step: "move up arm"
+      arm:
+        <<: *READY_TO_GET_GROUND_STONE
+        variable: [ 0, 0, 0, *JOINT4_UP_POSITION, 0, 0 ]
+    - step: "reverse joint7 up"
+      end_effector:
+        <<: *JOINT7_UP_POSITION
+    - step: "ready to place stone"
+      arm:
+        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
+        common:
+          <<: *NORMALLY_HIGH_TIMEOUT
+        tolerance:
+          <<: *MAX_TOLERANCE
+    - step: "place stone"
+      arm:
+        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_PLACE_STONE_POSITION ]
+        common:
+          <<: *NORMALLY_HIGH_TIMEOUT
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "change stone"
+      stone_num:
+        change: "MANUALLY"
+    - step: "reverse joint6 up"
+      arm:
+        <<: *REVERSE_ORE_UP
 
-  STORE_WHEN_ONE_STONE:
+  REVERSE_STONE_HIGH:
+    - step: "gimbal island"
+      gimbal:
+        <<: *ISLAND_POS
+    - step: "move joint1 up"
+      arm:
+        <<: *HOME
+        variable: [ *JOINT1_UP_POSITION, 0, 0, 0, 0, 0 ]
+
+  REVERSE_STONE_LOW:
+    - step: "gimbal island"
+      gimbal:
+        <<: *ISLAND_POS
+    - step: "move joint1 up"
+      arm:
+        <<: *HOME
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, 0, 0 ]
+
+  ########### STORE ############
+  STORE_WHEN_ZERO_STONE:
     - step: "island gimbal"
       gimbal:
         <<: *ISLAND_POS
     - step: "prepare"
       arm:
         <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ]
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_STORE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
     - step: "ready to store"
       arm:
         <<: *ACCESS_STORE
-        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_UP_POSITION ]
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_STORE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
     - step: "reversal z in"
       reversal:
         <<: *VELOCITY_Z_IN
     - step: "store"
       arm:
         <<: *ACCESS_STORE
-        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_STORE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
+    - step: "change stone"
+      stone_num:
+        change: "MANUALLY"
+    - step: "avoid_collision"
+      arm:
+        <<: *READY_TO_ACCESS_STORE
+        variable: [ *JOINT1_TWO_STONE_POSITION, 0, *JOINT3_STORE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
+    - step: "move joint6 up"
+      arm:
+        <<: *REVERSE_JOINT6_UP
     - step: "reversal z stop"
       reversal:
         <<: *VELOCITY_STOP
-    - step: "move out arm"
-      arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ]
-
-  STORE_WHEN_TWO_STONE:
+  STORE_WHEN_ONE_STONE:
     - step: "island gimbal"
       gimbal:
         <<: *ISLAND_POS
     - step: "prepare"
       arm:
         <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_STORE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
     - step: "ready to store"
       arm:
         <<: *ACCESS_STORE
-        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, *JOINT6_UP_POSITION ]
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_STORE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
     - step: "reversal z in"
       reversal:
         <<: *VELOCITY_Z_IN
     - step: "store"
       arm:
         <<: *ACCESS_STORE
-        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_STORE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
+    - step: "change stone"
+      stone_num:
+        change: "MANUALLY"
     - step: "reversal z stop"
       reversal:
         <<: *VELOCITY_STOP
-    - step: "move out arm"
-      arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
 
   EXCHANGE_WAIT:
+    - step: "island gimbal"
+      gimbal:
+        <<: *ISLAND_POS
     - step: "joint7 up"
       end_effector:
         <<: *JOINT7_UP_POSITION
     - step: "exchange wait"
       arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_LF_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
+        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_READY_TO_ACCESS_STONE_POSITION, *JOINT3_LF_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+  ######## GIMBAL ######
+  GIMBAL_ISLAND:
+    - step: "gimbal island pos"
+      gimbal:
+        <<: *ISLAND_POS
+  GIMBAL_SIDE:
+    - step: "gimbal side pos"
+      gimbal:
+        <<: *SIDE_POS
+  GIMBAL_GROUND:
+    - step: "gimbal ground pos"
+      gimbal:
+        <<: *GROUND_POS
+  GIMBAL_SMALL_SCREEN:
+    - step: "gimbal small screen"
+      gimbal:
+        <<: *EXCHANGE_POS
+  ###############   GRIPPER   ########
+  CLOSE_GRIPPER:
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
+  OPEN_GRIPPER:
+    - step: "open  gripper"
+      gripper:
+        <<: *OPEN_GRIPPER
+  ###############   HOME   ###########
+  HOME_ZERO_STONE:
+    - step: "gimbal look side"
+      gimbal:
+        <<: *SIDE_POS
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "joint7 up"
+      end_effector:
+        <<: *JOINT7_UP_POSITION
+    - step: "move joint6 up"
+      arm:
+        <<: *REVERSE_JOINT6_UP
+    - step: "home with no stone"
+      arm:
+        <<: *HOME
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+  HOME_ONE_STONE:
+    - step: "gimbal look side"
+      gimbal:
+        <<: *SIDE_POS
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "joint7 up"
+      end_effector:
+        <<: *JOINT7_UP_POSITION
+    - step: "move joint6 up"
+      arm:
+        <<: *REVERSE_JOINT6_UP
+    - step: "home with one stone"
+      arm:
+        <<: *HOME
+        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ]
+  HOME_TWO_STONE:
+    - step: "gimbal look side"
+      gimbal:
+        <<: *SIDE_POS
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "joint7 up"
+      end_effector:
+        <<: *JOINT7_UP_POSITION
+    - step: "move joint6 up"
+      arm:
+        <<: *REVERSE_JOINT6_UP
+    - step: "home with two stone"
+      arm:
+        <<: *HOME
+        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+
+      ###################   TAKE   ####################
+  ########### DRAG #######
+  DRAG_CAR:
+    - step: "ready to drag car"
+      gimbal:
+        <<: *DRAG_POSE1
+
+  DRAG_CAR0:
+    - step: "run away with car"
+      gimbal:
+        <<: *DRAG_POSE2
+
+  ########## EXCHANGE ############
+  OLD_EXCHANGE_SPHERE:
+    - step: "close to a sphere"
+      arm:
+        frame: exchanger
+        xyz: [ -0.4, 0., -0.25 ]
+        rpy: [ 0., 0., 0. ]
+        rpy_rectify: [ 0.01,0.1,0.03 ]
+        drift_dimensions: [ true, true, true, true, false, true ]
+        tolerance_position: 0.1
+        tolerance_orientation: 0.01
+        spacial_shape: SPHERE
+        radius: 0.5
+        point_resolution: 0.5
+        max_planning_times: 100
+        common:
+          <<: *QUICKLY
+
+  EXCHANGE_SPHERE:
+    - step: "test new exchange"
+      arm:
+        frame: exchanger
+        is_refer_planning_frame: true
+        xyz: [ -0.3, 0., 0. ]
+        rpy: [ 0., 0., 0. ]
+        drift_dimensions: [ true, true, true, true, false, true ]
+        tolerance_position: 0.05
+        tolerance_orientation: 0.05
+        spacial_shape: SPHERE
+        radius: 0.5
+        point_resolution: 0.5
+        max_planning_times: 100
         common:
           <<: *NORMALLY
+    - step: "test joint7"
+      end_effector:
+        original_tf: base_link
+        reference_tf: exchanger
+        direction: pitch
+    - step: "test new exchange"
+      arm:
+        frame: exchanger
+        is_refer_planning_frame: false
+        xyz: [ -0.2, 0., -0.18 ]
+        rpy: [ 0., 0., 0. ]
+        drift_dimensions: [ true, true, true, true, false, true ]
+        tolerance_position: 0.01
+        tolerance_orientation: 0.01
+        spacial_shape: BASICS
+        basics_length: [ 0.1, 0.1, 0.1 ]
+        point_resolution: 0.6
+        max_planning_times: 50
+        common:
+          <<: *NORMALLY
+  ########## ANTI DART ###########
+  ANTI_DART:
+    - step: "gimbal ground pos"
+      gimbal:
+        <<: *GROUND_POS
+    - step: "joint7 ready"
+      end_effector:
+        <<: *JOINT7_MID_POSITION_NO_DELAY
+    - step: "ready to get block"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_DOWN_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+  ANTI_DART0:
+    - step: "joint7 reverse up"
+      end_effector:
+        <<: *JOINT7_UP_POSITION_NO_DELAY
+    - step: "move up and rotate block"
+      arm:
+        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_BLOCK_DART_POSITION ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "move up block and ready to anti dart"
+      arm:
+        joints: [ *JOINT1_UP_POSITION, *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_BLOCK_DART_POSITION ]
+        common:
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
diff --git a/engineer_middleware/include/engineer_middleware/auto_exchange.h b/engineer_middleware/include/engineer_middleware/auto_exchange.h
index f118be0..e7ba8ca 100644
--- a/engineer_middleware/include/engineer_middleware/auto_exchange.h
+++ b/engineer_middleware/include/engineer_middleware/auto_exchange.h
@@ -3,260 +3,569 @@
 //
 #pragma once
 #include <rm_common/ros_utilities.h>
-#include <rm_common/decision/command_sender.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_ros/buffer.h>
 #include <rm_common/ori_tool.h>
+#include <rm_msgs/ExchangerMsg.h>
+#include <control_toolbox/pid.h>
+#include <angles/angles.h>
 
 namespace auto_exchange
 {
-enum FindProcess
-{
-  SWING,
-  FOUND,
-  LOCKED
-};
-
-enum AdjustProcess
-{
-  COMPUTER,
-  MOVE_CHASSIS
-};
-
-enum ServoMoveProcess
-{
-  YZ,
-  ROLL_YAW,
-  XYZ,
-  PITCH,
-  PUSH,
-  DONE
-};
-
-enum MotionMoveProcess
-{
-  SPHERE,
-  LINE,
-  POINT,
-  ACHIEVE
-};
-
-enum ExchangeProcess
-{
-  FIND,
-  PRE_ADJUST,
-  MOVE,
-  POST_ADJUST,
-  FINISH
-};
-
 class JointInfo
 {
 public:
   JointInfo(XmlRpc::XmlRpcValue& joint)
   {
-    //      joint1:
-    //        offset: 0.01
-    //        range: [0.,1.]
-    //        max_vel: 0.03
-    //        near_tolerance: 0.03
     offset_ = xmlRpcGetDouble(joint, "offset", 0.);
-    max_vel_ = xmlRpcGetDouble(joint, "max_vel", 1.0);
+    max_scale_ = xmlRpcGetDouble(joint, "max_scale", 1.0);
     near_tolerance_ = xmlRpcGetDouble(joint, "near_tolerance", 0.05);
     ROS_ASSERT(joint["range"].getType() == XmlRpc::XmlRpcValue::TypeArray);
     min_position_ = xmlRpcGetDouble(joint["range"], 0);
     max_position_ = xmlRpcGetDouble(joint["range"], 1);
+    move_direct_ = -1;
   }
-  bool judgeJointPosition()
+  bool judgeJointLimit()
   {
     return abs(current_position_ - offset_ - min_position_) <= near_tolerance_ ||
            abs(max_position_ + offset_ - current_position_) <= near_tolerance_;
   }
-  int judgeMoveDirect()
+  void judgeMoveDirect()
   {
-    return ((current_position_ - offset_) >= ((max_position_ - min_position_) / 2)) ? 1 : -1;
+    if (current_position_ - offset_ <= min_position_)
+      move_direct_ = 1;
+    else if (current_position_ - offset_ >= max_position_)
+      move_direct_ = -1;
   }
 
-private:
-  double offset_, max_position_, min_position_, current_position_, max_vel_, near_tolerance_;
+public:
+  int move_direct_;
+  double offset_, max_position_, min_position_, current_position_, max_scale_, near_tolerance_;
+};
+
+class SingleDirectionMove
+{
+public:
+  std::string name;
+  double tolerance, start_vel, offset_refer_exchanger, max_vel, error, pid_value;
+  control_toolbox::Pid pid;
+  void init(XmlRpc::XmlRpcValue& config, std::string config_name, ros::NodeHandle& nh)
+  {
+    name = config_name;
+    error = 1e10;
+    max_vel = config.hasMember("max_vel") ? (double)config["max_vel"] : 1e10;
+    start_vel = config.hasMember("start_vel") ? (double)config["start_vel"] : 0.;
+    tolerance = config.hasMember("tolerance") ? (double)config["tolerance"] : 1e10;
+    offset_refer_exchanger = config.hasMember("offset_refer_exchanger") ? (double)config["offset_refer_exchanger"] : 0.;
+    ros::NodeHandle pid_config = ros::NodeHandle(nh, name);
+    pid.init(ros::NodeHandle(pid_config, "pid"));
+  }
+  bool isFinish()
+  {
+    return abs(error) <= tolerance;
+  }
+  double computerVel(ros::Duration dt)
+  {
+    double vel = start_vel + abs(pid.computeCommand(error, dt));
+    int direction = error / abs(error);
+    return abs(vel) >= max_vel ? direction * max_vel : direction * vel;
+  }
+  void getPidValue(ros::Duration dt)
+  {
+    double vel = start_vel + abs(pid.computeCommand(error, dt));
+    int direction = error / abs(error);
+    pid_value = abs(vel) >= max_vel ? direction * max_vel : direction * vel;
+  }
 };
 
 class ProgressBase
 {
 public:
-  ProgressBase(XmlRpc::XmlRpcValue& progress, tf2_ros::Buffer& tf_buffer) : tf_buffer_(tf_buffer)
+  ProgressBase(XmlRpc::XmlRpcValue& progress, tf2_ros::Buffer& tf_buffer, ros::NodeHandle& nh)
+    : tf_buffer_(tf_buffer), nh_(nh)
   {
-    // progress:
-    //        timeout: 3.
-    time_out_ = xmlRpcGetDouble(progress, "timeout", 1e10);
+    time_out_ = xmlRpcGetDouble(progress, "time_out", 1e10);
+    if (nh_.hasParam("internal_time_out"))
+    {
+      internal_time_out_.resize((int)progress["internal_time_out"].size(), 1.);
+      for (int i = 0; i < (int)progress["internal_time_out"].size(); ++i)
+        internal_time_out_[i] = (progress["internal_time_out"][i]);
+    }
   }
-  virtual void init();
-  virtual void nextProcess();
-  virtual void manageProcess();
-  virtual void run();
-  virtual void printProcess();
-  bool checkTimeout(ros::Duration period)
+  virtual void init()
   {
-    if (period.toSec() > time_out_)
+    enter_flag_ = false;
+    is_finish_ = false;
+    is_recorded_time_ = false;
+    is_recorded_internal_time_ = false;
+  }
+  virtual void stateMachine() = 0;
+  virtual void run()
+  {
+    enter_flag_ = true;
+    if (!is_finish_)
     {
-      ROS_ERROR("Step timeout,it should be finish in %f seconds", time_out_);
-      return false;
+      checkTimeout();
+      if (nh_.hasParam("internal_time_out"))
+      {
+        checkInternalTimeout();
+      }
+      stateMachine();
+    }
+  }
+  virtual void printProcess() = 0;
+  bool getFinishFlag()
+  {
+    return is_finish_;
+  }
+  bool getEnterFlag()
+  {
+    return enter_flag_;
+  }
+  bool getTimeOutFlag()
+  {
+    return is_time_out_;
+  }
+  bool getProcess()
+  {
+    return process_;
+  }
+  void checkTimeout()
+  {
+    if (!is_recorded_time_)
+    {
+      is_recorded_time_ = true;
+      start_time_ = ros::Time::now();
+    }
+    if ((ros::Time::now() - start_time_).toSec() > time_out_)
+    {
+      ROS_ERROR("Progress timeout, should be finish in %f seconds", time_out_);
+      is_recorded_time_ = false;
+      is_finish_ = true;
+      is_time_out_ = true;
+    }
+  }
+  void checkInternalTimeout()
+  {
+    if (!is_recorded_internal_time_ || last_process_ != process_)
+    {
+      is_recorded_internal_time_ = true;
+      internal_start_time_ = ros::Time::now();
+      last_process_ = process_;
+    }
+    if ((ros::Time::now() - internal_start_time_).toSec() > internal_time_out_[process_])
+    {
+      ROS_ERROR("Inside progress timeout, should be finish in %f seconds", internal_time_out_[process_]);
+      //      ROS_INFO_STREAM("dt:  " <<  (ros::Time::now() - internal_start_time_).toSec());
+      is_recorded_internal_time_ = false;
+      if (process_ < (process_num_ - 1))
+        process_++;
+      else
+        is_finish_ = true;
     }
-    return true;
   }
 
 protected:
   tf2_ros::Buffer& tf_buffer_;
-  int process_;
-  bool is_finish_{ false };
+  int process_{}, last_process_{}, process_num_{};
+  bool is_finish_{ false }, is_recorded_time_{ false }, enter_flag_{ false }, is_recorded_internal_time_{ false },
+      is_time_out_{ false };
   double time_out_{};
-  ros::Time start_time_;
+  std::vector<double> internal_time_out_{};
+  ros::Time start_time_{}, internal_start_time_{};
+  ros::NodeHandle nh_{};
 };
 
 class Find : public ProgressBase
 {
 public:
-  Find(XmlRpc::XmlRpcValue& find, tf2_ros::Buffer& tf_buffer) : ProgressBase(find, tf_buffer)
-  {
-    //        find:
-    //          pitch:
-    //        offset: 0.01
-    //        range: [0.,1.]
-    //        max_vel: 0.03
-    //        near_tolerance: 0.03
-    //          confirm_lock_time
+  enum FindProcess
+  {
+    SWING,
+    ADJUST,
+    FINISH
+  };
+  Find(XmlRpc::XmlRpcValue& find, tf2_ros::Buffer& tf_buffer, ros::NodeHandle& nh) : ProgressBase(find, tf_buffer, nh)
+  {
     process_ = SWING;
+    last_process_ = process_;
+    process_num_ = 3;
+    gimbal_scale_.resize(2, 0);
+    chassis_scale_.resize(2, 0);
+    search_range_ = xmlRpcGetDouble(find, "search_range", 0.3);
     ROS_ASSERT(find["yaw"].getType() == XmlRpc::XmlRpcValue::TypeStruct);
-    ROS_ASSERT(find["yaw"].getType() == XmlRpc::XmlRpcValue::TypeStruct);
+    ROS_ASSERT(find["pitch"].getType() == XmlRpc::XmlRpcValue::TypeStruct);
     yaw_ = new JointInfo(find["yaw"]);
-    pitch_ = new JointInfo(find["pitch_"]);
-    confirm_lock_time_ = xmlRpcGetDouble(find, "confirm_lock_time", 10);
+    pitch_ = new JointInfo(find["pitch"]);
+    visual_recognition_sub_ =
+        nh_.subscribe<rm_msgs::ExchangerMsg>("/pnp_publisher", 1, &Find::visualRecognitionCallback, this);
+    ROS_INFO_STREAM("~~~~~~~~~~~~~FIND~~~~~~~~~~~~~~~~");
   }
   void init() override
   {
-    is_finish_ = false;
+    ProgressBase::init();
     process_ = { SWING };
+    initScales();
   }
-  void nextProcess() override
+  std::vector<double> getGimbalScale()
   {
-    process_++;
-    printProcess();
+    return gimbal_scale_;
   }
-  void manageProcess() override
+  std::vector<double> getChassisScale()
   {
-    if (process_ != LOCKED)
-      nextProcess();
-    else
-      is_finish_ = true;
+    return chassis_scale_;
+  }
+  void testAdjust()
+  {
+    gimbal_scale_[0] = middle_point_.x / 200;
+    gimbal_scale_[1] = middle_point_.y / 200;
+  }
+
+private:
+  void stateMachine() override
+  {
+    switch (process_)
+    {
+      case SWING:
+      {
+        autoSearch(false, true);
+        if (is_found_)
+        {
+          //            process_ = ADJUST;
+          process_ = FINISH;
+        }
+      }
+      break;
+      case ADJUST:
+      {
+        if (is_found_)
+        {
+          gimbal_scale_[0] = middle_point_.x / abs(middle_point_.x) * yaw_->max_scale_;
+          gimbal_scale_[1] = middle_point_.y / abs(middle_point_.y) * pitch_->max_scale_;
+          if (sqrt(pow(middle_point_.x, 2) + pow(middle_point_.y, 2)) < 200)
+            process_ = FINISH;
+        }
+        else
+          process_ = SWING;
+      }
+      break;
+      case FINISH:
+      {
+        is_finish_ = true;
+        ROS_INFO_STREAM("LOCKED");
+      }
+      break;
+    }
+  }
+
+  void autoSearch(bool enable_chassis, bool enable_gimbal)
+  {
+    if (enable_gimbal)
+    {
+      geometry_msgs::TransformStamped base2yaw, yaw2pitch;
+      base2yaw = tf_buffer_.lookupTransform("gimbal_base", "yaw", ros::Time(0));
+      yaw2pitch = tf_buffer_.lookupTransform("yaw", "pitch", ros::Time(0));
+
+      double yaw = yawFromQuat(base2yaw.transform.rotation);
+      double roll_temp, pitch, yaw_temp;
+      quatToRPY(yaw2pitch.transform.rotation, roll_temp, pitch, yaw_temp);
+      yaw_->current_position_ = yaw / search_range_;
+      pitch_->current_position_ = pitch / search_range_;
+      yaw_->judgeMoveDirect();
+      pitch_->judgeMoveDirect();
+      gimbal_scale_[0] = yaw_->move_direct_ * yaw_->max_scale_;
+      gimbal_scale_[1] = pitch_->move_direct_ * pitch_->max_scale_;
+    }
+    if (enable_chassis)
+    {
+      chassis_scale_[0] = gimbal_scale_[0];
+      chassis_scale_[1] = gimbal_scale_[1];
+    }
   }
   void printProcess() override
   {
     if (process_ == SWING)
       ROS_INFO_STREAM("SWING");
-    else if (process_ == FOUND)
+    else if (process_ == ADJUST)
       ROS_INFO_STREAM("FOUND");
-    else if (process_ == LOCKED)
-      ROS_INFO_STREAM("LOCKED");
+    else if (process_ == FINISH)
+      ROS_INFO_STREAM("FINISH");
   }
-  void run() override
+  void initScales()
   {
+    for (int i = 0; i < (int)gimbal_scale_.size(); ++i)
+    {
+      gimbal_scale_[i] = 0;
+      chassis_scale_[i] = 0;
+    }
+  }
+  void visualRecognitionCallback(const rm_msgs::ExchangerMsg ::ConstPtr& msg)
+  {
+    is_found_ = msg->flag;
+    middle_point_ = msg->middle_point;
   }
-
-private:
   JointInfo *yaw_{}, *pitch_{};
-  double search_angle_{}, confirm_lock_time_{};
+  bool is_found_{ false };
+  geometry_msgs::Point middle_point_;
+  std::vector<double> gimbal_scale_{}, chassis_scale_{};
+  ros::Subscriber visual_recognition_sub_{};
+  double search_range_{};
 };
 
-class ProAdjust
+class ProAdjust : public ProgressBase
 {
+public:
+  enum AdjustProcess
+  {
+    SET_GOAL,
+    CHASSIS_X,
+    CHASSIS_Y,
+    CHASSIS_YAW,
+    FINISH
+  };
+  ProAdjust(XmlRpc::XmlRpcValue& pre_adjust, tf2_ros::Buffer& tf_buffer, ros::NodeHandle& nh)
+    : ProgressBase(pre_adjust, tf_buffer, nh)
+  {
+    process_ = SET_GOAL;
+    last_process_ = process_;
+    process_num_ = 5;
+    x_.init(pre_adjust["x"], "x", nh);
+    y_.init(pre_adjust["y"], "y", nh);
+    yaw_.init(pre_adjust["yaw"], "yaw", nh);
+    ROS_INFO_STREAM("~~~~~~~~~~~~~PRE_ADJUST~~~~~~~~~~~~~~~~");
+  }
+  void init() override
+  {
+    ProgressBase::init();
+    process_ = SET_GOAL;
+    re_adjust_ = false;
+    initComputerValue();
+  }
+  void stateMachine() override
+  {
+    switch (process_)
+    {
+      case SET_GOAL:
+      {
+        set_goal();
+        process_ = CHASSIS_X;
+      }
+      break;
+      case CHASSIS_X:
+      {
+        computerChassisVel();
+        chassis_vel_cmd_.linear.y = 0.;
+        chassis_vel_cmd_.angular.z = 0.;
+        if (x_.isFinish())
+          process_ = re_adjust_ ? FINISH : CHASSIS_Y;
+      }
+      break;
+      case CHASSIS_Y:
+      {
+        computerChassisVel();
+        chassis_vel_cmd_.linear.x = 0.;
+        chassis_vel_cmd_.angular.z = 0.;
+        if (y_.isFinish())
+          process_ = CHASSIS_YAW;
+      }
+      break;
+      case CHASSIS_YAW:
+      {
+        computerChassisVel();
+        chassis_vel_cmd_.linear.x = 0.;
+        chassis_vel_cmd_.linear.y = 0.;
+        if (yaw_.isFinish())
+        {
+          process_ = SET_GOAL;
+          re_adjust_ = true;
+        }
+      }
+      break;
+      case FINISH:
+      {
+        is_finish_ = true;
+        re_adjust_ = false;
+        initComputerValue();
+        ROS_INFO_STREAM("PRE ADJUST FINISH");
+      }
+      break;
+    }
+  }
+  geometry_msgs::Twist getChassisVelMsg()
+  {
+    return chassis_vel_cmd_;
+  }
+  std::string getChassisCmdFrame()
+  {
+    return chassis_command_source_frame_;
+  }
+
+private:
+  void initComputerValue()
+  {
+    chassis_vel_cmd_.linear.x = 0;
+    chassis_vel_cmd_.linear.y = 0;
+    chassis_vel_cmd_.linear.z = 0;
+    chassis_vel_cmd_.angular.x = 0;
+    chassis_vel_cmd_.angular.y = 0;
+    chassis_vel_cmd_.angular.z = 0;
+  }
+  void printProcess() override
+  {
+    ROS_INFO_STREAM("PRE ADJUST");
+  }
+  void computerChassisVel()
+  {
+    geometry_msgs::TransformStamped current;
+    current = tf_buffer_.lookupTransform("map", "base_link", ros::Time(0));
+    x_.error = chassis_target_.pose.position.x - current.transform.translation.x;
+    y_.error = chassis_target_.pose.position.y - current.transform.translation.y;
+    double roll, pitch, yaw_current, yaw_goal;
+    quatToRPY(current.transform.rotation, roll, pitch, yaw_current);
+    quatToRPY(chassis_target_.pose.orientation, roll, pitch, yaw_goal);
+    yaw_.error = angles::shortest_angular_distance(yaw_current, yaw_goal);
+
+    ros::Duration dt = ros::Time::now() - last_time_;
+    chassis_vel_cmd_.linear.x = x_.computerVel(dt);
+    chassis_vel_cmd_.linear.y = y_.computerVel(dt);
+    chassis_vel_cmd_.angular.z = yaw_.computerVel(dt);
+
+    last_time_ = ros::Time::now();
+  }
+  void set_goal()
+  {
+    geometry_msgs::TransformStamped base2exchange;
+    double roll, pitch, yaw;
+    base2exchange = tf_buffer_.lookupTransform("base_link", "exchanger", ros::Time(0));
+    quatToRPY(base2exchange.transform.rotation, roll, pitch, yaw);
+
+    double goal_x = base2exchange.transform.translation.x - x_.offset_refer_exchanger;
+    double goal_y = base2exchange.transform.translation.y - y_.offset_refer_exchanger;
+    double goal_yaw = yaw * yaw_.offset_refer_exchanger;
+    chassis_original_target_.pose.position.x = goal_x;
+    chassis_original_target_.pose.position.y = goal_y;
+    tf2::Quaternion quat_tf;
+    quat_tf.setRPY(0, 0, goal_yaw);
+    geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf);
+    chassis_original_target_.pose.orientation = quat_msg;
+    chassis_target_ = chassis_original_target_;
+    tf2::doTransform(chassis_target_, chassis_target_, tf_buffer_.lookupTransform("map", "base_link", ros::Time(0)));
+  }
+
+  bool re_adjust_{ false };
+  ros::Time last_time_;
+  std::string chassis_command_source_frame_{ "base_link" };
+  geometry_msgs::Twist chassis_vel_cmd_{};
+  geometry_msgs::PoseStamped chassis_target_{}, chassis_original_target_{};
+  SingleDirectionMove x_, y_, yaw_;
 };
 
-class ServoMove : public ProgressBase
+class AutoServoMove : public ProgressBase
 {
 public:
-  ServoMove(XmlRpc::XmlRpcValue& servo_move, ros::NodeHandle& nh, tf2_ros::Buffer& tf_buffer)
-    : ProgressBase(servo_move, tf_buffer)
-  {
-    //      servo_move:
-    //        xyz_offset: [ 0.08, 0., -0.04]
-    //        link7_length: 0.1
-    //        servo_p: [ 6., 6., 7., 3., 2., 1. ]
-    //        servo_error_tolerance: [ 0.01, 0.01, 0.01, 0.01, 0.01, 0.01 ]
-    ROS_ASSERT(servo_move["xyz_offset"].getType() == XmlRpc::XmlRpcValue::TypeArray);
-    ROS_ASSERT(servo_move["servo_p"].getType() == XmlRpc::XmlRpcValue::TypeArray);
-    ROS_ASSERT(servo_move["servo_error_tolerance"].getType() == XmlRpc::XmlRpcValue::TypeArray);
-    ROS_ASSERT(servo_move["link7_length"].getType() == XmlRpc::XmlRpcValue::TypeDouble);
-    process_ = { YZ };
-    enter_servo_move_.data = false;
-    xyz_offset_.resize(3, 0.);
-    servo_p_.resize(6, 0.);
-    servo_errors_.resize(6, 0.);
+  enum ServoMoveProcess
+  {
+    YZ,
+    YAW,
+    ROLL,
+    REZ,
+    PITCH,
+    REY,
+    RREZ,
+    PUSH,
+    FINISH
+  };
+  AutoServoMove(XmlRpc::XmlRpcValue& auto_servo_move, tf2_ros::Buffer& tf_buffer, ros::NodeHandle& nh)
+    : ProgressBase(auto_servo_move, tf_buffer, nh), joint7_msg_(0.)
+  {
+    process_ = YZ;
+    last_process_ = process_;
+    process_num_ = 6;
+
+    x_.init(auto_servo_move["x"], "x", nh);
+    y_.init(auto_servo_move["y"], "y", nh);
+    z_.init(auto_servo_move["z"], "z", nh);
+    re_y_.init(auto_servo_move["re_y"], "re_y", nh);
+    re_z_.init(auto_servo_move["re_z"], "re_z", nh);
+    roll_.init(auto_servo_move["roll"], "roll", nh);
+    pitch_.init(auto_servo_move["pitch"], "pitch", nh);
+    yaw_.init(auto_servo_move["yaw"], "yaw", nh);
+    move_gather_.clear();
+    move_gather_.push_back(x_);
+    move_gather_.push_back(y_);
+    move_gather_.push_back(z_);
+    move_gather_.push_back(roll_);
+    move_gather_.push_back(pitch_);
+    move_gather_.push_back(yaw_);
+    move_gather_.push_back(re_y_);
+    move_gather_.push_back(re_z_);
+
+    servo_pid_value_.resize(6, 0.);
     servo_scales_.resize(6, 0.);
-    servo_error_tolerance_.resize(6, 0.01);
-    for (int i = 0; i < (int)xyz_offset_.size(); ++i)
-      xyz_offset_[i] = servo_move["xyz_offset"][i];
-    for (int i = 0; i < (int)servo_p_.size(); ++i)
-    {
-      servo_p_[i] = servo_move["servo_p"][i];
-      servo_error_tolerance_[i] = servo_move["servo_error_tolerance"][i];
-    }
-    exchanger_tf_update_pub_ = nh.advertise<std_msgs::Bool>("/is_update_exchanger", 1);
+    ROS_INFO_STREAM("~~~~~~~~~~~~~SERVO_MOVE~~~~~~~~~~~~~~~~");
   }
+  ~AutoServoMove() = default;
   void init() override
   {
-    is_finish_ = false;
+    ProgressBase::init();
     process_ = { YZ };
-    enter_servo_move_.data = false;
     initComputerValue();
+    joint7_msg_ = 0.;
   }
-
-  void nextProcess() override
+  double getJoint7Msg()
   {
-    process_++;
-    printProcess();
+    return joint7_msg_;
   }
-  void manageProcess() override
+  std::vector<double> getServoScale()
   {
-    if (process_ != DONE)
-      nextProcess();
-    else
-      is_finish_ = true;
+    return servo_scales_;
   }
+
   void printProcess() override
   {
     if (process_ == YZ)
       ROS_INFO_STREAM("YZ");
-    else if (process_ == ROLL_YAW)
-      ROS_INFO_STREAM("ROLL_YAW");
-    else if (process_ == XYZ)
-      ROS_INFO_STREAM("XYZ");
+    else if (process_ == YAW)
+      ROS_INFO_STREAM("YAW");
+    else if (process_ == ROLL)
+      ROS_INFO_STREAM("ROLL");
     else if (process_ == PITCH)
       ROS_INFO_STREAM("PITCH");
+    else if (process_ == REY)
+      ROS_INFO_STREAM("REY");
+    else if (process_ == REZ)
+      ROS_INFO_STREAM("REZ");
     else if (process_ == PUSH)
       ROS_INFO_STREAM("PUSH");
-  }
-  void run() override
-  {
-    enter_servo_move_.data = true;
-    exchanger_tf_update_pub_.publish(enter_servo_move_);
-    if (!is_finish_)
-    {
-      computeServoMoveScale();
-      manageProcess();
-    }
+    else if (process_ == FINISH)
+      ROS_INFO_STREAM("FINISH");
   }
 
 private:
+  void stateMachine() override
+  {
+    computeServoMoveScale();
+    manageServoMoveProcess();
+  }
   void initComputerValue()
   {
-    for (int i = 0; i < (int)servo_scales_.size(); ++i)
+    for (int i = 0; i < (int)servo_pid_value_.size(); ++i)
     {
-      servo_errors_[i] = 0;
-      servo_scales_[i] = 0;
+      servo_pid_value_[i] = 0.;
+      servo_scales_[i] = 0.;
     }
+    move_gather_[0] = x_;
+    move_gather_[1] = y_;
+    move_gather_[2] = z_;
+    move_gather_[3] = roll_;
+    move_gather_[4] = pitch_;
+    move_gather_[5] = yaw_;
+    move_gather_[6] = re_y_;
+    move_gather_[7] = re_z_;
   }
   void computeServoMoveError()
   {
+    initComputerValue();
     double roll, pitch, yaw;
     std::vector<double> errors;
     geometry_msgs::TransformStamped tools2exchanger;
@@ -269,14 +578,31 @@ class ServoMove : public ProgressBase
     {
       ROS_WARN("%s", ex.what());
     }
-    initComputerValue();
-    servo_errors_[0] = (process_ == PUSH ? (tools2exchanger.transform.translation.x) :
-                                           (tools2exchanger.transform.translation.x - xyz_offset_[0]));
-    servo_errors_[1] = tools2exchanger.transform.translation.y - xyz_offset_[1] + 0.06 * sin(roll + M_PI_2);
-    servo_errors_[2] = tools2exchanger.transform.translation.z - xyz_offset_[2];
-    servo_errors_[3] = roll;
-    servo_errors_[4] = pitch;
-    servo_errors_[5] = yaw;
+    x_.error = (process_ == PUSH ? (tools2exchanger.transform.translation.x) :
+                                   (tools2exchanger.transform.translation.x - x_.offset_refer_exchanger));
+    y_.error = tools2exchanger.transform.translation.y - y_.offset_refer_exchanger;
+    re_y_.error = y_.error;
+    z_.error = tools2exchanger.transform.translation.z - z_.offset_refer_exchanger;
+    re_z_.error = z_.error;
+    roll_.error = roll * roll_.offset_refer_exchanger;
+    pitch_.error = pitch * pitch_.offset_refer_exchanger;
+    yaw_.error = yaw * yaw_.offset_refer_exchanger;
+
+    ros::Duration dt = ros::Time::now() - last_time_;
+    last_time_ = ros::Time::now();
+
+    servo_pid_value_[0] = x_.computerVel(dt);
+    if (process_ != REY)
+      servo_pid_value_[1] = y_.computerVel(dt);
+    else
+      servo_pid_value_[1] = re_y_.computerVel(dt);
+    if (process_ == REZ || process_ == RREZ)
+      servo_pid_value_[2] = re_z_.computerVel(dt);
+    else
+      servo_pid_value_[2] = z_.computerVel(dt);
+    servo_pid_value_[3] = roll_.computerVel(dt);
+    servo_pid_value_[4] = pitch_.computerVel(dt);
+    servo_pid_value_[5] = yaw_.computerVel(dt);
   }
   void computeServoMoveScale()
   {
@@ -285,32 +611,43 @@ class ServoMove : public ProgressBase
     {
       case YZ:
       {
-        for (int i = 1; i < 3; ++i)
-        {
-          servo_scales_[i] = servo_errors_[i] * servo_p_[i];
-        }
+        servo_scales_[1] = servo_pid_value_[1];
+        servo_scales_[2] = servo_pid_value_[2];
       }
       break;
-      case ROLL_YAW:
+      case YAW:
       {
-        servo_scales_[3] = servo_errors_[3] * servo_p_[3];
-        servo_scales_[5] = servo_errors_[5] * servo_p_[5];
+        servo_scales_[5] = servo_pid_value_[5];
       }
       break;
-      case XYZ:
+      case ROLL:
       {
-        for (int i = 0; i < 3; ++i)
-          servo_scales_[i] = servo_errors_[i] * servo_p_[i];
+        servo_scales_[3] = servo_pid_value_[3];
+      }
+      break;
+      case REZ:
+      {
+        servo_scales_[2] = servo_pid_value_[2];
       }
       break;
       case PITCH:
       {
-        joint7_msg_ = servo_errors_[4];
+        joint7_msg_ = pitch_.error;
+      }
+      break;
+      case REY:
+      {
+        servo_scales_[1] = servo_pid_value_[1];
+      }
+      break;
+      case RREZ:
+      {
+        servo_scales_[2] = servo_pid_value_[2];
       }
       break;
       case PUSH:
       {
-        servo_scales_[0] = servo_errors_[0] * servo_p_[0];
+        servo_scales_[0] = servo_pid_value_[0];
       }
       break;
     }
@@ -323,43 +660,348 @@ class ServoMove : public ProgressBase
       if (servo_scales_[i] != 0)
       {
         move_joint_num++;
-        if (abs(servo_errors_[i]) <= servo_error_tolerance_[i])
-          arrived_joint_num++;
+        if (process_ == YZ)
+        {
+          if (abs(move_gather_[i].error) <= (y_.tolerance + z_.tolerance) / 2)
+            arrived_joint_num++;
+        }
+        else if (process_ == REZ)
+        {
+          if (abs(re_z_.error) <= re_z_.tolerance)
+            arrived_joint_num++;
+        }
+        else if (process_ == REY)
+        {
+          if (abs(re_y_.error) <= re_y_.tolerance)
+            arrived_joint_num++;
+        }
+        else
+        {
+          if (abs(move_gather_[i].error) <= move_gather_[i].tolerance)
+            arrived_joint_num++;
+        }
       }
     }
     if (arrived_joint_num == move_joint_num)
-      nextProcess();
-    //        if (checkTimeout())
-    //        {
-    //            switch ()
-    //            {
-    //            }
-    //            nextProcess();
-    //            ROS_INFO_STREAM("TIME OUT");
-    //        }
-    //        else if (arrived_joint_num == move_joint_num)
-    //            nextProcess();
+    {
+      if (process_ != FINISH)
+        process_++;
+      else
+        is_finish_ = true;
+    }
   }
-  double getJoint7Msg()
+  void rectifyForLink7(double theta, double link7_length)
   {
-    return joint7_msg_;
+    rectify_x_ = link7_length_ * pow(sin(theta), 2) / (tan(M_PI_2 - theta / 2));
+    rectify_z_ = link7_length_ * sin(theta) * cos(theta) / (tan(M_PI_2 - theta / 2));
   }
-  std_msgs::Bool enter_servo_move_{};
-  double link7_length_{}, joint7_msg_{};
-  ros::Publisher exchanger_tf_update_pub_;
-  std::vector<double> xyz_offset_{}, servo_p_{}, servo_errors_{}, servo_scales_{}, servo_error_tolerance_{};
-};
 
-class MotionMove
-{
+  SingleDirectionMove x_, y_, z_, roll_, pitch_, yaw_, re_y_, re_z_;
+  std::vector<SingleDirectionMove> move_gather_{};
+  ros::Time last_time_;
+  double link7_length_{}, joint7_msg_{}, rectify_x_, rectify_z_;
+  std::vector<double> servo_errors_{}, servo_scales_{}, servo_pid_value_{};
 };
 
-class PostAdjust
+class UnionMove : public ProgressBase
 {
+public:
+  enum UnionMoveProcess
+  {
+    MOTION,
+    SERVO_Z,
+    SERVO_Y,
+    SERVO_X,
+    FINISH
+  };
+  UnionMove(XmlRpc::XmlRpcValue& union_move, tf2_ros::Buffer& tf_buffer, ros::NodeHandle& nh)
+    : ProgressBase(union_move, tf_buffer, nh)
+  {
+    process_ = MOTION;
+    last_process_ = process_;
+    process_num_ = 4;
+    motion_name_ = union_move.hasMember("motion_name") ? std::string(union_move["motion_name"]) : "AUTO_EXCHANGE";
+    x_.init(union_move["x"], "x", nh);
+    y_.init(union_move["y"], "y", nh);
+    z_.init(union_move["z"], "z", nh);
+    move_gather_.clear();
+    move_gather_.push_back(x_);
+    move_gather_.push_back(y_);
+    move_gather_.push_back(z_);
+    servo_scales_.resize(3, 0.);
+  }
+  std::string getMotionName()
+  {
+    return motion_name_;
+  }
+  bool getIsMotionFinish()
+  {
+    return is_motion_finish_;
+  }
+  bool getIsMotionStart()
+  {
+    return is_motion_start_;
+  }
+  void changeIsMotionFinish(bool state)
+  {
+    is_motion_finish_ = state;
+  }
+  void changeIsMotionStart(bool state)
+  {
+    is_motion_start_ = state;
+  }
+  std::vector<double> getServoScale()
+  {
+    return servo_scales_;
+  }
+  void init() override
+  {
+    ProgressBase::init();
+    initComputerValue();
+    process_ = MOTION;
+    is_motion_start_ = false;
+    is_motion_finish_ = false;
+  }
+
+private:
+  void printProcess() override
+  {
+    ROS_INFO_STREAM(process_);
+  }
+  void stateMachine() override
+  {
+    if (is_motion_finish_)
+      process_ = SERVO_Z;
+    if (process_ != MOTION)
+    {
+      computeServoMoveScale();
+      manageServoMoveProcess();
+    }
+  }
+  void initComputerValue()
+  {
+    for (int i = 0; i < (int)servo_scales_.size(); ++i)
+    {
+      servo_scales_[i] = 0.;
+    }
+    move_gather_[0] = x_;
+    move_gather_[1] = y_;
+    move_gather_[2] = z_;
+  }
+  void computeServoMoveError()
+  {
+    initComputerValue();
+    geometry_msgs::TransformStamped tools2exchanger;
+    try
+    {
+      tools2exchanger = tf_buffer_.lookupTransform("tools_link", "exchanger", ros::Time(0));
+    }
+    catch (tf2::TransformException& ex)
+    {
+      ROS_WARN("%s", ex.what());
+    }
+    x_.error = tools2exchanger.transform.translation.x - x_.offset_refer_exchanger;
+    y_.error = tools2exchanger.transform.translation.y - y_.offset_refer_exchanger;
+    z_.error = tools2exchanger.transform.translation.z - z_.offset_refer_exchanger;
+
+    ros::Duration dt = ros::Time::now() - last_time_;
+    last_time_ = ros::Time::now();
+
+    x_.getPidValue(dt);
+    y_.getPidValue(dt);
+    z_.getPidValue(dt);
+  }
+  void computeServoMoveScale()
+  {
+    computeServoMoveError();
+    switch (process_)
+    {
+      case SERVO_Z:
+      {
+        servo_scales_[0] = 0.;
+        servo_scales_[1] = 0.;
+        servo_scales_[2] = z_.pid_value;
+      }
+      break;
+      case SERVO_Y:
+      {
+        servo_scales_[0] = 0.;
+        servo_scales_[1] = y_.pid_value;
+        servo_scales_[2] = 0.;
+      }
+      break;
+      case SERVO_X:
+      {
+        servo_scales_[0] = x_.pid_value;
+        servo_scales_[1] = 0.;
+        servo_scales_[2] = 0.;
+      }
+      break;
+    }
+  }
+  void manageServoMoveProcess()
+  {
+    int move_joint_num = 0, arrived_joint_num = 0;
+    for (int i = 0; i < (int)servo_scales_.size(); ++i)
+    {
+      if (servo_scales_[i] != 0)
+      {
+        move_joint_num++;
+        if (abs(move_gather_[i].error) <= move_gather_[i].tolerance)
+          arrived_joint_num++;
+      }
+    }
+    if (arrived_joint_num == move_joint_num)
+    {
+      if (process_ != FINISH)
+        process_++;
+      else
+        is_finish_ = true;
+    }
+  }
+  bool is_motion_finish_{ false }, is_motion_start_{ false };
+  ros::Time last_time_;
+  SingleDirectionMove x_, y_, z_;
+  std::string motion_name_;
+  std::vector<double> servo_scales_{};
+  std::vector<SingleDirectionMove> move_gather_{};
 };
 
-class Finish
+// class MotionMove : public ProgressBase
+//{
+// public:
+//     enum MotionMoveProcess
+//     {
+//         SPHERE,
+//         LINE,
+//         POINT,
+//         FINISH
+//     };
+//     MotionMove(XmlRpc::XmlRpcValue& auto_servo_move, tf2_ros::Buffer& tf_buffer, ros::NodeHandle& nh)
+//     : ProgressBase(auto_servo_move, tf_buffer, nh) {
+//         process_ = SPHERE;
+//         last_process_ = process_;
+//         process_num_ = 4;
+//     }
+//
+//};
+//
+// class PostAdjust
+//{
+//};
+//
+// class Finish
+//{
+//};
+
+class AutoExchange : public ProgressBase
 {
-};
+public:
+  enum ExchangeProcess
+  {
+    FIND,
+    PRE_ADJUST,
+    MOVE,
+    FINISH
+  };
+  AutoExchange(XmlRpc::XmlRpcValue& auto_exchange, tf2_ros::Buffer& tf_buffer, ros::NodeHandle& nh)
+    : ProgressBase(auto_exchange, tf_buffer, nh)
+  {
+    process_ = FIND;
+    last_process_ = process_;
+    process_num_ = 4;
+    ros::NodeHandle nh_auto_find(nh, "auto_find");
+    ros::NodeHandle nh_auto_pre_adjust(nh, "auto_pre_adjust");
+    ros::NodeHandle nh_auto_servo_move(nh, "auto_servo_move");
+    ros::NodeHandle nh_union_move(nh, "union_move");
+    find_ = new Find(auto_exchange["auto_find"], tf_buffer, nh_auto_find);
+    pre_adjust_ = new ProAdjust(auto_exchange["auto_pre_adjust"], tf_buffer, nh_auto_pre_adjust);
+    auto_servo_move_ = new AutoServoMove(auto_exchange["auto_servo_move"], tf_buffer, nh_auto_servo_move);
+    union_move_ = new UnionMove(auto_exchange["union_move"], tf_buffer, nh_union_move);
+    exchanger_tf_update_pub_ = nh_.advertise<std_msgs::Bool>("/is_update_exchanger", 1);
+  }
+  void init() override
+  {
+    ProgressBase::init();
+    process_ = FIND;
+    find_->init();
+    pre_adjust_->init();
+    union_move_->init();
+    exchangerTfUpdate(true);
+  }
 
+public:
+  Find* find_{};
+  ProAdjust* pre_adjust_{};
+  AutoServoMove* auto_servo_move_{};
+  UnionMove* union_move_{};
+
+private:
+  void exchangerTfUpdate(bool is_exchanger_tf_update)
+  {
+    is_exchanger_tf_update_.data = is_exchanger_tf_update;
+    exchanger_tf_update_pub_.publish(is_exchanger_tf_update_);
+  }
+  void stateMachine() override
+  {
+    switch (process_)
+    {
+      case FIND:
+      {
+        exchangerTfUpdate(true);
+        find_->run();
+        if (find_->getFinishFlag())
+        {
+          if (find_->getTimeOutFlag() && find_->getProcess() == 0)
+          {
+            is_finish_ = true;
+            find_->init();
+          }
+          else
+          {
+            process_ = PRE_ADJUST;
+            find_->init();
+          }
+        }
+      }
+      break;
+      case PRE_ADJUST:
+      {
+        exchangerTfUpdate(true);
+        pre_adjust_->run();
+        if (pre_adjust_->getFinishFlag())
+        {
+          process_ = MOVE;
+          pre_adjust_->init();
+        }
+      }
+      break;
+      case MOVE:
+      {
+        exchangerTfUpdate(false);
+        union_move_->run();
+        if (union_move_->getFinishFlag())
+        {
+          is_finish_ = true;
+          union_move_->init();
+        }
+      }
+      break;
+    }
+  }
+  void printProcess() override
+  {
+    if (process_ == FIND)
+      ROS_INFO_STREAM("FIND");
+    else if (process_ == PRE_ADJUST)
+      ROS_INFO_STREAM("PRE_ADJUST");
+    else if (process_ == MOVE)
+      ROS_INFO_STREAM("MOVE");
+    else if (process_ == FINISH)
+      ROS_INFO_STREAM("FINISH");
+  }
+  // tf update
+  std_msgs::Bool is_exchanger_tf_update_{};
+  ros::Publisher exchanger_tf_update_pub_;
+};
 }  // namespace auto_exchange
diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 677237f..cc14eb4 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -207,7 +207,7 @@ class EndEffectorMotion : public MoveitMotionBase
     }
   }
 
-private:
+protected:
   bool isReachGoal() override
   {
     geometry_msgs::Pose pose = interface_.getCurrentPose().pose;
@@ -235,10 +235,18 @@ class SpaceEeMotion : public EndEffectorMotion
 public:
   SpaceEeMotion(XmlRpc::XmlRpcValue& motion, moveit::planning_interface::MoveGroupInterface& interface,
                 tf2_ros::Buffer& tf)
-    : EndEffectorMotion(motion, interface, tf), tf_(tf)
+    : EndEffectorMotion(motion, interface, tf)
   {
+    drift_dimensions_.resize(6, true);
     radius_ = xmlRpcGetDouble(motion, "radius", 0.1);
     link7_length_ = xmlRpcGetDouble(motion, "link7_length", 0.);
+    if (motion.hasMember("drift_dimensions"))
+    {
+      for (int i = 0; i < (int)drift_dimensions_.size(); ++i)
+      {
+        drift_dimensions_[i] = motion["drift_dimensions"][i];
+      }
+    }
     if (motion.hasMember("basics_length"))
     {
       ROS_ASSERT(motion["basics_length"].getType() == XmlRpc::XmlRpcValue::TypeArray);
@@ -267,6 +275,10 @@ class SpaceEeMotion : public EndEffectorMotion
         ROS_ERROR("NO SUCH SHAPE");
       points_.generateGeometryPoints();
     }
+    if (motion.hasMember("is_refer_planning_frame"))
+      is_refer_planning_frame_ = motion["is_refer_planning_frame"];
+    else
+      is_refer_planning_frame_ = false;
   }
   bool move() override
   {
@@ -278,39 +290,85 @@ class SpaceEeMotion : public EndEffectorMotion
     {
       if (!target_.header.frame_id.empty())
       {
-        try
+        if (!is_refer_planning_frame_)
         {
-          double roll, pitch, yaw, roll_exchange2base, pitch_exchange2base, yaw__exchange2base;
-          exchange2base_ = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
-          quatToRPY(exchange2base_.transform.rotation, roll_exchange2base, pitch_exchange2base, yaw__exchange2base);
-          quatToRPY(target_.pose.orientation, roll, pitch, yaw);
-          pitch -= pitch_exchange2base;
-          tf2::Quaternion tmp_tf_quaternion;
-          tmp_tf_quaternion.setRPY(roll, pitch, yaw);
-          geometry_msgs::Quaternion quat_tf = tf2::toMsg(tmp_tf_quaternion);
-          target_.pose.orientation = quat_tf;
-          quatToRPY(target_.pose.orientation, roll, pitch, yaw);
-          points_.rectifyForRPY(pitch_exchange2base, yaw__exchange2base, k_x_, k_theta_, k_beta_);
-          if (link7_length_ > 0)
-            points_.rectifyForLink7(pitch_exchange2base, link7_length_);
-          target_.pose.position.x = points_.getPoints()[i].x;
-          target_.pose.position.y = points_.getPoints()[i].y;
-          target_.pose.position.z = points_.getPoints()[i].z;
-          tf2::doTransform(target_.pose, final_target_.pose,
-                           tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0)));
-          final_target_.header.frame_id = interface_.getPlanningFrame();
+          try
+          {
+            double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
+            geometry_msgs::TransformStamped exchange2base;
+            exchange2base = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
+            quatToRPY(exchange2base.transform.rotation, roll_temp, pitch_temp, yaw_temp);
+            quatToRPY(target_.pose.orientation, roll, pitch, yaw);
+            roll = drift_dimensions_[3] ? roll : (roll - roll_temp);
+            pitch = drift_dimensions_[4] ? pitch : (pitch - pitch_temp);
+            yaw = drift_dimensions_[5] ? yaw : (yaw - yaw_temp);
+
+            tf2::Quaternion tmp_tf_quaternion;
+            tmp_tf_quaternion.setRPY(roll, pitch, yaw);
+            geometry_msgs::Quaternion quat_tf = tf2::toMsg(tmp_tf_quaternion);
+            quatToRPY(quat_tf, roll, pitch, yaw);
+            points_.rectifyForRPY(pitch_temp, yaw_temp, k_x_, k_theta_, k_beta_);
+            if (link7_length_)
+              points_.rectifyForLink7(pitch_temp, link7_length_);
+            target_.pose.position.x = points_.getPoints()[i].x;
+            target_.pose.position.y = points_.getPoints()[i].y;
+            target_.pose.position.z = points_.getPoints()[i].z;
+            geometry_msgs::PoseStamped temp_target;
+            temp_target.pose.position = target_.pose.position;
+            temp_target.pose.orientation = quat_tf;
+            tf2::doTransform(temp_target.pose, final_target_.pose,
+                             tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0)));
+            final_target_.pose.position.x = drift_dimensions_[0] ? final_target_.pose.position.x : 0.;
+            final_target_.pose.position.y = drift_dimensions_[1] ? final_target_.pose.position.y : 0.;
+            final_target_.pose.position.z = drift_dimensions_[2] ? final_target_.pose.position.z : 0.;
+            final_target_.header.frame_id = interface_.getPlanningFrame();
+          }
+          catch (tf2::TransformException& ex)
+          {
+            ROS_WARN("%s", ex.what());
+            return false;
+          }
         }
-        catch (tf2::TransformException& ex)
+        else
         {
-          ROS_WARN("%s", ex.what());
-          return false;
+          try
+          {
+            double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
+            geometry_msgs::TransformStamped base2exchange;
+            base2exchange = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
+            target_.pose.position.x = points_.getPoints()[i].x;
+            target_.pose.position.y = points_.getPoints()[i].y;
+            target_.pose.position.z = points_.getPoints()[i].z;
+            quatToRPY(base2exchange.transform.rotation, roll, pitch, yaw);
+            quatToRPY(target_.pose.orientation, roll_temp, pitch_temp, yaw_temp);
+            roll = drift_dimensions_[3] ? (roll + roll_temp) : 0.;
+            pitch = drift_dimensions_[4] ? (pitch + pitch_temp) : 0.;
+            yaw = drift_dimensions_[5] ? (yaw + yaw_temp) : 0.;
+            tf2::Quaternion tf_quaternion;
+            tf_quaternion.setRPY(roll, pitch, yaw);
+            geometry_msgs::Quaternion quat_tf = tf2::toMsg(tf_quaternion);
+
+            final_target_.pose.position.x =
+                drift_dimensions_[0] ? base2exchange.transform.translation.x + target_.pose.position.x : 0.;
+            final_target_.pose.position.y =
+                drift_dimensions_[1] ? base2exchange.transform.translation.y + target_.pose.position.y : 0.;
+            final_target_.pose.position.z =
+                drift_dimensions_[2] ? base2exchange.transform.translation.z + target_.pose.position.z : 0.;
+            final_target_.pose.orientation = quat_tf;
+            final_target_.header.frame_id = interface_.getPlanningFrame();
+          }
+          catch (tf2::TransformException& ex)
+          {
+            ROS_WARN("%s", ex.what());
+            return false;
+          }
         }
+        interface_.setPoseTarget(final_target_);
+        moveit::planning_interface::MoveGroupInterface::Plan plan;
+        msg_.data = interface_.plan(plan).val;
+        if (msg_.data == 1)
+          return interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
       }
-      interface_.setPoseTarget(final_target_);
-      moveit::planning_interface::MoveGroupInterface::Plan plan;
-      msg_.data = interface_.plan(plan).val;
-      if (msg_.data == 1)
-        return interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
     }
     return false;
   }
@@ -321,22 +379,21 @@ class SpaceEeMotion : public EndEffectorMotion
     geometry_msgs::Pose pose = interface_.getCurrentPose().pose;
     double roll_current, pitch_current, yaw_current, roll_goal, pitch_goal, yaw_goal;
     quatToRPY(pose.orientation, roll_current, pitch_current, yaw_current);
-    quatToRPY(target_.pose.orientation, roll_goal, pitch_goal, yaw_goal);
-    return (std::pow(pose.position.x - target_.pose.position.x, 2) +
-                    std::pow(pose.position.y - target_.pose.position.y, 2) +
-                    std::pow(pose.position.z - target_.pose.position.z, 2) <
+    quatToRPY(final_target_.pose.orientation, roll_goal, pitch_goal, yaw_goal);
+    return (std::pow(pose.position.x - final_target_.pose.position.x, 2) +
+                    std::pow(pose.position.y - final_target_.pose.position.y, 2) +
+                    std::pow(pose.position.z - final_target_.pose.position.z, 2) <
                 tolerance_position_ &&
             std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) +
                     std::abs(angles::shortest_angular_distance(pitch_current, pitch_goal)) +
                     std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) <
                 tolerance_orientation_);
   }
-  tf2_ros::Buffer& tf_;
-  geometry_msgs::PoseStamped target_, final_target_;
-  geometry_msgs::TransformStamped exchange2base_;
+  bool is_refer_planning_frame_;
+  std::vector<bool> drift_dimensions_;
+  geometry_msgs::PoseStamped final_target_;
   int max_planning_times_{};
-  double radius_, point_resolution_, x_length_, y_length_, z_length_, k_theta_, k_beta_, k_x_, tolerance_position_,
-      tolerance_orientation_, link7_length_;
+  double radius_, point_resolution_, x_length_, y_length_, z_length_, k_theta_, k_beta_, k_x_, link7_length_;
 };
 
 class JointMotion : public MoveitMotionBase
@@ -350,11 +407,14 @@ class JointMotion : public MoveitMotionBase
       ROS_ASSERT(motion["joints"].getType() == XmlRpc::XmlRpcValue::TypeArray);
       for (int i = 0; i < motion["joints"].size(); ++i)
       {
-        ROS_ASSERT(motion["joints"][i].getType() == XmlRpc::XmlRpcValue::TypeDouble || motion["joints"][i] == "KEEP");
         if (motion["joints"][i].getType() == XmlRpc::XmlRpcValue::TypeDouble)
           target_.push_back(motion["joints"][i]);
         else if (motion["joints"][i] == "KEEP")
           target_.push_back(NAN);
+        else if (motion["joints"][i] == "VARIABLE")
+          target_.push_back(motion["variable"][i]);
+        else
+          ROS_ERROR("ERROR TYPE OR STRING!!!");
       }
     }
     if (motion.hasMember("tolerance"))
@@ -375,9 +435,12 @@ class JointMotion : public MoveitMotionBase
       if (!std::isnormal(target_[i]))
       {
         final_target_.push_back(interface_.getCurrentJointValues()[i]);
+        ROS_INFO_STREAM(target_[i]);
       }
       else
+      {
         final_target_.push_back(target_[i]);
+      }
     }
     interface_.setJointValueTarget(final_target_);
     moveit::planning_interface::MoveGroupInterface::Plan plan;
@@ -498,7 +561,7 @@ class JointPositionMotion : public PublishMotion<std_msgs::Float64>
   {
     double roll, pitch, yaw;
     geometry_msgs::TransformStamped tf;
-    tf = tf_.lookupTransform(reference_tf_, original_tf_, ros::Time(0));
+    tf = tf_.lookupTransform(original_tf_, reference_tf_, ros::Time(0));
     quatToRPY(tf.transform.rotation, roll, pitch, yaw);
     start_time_ = ros::Time::now();
     if (direction_ == "roll")
@@ -548,8 +611,8 @@ class ChassisMotion : public MotionBase<ChassisInterface>
   ChassisMotion(XmlRpc::XmlRpcValue& motion, ChassisInterface& interface)
     : MotionBase<ChassisInterface>(motion, interface)
   {
-    chassis_tolerance_position_ = xmlRpcGetDouble(motion, "chassis_tolerance_position_", 0.01);
-    chassis_tolerance_angular_ = xmlRpcGetDouble(motion, "chassis_tolerance_angular_", 0.01);
+    chassis_tolerance_position_ = xmlRpcGetDouble(motion, "chassis_tolerance_position", 0.01);
+    chassis_tolerance_angular_ = xmlRpcGetDouble(motion, "chassis_tolerance_angular", 0.01);
     if (motion.hasMember("frame"))
       target_.header.frame_id = std::string(motion["frame"]);
     if (motion.hasMember("position"))
diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h
index b8d7ef3..c87162a 100644
--- a/engineer_middleware/include/engineer_middleware/step.h
+++ b/engineer_middleware/include/engineer_middleware/step.h
@@ -150,6 +150,8 @@ class Step
       success &= chassis_motion_->isFinish();
     if (gimbal_motion_)
       success &= gimbal_motion_->isFinish();
+    if (reversal_motion_)
+      success &= reversal_motion_->isFinish();
     return success;
   }
   bool checkTimeout(ros::Duration period)

From 58e6edea120d7b7d1538b819c91bfb219a0e2490 Mon Sep 17 00:00:00 2001
From: LSY <2028238109@qq.com>
Date: Sat, 12 Aug 2023 08:58:48 +0800
Subject: [PATCH 021/147] record.

---
 .../include/engineer_middleware/chassis_interface.h         | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/chassis_interface.h b/engineer_middleware/include/engineer_middleware/chassis_interface.h
index 0838806..95e437f 100644
--- a/engineer_middleware/include/engineer_middleware/chassis_interface.h
+++ b/engineer_middleware/include/engineer_middleware/chassis_interface.h
@@ -136,8 +136,10 @@ class ChassisInterface
     quatToRPY(goal_.pose.orientation, roll, pitch, yaw_goal);
     error_yaw_ = angles::shortest_angular_distance(yaw_current, yaw_goal);
     geometry_msgs::Twist cmd_vel{};
-    cmd_vel.linear.x = pid_x_.computeCommand(error.x, period) <= max_vel_ ? pid_x_.computeCommand(error.x, period) : 0;
-    cmd_vel.linear.y = pid_y_.computeCommand(error.y, period) <= max_vel_ ? pid_y_.computeCommand(error.y, period) : 0;
+    cmd_vel.linear.x =
+        abs(pid_x_.computeCommand(error.x, period)) <= max_vel_ ? pid_x_.computeCommand(error.x, period) : 0;
+    cmd_vel.linear.y =
+        abs(pid_y_.computeCommand(error.y, period)) <= max_vel_ ? pid_y_.computeCommand(error.y, period) : 0;
     cmd_vel.angular.z = abs(pid_yaw_.computeCommand(error_yaw_, period)) >= yaw_start_threshold_ ?
                             pid_yaw_.computeCommand(error_yaw_, period) :
                             0;

From 214efaf8edc40dd004f8cec40bac5e75dfaae0cb Mon Sep 17 00:00:00 2001
From: cc0h <792166012@qq.com>
Date: Sat, 4 Nov 2023 11:45:31 +0800
Subject: [PATCH 022/147] change kinematics_solver.

---
 engineer_arm_config/config/kinematics.yaml |   6 +-
 engineer_arm_config/launch/moveit.rviz     | 147 ++++++---------------
 2 files changed, 41 insertions(+), 112 deletions(-)

diff --git a/engineer_arm_config/config/kinematics.yaml b/engineer_arm_config/config/kinematics.yaml
index d89ddcb..a4b51af 100644
--- a/engineer_arm_config/config/kinematics.yaml
+++ b/engineer_arm_config/config/kinematics.yaml
@@ -1,4 +1,4 @@
 engineer_arm:
-  kinematics_solver: engineer_arm/IKFastKinematicsPlugin
-  kinematics_solver_search_resolution: 0.03
-  kinematics_solver_timeout: 0.03
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.005
diff --git a/engineer_arm_config/launch/moveit.rviz b/engineer_arm_config/launch/moveit.rviz
index 9943155..60c0e48 100644
--- a/engineer_arm_config/launch/moveit.rviz
+++ b/engineer_arm_config/launch/moveit.rviz
@@ -6,10 +6,11 @@ Panels:
       Expanded:
         - /MotionPlanning1
         - /MotionPlanning1/Planning Metrics1
+        - /MotionPlanning1/Planned Path1
         - /TF1
         - /TF1/Frames1
       Splitter Ratio: 0.5
-    Tree Height: 357
+    Tree Height: 315
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -87,7 +88,7 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          fake_link5:
+          drag_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
@@ -141,6 +142,10 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          mimic_drag_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           pitch:
             Alpha: 1
             Show Axes: false
@@ -176,6 +181,10 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
+          tools_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           yaw:
             Alpha: 1
             Show Axes: false
@@ -185,7 +194,7 @@ Visualization Manager:
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
         Show Robot Collision: false
-        Show Robot Visual: false
+        Show Robot Visual: true
         Show Trail: false
         State Display Time: 0.05 s
         Trail Step Size: 1
@@ -241,7 +250,7 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          fake_link5:
+          drag_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
@@ -295,6 +304,10 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          mimic_drag_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           pitch:
             Alpha: 1
             Show Axes: false
@@ -330,6 +343,10 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
+          tools_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           yaw:
             Alpha: 1
             Show Axes: false
@@ -341,62 +358,12 @@ Visualization Manager:
       Value: true
       Velocity_Scaling_Factor: 0.1
     - Class: rviz/TF
-      Enabled: true
+      Enabled: false
+      Filter (blacklist): ""
+      Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: false
-        base_link:
-          Value: true
-        camera_link:
-          Value: false
-        camera_optical_frame:
-          Value: true
-        exchanger:
-          Value: true
-        fake_link5:
-          Value: false
-        gimbal_base:
-          Value: false
-        left_back_wheel:
-          Value: false
-        left_front_wheel:
-          Value: false
-        link1:
-          Value: true
-        link2:
-          Value: true
-        link3:
-          Value: true
-        link4:
-          Value: true
-        link5:
-          Value: true
-        link6:
-          Value: true
-        link7:
-          Value: true
-        map:
-          Value: true
-        odom:
-          Value: true
-        pitch:
-          Value: false
-        pitch_behind_link:
-          Value: false
-        pitch_front_link:
-          Value: false
-        right_back_wheel:
-          Value: false
-        right_front_wheel:
-          Value: false
-        roll_left_link:
-          Value: false
-        roll_right_link:
-          Value: false
-        tools_link:
-          Value: true
-        yaw:
-          Value: false
       Marker Alpha: 1
       Marker Scale: 1
       Name: TF
@@ -404,47 +371,9 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        map:
-          exchanger:
-            {}
-          odom:
-            base_link:
-              left_back_wheel:
-                {}
-              left_front_wheel:
-                {}
-              link1:
-                gimbal_base:
-                  yaw:
-                    pitch:
-                      camera_link:
-                        {}
-                      camera_optical_frame:
-                        {}
-                link2:
-                  link3:
-                    link4:
-                      link5:
-                        fake_link5:
-                          {}
-                        link6:
-                          link7:
-                            tools_link:
-                              {}
-              pitch_behind_link:
-                {}
-              pitch_front_link:
-                {}
-              right_back_wheel:
-                {}
-              right_front_wheel:
-                {}
-              roll_left_link:
-                {}
-              roll_right_link:
-                {}
+        {}
       Update Interval: 0
-      Value: true
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -461,7 +390,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 4.128435134887695
+      Distance: 1.5691611766815186
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -469,33 +398,33 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: -0.17658083140850067
-        Y: 0.4902980625629425
-        Z: 0.38593053817749023
+        X: 0.2072981297969818
+        Y: -0.0745711624622345
+        Z: 0.24762113392353058
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3947972059249878
+      Pitch: 0.49479714035987854
       Target Frame: base_link
-      Yaw: 1.5382111072540283
+      Yaw: 5.426367282867432
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: true
+    collapsed: false
   Height: 1016
   Help:
     collapsed: false
-  Hide Left Dock: true
+  Hide Left Dock: false
   Hide Right Dock: false
   MotionPlanning:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f30000039efc0200000007fb000000100044006900730070006c006100790073000000003d0000039e000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001ab000002300000017d00ffffff000007320000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f30000039efc0200000007fb000000100044006900730070006c006100790073010000003d000001cc000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000020f000001cc0000017d00ffffff000004880000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
-  Width: 1842
-  X: 78
-  Y: 27
+  Width: 1665
+  X: 465
+  Y: 245

From 5b20911e41d005b19317e502b9dd866375e37fe0 Mon Sep 17 00:00:00 2001
From: cc0h <792166012@qq.com>
Date: Sun, 5 Nov 2023 22:30:23 +0800
Subject: [PATCH 023/147] URDF for new arm on 2023 engineer.

---
 engineer_arm_config/launch/moveit.rviz | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/engineer_arm_config/launch/moveit.rviz b/engineer_arm_config/launch/moveit.rviz
index 60c0e48..7375e5e 100644
--- a/engineer_arm_config/launch/moveit.rviz
+++ b/engineer_arm_config/launch/moveit.rviz
@@ -213,7 +213,7 @@ Visualization Manager:
         Goal State Color: 250; 128; 0
         Interactive Marker Size: 0
         Joint Violation Color: 255; 0; 255
-        Planning Group: engineer_arm
+        Planning Group: ""
         Query Goal State: true
         Query Start State: false
         Show Workspace: false
@@ -390,7 +390,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.5691611766815186
+      Distance: 0.9410187602043152
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -398,17 +398,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: 0.2072981297969818
-        Y: -0.0745711624622345
-        Z: 0.24762113392353058
+        X: 0.3315882384777069
+        Y: 0.12195970118045807
+        Z: 0.27018725872039795
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.49479714035987854
+      Pitch: 0.4797971844673157
       Target Frame: base_link
-      Yaw: 5.426367282867432
+      Yaw: 5.201375961303711
     Saved: ~
 Window Geometry:
   Displays:
@@ -426,5 +426,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1665
-  X: 465
-  Y: 245
+  X: 575
+  Y: 235

From ae1b7f917b880f943950dd219edab18a4a3dd657 Mon Sep 17 00:00:00 2001
From: cc0h <792166012@qq.com>
Date: Wed, 15 Nov 2023 16:04:34 +0800
Subject: [PATCH 024/147] update engineer arm kinematic config.

---
 engineer_arm_config/config/engineer.srdf      |  4 ++--
 .../config/fake_controllers.yaml              |  1 +
 engineer_arm_config/config/joint_limits.yaml  |  5 +++++
 .../config/ros_controllers.yaml               |  2 ++
 .../launch/load_controllers.launch            |  1 +
 engineer_arm_config/launch/moveit.rviz        | 19 ++++++++++---------
 engineer_middleware/config/steps_list.yaml    |  6 +++---
 7 files changed, 24 insertions(+), 14 deletions(-)

diff --git a/engineer_arm_config/config/engineer.srdf b/engineer_arm_config/config/engineer.srdf
index 6d6db13..9818652 100644
--- a/engineer_arm_config/config/engineer.srdf
+++ b/engineer_arm_config/config/engineer.srdf
@@ -16,6 +16,7 @@
         <joint name="joint4"/>
         <joint name="joint5"/>
         <joint name="joint6"/>
+        <joint name="joint7"/>
     </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="home" group="engineer_arm">
@@ -25,6 +26,7 @@
         <joint name="joint4" value="0"/>
         <joint name="joint5" value="1.92225"/>
         <joint name="joint6" value="0"/>
+        <joint name="joint7" value="0"/>
     </group_state>
         <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
         <disable_collisions link1="base_link" link2="gimbal_base" reason="Adjacent"/>
@@ -42,11 +44,9 @@
         <disable_collisions link1="link2" link2="link4" reason="Never"/>
         <disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
         <disable_collisions link1="link3" link2="link5" reason="Default"/>
-        <disable_collisions link1="link3" link2="link6" reason="Never"/>
         <disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
         <disable_collisions link1="link4" link2="link6" reason="Never"/>
         <disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
-        <disable_collisions link1="link3" link2="link7" reason="Never"/>
         <disable_collisions link1="link6" link2="link7" reason="Never"/>
         <disable_collisions link1="link5" link2="link7" reason="Never"/>
 </robot>
diff --git a/engineer_arm_config/config/fake_controllers.yaml b/engineer_arm_config/config/fake_controllers.yaml
index 4167cc8..82506df 100644
--- a/engineer_arm_config/config/fake_controllers.yaml
+++ b/engineer_arm_config/config/fake_controllers.yaml
@@ -8,6 +8,7 @@ controller_list:
       - joint4
       - joint5
       - joint6
+      - joint7
 initial:  # Define initial robot poses per group
   - group: engineer_arm
     pose: home
diff --git a/engineer_arm_config/config/joint_limits.yaml b/engineer_arm_config/config/joint_limits.yaml
index f0cf0d3..a1ea2d6 100644
--- a/engineer_arm_config/config/joint_limits.yaml
+++ b/engineer_arm_config/config/joint_limits.yaml
@@ -38,3 +38,8 @@ joint_limits:
     max_velocity: 50
     has_acceleration_limits: true
     max_acceleration: 90
+  joint7:
+    has_velocity_limits: true
+    max_velocity: 50
+    has_acceleration_limits: true
+    max_acceleration: 90
diff --git a/engineer_arm_config/config/ros_controllers.yaml b/engineer_arm_config/config/ros_controllers.yaml
index a4837dc..52ad421 100644
--- a/engineer_arm_config/config/ros_controllers.yaml
+++ b/engineer_arm_config/config/ros_controllers.yaml
@@ -15,6 +15,7 @@ hardware_interface:
     - joint4
     - joint5
     - joint6
+    - joint7
   sim_control_mode: 1  # 0: position, 1: velocity
 
 controller_list:
@@ -29,3 +30,4 @@ controller_list:
       - joint4
       - joint5
       - joint6
+      - joint7
diff --git a/engineer_arm_config/launch/load_controllers.launch b/engineer_arm_config/launch/load_controllers.launch
index 5237054..47d0bfb 100644
--- a/engineer_arm_config/launch/load_controllers.launch
+++ b/engineer_arm_config/launch/load_controllers.launch
@@ -18,6 +18,7 @@
                 controllers/joint4_calibration_controller
                 controllers/joint5_calibration_controller
                 controllers/joint6_calibration_controller
+                controllers/joint7_calibration_controller
                 controllers/arm_trajectory_controller
                 controllers/chassis_controller
                 controllers/gpio_controller
diff --git a/engineer_arm_config/launch/moveit.rviz b/engineer_arm_config/launch/moveit.rviz
index 7375e5e..ab46ed2 100644
--- a/engineer_arm_config/launch/moveit.rviz
+++ b/engineer_arm_config/launch/moveit.rviz
@@ -5,6 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /MotionPlanning1
+        - /MotionPlanning1/Planning Request1
         - /MotionPlanning1/Planning Metrics1
         - /MotionPlanning1/Planned Path1
         - /TF1
@@ -213,7 +214,7 @@ Visualization Manager:
         Goal State Color: 250; 128; 0
         Interactive Marker Size: 0
         Joint Violation Color: 255; 0; 255
-        Planning Group: ""
+        Planning Group: engineer_arm
         Query Goal State: true
         Query Start State: false
         Show Workspace: false
@@ -390,7 +391,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.9410187602043152
+      Distance: 0.8833990097045898
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -398,17 +399,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: 0.3315882384777069
-        Y: 0.12195970118045807
-        Z: 0.27018725872039795
+        X: 0.7602222561836243
+        Y: -0.15494105219841003
+        Z: 0.9399657249450684
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4797971844673157
+      Pitch: 0.5947970747947693
       Target Frame: base_link
-      Yaw: 5.201375961303711
+      Yaw: 1.4263750314712524
     Saved: ~
 Window Geometry:
   Displays:
@@ -426,5 +427,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1665
-  X: 575
-  Y: 235
+  X: 757
+  Y: 84
diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 85d173e..b748543 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -48,7 +48,7 @@ common:
                      0.43
     home:
       zero_stone_position: &JOINT1_ZERO_STONE_POSITION
-                             0.001
+                             0.08
       one_stone_position: &JOINT1_ONE_STONE_POSITION
                             0.08
       two_stone_position: &JOINT1_TWO_STONE_POSITION
@@ -132,7 +132,7 @@ common:
       right_position: &JOINT5_RT_POSITION
                         -1.54
       mid_position: &JOINT5_MID_POSITION
-                      0.01
+                      -1.538
       left_position: &JOINT5_LF_POSITION
                        1.25
 
@@ -246,7 +246,7 @@ common:
         <<: *NORMAL_TOLERANCE
 
     home: &HOME
-      joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
+      joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_LF_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
       common:
         <<: *QUICKLY
       tolerance:

From 9cb68a4711b95802622d339f2f34244ccd62d930 Mon Sep 17 00:00:00 2001
From: cc0h <792166012@qq.com>
Date: Sun, 26 Nov 2023 18:55:27 +0800
Subject: [PATCH 025/147] test record.

---
 engineer_middleware/config/steps_list.yaml    | 42 +++++++++++++++++--
 .../include/engineer_middleware/motion.h      |  1 -
 2 files changed, 38 insertions(+), 5 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index b748543..7876482 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -31,9 +31,9 @@ common:
 
   tolerance:
     small_tolerance: &SMALL_TOLERANCE
-      tolerance_joints: [ 0.005, 0.008, 0.01, 0.1, 0.1, 0.1 ]
+      tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1 ]
     normal_tolerance: &NORMAL_TOLERANCE
-      tolerance_joints: [ 0.005, 0.010, 0.01, 0.2, 0.15, 0.15 ]
+      tolerance_joints: [ 0.01, 0.010, 0.015, 0.3, 0.2, 0.15, 0.1 ]
     bigger_tolerance: &BIGGER_TOLERANCE
       tolerance_joints: [ 0.005, 0.010, 0.01, 0.3, 0.2, 0.2 ]
     max_tolerance: &MAX_TOLERANCE
@@ -248,9 +248,9 @@ common:
     home: &HOME
       joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_LF_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
       common:
-        <<: *QUICKLY
+        <<: *NORMALLY
       tolerance:
-        <<: *SMALL_TOLERANCE
+        <<: *NORMAL_TOLERANCE
 
 
     get_ore:
@@ -1428,3 +1428,37 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+
+  HOME:
+    - step: "home test"
+      arm:
+        joints: [ 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  HOME2:
+    - step: "home2"
+      arm:
+        joints: [ 0.3, 0.2, 0.3, 0.01, 0.31, 0.31, 2.31 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  TEST1:
+    - step: "home test"
+      arm:
+        joints: [ 0.1, 0.1, 0.01, 0.01, 0.01, 0.01, 0.01 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "home2"
+      arm:
+        joints: [ 0.3, 0.2, 0.3, 0.01, 0.31, 0.31, 2.31 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index cc14eb4..591ca8c 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -435,7 +435,6 @@ class JointMotion : public MoveitMotionBase
       if (!std::isnormal(target_[i]))
       {
         final_target_.push_back(interface_.getCurrentJointValues()[i]);
-        ROS_INFO_STREAM(target_[i]);
       }
       else
       {

From 4c84e89c538e63d9baba430d3c2b5299e492d9db Mon Sep 17 00:00:00 2001
From: cc0h <792166012@qq.com>
Date: Tue, 5 Dec 2023 22:30:35 +0800
Subject: [PATCH 026/147] add missing head files.

---
 engineer_middleware/include/engineer_middleware/auto_exchange.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/engineer_middleware/include/engineer_middleware/auto_exchange.h b/engineer_middleware/include/engineer_middleware/auto_exchange.h
index e7ba8ca..52af56c 100644
--- a/engineer_middleware/include/engineer_middleware/auto_exchange.h
+++ b/engineer_middleware/include/engineer_middleware/auto_exchange.h
@@ -9,6 +9,8 @@
 #include <rm_msgs/ExchangerMsg.h>
 #include <control_toolbox/pid.h>
 #include <angles/angles.h>
+#include <geometry_msgs/Twist.h>
+#include <std_msgs/Bool.h>
 
 namespace auto_exchange
 {

From a9059d315349ab40caa59ded0cb705c4e058a6bb Mon Sep 17 00:00:00 2001
From: cc0h <792166012@qq.com>
Date: Sat, 9 Dec 2023 22:32:11 +0800
Subject: [PATCH 027/147] new step list for engineer.

---
 engineer_middleware/config/steps_list.yaml | 1360 ++++----------------
 1 file changed, 257 insertions(+), 1103 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 7876482..e38c049 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -1,9 +1,5 @@
 common:
   speed:
-    slowly_low_timeout: &SLOWLY_LOW_TIMEOUT
-      speed: 0.4
-      accel: 0.3
-      timeout: 3.
     slowly: &SLOWLY
       speed: 0.15
       accel: 0.1
@@ -12,22 +8,10 @@ common:
       speed: 0.4
       accel: 0.3
       timeout: 3
-    normally_high_timeout: &NORMALLY_HIGH_TIMEOUT
-      speed: 0.4
-      accel: 0.3
-      timeout: 4
     quickly: &QUICKLY
-      speed: 0.8
-      accel: 0.8
-      timeout: 7.
-    little_quickly: &LITTLE_QUICKLY
       speed: 0.6
       accel: 0.6
-      timeout: 1.
-    joint6_normally: &JOINT6_NORMALLY
-      speed: 0.4
-      accel: 0.3
-      timeout: 2.
+      timeout: 7.
 
   tolerance:
     small_tolerance: &SMALL_TOLERANCE
@@ -39,7 +23,6 @@ common:
     max_tolerance: &MAX_TOLERANCE
       tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 ]
 
-
   joint1:
     mechanical:
       down_position: &JOINT1_DOWN_POSITION
@@ -57,18 +40,13 @@ common:
                               0.35
     get_ore:
       ready_get_ore: &JOINT1_READY_TO_GET_ORE_POSITION
-                       0.44
+                       0.15
       get_ore: &JOINT1_GET_ORE_POSITION
-                 0.265
+                 0.15
       store_ore_position: &JOINT1_STORE_ORE_POSITION
                             0.25
     exchange_position: &JOINT1_EXCHANGE_POSITION
                          0.4
-    ground_stone:
-      ready_to_get_ground_stone: &JOINT1_READY_TO_GET_GROUND_STONE
-                                   0.15184
-      get_ground_stone: &JOINT1_GET_GROUND_STONE
-                          0.02545
 
   joint2:
     mechanical:
@@ -79,123 +57,66 @@ common:
     store:
       access_store_position: &JOINT2_ACCESS_STORE_POSITION
                                0.00001
-      ready_to_access_stone_position: &JOINT2_READY_TO_ACCESS_STONE_POSITION
+      ready_to_access_stone_position: &JOINT2_READY_TO_ACCESS_STORE_POSITION
                                         0.06
     exchange_position: &JOINT2_EXCHANGE_POSITION
                          0.05
     get_ore_position: &JOINT2_GET_ORE_POSITION
-                        0.002
+                        0.05
 
   joint3:
     mechanical:
-      left_position: &JOINT3_LF_POSITION
+      left_position: &JOINT3_L_POSITION
                        0.28
       mid_position: &JOINT3_MID_POSITION
                       0.0001
-      right_position: &JOINT3_RT_POSITION
+      right_position: &JOINT3_R_POSITION
                         -0.275
     get_ore:
-      lf_get_ore_position: &JOINT3_LF_GET_ORE_POSITION
-                             0.2560
+      lf_get_ore_position: &JOINT3_L_GET_ORE_POSITION
+                             0.28
       mid_get_ore_position: &JOINT3_MID_GET_ORE_POSITION
-                              0.00
-      rt_get_ore_position: &JOINT3_RT_GET_ORE_POSITION
-                             -0.266600
-      lf_avoid_other_ore: &JOINT3_LF_AVOID_ORE
-                            0.3
-      mid_avoid_other_ore: &JOINT3_MID_AVOID_ORE
-                             0.1
-      rt_avoid_other_ore: &JOINT3_RT_AVOID_ORE
-                            -0.2
-      sky_store_position: &JOINT3_SKY_STORE_POSITION
-                            0.2515
-      gold_ore_position: &JOINT3_GOLD_ORE_POSITION
-                           0.24564
-
+                              0.124
+      rt_get_ore_position: &JOINT3_R_GET_ORE_POSITION
+                             -0.1
     store_and_take:
       store_stone_position: &JOINT3_STORE_STONE_POSITION
-                              0.187890
+                              0.235
       take_stone_position: &JOINT3_TAKE_STONE_POSITION
-                             0.255230
+                             0.235230
 
   joint4:
     mechanical:
-      up_position: &JOINT4_UP_POSITION
-                     0.0001
+      left_position: &JOINT4_L_POSITION
+                       1.00
       mid_position: &JOINT4_MID_POSITION
-                      0.7
-      down_position: &JOINT4_DOWN_POSITION
-                       1.55
-
+                      0.001
+      right_position: &JOINT4_R_POSITION
+                        -1.59
   joint5:
     mechanical:
-      right_position: &JOINT5_RT_POSITION
-                        -1.54
       mid_position: &JOINT5_MID_POSITION
-                      -1.538
-      left_position: &JOINT5_LF_POSITION
-                       1.25
-
+                      0.001
+      left_90_position: &JOINT5_L90_POSITION
+                          -1.54
+      right_90_position: &JOINT5_R90_POSITION
+                           1.597
   joint6:
-    up_position: &JOINT6_UP_POSITION
-                   0.0001
-    down_position: &JOINT6_DOWN_POSITION
-                     3.1415926
-    access_stone: &JOINT6_ACCESS_STONE_POSITION
-                    1.562120
-    little_back_position: &JOINT6_LITTLE_BACK_POSITION
-                            2.167400
-    place_on_reverse: &JOINT6_PLACE_STONE_POSITION
-                        1.047
-    block_dart_position: &JOINT6_BLOCK_DART_POSITION
-                           0.5236
-
+    mechanical:
+      mid_position: &JOINT6_MID_POSITION
+                      0.001
+      up_position: &JOINT6_UP_POSITION
+                     -1.55
+      down_position: &JOINT6_DOWN_POSITION
+                       1.569
   joint7:
-    use_for_delay: &DELAY_0_1
-      target: 0.001
-      delay: 0.1
-    up_position: &JOINT7_UP_POSITION
-      target: 0.001
-      delay: 0.1
-    up_position_no_delay: &JOINT7_UP_POSITION_NO_DELAY
-      target: 0.001
-      delay: 0.
-    mid_more_position: &JOINT7_MID_MORE_POSITION
-      target: 0.25
-      delay: 0.1
-    mid_more_position_no_delay: &JOINT7_MID_MORE_POSITION_NO_DELAY
-      target: 0.25
-      delay: 0.
-    mid_position: &JOINT7_MID_POSITION
-      target: 0.5
-      delay: 0.1
-    mid_position_no_delay: &JOINT7_MID_POSITION_NO_DELAY
-      target: 0.5
-      delay: 0.
-    mid_less_position: &JOINT7_MID_LESS_POSITION
-      target: 0.75
-      delay: 0.1
-    mid_less_position_no_delay: &JOINT7_MID_LESS_POSITION_NO_DELAY
-      target: 0.75
-      delay: 0.
-    down_position: &JOINT7_DOWN_POSITION
-      target: 1.0
-      delay: 0.1
-    down_position_no_delay: &JOINT7_DOWN_POSITION_NO_DELAY
-      target: 1.575
-      delay: 0.
-    auto_exchange_position: &JOINT7_AUTO_EXCHANGE_POSITION
-      original_tf: link4
-      reference_tf: base_link
-      direction: pitch
-      delay: 0.05
-
-  gripper:
-    open_gripper: &OPEN_GRIPPER
-      state: true
-    close_gripper: &CLOSE_GRIPPER
-      state: false
-
+    mechanical:
+      mid_position: &JOINT7_MID_POSITION
+                      0.001
+      left_90_position: &JOINT7_L90_POSITION
+                          -1.59
+      right_90_position: &JOINT7_R90_POSITION
+                           1.59
   gimbal:
     side_pos: &SIDE_POS
       frame: gimbal_base
@@ -206,647 +127,301 @@ common:
     ground_pos: &GROUND_POS
       frame: gimbal_base
       position: [ 2, 0.001, -0.8 ]
-    exchange_pos: &EXCHANGE_POS
-      frame: gimbal_base
-      position: [ 0, -3, -2.3 ]
-    drag_pos1: &DRAG_POSE1
-      frame: gimbal_base
-      position: [ 0.001, 2, -0.8 ]
-    drag_pos2: &DRAG_POSE2
-      frame: gimbal_base
-      position: [ 0.001, -2, 0.001 ]
-
-  arm:
-    reverse_joint6_up: &REVERSE_JOINT6_UP
-      joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_UP_POSITION ]
-      common:
-        <<: *QUICKLY
-      tolerance:
-        <<: *NORMAL_TOLERANCE
 
-    reverse_joint6_down: &REVERSE_JOINT6_DOWN
-      joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_DOWN_POSITION ]
-      common:
-        <<: *QUICKLY
-      tolerance:
-        <<: *NORMAL_TOLERANCE
-
-    reverse_ore_up: &REVERSE_ORE_UP
-      joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_UP_POSITION ]
-      common:
-        <<: *NORMALLY
-      tolerance:
-        <<: *NORMAL_TOLERANCE
+  gripper:
+    open_gripper: &OPEN_GRIPPER
+      state: true
+    close_gripper: &CLOSE_GRIPPER
+      state: false
 
-    take_back_ore_after_catch_drop: &TAKE_BACK_ORE
-      joints: [ "KEEP", *JOINT2_READY_TO_ACCESS_STONE_POSITION, "KEEP", "KEEP", "KEEP", "KEEP" ]
+  arm:
+    reverse_joint5_up: &REVERSE_JOINT5_UP
+      joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
-
-    home: &HOME
-      joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_LF_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
+    home: &HOME_1
+      joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
 
-
     get_ore:
-      ready_to_get_ore: &READY_TO_GET_ORE
-        joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, "VARIABLE" ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *BIGGER_TOLERANCE
-      get_ore: &GET_ORE
-        joints: [ *JOINT1_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *SMALL_TOLERANCE
-      catch_drop: &CATCH_DROP
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_FURTHEST_POSITION, *JOINT3_MID_GET_ORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
+      ready_to_get_ore: &READY_TO_GET_ORE_3
+        joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_BACK_POSITION, "VARIABLE", *JOINT4_R_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_R90_POSITION ]
         common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *SMALL_TOLERANCE
-      ready_to_get_gold_ore: &READY_TO_GET_GOLD_ORE
-        joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_FURTHEST_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, "VARIABLE" ]
-        common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
-          <<: *BIGGER_TOLERANCE
-      get_gold_ore: &GET_GOLD_ORE
-        joints: [ *JOINT1_GET_ORE_POSITION, *JOINT2_FURTHEST_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_DOWN_POSITION ]
+          <<: *NORMAL_TOLERANCE
+      get_ore: &GET_ORE_3
+        joints: [ *JOINT1_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, "VARIABLE", *JOINT4_R_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_R90_POSITION ]
         common:
           <<: *QUICKLY
         tolerance:
           <<: *SMALL_TOLERANCE
-
-      ready_to_get_ground_stone: &READY_TO_GET_GROUND_STONE
-        joints: [ *JOINT1_READY_TO_GET_GROUND_STONE, *JOINT2_READY_TO_ACCESS_STONE_POSITION, *JOINT3_MID_POSITION, "VARIABLE", *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-      get_ground_stone: &GET_GROUND_STONE
-        joints: [ *JOINT1_GET_GROUND_STONE, *JOINT2_READY_TO_ACCESS_STONE_POSITION, *JOINT3_MID_POSITION, *JOINT4_DOWN_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
+      move_up_ore: &MOVE_UP_ORE_3
+        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_GET_ORE_POSITION, "VARIABLE", *JOINT4_R_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_R90_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
       ready_to_store_stone_after_get_ore: &READY_TO_STORE_STONE_AFTER_GET_ORE
-        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-      ready_to_store_stone_after_catch_drop: &READY_TO_STORE_STONE_AFTER_CATCH_DROP
-        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_SKY_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
+        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_L90_POSITION ]
         common:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
       store_stone_after_get_ore: &STORE_STONE_AFTER_GET_ORE
-        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_ACCESS_STONE_POSITION ]
-        common:
-          <<: *NORMALLY_HIGH_TIMEOUT
-        tolerance:
-          <<: *MAX_TOLERANCE
-      ready_to_store_gold_stone_after_get_ore: &READY_TO_STORE_GOLD_STONE_AFTER_GET_ORE
-        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_GOLD_ORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
+        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_L90_POSITION ]
         common:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-      store_gold_stone_after_get_ore: &STORE_GOLD_STONE_AFTER_GET_ORE
-        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_GOLD_ORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_ACCESS_STONE_POSITION ]
-        common:
-          <<: *NORMALLY_HIGH_TIMEOUT
-        tolerance:
-          <<: *MAX_TOLERANCE
-      store_stone_after_catch_drop: &STORE_STONE_AFTER_CATCH_DROP
-        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_SKY_STORE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_ACCESS_STONE_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *MAX_TOLERANCE
+
     access_store:
-      ready_to_access_store: &READY_TO_ACCESS_STORE
-        joints: [ "VARIABLE", *JOINT2_READY_TO_ACCESS_STONE_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, "VARIABLE" ]
+      ready_to_access_store: &READY_TO_ACCESS_STORE_1
+        joints: [ "VARIABLE", *JOINT2_READY_TO_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_L90_POSITION ]
         common:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-      access_store: &ACCESS_STORE
-        joints: [ "VARIABLE", *JOINT2_ACCESS_STORE_POSITION, "VARIABLE", *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, "VARIABLE" ]
+      access_store: &ACCESS_STORE_15
+        joints: [ "VARIABLE", *JOINT2_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, "VARIABLE", *JOINT6_UP_POSITION, *JOINT7_L90_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
     exchange_position: &EXCHANGE_POSITION
-      joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_LF_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
+      joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *BIGGER_TOLERANCE
 
-  reversal:
-    velocity_stop: &VELOCITY_STOP
-      mode: "VELOCITY"
-      values: [ 0.,0.,0.,0.,0.,0. ]
-    velocity_z_out: &VELOCITY_Z_OUT
-      mode: "VELOCITY"
-      values: [ 0.,0.,3.,0.,0.,0. ]
-    velocity_z_out_store: &VELOCITY_Z_OUT_STORE
-      mode: "VELOCITY"
-      values: [ 0.,0.,0.2,0.,0.,0. ]
-    velocity_z_in: &VELOCITY_Z_IN
-      mode: "VELOCITY"
-      values: [ 0.,0.,-1.2,0.,-0.01,0. ]
-    velocity_z_in_quickly: &VELOCITY_Z_IN_QUICKLY
-      mode: "VELOCITY"
-      values: [ 0.,0.,-3.0,0.,-0.01,0. ]
-    velocity_z_in_quickly_with_delay: &VELOCITY_Z_IN_QUICKLY_WITH_DELAY
-      mode: "VELOCITY"
-      values: [ 0.,0.,-3.0,0.,-0.01,0. ]
-      delay: 0.3
-    velocity_z_pitch_in: &VELOCITY_PITCH_Z_IN
-      mode: "VELOCITY"
-      values: [ 0.,0.,0.,0.,-5.0,0. ]
-      delay: 0.85
-    velocity_z_pitch_in_90: &VELOCITY_PITCH_Z_IN_90
-      mode: "VELOCITY"
-      values: [ 0.,0.,0.,0.,-5.0,0. ]
-      delay: 0.5
-    velocity_z_in_slowly: &VELOCITY_Z_IN_SLOWLY
-      mode: "VELOCITY"
-      values: [ 0.,0.,-0.5,0.,-0.01,0. ]
-    velocity_z_in_delay: &VELOCITY_Z_IN_DELAY
-      mode: "VELOCITY"
-      values: [ 0.,0.,-1.,0.,0.,0. ]
-      delay: 1.1
-    position_z_out: &POSITION_Z_OUT
-      mode: "POSITION"
-      values: [ 0.,0.,7.,0.,0.,0. ]
-      delay: 0.2
-    position_z_out_little: &POSITION_Z_OUT_LITTLE
-      mode: "POSITION"
-      values: [ 0.,0.,3.,0.,0.,0. ]
-      delay: 0.4
-    position_z_out_little2: &POSITION_Z_OUT_LITTLE2
-      mode: "POSITION"
-      values: [ 0.,0.,4.,0.,0.,0. ]
-      delay: 0.4
-    position_pitch_out: &POSITION_PITCH_OUT
-      mode: "POSITION"
-      values: [ 0.,0.,10.,0.,-1.,0. ]
-    position_pitch_reversal_pi_2: &POSITION_PITCH_REVERSAL_PI_2
-      mode: "POSITION"
-      values: [ 0.,0.,0.,0.,10.,0. ]
-      delay: 0.6
-    position_pitch_reversal_pi: &POSITION_PITCH_REVERSAL_PI
-      mode: "POSITION"
-      values: [ 0.,0.,0.,0.,-100,0. ]
-      delay: 1
-
-  chassis_move:
-    chassis_backward_30: &CHASSIS_BACKWARD_25
-      frame: base_link
-      position: [ -0.35,0. ]
-      yaw: 0.0
-      chassis_tolerance_position: 0.1
-      chassis_tolerance_angular: 0.3
-      common:
-        timeout: 2.
-    chassis_180: &CHASSIS_180
-      frame: base_link
-      position: [ 0., 0. ]
-      yaw: 2.80
-      chassis_tolerance_position: 0.1
-      chassis_tolerance_angular: 0.2
-      common:
-        timeout: 2.
 
+    reversal:
+      velocity_stop: &VELOCITY_STOP
+        mode: "VELOCITY"
+        values: [ 0.,0.,0.,0.,0.,0. ]
+      velocity_z_out: &VELOCITY_Z_OUT
+        mode: "VELOCITY"
+        values: [ 0.,0.,3.,0.,0.,0. ]
+      velocity_z_out_store: &VELOCITY_Z_OUT_STORE
+        mode: "VELOCITY"
+        values: [ 0.,0.,0.2,0.,0.,0. ]
+      velocity_z_in: &VELOCITY_Z_IN
+        mode: "VELOCITY"
+        values: [ 0.,0.,-1.2,0.,-0.01,0. ]
+      velocity_z_in_quickly: &VELOCITY_Z_IN_QUICKLY
+        mode: "VELOCITY"
+        values: [ 0.,0.,-3.0,0.,-0.01,0. ]
+      velocity_z_in_quickly_with_delay: &VELOCITY_Z_IN_QUICKLY_WITH_DELAY
+        mode: "VELOCITY"
+        values: [ 0.,0.,-3.0,0.,-0.01,0. ]
+        delay: 0.3
+      position_z_out: &POSITION_Z_OUT
+        mode: "POSITION"
+        values: [ 0.,0.,7.,0.,0.,0. ]
+        delay: 0.2
+      position_z_out_little: &POSITION_Z_OUT_LITTLE
+        mode: "POSITION"
+        values: [ 0.,0.,3.,0.,0.,0. ]
+        delay: 0.4
+
+    chassis_move:
+      chassis_backward_30: &CHASSIS_BACKWARD_25
+        frame: base_link
+        position: [ -0.35,0. ]
+        yaw: 0.0
+        chassis_tolerance_position: 0.1
+        chassis_tolerance_angular: 0.3
+        common:
+          timeout: 2.
+      chassis_180: &CHASSIS_180
+        frame: base_link
+        position: [ 0., 0. ]
+        yaw: 2.80
+        chassis_tolerance_position: 0.1
+        chassis_tolerance_angular: 0.2
+        common:
+          timeout: 2.
 
 steps_list:
-  #############  SMALL_ISLAND  ################
-  ########### LF  ############
-  SMALL_ISLAND_LF:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION_NO_DELAY
+  ###################### SMALL_ISLAND #######################
+  ############ MID #############
+  MID_SMALL_ISLAND:
     - step: "island gimbal"
       gimbal:
         <<: *GROUND_POS
     - step: "ready_to_get_ore"
       arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+        <<: *READY_TO_GET_ORE_3
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0, 0 ]
     - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
-  SMALL_ISLAND_LF0:
-    - step: "move down to get ore"
+  MID_SMALL_ISLAND0:
+    - step: "get ore"
       arm:
-        <<: *GET_ORE
-        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, 0 ]
+        <<: *GET_ORE_3
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0, 0 ]
     - step: "move up ore"
       arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
-    - step: "reverse ore"
-      arm:
-        <<: *REVERSE_ORE_UP
-    - step: "reversal move"
-      reversal:
-        <<: *VELOCITY_Z_IN_QUICKLY_WITH_DELAY
-    - step: "ready to store stone"
+        <<: *MOVE_UP_ORE_3
+        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0, 0 ]
+    - step: "ready store ore"
       arm:
         <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
-    - step: "store stone"
-      arm:
-        <<: *STORE_STONE_AFTER_GET_ORE
-    - step: "change stone"
-      stone_num:
-        change: "WHITE"
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "move joint6 up"
-      arm:
-        <<: *REVERSE_JOINT6_UP
-    - step: "reverse joint6"
-      arm:
-        <<: *HOME
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, 0, 0 ]
-    - step: "reversal stop"
-      reversal:
-        <<: *VELOCITY_STOP
-  ########### MID ############
-  SMALL_ISLAND_MID:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION_NO_DELAY
-    - step: "island gimbal"
-      gimbal:
-        <<: *GROUND_POS
-    - step: "ready_to_get_ore"
-      arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-  SMALL_ISLAND_MID0:
-    - step: "move down to get ore"
-      arm:
-        <<: *GET_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0 ]
-    - step: "move up ore"
-      arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
-    - step: "reverse ore"
-      arm:
-        <<: *REVERSE_ORE_UP
     - step: "reversal move"
       reversal:
         <<: *VELOCITY_Z_IN_QUICKLY_WITH_DELAY
-    - step: "ready to store stone"
-      arm:
-        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
-    - step: "store stone"
+    - step: "store ore"
       arm:
         <<: *STORE_STONE_AFTER_GET_ORE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "move joint6 up"
+    - step: "reverse joint5 up"
       arm:
-        <<: *REVERSE_JOINT6_UP
-    - step: "change stone"
+        <<: *REVERSE_JOINT5_UP
+    - step: "add store num"
       stone_num:
-        change: "WHITE"
-    - step: "reverse joint6"
-      arm:
-        <<: *HOME
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, 0, 0 ]
+        change: "+1"
     - step: "reversal stop"
       reversal:
         <<: *VELOCITY_STOP
-    - step: "chassis backward"
-      chassis:
-        <<: *CHASSIS_BACKWARD_25
     - step: "home one stone"
       arm:
-        <<: *HOME
+        <<: *HOME_1
         variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
     - step: "gimbal side"
       gimbal:
         <<: *SIDE_POS
-    - step: "chassis 180"
-      chassis:
-        <<: *CHASSIS_180
-    ########### RT  ############
-  SMALL_ISLAND_RT:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION_NO_DELAY
+  ############ LEFT #############
+  L_SMALL_ISLAND:
     - step: "island gimbal"
       gimbal:
         <<: *GROUND_POS
     - step: "ready_to_get_ore"
       arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+        <<: *READY_TO_GET_ORE_3
+        variable: [ 0, 0, *JOINT3_L_GET_ORE_POSITION, 0, 0, 0, 0 ]
     - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
-  SMALL_ISLAND_RT0:
-    - step: "move down to get ore"
+  L_SMALL_ISLAND0:
+    - step: "get ore"
       arm:
-        <<: *GET_ORE
-        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, 0 ]
+        <<: *GET_ORE_3
+        variable: [ 0, 0, *JOINT3_L_GET_ORE_POSITION, 0, 0, 0, 0 ]
     - step: "move up ore"
       arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
-    - step: "reverse ore"
+        <<: *MOVE_UP_ORE_3
+        variable: [ 0, 0, *JOINT3_L_GET_ORE_POSITION, 0, 0, 0, 0 ]
+    - step: "ready store ore"
       arm:
-        <<: *REVERSE_ORE_UP
+        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
     - step: "reversal move"
       reversal:
         <<: *VELOCITY_Z_IN_QUICKLY_WITH_DELAY
-    - step: "ready to store stone"
-      arm:
-        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
-    - step: "store stone"
+    - step: "store ore"
       arm:
         <<: *STORE_STONE_AFTER_GET_ORE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "change stone"
-      stone_num:
-        change: "WHITE"
-    - step: "move joint6 up"
-      arm:
-        <<: *REVERSE_JOINT6_UP
-    - step: "reverse joint6"
-      arm:
-        <<: *HOME
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, 0, 0 ]
-    - step: "reversal stop"
-      reversal:
-        <<: *VELOCITY_STOP
-
-      ###################   TAKE   ####################
-  ########## GET DUAL ORE #######
-  SMALL_ISLAND_TWO_ORE_L:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION_NO_DELAY
-    - step: "island gimbal"
-      gimbal:
-        <<: *GROUND_POS
-    - step: "ready_to_get_mid_ore"
+    - step: "reverse joint5 up"
       arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-  SMALL_ISLAND_TWO_ORE_L0:
-    ##### GET MID ORE
-    - step: "move down to get mid ore"
-      arm:
-        <<: *GET_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0 ]
-    - step: "move up ore"
-      arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
-    - step: "reverse ore"
-      arm:
-        <<: *REVERSE_ORE_UP
-    - step: "reversal move"
-      reversal:
-        <<: *VELOCITY_Z_IN_QUICKLY
-    - step: "ready to store stone"
-      arm:
-        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
-    - step: "store stone"
-      arm:
-        <<: *STORE_STONE_AFTER_GET_ORE
-    - step: "change stone"
+        <<: *REVERSE_JOINT5_UP
+    - step: "add store num"
       stone_num:
-        change: "WHITE"
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "move joint6 up"
-      arm:
-        <<: *REVERSE_JOINT6_UP
-  SMALL_ISLAND_TWO_ORE_L00:
-    - step: "ready_to_get_left_ore"
-      arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+        change: "+1"
     - step: "reversal stop"
       reversal:
         <<: *VELOCITY_STOP
-    ###### GET LEFT ORE
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "move down to get ore"
-      arm:
-        <<: *GET_ORE
-        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, 0 ]
-    - step: "move up ore"
-      arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_LF_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
-    - step: "reverse ore"
-      arm:
-        <<: *REVERSE_ORE_UP
-    - step: "reversal move"
-      reversal:
-        <<: *VELOCITY_Z_IN_QUICKLY
-    - step: "ready to store stone"
-      arm:
-        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
-    - step: "store stone"
-      arm:
-        <<: *STORE_STONE_AFTER_GET_ORE
-    - step: "change stone"
-      stone_num:
-        change: "WHITE"
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "move joint6 up"
-      arm:
-        <<: *REVERSE_JOINT6_UP
-    - step: "chassis backward"
-      chassis:
-        <<: *CHASSIS_BACKWARD_25
-    - step: "reversal stop"
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "home two stone"
+    - step: "home one stone"
       arm:
-        <<: *HOME
-        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+        <<: *HOME_1
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
     - step: "gimbal side"
       gimbal:
         <<: *SIDE_POS
-    - step: "chassis 180"
-      chassis:
-        <<: *CHASSIS_180
-
-  SMALL_ISLAND_TWO_ORE_R:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION_NO_DELAY
+  ############ RIGHT #############
+  R_SMALL_ISLAND:
     - step: "island gimbal"
       gimbal:
         <<: *GROUND_POS
-    - step: "ready_to_get_mid_ore"
+    - step: "ready_to_get_ore"
       arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
+        <<: *READY_TO_GET_ORE_3
+        variable: [ 0, 0, *JOINT3_R_GET_ORE_POSITION, 0, 0, 0, 0 ]
     - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
-  SMALL_ISLAND_TWO_ORE_R0:
-    ##### GET RIGHT ORE
-    - step: "move down to get right ore"
+  R_SMALL_ISLAND0:
+    - step: "get ore"
       arm:
-        <<: *GET_ORE
-        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, 0 ]
+        <<: *GET_ORE_3
+        variable: [ 0, 0, *JOINT3_R_GET_ORE_POSITION, 0, 0, 0, 0 ]
     - step: "move up ore"
       arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
-    - step: "reverse ore"
-      arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_RT_GET_ORE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
-    - step: "reversal move"
-      reversal:
-        <<: *VELOCITY_Z_IN_QUICKLY
-    - step: "ready to store stone"
+        <<: *MOVE_UP_ORE_3
+        variable: [ 0, 0, *JOINT3_R_GET_ORE_POSITION, 0, 0, 0, 0 ]
+    - step: "ready store ore"
       arm:
         <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
-    - step: "store stone"
-      arm:
-        <<: *STORE_STONE_AFTER_GET_ORE
-    - step: "change stone"
-      stone_num:
-        change: "WHITE"
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-  SMALL_ISLAND_TWO_ORE_R00:
-    - step: "move joint6 up"
-      arm:
-        <<: *REVERSE_JOINT6_UP
-    - step: "move up"
-      arm:
-        <<: *HOME
-        variable: [ *JOINT1_READY_TO_GET_ORE_POSITION, 0, 0, 0, 0, 0 ]
-    - step: "ready_to_get_mid_ore"
-      arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
-    - step: "reversal stop"
-      reversal:
-        <<: *VELOCITY_STOP
-    ###### GET MID ORE
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "move down to get ore"
-      arm:
-        <<: *GET_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0 ]
-    - step: "move up ore"
-      arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
-    - step: "reverse ore"
-      arm:
-        <<: *READY_TO_GET_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
     - step: "reversal move"
       reversal:
-        <<: *VELOCITY_Z_IN_QUICKLY
-    - step: "ready to store stone"
-      arm:
-        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
-    - step: "store stone"
+        <<: *VELOCITY_Z_IN_QUICKLY_WITH_DELAY
+    - step: "store ore"
       arm:
         <<: *STORE_STONE_AFTER_GET_ORE
-    - step: "change stone"
-      stone_num:
-        change: "WHITE"
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "move joint6 up"
-      arm:
-        <<: *REVERSE_JOINT6_UP
-    - step: "reverse joint6"
+    - step: "reverse joint5 up"
       arm:
-        <<: *HOME
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, 0, 0 ]
+        <<: *REVERSE_JOINT5_UP
+    - step: "add store num"
+      stone_num:
+        change: "+1"
     - step: "reversal stop"
       reversal:
         <<: *VELOCITY_STOP
-  ########### TAKE STORED STONE ###########
-  ##### AUTO REVERSE ####
-  TAKE_WHEN_ONE_STONE_AUTO_REVERSE:
+    - step: "home one stone"
+      arm:
+        <<: *HOME_1
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+    - step: "gimbal side"
+      gimbal:
+        <<: *SIDE_POS
+  ##################### TAKE STORED STONE ####################
+  TAKE_WHEN_ONE_STONE:
     - step: "island gimbal"
       gimbal:
         <<: *ISLAND_POS
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION_NO_DELAY
     - step: "move down to get stone"
       arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        <<: *READY_TO_ACCESS_STORE_1
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0, 0 ]
     - step: "REVERSAL OUT"
       reversal:
         <<: *POSITION_Z_OUT
     - step: "move up with stone and ready to get stone"
       arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
-  #    - step: "reversal pitch pi"
-  #      reversal:
-  #        <<: *VELOCITY_PITCH_Z_IN
-  #    - step: "stop reversal"
-  #      reversal:
-  #        <<: *VELOCITY_STOP
-  TAKE_WHEN_ONE_STONE_AUTO_REVERSE0:
-    - step: "move up with stone and ready to get stone"
-      arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        <<: *READY_TO_ACCESS_STORE_1
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, 0, 0, 0 ]
     - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
     - step: "move joint2 to get stone"
       arm:
-        <<: *ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        <<: *ACCESS_STORE_15
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_R90_POSITION, 0, 0 ]
     - step: "wait gripper get stone steady"
       reversal:
         <<: *VELOCITY_STOP
@@ -854,10 +429,13 @@ steps_list:
     - step: "REVERSAL Z OUT"
       reversal:
         <<: *VELOCITY_Z_OUT
-    - step: "reverse joint6"
+    - step: "reverse joint5"
       arm:
-        <<: *ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
+        <<: *REVERSE_JOINT5_UP
+    - step: "reverse ore"
+      arm:
+        <<: *ACCESS_STORE_15
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_MID_POSITION, 0, 0 ]
     - step: "REVERSAL OUT"
       reversal:
         <<: *VELOCITY_STOP
@@ -867,416 +445,118 @@ steps_list:
     - step: "move to exchange wait state"
       arm:
         <<: *EXCHANGE_POSITION
-
-  TAKE_WHEN_TWO_STONE_AUTO_REVERSE:
+  TAKE_WHEN_TWO_STONE:
     - step: "island gimbal"
       gimbal:
         <<: *ISLAND_POS
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION_NO_DELAY
     - step: "move down to get stone"
       arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_TWO_STONE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        <<: *READY_TO_ACCESS_STORE_1
+        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0, 0 ]
     - step: "reversal out little"
       reversal:
         <<: *POSITION_Z_OUT_LITTLE
     - step: "move up with stone and ready to get stone"
       arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
-    #    - step: "reversal pitch pi"
-    #      reversal:
-    #        <<: *VELOCITY_PITCH_Z_IN
-    #    - step: "stop reversal"
-    #      reversal:
-    #        <<: *VELOCITY_STOP
-    - step: "move up with stone and ready to get stone"
-      arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
-  TAKE_WHEN_TWO_STONE_AUTO_REVERSE0:
+        <<: *READY_TO_ACCESS_STORE_1
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, 0, 0, 0 ]
     - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
     - step: "move joint2 to get stone"
       arm:
-        <<: *ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        <<: *ACCESS_STORE_15
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_R90_POSITION, 0, 0 ]
     - step: "wait gripper get stone steady"
       reversal:
         <<: *VELOCITY_STOP
         delay: 0.3
-    - step: "REVERSAL OUT"
-      reversal:
-        <<: *VELOCITY_Z_OUT
-    - step: "move joint6"
-      arm:
-        <<: *ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
-    - step: "change stone"
-      stone_num:
-        change: "-1"
-    - step: "reversal stop"
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "move to exchange wait state"
-      arm:
-        <<: *EXCHANGE_POSITION
-  ##### NO REVERSE ####
-  TAKE_WHEN_ONE_STONE_NO_REVERSE:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION_NO_DELAY
-    - step: "move down to get stone"
-      arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
-    - step: "REVERSAL OUT"
-      reversal:
-        <<: *POSITION_Z_OUT
-    - step: "move up with stone and ready to get stone"
-      arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
-  TAKE_WHEN_ONE_STONE_NO_REVERSE0:
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "move joint2 to get stone"
+    - step: "reverse joint5"
       arm:
-        <<: *ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        <<: *REVERSE_JOINT5_UP
     - step: "REVERSAL Z OUT"
       reversal:
         <<: *VELOCITY_Z_OUT
-    - step: "reverse joint6"
+    - step: "reverse joint5"
       arm:
-        <<: *ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
-    - step: "REVERSAL STOP"
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "change stone"
-      stone_num:
-        change: "-1"
-    - step: "move to exchange wait state"
-      arm:
-        <<: *EXCHANGE_POSITION
-
-  TAKE_WHEN_TWO_STONE_NO_REVERSE:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION_NO_DELAY
-    - step: "move down to get stone"
-      arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_TWO_STONE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
-    - step: "move up with stone and ready to get stone"
-      arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
-  TAKE_WHEN_TWO_STONE_NO_REVERSE0:
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "move joint2 to get stone"
-      arm:
-        <<: *ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        <<: *ACCESS_STORE_15
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_MID_POSITION, 0, 0 ]
     - step: "REVERSAL OUT"
       reversal:
-        <<: *VELOCITY_Z_OUT
-    - step: "move joint6"
-      arm:
-        <<: *ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_TAKE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
+        <<: *VELOCITY_STOP
     - step: "change stone"
       stone_num:
         change: "-1"
-    - step: "reversal stop"
-      reversal:
-        <<: *VELOCITY_STOP
     - step: "move to exchange wait state"
       arm:
         <<: *EXCHANGE_POSITION
 
-  ##### BIG ISLAND ########
-  BIG_ISLAND:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION_NO_DELAY
-    - step: "island gimbal"
-      gimbal:
-        <<: *GROUND_POS
-    - step: "move up to avoid collision"
-      arm:
-        <<: *HOME
-        variable: [ *JOINT1_READY_TO_GET_ORE_POSITION, 0, 0, 0, 0, 0 ]
-    - step: "ready_to_get_mid_ore"
-      arm:
-        <<: *READY_TO_GET_GOLD_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-  BIG_ISLAND0:
-    ##### GET MID ORE
-    - step: "move down to get mid ore"
-      arm:
-        <<: *GET_GOLD_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0 ]
-    - step: "move up ore"
-      arm:
-        <<: *READY_TO_GET_GOLD_ORE
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, *JOINT6_DOWN_POSITION ]
-    - step: "reverse ore"
-      arm:
-        <<: *REVERSE_ORE_UP
-    - step: "reversal move"
-      reversal:
-        <<: *VELOCITY_Z_IN_QUICKLY
-    - step: "ready to store stone"
-      arm:
-        <<: *READY_TO_STORE_GOLD_STONE_AFTER_GET_ORE
-    - step: "store stone"
-      arm:
-        <<: *STORE_GOLD_STONE_AFTER_GET_ORE
-    - step: "change stone"
-      stone_num:
-        change: "WHITE"
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "move joint6 up"
-      arm:
-        <<: *REVERSE_JOINT6_UP
-
-
-  CATCH_DROP:
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION_NO_DELAY
-    - step: "gimbal island pos"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "ready to catch drop"
-      arm:
-        <<: *CATCH_DROP
-  CATCH_DROP0:
-    - step: "take back ore"
-      arm:
-        <<: *TAKE_BACK_ORE
-    - step: "ready to store stone"
-      arm:
-        <<: *READY_TO_STORE_STONE_AFTER_CATCH_DROP
-    - step: "reversal in"
-      reversal:
-        <<: *VELOCITY_Z_IN_QUICKLY
-    - step: "place stone on reversal"
-      arm:
-        <<: *STORE_STONE_AFTER_CATCH_DROP
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "change stone"
-      stone_num:
-        change: "YELLOW"
-    - step: "move joint6 up"
-      arm:
-        <<: *REVERSE_JOINT6_UP
-    - step: "stop reversal"
-      reversal:
-        <<: *VELOCITY_STOP
-
-  GROUND_STONE:
-    - step: "ground gimbal"
-      gimbal:
-        <<: *GROUND_POS
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "reverse joint7"
-      end_effector:
-        <<: *JOINT7_DOWN_POSITION_NO_DELAY
-    - step: "ready to get ground stone"
-      arm:
-        <<: *READY_TO_GET_GROUND_STONE
-        variable: [ 0, 0, 0, *JOINT4_DOWN_POSITION, 0, 0 ]
-  GROUND_STONE0:
-    - step: "move down to get ore"
-      arm:
-        <<: *GET_GROUND_STONE
-    - step: "move up ore"
-      arm:
-        <<: *READY_TO_GET_GROUND_STONE
-        variable: [ 0, 0, 0, *JOINT4_DOWN_POSITION, 0, 0 ]
-  GROUND_STONE00:
-    - step: "move up arm"
-      arm:
-        <<: *READY_TO_GET_GROUND_STONE
-        variable: [ 0, 0, 0, *JOINT4_UP_POSITION, 0, 0 ]
-    - step: "reverse joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION
-    - step: "ready to place stone"
-      arm:
-        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY_HIGH_TIMEOUT
-        tolerance:
-          <<: *MAX_TOLERANCE
-    - step: "place stone"
-      arm:
-        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_PLACE_STONE_POSITION ]
-        common:
-          <<: *NORMALLY_HIGH_TIMEOUT
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "change stone"
-      stone_num:
-        change: "MANUALLY"
-    - step: "reverse joint6 up"
-      arm:
-        <<: *REVERSE_ORE_UP
-
-  REVERSE_STONE_HIGH:
-    - step: "gimbal island"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "move joint1 up"
-      arm:
-        <<: *HOME
-        variable: [ *JOINT1_UP_POSITION, 0, 0, 0, 0, 0 ]
-
-  REVERSE_STONE_LOW:
-    - step: "gimbal island"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "move joint1 up"
-      arm:
-        <<: *HOME
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, 0, 0 ]
-
-  ########### STORE ############
   STORE_WHEN_ZERO_STONE:
     - step: "island gimbal"
       gimbal:
         <<: *ISLAND_POS
-    - step: "prepare"
-      arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_STORE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
-    - step: "ready to store"
-      arm:
-        <<: *ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_STORE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
     - step: "reversal z in"
       reversal:
         <<: *VELOCITY_Z_IN
+    - step: "ready to store"
+      arm:
+        <<: *ACCESS_STORE_15
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_MID_POSITION, 0, 0 ]
     - step: "store"
       arm:
-        <<: *ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_STORE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        <<: *ACCESS_STORE_15
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_R90_POSITION, 0, 0 ]
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
+    - step: "avoid collision"
+      arm:
+        <<: *ACCESS_STORE_15
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_MID_POSITION, 0, 0 ]
     - step: "change stone"
       stone_num:
-        change: "MANUALLY"
-    - step: "avoid_collision"
-      arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_TWO_STONE_POSITION, 0, *JOINT3_STORE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
-    - step: "move joint6 up"
-      arm:
-        <<: *REVERSE_JOINT6_UP
+        change: "+1"
     - step: "reversal z stop"
       reversal:
         <<: *VELOCITY_STOP
+    - step: "home one stone"
+      arm:
+        <<: *HOME_1
+        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0, 0 ]
   STORE_WHEN_ONE_STONE:
     - step: "island gimbal"
       gimbal:
         <<: *ISLAND_POS
-    - step: "prepare"
-      arm:
-        <<: *READY_TO_ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_STORE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
     - step: "ready to store"
       arm:
-        <<: *ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_STORE_STONE_POSITION, 0, 0, *JOINT6_UP_POSITION ]
+        <<: *ACCESS_STORE_15
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_MID_POSITION, 0, 0 ]
     - step: "reversal z in"
       reversal:
         <<: *VELOCITY_Z_IN
     - step: "store"
       arm:
-        <<: *ACCESS_STORE
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, *JOINT3_STORE_STONE_POSITION, 0, 0, *JOINT6_ACCESS_STONE_POSITION ]
+        <<: *ACCESS_STORE_15
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_R90_POSITION, 0, 0 ]
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
     - step: "change stone"
       stone_num:
-        change: "MANUALLY"
+        change: "+1"
+    - step: "avoid collision"
+      arm:
+        <<: *ACCESS_STORE_15
+        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_MID_POSITION, 0, 0 ]
     - step: "reversal z stop"
       reversal:
         <<: *VELOCITY_STOP
-
-  EXCHANGE_WAIT:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION
-    - step: "exchange wait"
+    - step: "home one stone"
       arm:
-        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_READY_TO_ACCESS_STONE_POSITION, *JOINT3_LF_POSITION, *JOINT4_UP_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-  ######## GIMBAL ######
-  GIMBAL_ISLAND:
-    - step: "gimbal island pos"
-      gimbal:
-        <<: *ISLAND_POS
-  GIMBAL_SIDE:
-    - step: "gimbal side pos"
-      gimbal:
-        <<: *SIDE_POS
-  GIMBAL_GROUND:
-    - step: "gimbal ground pos"
-      gimbal:
-        <<: *GROUND_POS
-  GIMBAL_SMALL_SCREEN:
-    - step: "gimbal small screen"
-      gimbal:
-        <<: *EXCHANGE_POS
-  ###############   GRIPPER   ########
-  CLOSE_GRIPPER:
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-  OPEN_GRIPPER:
-    - step: "open  gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-  ###############   HOME   ###########
+        <<: *HOME_1
+        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0, 0 ]
+
+  ############################ HOME ##########################
   HOME_ZERO_STONE:
     - step: "gimbal look side"
       gimbal:
@@ -1284,15 +564,9 @@ steps_list:
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION
-    - step: "move joint6 up"
-      arm:
-        <<: *REVERSE_JOINT6_UP
     - step: "home with no stone"
       arm:
-        <<: *HOME
+        <<: *HOME_1
         variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
   HOME_ONE_STONE:
     - step: "gimbal look side"
@@ -1301,15 +575,9 @@ steps_list:
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION
-    - step: "move joint6 up"
-      arm:
-        <<: *REVERSE_JOINT6_UP
     - step: "home with one stone"
       arm:
-        <<: *HOME
+        <<: *HOME_1
         variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ]
   HOME_TWO_STONE:
     - step: "gimbal look side"
@@ -1318,147 +586,33 @@ steps_list:
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "joint7 up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION
-    - step: "move joint6 up"
-      arm:
-        <<: *REVERSE_JOINT6_UP
     - step: "home with two stone"
       arm:
-        <<: *HOME
+        <<: *HOME_1
         variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
-
-      ###################   TAKE   ####################
-  ########### DRAG #######
-  DRAG_CAR:
-    - step: "ready to drag car"
-      gimbal:
-        <<: *DRAG_POSE1
-
-  DRAG_CAR0:
-    - step: "run away with car"
-      gimbal:
-        <<: *DRAG_POSE2
-
-  ########## EXCHANGE ############
-  OLD_EXCHANGE_SPHERE:
-    - step: "close to a sphere"
-      arm:
-        frame: exchanger
-        xyz: [ -0.4, 0., -0.25 ]
-        rpy: [ 0., 0., 0. ]
-        rpy_rectify: [ 0.01,0.1,0.03 ]
-        drift_dimensions: [ true, true, true, true, false, true ]
-        tolerance_position: 0.1
-        tolerance_orientation: 0.01
-        spacial_shape: SPHERE
-        radius: 0.5
-        point_resolution: 0.5
-        max_planning_times: 100
-        common:
-          <<: *QUICKLY
-
-  EXCHANGE_SPHERE:
-    - step: "test new exchange"
-      arm:
-        frame: exchanger
-        is_refer_planning_frame: true
-        xyz: [ -0.3, 0., 0. ]
-        rpy: [ 0., 0., 0. ]
-        drift_dimensions: [ true, true, true, true, false, true ]
-        tolerance_position: 0.05
-        tolerance_orientation: 0.05
-        spacial_shape: SPHERE
-        radius: 0.5
-        point_resolution: 0.5
-        max_planning_times: 100
-        common:
-          <<: *NORMALLY
-    - step: "test joint7"
-      end_effector:
-        original_tf: base_link
-        reference_tf: exchanger
-        direction: pitch
-    - step: "test new exchange"
-      arm:
-        frame: exchanger
-        is_refer_planning_frame: false
-        xyz: [ -0.2, 0., -0.18 ]
-        rpy: [ 0., 0., 0. ]
-        drift_dimensions: [ true, true, true, true, false, true ]
-        tolerance_position: 0.01
-        tolerance_orientation: 0.01
-        spacial_shape: BASICS
-        basics_length: [ 0.1, 0.1, 0.1 ]
-        point_resolution: 0.6
-        max_planning_times: 50
-        common:
-          <<: *NORMALLY
-  ########## ANTI DART ###########
-  ANTI_DART:
-    - step: "gimbal ground pos"
-      gimbal:
-        <<: *GROUND_POS
-    - step: "joint7 ready"
-      end_effector:
-        <<: *JOINT7_MID_POSITION_NO_DELAY
-    - step: "ready to get block"
-      arm:
-        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_DOWN_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-  ANTI_DART0:
-    - step: "joint7 reverse up"
-      end_effector:
-        <<: *JOINT7_UP_POSITION_NO_DELAY
-    - step: "move up and rotate block"
-      arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_BLOCK_DART_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "move up block and ready to anti dart"
-      arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_UP_POSITION, *JOINT5_RT_POSITION, *JOINT6_BLOCK_DART_POSITION ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-
-  HOME:
-    - step: "home test"
-      arm:
-        joints: [ 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01 ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-
-  HOME2:
-    - step: "home2"
-      arm:
-        joints: [ 0.3, 0.2, 0.3, 0.01, 0.31, 0.31, 2.31 ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
+  OPEN_GRIPPER:
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_GRIPPER
+  CLOSE_GRIPPER:
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
 
   TEST1:
-    - step: "home test"
-      arm:
-        joints: [ 0.1, 0.1, 0.01, 0.01, 0.01, 0.01, 0.01 ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "home2"
-      arm:
-        joints: [ 0.3, 0.2, 0.3, 0.01, 0.31, 0.31, 2.31 ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
+    - step: "a"
+      reversal:
+        <<: *VELOCITY_Z_IN
+  TEST2:
+    - step: "a"
+      reversal:
+        <<: *VELOCITY_Z_IN_QUICKLY
+
+  TEST3:
+    - step: "a"
+      reversal:
+        <<: *VELOCITY_Z_OUT
+  TEST4:
+    - step: "a"
+      reversal:
+        <<: *VELOCITY_STOP

From 3e3aa67adb719da33bf6fcdd3bcc7289a90f5be7 Mon Sep 17 00:00:00 2001
From: cc0h <792166012@qq.com>
Date: Mon, 11 Dec 2023 23:12:07 +0800
Subject: [PATCH 028/147] add auto exchange motion

---
 engineer_middleware/config/steps_list.yaml | 54 ++++++++++++++++++++++
 1 file changed, 54 insertions(+)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index e38c049..dd07a33 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -599,6 +599,60 @@ steps_list:
       gripper:
         <<: *CLOSE_GRIPPER
 
+  ########## EXCHANGE ############
+  OLD_EXCHANGE_SPHERE:
+    - step: "close to a sphere"
+      arm:
+        frame: exchanger
+        xyz: [ -0.4, 0., -0.25 ]
+        rpy: [ 0., 0., 0. ]
+        rpy_rectify: [ 0.01,0.1,0.03 ]
+        drift_dimensions: [ true, true, true, true, false, true ]
+        tolerance_position: 0.1
+        tolerance_orientation: 0.01
+        spacial_shape: SPHERE
+        radius: 0.5
+        point_resolution: 0.5
+        max_planning_times: 100
+        common:
+          <<: *QUICKLY
+
+  EXCHANGE_SPHERE:
+    - step: "test new exchange"
+      arm:
+        frame: exchanger
+        is_refer_planning_frame: true
+        xyz: [ -0.3, 0., 0. ]
+        rpy: [ 0., 0., 0. ]
+        drift_dimensions: [ true, true, true, true, false, true ]
+        tolerance_position: 0.05
+        tolerance_orientation: 0.05
+        spacial_shape: SPHERE
+        radius: 0.5
+        point_resolution: 0.5
+        max_planning_times: 100
+        common:
+          <<: *NORMALLY
+    - step: "test joint7"
+      end_effector:
+        original_tf: base_link
+        reference_tf: exchanger
+        direction: pitch
+    - step: "test new exchange"
+      arm:
+        frame: exchanger
+        is_refer_planning_frame: false
+        xyz: [ -0.2, 0., -0.18 ]
+        rpy: [ 0., 0., 0. ]
+        drift_dimensions: [ true, true, true, true, false, true ]
+        tolerance_position: 0.01
+        tolerance_orientation: 0.01
+        spacial_shape: BASICS
+        basics_length: [ 0.1, 0.1, 0.1 ]
+        point_resolution: 0.6
+        max_planning_times: 50
+        common:
+          <<: *NORMALLY
   TEST1:
     - step: "a"
       reversal:

From 4c326d2c3360d460118d6a8f80fe152ec2544992 Mon Sep 17 00:00:00 2001
From: cch <792166012@qq.com>
Date: Wed, 13 Dec 2023 22:56:17 +0800
Subject: [PATCH 029/147] modify some function.

---
 .../include/engineer_middleware/auto_exchange.h          | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/auto_exchange.h b/engineer_middleware/include/engineer_middleware/auto_exchange.h
index 52af56c..68fccee 100644
--- a/engineer_middleware/include/engineer_middleware/auto_exchange.h
+++ b/engineer_middleware/include/engineer_middleware/auto_exchange.h
@@ -17,7 +17,7 @@ namespace auto_exchange
 class JointInfo
 {
 public:
-  JointInfo(XmlRpc::XmlRpcValue& joint)
+  explicit JointInfo(XmlRpc::XmlRpcValue& joint)
   {
     offset_ = xmlRpcGetDouble(joint, "offset", 0.);
     max_scale_ = xmlRpcGetDouble(joint, "max_scale", 1.0);
@@ -25,9 +25,10 @@ class JointInfo
     ROS_ASSERT(joint["range"].getType() == XmlRpc::XmlRpcValue::TypeArray);
     min_position_ = xmlRpcGetDouble(joint["range"], 0);
     max_position_ = xmlRpcGetDouble(joint["range"], 1);
+    current_position_ = 0;
     move_direct_ = -1;
   }
-  bool judgeJointLimit()
+  bool judgeJointLimit() const
   {
     return abs(current_position_ - offset_ - min_position_) <= near_tolerance_ ||
            abs(max_position_ + offset_ - current_position_) <= near_tolerance_;
@@ -49,7 +50,7 @@ class SingleDirectionMove
 {
 public:
   std::string name;
-  double tolerance, start_vel, offset_refer_exchanger, max_vel, error, pid_value;
+  double tolerance{}, start_vel{}, offset_refer_exchanger{}, max_vel{}, error{}, pid_value{};
   control_toolbox::Pid pid;
   void init(XmlRpc::XmlRpcValue& config, std::string config_name, ros::NodeHandle& nh)
   {
@@ -62,7 +63,7 @@ class SingleDirectionMove
     ros::NodeHandle pid_config = ros::NodeHandle(nh, name);
     pid.init(ros::NodeHandle(pid_config, "pid"));
   }
-  bool isFinish()
+  bool isFinish() const
   {
     return abs(error) <= tolerance;
   }

From 6127e54524a2bfebcabf3b723a40acc729c34e5b Mon Sep 17 00:00:00 2001
From: =cc0h <=792166012@qq.com>
Date: Fri, 29 Dec 2023 21:28:47 +0800
Subject: [PATCH 030/147] test exchange sphere.

---
 engineer_middleware/config/steps_list.yaml | 70 +++++++++++-----------
 1 file changed, 35 insertions(+), 35 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index dd07a33..bfbdf17 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -607,7 +607,7 @@ steps_list:
         xyz: [ -0.4, 0., -0.25 ]
         rpy: [ 0., 0., 0. ]
         rpy_rectify: [ 0.01,0.1,0.03 ]
-        drift_dimensions: [ true, true, true, true, false, true ]
+        drift_dimensions: [ true, true, true, true, true, true ]
         tolerance_position: 0.1
         tolerance_orientation: 0.01
         spacial_shape: SPHERE
@@ -617,14 +617,14 @@ steps_list:
         common:
           <<: *QUICKLY
 
-  EXCHANGE_SPHERE:
+  EXCHANGE_DOWN:
     - step: "test new exchange"
       arm:
         frame: exchanger
         is_refer_planning_frame: true
-        xyz: [ -0.3, 0., 0. ]
-        rpy: [ 0., 0., 0. ]
-        drift_dimensions: [ true, true, true, true, false, true ]
+        xyz: [ -0.23, 0., -0.12 ]
+        rpy: [ 0., -1.5707963, 0. ]
+        drift_dimensions: [ true, true, true, true, true, true ]
         tolerance_position: 0.05
         tolerance_orientation: 0.05
         spacial_shape: SPHERE
@@ -633,40 +633,40 @@ steps_list:
         max_planning_times: 100
         common:
           <<: *NORMALLY
-    - step: "test joint7"
-      end_effector:
-        original_tf: base_link
-        reference_tf: exchanger
-        direction: pitch
+  EXCHANGE_STRAIGHT:
     - step: "test new exchange"
       arm:
         frame: exchanger
-        is_refer_planning_frame: false
-        xyz: [ -0.2, 0., -0.18 ]
-        rpy: [ 0., 0., 0. ]
+        is_refer_planning_frame: true
+        xyz: [ -0.3, 0., 0. ]
+        rpy: [ 0., 3.1415926, 0. ]
         drift_dimensions: [ true, true, true, true, false, true ]
-        tolerance_position: 0.01
-        tolerance_orientation: 0.01
-        spacial_shape: BASICS
-        basics_length: [ 0.1, 0.1, 0.1 ]
-        point_resolution: 0.6
-        max_planning_times: 50
+        tolerance_position: 0.05
+        tolerance_orientation: 0.05
+        spacial_shape: SPHERE
+        radius: 0.5
+        point_resolution: 0.5
+        max_planning_times: 100
         common:
           <<: *NORMALLY
-  TEST1:
-    - step: "a"
-      reversal:
-        <<: *VELOCITY_Z_IN
-  TEST2:
-    - step: "a"
-      reversal:
-        <<: *VELOCITY_Z_IN_QUICKLY
+  WAIT:
+    - step: "move to exchange wait state"
+      arm:
+        <<: *EXCHANGE_POSITION
+
+#    - step: "test new exchange"
+#      arm:
+#        frame: exchanger
+#        is_refer_planning_frame: false
+#        xyz: [ -0.2, 0., -0.18 ]
+#        rpy: [ 0., 3.141592, 0. ]
+#        drift_dimensions: [ true, true, true, true, false, true ]
+#        tolerance_position: 0.01
+#        tolerance_orientation: 0.01
+#        spacial_shape: BASICS
+#        basics_length: [ 0.1, 0.1, 0.1 ]
+#        point_resolution: 0.6
+#        max_planning_times: 50
+#        common:
+#          <<: *NORMALLY
 
-  TEST3:
-    - step: "a"
-      reversal:
-        <<: *VELOCITY_Z_OUT
-  TEST4:
-    - step: "a"
-      reversal:
-        <<: *VELOCITY_STOP

From e598230d1727a45c9766deeb84ac59e294a88f63 Mon Sep 17 00:00:00 2001
From: =cc0h <=792166012@qq.com>
Date: Sun, 28 Jan 2024 17:04:20 +0800
Subject: [PATCH 031/147] little optimize.

---
 .../engineer_middleware/auto_exchange.h       | 44 +++++++++----------
 1 file changed, 22 insertions(+), 22 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/auto_exchange.h b/engineer_middleware/include/engineer_middleware/auto_exchange.h
index 68fccee..ee1b25b 100644
--- a/engineer_middleware/include/engineer_middleware/auto_exchange.h
+++ b/engineer_middleware/include/engineer_middleware/auto_exchange.h
@@ -70,13 +70,13 @@ class SingleDirectionMove
   double computerVel(ros::Duration dt)
   {
     double vel = start_vel + abs(pid.computeCommand(error, dt));
-    int direction = error / abs(error);
+    int direction = (error > 0) ? 1 : -1;
     return abs(vel) >= max_vel ? direction * max_vel : direction * vel;
   }
   void getPidValue(ros::Duration dt)
   {
     double vel = start_vel + abs(pid.computeCommand(error, dt));
-    int direction = error / abs(error);
+    int direction = (error > 0) ? 1 : -1;
     pid_value = abs(vel) >= max_vel ? direction * max_vel : direction * vel;
   }
 };
@@ -117,19 +117,19 @@ class ProgressBase
     }
   }
   virtual void printProcess() = 0;
-  bool getFinishFlag()
+  bool getFinishFlag() const
   {
     return is_finish_;
   }
-  bool getEnterFlag()
+  bool getEnterFlag() const
   {
     return enter_flag_;
   }
-  bool getTimeOutFlag()
+  bool getTimeOutFlag() const
   {
     return is_time_out_;
   }
-  bool getProcess()
+  bool getProcess() const
   {
     return process_;
   }
@@ -341,7 +341,7 @@ class ProAdjust : public ProgressBase
   {
     ProgressBase::init();
     process_ = SET_GOAL;
-    re_adjust_ = false;
+    re_adjusted_ = false;
     initComputerValue();
   }
   void stateMachine() override
@@ -360,7 +360,7 @@ class ProAdjust : public ProgressBase
         chassis_vel_cmd_.linear.y = 0.;
         chassis_vel_cmd_.angular.z = 0.;
         if (x_.isFinish())
-          process_ = re_adjust_ ? FINISH : CHASSIS_Y;
+          process_ = re_adjusted_ ? FINISH : CHASSIS_Y;
       }
       break;
       case CHASSIS_Y:
@@ -380,14 +380,14 @@ class ProAdjust : public ProgressBase
         if (yaw_.isFinish())
         {
           process_ = SET_GOAL;
-          re_adjust_ = true;
+          re_adjusted_ = true;
         }
       }
       break;
       case FINISH:
       {
         is_finish_ = true;
-        re_adjust_ = false;
+        re_adjusted_ = false;
         initComputerValue();
         ROS_INFO_STREAM("PRE ADJUST FINISH");
       }
@@ -455,7 +455,7 @@ class ProAdjust : public ProgressBase
     tf2::doTransform(chassis_target_, chassis_target_, tf_buffer_.lookupTransform("map", "base_link", ros::Time(0)));
   }
 
-  bool re_adjust_{ false };
+  bool re_adjusted_{ false };
   ros::Time last_time_;
   std::string chassis_command_source_frame_{ "base_link" };
   geometry_msgs::Twist chassis_vel_cmd_{};
@@ -515,7 +515,7 @@ class AutoServoMove : public ProgressBase
     initComputerValue();
     joint7_msg_ = 0.;
   }
-  double getJoint7Msg()
+  double getJoint7Msg() const
   {
     return joint7_msg_;
   }
@@ -737,11 +737,11 @@ class UnionMove : public ProgressBase
   {
     return motion_name_;
   }
-  bool getIsMotionFinish()
+  bool getIsMotionFinish() const
   {
     return is_motion_finish_;
   }
-  bool getIsMotionStart()
+  bool getIsMotionStart() const
   {
     return is_motion_start_;
   }
@@ -783,9 +783,9 @@ class UnionMove : public ProgressBase
   }
   void initComputerValue()
   {
-    for (int i = 0; i < (int)servo_scales_.size(); ++i)
+    for (double & servo_scale : servo_scales_)
     {
-      servo_scales_[i] = 0.;
+      servo_scale = 0.;
     }
     move_gather_[0] = x_;
     move_gather_[1] = y_;
@@ -918,7 +918,7 @@ class AutoExchange : public ProgressBase
     ros::NodeHandle nh_auto_servo_move(nh, "auto_servo_move");
     ros::NodeHandle nh_union_move(nh, "union_move");
     find_ = new Find(auto_exchange["auto_find"], tf_buffer, nh_auto_find);
-    pre_adjust_ = new ProAdjust(auto_exchange["auto_pre_adjust"], tf_buffer, nh_auto_pre_adjust);
+    pre_adjusted_ = new ProAdjust(auto_exchange["auto_pre_adjust"], tf_buffer, nh_auto_pre_adjust);
     auto_servo_move_ = new AutoServoMove(auto_exchange["auto_servo_move"], tf_buffer, nh_auto_servo_move);
     union_move_ = new UnionMove(auto_exchange["union_move"], tf_buffer, nh_union_move);
     exchanger_tf_update_pub_ = nh_.advertise<std_msgs::Bool>("/is_update_exchanger", 1);
@@ -928,14 +928,14 @@ class AutoExchange : public ProgressBase
     ProgressBase::init();
     process_ = FIND;
     find_->init();
-    pre_adjust_->init();
+    pre_adjusted_->init();
     union_move_->init();
     exchangerTfUpdate(true);
   }
 
 public:
   Find* find_{};
-  ProAdjust* pre_adjust_{};
+  ProAdjust* pre_adjusted_{};
   AutoServoMove* auto_servo_move_{};
   UnionMove* union_move_{};
 
@@ -971,11 +971,11 @@ class AutoExchange : public ProgressBase
       case PRE_ADJUST:
       {
         exchangerTfUpdate(true);
-        pre_adjust_->run();
-        if (pre_adjust_->getFinishFlag())
+        pre_adjusted_->run();
+        if (pre_adjusted_->getFinishFlag())
         {
           process_ = MOVE;
-          pre_adjust_->init();
+          pre_adjusted_->init();
         }
       }
       break;

From d364b9aae1bbb17253c2909b76c769701143ee9a Mon Sep 17 00:00:00 2001
From: =cc0h <=792166012@qq.com>
Date: Fri, 23 Feb 2024 23:32:47 +0800
Subject: [PATCH 032/147] new step list. disable collision check between camera
 and link1.

---
 engineer_arm_config/config/engineer.srdf   |   1 +
 engineer_middleware/config/steps_list.yaml | 445 +++------------------
 2 files changed, 48 insertions(+), 398 deletions(-)

diff --git a/engineer_arm_config/config/engineer.srdf b/engineer_arm_config/config/engineer.srdf
index 9818652..b4dde6c 100644
--- a/engineer_arm_config/config/engineer.srdf
+++ b/engineer_arm_config/config/engineer.srdf
@@ -49,4 +49,5 @@
         <disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
         <disable_collisions link1="link6" link2="link7" reason="Never"/>
         <disable_collisions link1="link5" link2="link7" reason="Never"/>
+        <disable_collisions link1="link1" link2="camera_link" reason="Never"/>
 </robot>
diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index bfbdf17..513a098 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -26,43 +26,27 @@ common:
   joint1:
     mechanical:
       down_position: &JOINT1_DOWN_POSITION
-                       0.
+                       0.015
       up_position: &JOINT1_UP_POSITION
-                     0.43
+                     0.22
     home:
       zero_stone_position: &JOINT1_ZERO_STONE_POSITION
                              0.08
       one_stone_position: &JOINT1_ONE_STONE_POSITION
-                            0.08
+                            0.156
       two_stone_position: &JOINT1_TWO_STONE_POSITION
-                            0.145700
+                            0.22
       three_stone_position: &JOINT1_THREE_STONE_POSITION
-                              0.35
-    get_ore:
-      ready_get_ore: &JOINT1_READY_TO_GET_ORE_POSITION
-                       0.15
-      get_ore: &JOINT1_GET_ORE_POSITION
-                 0.15
-      store_ore_position: &JOINT1_STORE_ORE_POSITION
-                            0.25
-    exchange_position: &JOINT1_EXCHANGE_POSITION
-                         0.4
+                              0.22
 
   joint2:
     mechanical:
       back_position: &JOINT2_BACK_POSITION
-                       0.0001
+                       0.01
+      get_ore_position: &JOINT2_GET_ORE_POSITION
+                           0.1
       furthest_position: &JOINT2_FURTHEST_POSITION
-                           0.270000
-    store:
-      access_store_position: &JOINT2_ACCESS_STORE_POSITION
-                               0.00001
-      ready_to_access_stone_position: &JOINT2_READY_TO_ACCESS_STORE_POSITION
-                                        0.06
-    exchange_position: &JOINT2_EXCHANGE_POSITION
-                         0.05
-    get_ore_position: &JOINT2_GET_ORE_POSITION
-                        0.05
+                           0.2575
 
   joint3:
     mechanical:
@@ -72,18 +56,6 @@ common:
                       0.0001
       right_position: &JOINT3_R_POSITION
                         -0.275
-    get_ore:
-      lf_get_ore_position: &JOINT3_L_GET_ORE_POSITION
-                             0.28
-      mid_get_ore_position: &JOINT3_MID_GET_ORE_POSITION
-                              0.124
-      rt_get_ore_position: &JOINT3_R_GET_ORE_POSITION
-                             -0.1
-    store_and_take:
-      store_stone_position: &JOINT3_STORE_STONE_POSITION
-                              0.235
-      take_stone_position: &JOINT3_TAKE_STONE_POSITION
-                             0.235230
 
   joint4:
     mechanical:
@@ -92,7 +64,7 @@ common:
       mid_position: &JOINT4_MID_POSITION
                       0.001
       right_position: &JOINT4_R_POSITION
-                        -1.59
+                        -1.45
   joint5:
     mechanical:
       mid_position: &JOINT5_MID_POSITION
@@ -100,13 +72,13 @@ common:
       left_90_position: &JOINT5_L90_POSITION
                           -1.54
       right_90_position: &JOINT5_R90_POSITION
-                           1.597
+                           1.59
   joint6:
     mechanical:
       mid_position: &JOINT6_MID_POSITION
                       0.001
       up_position: &JOINT6_UP_POSITION
-                     -1.55
+                     -1.49
       down_position: &JOINT6_DOWN_POSITION
                        1.569
   joint7:
@@ -116,7 +88,7 @@ common:
       left_90_position: &JOINT7_L90_POSITION
                           -1.59
       right_90_position: &JOINT7_R90_POSITION
-                           1.59
+                           1.49
   gimbal:
     side_pos: &SIDE_POS
       frame: gimbal_base
@@ -135,12 +107,6 @@ common:
       state: false
 
   arm:
-    reverse_joint5_up: &REVERSE_JOINT5_UP
-      joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
-      common:
-        <<: *NORMALLY
-      tolerance:
-        <<: *NORMAL_TOLERANCE
     home: &HOME_1
       joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
       common:
@@ -148,60 +114,6 @@ common:
       tolerance:
         <<: *NORMAL_TOLERANCE
 
-    get_ore:
-      ready_to_get_ore: &READY_TO_GET_ORE_3
-        joints: [ *JOINT1_READY_TO_GET_ORE_POSITION, *JOINT2_BACK_POSITION, "VARIABLE", *JOINT4_R_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_R90_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-      get_ore: &GET_ORE_3
-        joints: [ *JOINT1_GET_ORE_POSITION, *JOINT2_GET_ORE_POSITION, "VARIABLE", *JOINT4_R_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_R90_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *SMALL_TOLERANCE
-      move_up_ore: &MOVE_UP_ORE_3
-        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_GET_ORE_POSITION, "VARIABLE", *JOINT4_R_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_R90_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-      ready_to_store_stone_after_get_ore: &READY_TO_STORE_STONE_AFTER_GET_ORE
-        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_L90_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-      store_stone_after_get_ore: &STORE_STONE_AFTER_GET_ORE
-        joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_L90_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-
-    access_store:
-      ready_to_access_store: &READY_TO_ACCESS_STORE_1
-        joints: [ "VARIABLE", *JOINT2_READY_TO_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_L90_POSITION ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-
-      access_store: &ACCESS_STORE_15
-        joints: [ "VARIABLE", *JOINT2_ACCESS_STORE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, "VARIABLE", *JOINT6_UP_POSITION, *JOINT7_L90_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-
-    exchange_position: &EXCHANGE_POSITION
-      joints: [ *JOINT1_STORE_ORE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
-      common:
-        <<: *NORMALLY
-      tolerance:
-        <<: *BIGGER_TOLERANCE
-
 
     reversal:
       velocity_stop: &VELOCITY_STOP
@@ -252,309 +164,48 @@ common:
 
 steps_list:
   ###################### SMALL_ISLAND #######################
-  ############ MID #############
-  MID_SMALL_ISLAND:
-    - step: "island gimbal"
-      gimbal:
-        <<: *GROUND_POS
-    - step: "ready_to_get_ore"
-      arm:
-        <<: *READY_TO_GET_ORE_3
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0, 0 ]
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-  MID_SMALL_ISLAND0:
-    - step: "get ore"
-      arm:
-        <<: *GET_ORE_3
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0, 0 ]
-    - step: "move up ore"
-      arm:
-        <<: *MOVE_UP_ORE_3
-        variable: [ 0, 0, *JOINT3_MID_GET_ORE_POSITION, 0, 0, 0, 0 ]
-    - step: "ready store ore"
+  SMALL_ISLAND_DEMO:
+    - step: "home pos"
       arm:
-        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
-    - step: "reversal move"
-      reversal:
-        <<: *VELOCITY_Z_IN_QUICKLY_WITH_DELAY
-    - step: "store ore"
-      arm:
-        <<: *STORE_STONE_AFTER_GET_ORE
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "reverse joint5 up"
-      arm:
-        <<: *REVERSE_JOINT5_UP
-    - step: "add store num"
-      stone_num:
-        change: "+1"
-    - step: "reversal stop"
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "home one stone"
-      arm:
-        <<: *HOME_1
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
-    - step: "gimbal side"
-      gimbal:
-        <<: *SIDE_POS
-  ############ LEFT #############
-  L_SMALL_ISLAND:
-    - step: "island gimbal"
-      gimbal:
-        <<: *GROUND_POS
-    - step: "ready_to_get_ore"
-      arm:
-        <<: *READY_TO_GET_ORE_3
-        variable: [ 0, 0, *JOINT3_L_GET_ORE_POSITION, 0, 0, 0, 0 ]
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-  L_SMALL_ISLAND0:
-    - step: "get ore"
-      arm:
-        <<: *GET_ORE_3
-        variable: [ 0, 0, *JOINT3_L_GET_ORE_POSITION, 0, 0, 0, 0 ]
-    - step: "move up ore"
-      arm:
-        <<: *MOVE_UP_ORE_3
-        variable: [ 0, 0, *JOINT3_L_GET_ORE_POSITION, 0, 0, 0, 0 ]
-    - step: "ready store ore"
-      arm:
-        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
-    - step: "reversal move"
-      reversal:
-        <<: *VELOCITY_Z_IN_QUICKLY_WITH_DELAY
-    - step: "store ore"
-      arm:
-        <<: *STORE_STONE_AFTER_GET_ORE
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "reverse joint5 up"
-      arm:
-        <<: *REVERSE_JOINT5_UP
-    - step: "add store num"
-      stone_num:
-        change: "+1"
-    - step: "reversal stop"
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "home one stone"
-      arm:
-        <<: *HOME_1
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
-    - step: "gimbal side"
-      gimbal:
-        <<: *SIDE_POS
-  ############ RIGHT #############
-  R_SMALL_ISLAND:
-    - step: "island gimbal"
-      gimbal:
-        <<: *GROUND_POS
-    - step: "ready_to_get_ore"
-      arm:
-        <<: *READY_TO_GET_ORE_3
-        variable: [ 0, 0, *JOINT3_R_GET_ORE_POSITION, 0, 0, 0, 0 ]
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "open gripper"
       gripper:
         <<: *OPEN_GRIPPER
-  R_SMALL_ISLAND0:
-    - step: "get ore"
+    - step: "ready to get ore"
       arm:
-        <<: *GET_ORE_3
-        variable: [ 0, 0, *JOINT3_R_GET_ORE_POSITION, 0, 0, 0, 0 ]
+        joints: [ *JOINT1_ONE_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_R90_POSITION]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  SMALL_ISLAND_DEMO0:
     - step: "move up ore"
       arm:
-        <<: *MOVE_UP_ORE_3
-        variable: [ 0, 0, *JOINT3_R_GET_ORE_POSITION, 0, 0, 0, 0 ]
-    - step: "ready store ore"
-      arm:
-        <<: *READY_TO_STORE_STONE_AFTER_GET_ORE
-    - step: "reversal move"
-      reversal:
-        <<: *VELOCITY_Z_IN_QUICKLY_WITH_DELAY
-    - step: "store ore"
-      arm:
-        <<: *STORE_STONE_AFTER_GET_ORE
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "reverse joint5 up"
-      arm:
-        <<: *REVERSE_JOINT5_UP
-    - step: "add store num"
-      stone_num:
-        change: "+1"
-    - step: "reversal stop"
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "home one stone"
-      arm:
-        <<: *HOME_1
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
-    - step: "gimbal side"
-      gimbal:
-        <<: *SIDE_POS
-  ##################### TAKE STORED STONE ####################
-  TAKE_WHEN_ONE_STONE:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "move down to get stone"
-      arm:
-        <<: *READY_TO_ACCESS_STORE_1
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0, 0 ]
-    - step: "REVERSAL OUT"
-      reversal:
-        <<: *POSITION_Z_OUT
-    - step: "move up with stone and ready to get stone"
-      arm:
-        <<: *READY_TO_ACCESS_STORE_1
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, 0, 0, 0 ]
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "move joint2 to get stone"
-      arm:
-        <<: *ACCESS_STORE_15
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_R90_POSITION, 0, 0 ]
-    - step: "wait gripper get stone steady"
-      reversal:
-        <<: *VELOCITY_STOP
-        delay: 0.3
-    - step: "REVERSAL Z OUT"
-      reversal:
-        <<: *VELOCITY_Z_OUT
-    - step: "reverse joint5"
-      arm:
-        <<: *REVERSE_JOINT5_UP
+        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_R90_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "reverse ore"
       arm:
-        <<: *ACCESS_STORE_15
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_MID_POSITION, 0, 0 ]
-    - step: "REVERSAL OUT"
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "change stone"
-      stone_num:
-        change: "-1"
-    - step: "move to exchange wait state"
-      arm:
-        <<: *EXCHANGE_POSITION
-  TAKE_WHEN_TWO_STONE:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "move down to get stone"
-      arm:
-        <<: *READY_TO_ACCESS_STORE_1
-        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0, 0 ]
-    - step: "reversal out little"
-      reversal:
-        <<: *POSITION_Z_OUT_LITTLE
-    - step: "move up with stone and ready to get stone"
-      arm:
-        <<: *READY_TO_ACCESS_STORE_1
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, 0, 0, 0 ]
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "move joint2 to get stone"
-      arm:
-        <<: *ACCESS_STORE_15
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_R90_POSITION, 0, 0 ]
-    - step: "wait gripper get stone steady"
-      reversal:
-        <<: *VELOCITY_STOP
-        delay: 0.3
-    - step: "reverse joint5"
-      arm:
-        <<: *REVERSE_JOINT5_UP
-    - step: "REVERSAL Z OUT"
-      reversal:
-        <<: *VELOCITY_Z_OUT
-    - step: "reverse joint5"
-      arm:
-        <<: *ACCESS_STORE_15
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_MID_POSITION, 0, 0 ]
-    - step: "REVERSAL OUT"
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "change stone"
-      stone_num:
-        change: "-1"
-    - step: "move to exchange wait state"
-      arm:
-        <<: *EXCHANGE_POSITION
-
-  STORE_WHEN_ZERO_STONE:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "reversal z in"
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "ready to store"
-      arm:
-        <<: *ACCESS_STORE_15
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_MID_POSITION, 0, 0 ]
-    - step: "store"
-      arm:
-        <<: *ACCESS_STORE_15
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_R90_POSITION, 0, 0 ]
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "avoid collision"
-      arm:
-        <<: *ACCESS_STORE_15
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_MID_POSITION, 0, 0 ]
-    - step: "change stone"
-      stone_num:
-        change: "+1"
-    - step: "reversal z stop"
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "home one stone"
-      arm:
-        <<: *HOME_1
-        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0, 0 ]
-  STORE_WHEN_ONE_STONE:
-    - step: "island gimbal"
-      gimbal:
-        <<: *ISLAND_POS
-    - step: "ready to store"
-      arm:
-        <<: *ACCESS_STORE_15
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_MID_POSITION, 0, 0 ]
-    - step: "reversal z in"
-      reversal:
-        <<: *VELOCITY_Z_IN
-    - step: "store"
-      arm:
-        <<: *ACCESS_STORE_15
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_R90_POSITION, 0, 0 ]
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "change stone"
-      stone_num:
-        change: "+1"
-    - step: "avoid collision"
-      arm:
-        <<: *ACCESS_STORE_15
-        variable: [ *JOINT1_STORE_ORE_POSITION, 0, 0, 0, *JOINT5_MID_POSITION, 0, 0 ]
-    - step: "reversal z stop"
-      reversal:
-        <<: *VELOCITY_STOP
-    - step: "home one stone"
+        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_R90_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "move ore back"
       arm:
-        <<: *HOME_1
-        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0, 0 ]
+        joints: [ *JOINT1_ONE_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_R90_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+  ############ MID #############
 
   ############################ HOME ##########################
   HOME_ZERO_STONE:
@@ -590,6 +241,7 @@ steps_list:
       arm:
         <<: *HOME_1
         variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+
   OPEN_GRIPPER:
     - step: "open gripper"
       gripper:
@@ -649,10 +301,7 @@ steps_list:
         max_planning_times: 100
         common:
           <<: *NORMALLY
-  WAIT:
-    - step: "move to exchange wait state"
-      arm:
-        <<: *EXCHANGE_POSITION
+
 
 #    - step: "test new exchange"
 #      arm:

From 9e7a9f73d6045d4e0f28c32ffc16bfa093fc62cd Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 16 Mar 2024 14:55:43 +0800
Subject: [PATCH 033/147] add a class to control gimbal lifter, ore bin lifter
 and rotator.

---
 engineer_middleware/config/steps_list.yaml    | 85 ++++++++++++++++---
 .../include/engineer_middleware/middleware.h  |  2 +-
 .../include/engineer_middleware/motion.h      | 30 +++++--
 .../include/engineer_middleware/step.h        | 15 +++-
 .../include/engineer_middleware/step_queue.h  |  6 +-
 engineer_middleware/src/middleware.cpp        |  5 +-
 6 files changed, 119 insertions(+), 24 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 513a098..60b1cdd 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -47,6 +47,8 @@ common:
                            0.1
       furthest_position: &JOINT2_FURTHEST_POSITION
                            0.2575
+      exchange_position: &JOINT2_EXCHANGE_POSITION
+                          0.07
 
   joint3:
     mechanical:
@@ -91,19 +93,29 @@ common:
                            1.49
   gimbal:
     side_pos: &SIDE_POS
-      frame: gimbal_base
+      frame: gimbal_lifter
       position: [ 0.03 ,3, -0.02 ]
     island_pos: &ISLAND_POS
-      frame: gimbal_base
+      frame: gimbal_lifter
       position: [ 2, 0.001, 0.001 ]
     ground_pos: &GROUND_POS
-      frame: gimbal_base
+      frame: gimbal_lifter
       position: [ 2, 0.001, -0.8 ]
 
+  gimbal_lifter:
+    button_pos: &BUTTON_POS
+      0.01
+    mid_pos: &MID_POS
+      0.1
+    tallest_pos: &TALLEST_POS
+      0.28
+
   gripper:
     open_gripper: &OPEN_GRIPPER
+      pin: 0
       state: true
     close_gripper: &CLOSE_GRIPPER
+      pin: 0
       state: false
 
   arm:
@@ -205,13 +217,34 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+
+  EXCHANGE_POS:
+    - step: "gimbal look front"
+      gimbal:
+        <<: *ISLAND_POS
+    - step: "move arm"
+      arm:
+        joints: [*JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  GIMBAL_FRONT:
+    - step: "gimbal look front"
+      gimbal:
+        <<: *ISLAND_POS
+  GIMBAL_LEFT:
+    - step: "gimbal look left"
+      gimbal:
+        <<: *SIDE_POS
   ############ MID #############
 
   ############################ HOME ##########################
   HOME_ZERO_STONE:
     - step: "gimbal look side"
       gimbal:
-        <<: *SIDE_POS
+        <<: *ISLAND_POS
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -222,7 +255,7 @@ steps_list:
   HOME_ONE_STONE:
     - step: "gimbal look side"
       gimbal:
-        <<: *SIDE_POS
+        <<: *ISLAND_POS
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -233,7 +266,7 @@ steps_list:
   HOME_TWO_STONE:
     - step: "gimbal look side"
       gimbal:
-        <<: *SIDE_POS
+        <<: *ISLAND_POS
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -274,11 +307,26 @@ steps_list:
       arm:
         frame: exchanger
         is_refer_planning_frame: true
-        xyz: [ -0.23, 0., -0.12 ]
+        xyz: [ -0.3, 0.2, -0.2 ]
         rpy: [ 0., -1.5707963, 0. ]
         drift_dimensions: [ true, true, true, true, true, true ]
-        tolerance_position: 0.05
-        tolerance_orientation: 0.05
+        tolerance_position: 0.01
+        tolerance_orientation: 0.001
+        spacial_shape: SPHERE
+        radius: 0.5
+        point_resolution: 0.5
+        max_planning_times: 100
+        common:
+          <<: *NORMALLY
+  EXCHANGE_ORI:
+    - step: "test new exchange"
+      arm:
+        frame: exchanger
+        is_refer_planning_frame: true
+        rpy: [ 0., -1.5707963, 0. ]
+        drift_dimensions: [ true, true, true, true, true, true ]
+        tolerance_position: 0.01
+        tolerance_orientation: 0.001
         spacial_shape: SPHERE
         radius: 0.5
         point_resolution: 0.5
@@ -292,9 +340,9 @@ steps_list:
         is_refer_planning_frame: true
         xyz: [ -0.3, 0., 0. ]
         rpy: [ 0., 3.1415926, 0. ]
-        drift_dimensions: [ true, true, true, true, false, true ]
-        tolerance_position: 0.05
-        tolerance_orientation: 0.05
+        drift_dimensions: [ true, true, true, true, true, true ]
+        tolerance_position: 0.01
+        tolerance_orientation: 0.001
         spacial_shape: SPHERE
         radius: 0.5
         point_resolution: 0.5
@@ -302,6 +350,19 @@ steps_list:
         common:
           <<: *NORMALLY
 
+  GTESTL:
+    - step: "test"
+      gimbal_lifter:
+        target: *BUTTON_POS
+  GTESTM:
+    - step: "test"
+      gimbal_lifter:
+        target: *MID_POS
+  GTESTH:
+    - step: "test"
+      gimbal_lifter:
+        target: *TALLEST_POS
+
 
 #    - step: "test new exchange"
 #      arm:
diff --git a/engineer_middleware/include/engineer_middleware/middleware.h b/engineer_middleware/include/engineer_middleware/middleware.h
index b588530..58bcb2d 100644
--- a/engineer_middleware/include/engineer_middleware/middleware.h
+++ b/engineer_middleware/include/engineer_middleware/middleware.h
@@ -82,7 +82,7 @@ class Middleware
   moveit::planning_interface::MoveGroupInterface arm_group_;
   ChassisInterface chassis_interface_;
   ros::Publisher hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, planning_result_pub_,
-      stone_num_pub_, point_cloud_pub_;
+      stone_num_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub;
   std::unordered_map<std::string, StepQueue> step_queues_;
   tf2_ros::Buffer tf_;
   tf2_ros::TransformListener tf_listener_;
diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 591ca8c..cbc3929 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -216,13 +216,11 @@ class EndEffectorMotion : public MoveitMotionBase
     quatToRPY(target_.pose.orientation, roll_goal, pitch_goal, yaw_goal);
     // TODO: Add orientation error check
     return (std::pow(pose.position.x - target_.pose.position.x, 2) +
-                    std::pow(pose.position.y - target_.pose.position.y, 2) +
-                    std::pow(pose.position.z - target_.pose.position.z, 2) <
-                tolerance_position_ &&
-            std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) +
-                    std::abs(angles::shortest_angular_distance(pitch_current, pitch_goal)) +
-                    std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) <
-                tolerance_orientation_);
+            std::pow(pose.position.y - target_.pose.position.y, 2) +
+            std::pow(pose.position.z - target_.pose.position.z, 2) < tolerance_position_ &&
+            std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) < tolerance_orientation_ &&
+            std::abs(angles::shortest_angular_distance(pitch_current, pitch_goal)) < tolerance_orientation_ &&
+            std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) <tolerance_orientation_);
   }
   tf2_ros::Buffer& tf_;
   bool has_pos_, has_ori_, is_cartesian_;
@@ -702,4 +700,22 @@ class ReversalMotion : public PublishMotion<rm_msgs::MultiDofCmd>
   rm_msgs::MultiDofCmd zero_msg_;
 };
 
+class JointPointMotion : public PublishMotion<std_msgs::Float64>
+{
+public:
+  JointPointMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface)
+    : PublishMotion<std_msgs::Float64>(motion, interface)
+  {
+    ROS_ASSERT(motion.hasMember("target"));
+    target_ = xmlRpcGetDouble(motion, "target", 0.0);
+  }
+  bool move() override
+  {
+    msg_.data = target_;
+    return PublishMotion::move();
+  }
+private:
+  double target_;
+};
+
 };  // namespace engineer_middleware
diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h
index c87162a..24fdc4e 100644
--- a/engineer_middleware/include/engineer_middleware/step.h
+++ b/engineer_middleware/include/engineer_middleware/step.h
@@ -50,7 +50,7 @@ class Step
        moveit::planning_interface::MoveGroupInterface& arm_group, ChassisInterface& chassis_interface,
        ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub,
        ros::Publisher& reversal_pub, ros::Publisher& stone_num_pub, ros::Publisher& planning_result_pub,
-       ros::Publisher& point_cloud_pub)
+       ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub)
     : planning_result_pub_(planning_result_pub), point_cloud_pub_(point_cloud_pub), arm_group_(arm_group)
   {
     ROS_ASSERT(step.hasMember("step"));
@@ -84,6 +84,12 @@ class Step
         if (step["scene_name"] == it->first)
           planning_scene_ = new PlanningScene(it->second, arm_group);
     }
+    if (step.hasMember("ore_rotater"))
+      ore_rotate_motion_ = new JointPointMotion(step["ore_rotator"], ore_rotate_pub);
+    if (step.hasMember("ore_lifter"))
+      ore_lift_motion_ = new JointPointMotion(step["ore_lifter"], ore_lift_pub);
+    if (step.hasMember("gimbal_lifter"))
+      gimbal_lift_motion_ = new JointPointMotion(step["gimbal_lifter"], gimbal_lift_pub);
   }
   bool move()
   {
@@ -115,6 +121,12 @@ class Step
       success &= reversal_motion_->move();
     if (planning_scene_)
       planning_scene_->add();
+    if (ore_lift_motion_)
+      success &= ore_lift_motion_->move();
+    if (ore_rotate_motion_)
+      success &= ore_rotate_motion_->move();
+    if (gimbal_lift_motion_)
+      success &= gimbal_lift_motion_->move();
     return success;
   }
   void stop()
@@ -182,6 +194,7 @@ class Step
   MoveitMotionBase* arm_motion_{};
   HandMotion* hand_motion_{};
   JointPositionMotion* end_effector_motion_{};
+  JointPointMotion* ore_rotate_motion_{},* ore_lift_motion_{},* gimbal_lift_motion_{};
   StoneNumMotion* stone_num_motion_{};
   ChassisMotion* chassis_motion_{};
   GimbalMotion* gimbal_motion_{};
diff --git a/engineer_middleware/include/engineer_middleware/step_queue.h b/engineer_middleware/include/engineer_middleware/step_queue.h
index f1adbd8..2c6b63c 100644
--- a/engineer_middleware/include/engineer_middleware/step_queue.h
+++ b/engineer_middleware/include/engineer_middleware/step_queue.h
@@ -55,13 +55,15 @@ class StepQueue
             moveit::planning_interface::MoveGroupInterface& arm_group, ChassisInterface& chassis_interface,
             ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& stone_num_pub,
             ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub, ros::Publisher& reversal_pub,
-            ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub)
+            ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub,
+            ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub)
     : chassis_interface_(chassis_interface)
   {
     ROS_ASSERT(steps.getType() == XmlRpc::XmlRpcValue::TypeArray);
     for (int i = 0; i < steps.size(); ++i)
       queue_.emplace_back(steps[i], scenes, tf, arm_group, chassis_interface, hand_pub, end_effector_pub, stone_num_pub,
-                          gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, point_cloud_pub);
+                          gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, point_cloud_pub, ore_rotate_pub,
+                          ore_lift_pub, gimbal_lift_pub);
   }
   bool run(actionlib::SimpleActionServer<rm_msgs::EngineerAction>& as)
   {
diff --git a/engineer_middleware/src/middleware.cpp b/engineer_middleware/src/middleware.cpp
index d506671..f57dee3 100644
--- a/engineer_middleware/src/middleware.cpp
+++ b/engineer_middleware/src/middleware.cpp
@@ -53,6 +53,9 @@ Middleware::Middleware(ros::NodeHandle& nh)
   , planning_result_pub_(nh.advertise<std_msgs::Int32>("/planning_result", 10))
   , stone_num_pub_(nh.advertise<std_msgs::String>("/stone_num", 10))
   , point_cloud_pub_(nh.advertise<sensor_msgs::PointCloud2>("/cloud", 100))
+  , ore_rotate_pub_(nh.advertise<std_msgs::Float64>("/controllers/ore_bin_rotate_controller/command", 10))
+  , ore_lift_pub_(nh.advertise<std_msgs::Float64>("/controllers/ore_bin_lifter_controller/command", 10))
+  , gimbal_lift_pub(nh.advertise<std_msgs::Float64>("/controllers/gimbal_lifter_controller/command", 10))
   , tf_listener_(tf_)
   , is_middleware_control_(false)
 {
@@ -69,7 +72,7 @@ Middleware::Middleware(ros::NodeHandle& nh)
       step_queues_.insert(
           std::make_pair(it->first, StepQueue(it->second, scenes_list, tf_, arm_group_, chassis_interface_, hand_pub_,
                                               end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, stone_num_pub_,
-                                              planning_result_pub_, point_cloud_pub_)));
+                                              planning_result_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub)));
     }
   }
   else

From 2f1bfe471cc353bd486fc59fde643639b260b652 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sun, 17 Mar 2024 22:37:14 +0800
Subject: [PATCH 034/147] fix roll error didn't being checked.

---
 .../include/engineer_middleware/motion.h           | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index cbc3929..71b4cd5 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -58,7 +58,7 @@ class MotionBase
 public:
   MotionBase(XmlRpc::XmlRpcValue& motion, Interface& interface) : interface_(interface)
   {
-    time_out_ = xmlRpcGetDouble(motion["common"], "timeout", 1e10);
+    time_out_ = xmlRpcGetDouble(motion["common"], "timeout", 3);
   };
   ~MotionBase() = default;
   virtual bool move() = 0;
@@ -216,11 +216,12 @@ class EndEffectorMotion : public MoveitMotionBase
     quatToRPY(target_.pose.orientation, roll_goal, pitch_goal, yaw_goal);
     // TODO: Add orientation error check
     return (std::pow(pose.position.x - target_.pose.position.x, 2) +
-            std::pow(pose.position.y - target_.pose.position.y, 2) +
-            std::pow(pose.position.z - target_.pose.position.z, 2) < tolerance_position_ &&
+                    std::pow(pose.position.y - target_.pose.position.y, 2) +
+                    std::pow(pose.position.z - target_.pose.position.z, 2) <
+                tolerance_position_ &&
             std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) < tolerance_orientation_ &&
             std::abs(angles::shortest_angular_distance(pitch_current, pitch_goal)) < tolerance_orientation_ &&
-            std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) <tolerance_orientation_);
+            std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) < tolerance_orientation_);
   }
   tf2_ros::Buffer& tf_;
   bool has_pos_, has_ori_, is_cartesian_;
@@ -259,7 +260,7 @@ class SpaceEeMotion : public EndEffectorMotion
       k_beta_ = xmlRpcGetDouble(motion["rpy_rectify"], 2);
     }
     point_resolution_ = xmlRpcGetDouble(motion, "point_resolution", 0.01);
-    max_planning_times_ = (int)xmlRpcGetDouble(motion, "max_planning_times", 100);
+    max_planning_times_ = (int)xmlRpcGetDouble(motion, "max_planning_times", 10);
     if (motion.hasMember("spacial_shape"))
     {
       points_.cleanPoints();
@@ -382,7 +383,7 @@ class SpaceEeMotion : public EndEffectorMotion
                     std::pow(pose.position.y - final_target_.pose.position.y, 2) +
                     std::pow(pose.position.z - final_target_.pose.position.z, 2) <
                 tolerance_position_ &&
-            std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) +
+            std::abs(angles::shortest_angular_distance(roll_current, roll_goal)) +
                     std::abs(angles::shortest_angular_distance(pitch_current, pitch_goal)) +
                     std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) <
                 tolerance_orientation_);
@@ -714,6 +715,7 @@ class JointPointMotion : public PublishMotion<std_msgs::Float64>
     msg_.data = target_;
     return PublishMotion::move();
   }
+
 private:
   double target_;
 };

From fc5279ea1660b1680e520f77a5bba1b9a18f1b8a Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Fri, 22 Mar 2024 22:43:46 +0800
Subject: [PATCH 035/147] Add small island get stone.

---
 engineer_middleware/config/steps_list.yaml | 133 ++++++++++++++++++++-
 1 file changed, 132 insertions(+), 1 deletion(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 60b1cdd..2113ebc 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -27,8 +27,12 @@ common:
     mechanical:
       down_position: &JOINT1_DOWN_POSITION
                        0.015
+      ready_position: &JOINT1_READY_POSITION
+                      0.145
       up_position: &JOINT1_UP_POSITION
-                     0.22
+                   0.252
+      ready_store: &JOINT1_READY_STORE_POSITION
+                   0.162
     home:
       zero_stone_position: &JOINT1_ZERO_STONE_POSITION
                              0.08
@@ -49,6 +53,10 @@ common:
                            0.2575
       exchange_position: &JOINT2_EXCHANGE_POSITION
                           0.07
+      lift_stone_position: &JOINT2_LIFT_STONE_POSITION
+                            0.085
+      store_stone_position: &JOINT2_STORE_STONE_POSITION
+                              0.02
 
   joint3:
     mechanical:
@@ -58,6 +66,11 @@ common:
                       0.0001
       right_position: &JOINT3_R_POSITION
                         -0.275
+      ready_position: &JOINT3_READY_POSITION
+                        0.178
+      get_stone_position: &JOINT3_GET_STONE_POSITION
+                            0.034
+
 
   joint4:
     mechanical:
@@ -380,3 +393,121 @@ steps_list:
 #        common:
 #          <<: *NORMALLY
 
+  ###################SMALL_ISLAND_FROM_TOP################
+  ONE_STONE_SMALL_ISLAND:
+    - gimbal:
+        <<:
+    - step: "SI_ARM_READY"
+      arm:
+        joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "SI_GET_STONE"
+      arm:
+        joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "SI_ARM_LIFT_STONE"
+      arm:
+        joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_TO_STORE_STONE"
+      arm:
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ORE_LIFTER_UP"
+      ore_lifter:
+
+    - step: "STORE_STONE"
+      arm:
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "ARM_RESET"
+      arm:
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  #################BIG_ISLAND#############
+#  ONE_STONE_BIG_ISLAND：
+#  - gimbal:
+#      <<: *NORMALLY
+#  - step: "BI_ARM_READY"
+#    arm:
+#      joints: [ ]
+#      common:
+#        <<: *NORMALLY
+#      tolerance:
+#        <<: *NORMAL_TOLERANCE
+#  - step: "OPEN_GRIPPER"
+#    gripper:
+#      <<: *OPEN_GRIPPER
+#  - step: "BI_GET_STONE"
+#    arm:
+#      joints: [ ]
+#      common:
+#        <<: *NORMALLY
+#      tolerance:
+#        <<: *NORMAL_TOLERANCE
+#  - step: "PULL_OUT_STONE"
+#    arm:
+#      joints: [ ]
+#      common:
+#        <<: *NORMALLY
+#      tolerance:
+#        <<: *NORMAL_TOLERANCE
+#  - step: "BI_ARM_UP_STONE"         #########joint6抬起，无需转动joint7#######
+#    arm:
+#      joints: [ ]
+#      common:
+#        <<: *NORMALLY
+#      tolerance:
+#        <<: *NORMAL_TOLERANCE
+#  - step: "ORE_LIFTER_UP"
+#    ore_lifter:
+#
+#  - step: "STORE_STONE"
+#    arm:
+#      joints: [ ]
+#      common:
+#        <<: *NORMALLY
+#      tolerance:
+#        <<: *NORMAL_TOLERANCE
+#  - step: "CLOSE_GRIPPER"
+#    gripper:
+#      <<: *CLOSE_GRIPPER
+#  - step: "ARM_RESET"
+#    arm:
+#      joints: [ ]
+#      common:
+#        <<: *NORMALLY
+#      tolerance:
+#        <<: *NORMAL_TOLERANCE
\ No newline at end of file

From 1476e70504a506657d85ee027266acca382780f6 Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Fri, 22 Mar 2024 23:06:27 +0800
Subject: [PATCH 036/147] Update.

---
 engineer_middleware/config/steps_list.yaml | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 2113ebc..82e2d29 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -395,7 +395,10 @@ steps_list:
 
   ###################SMALL_ISLAND_FROM_TOP################
   ONE_STONE_SMALL_ISLAND:
-    - gimbal:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *SIDE_POS
+      gimbal_lifter:
         <<:
     - step: "SI_ARM_READY"
       arm:

From 5f3cd6d40adae87117920dfe856be6f108ae4330 Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Sat, 23 Mar 2024 17:27:04 +0800
Subject: [PATCH 037/147] Add small island two stone.

---
 engineer_middleware/config/steps_list.yaml | 222 ++++++++++++++++-----
 1 file changed, 169 insertions(+), 53 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 82e2d29..8c295fd 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -28,9 +28,9 @@ common:
       down_position: &JOINT1_DOWN_POSITION
                        0.015
       ready_position: &JOINT1_READY_POSITION
-                      0.145
+                      0.175
       up_position: &JOINT1_UP_POSITION
-                   0.252
+                   0.312
       ready_store: &JOINT1_READY_STORE_POSITION
                    0.162
     home:
@@ -67,7 +67,7 @@ common:
       right_position: &JOINT3_R_POSITION
                         -0.275
       ready_position: &JOINT3_READY_POSITION
-                        0.178
+                        0.098
       get_stone_position: &JOINT3_GET_STONE_POSITION
                             0.034
 
@@ -140,52 +140,53 @@ common:
         <<: *NORMAL_TOLERANCE
 
 
-    reversal:
-      velocity_stop: &VELOCITY_STOP
-        mode: "VELOCITY"
-        values: [ 0.,0.,0.,0.,0.,0. ]
-      velocity_z_out: &VELOCITY_Z_OUT
-        mode: "VELOCITY"
-        values: [ 0.,0.,3.,0.,0.,0. ]
-      velocity_z_out_store: &VELOCITY_Z_OUT_STORE
-        mode: "VELOCITY"
-        values: [ 0.,0.,0.2,0.,0.,0. ]
-      velocity_z_in: &VELOCITY_Z_IN
-        mode: "VELOCITY"
-        values: [ 0.,0.,-1.2,0.,-0.01,0. ]
-      velocity_z_in_quickly: &VELOCITY_Z_IN_QUICKLY
-        mode: "VELOCITY"
-        values: [ 0.,0.,-3.0,0.,-0.01,0. ]
-      velocity_z_in_quickly_with_delay: &VELOCITY_Z_IN_QUICKLY_WITH_DELAY
-        mode: "VELOCITY"
-        values: [ 0.,0.,-3.0,0.,-0.01,0. ]
-        delay: 0.3
-      position_z_out: &POSITION_Z_OUT
-        mode: "POSITION"
-        values: [ 0.,0.,7.,0.,0.,0. ]
-        delay: 0.2
-      position_z_out_little: &POSITION_Z_OUT_LITTLE
-        mode: "POSITION"
-        values: [ 0.,0.,3.,0.,0.,0. ]
-        delay: 0.4
+  reversal:
+    velocity_stop: &VELOCITY_STOP
+      mode: "VELOCITY"
+      values: [ 0.,0.,0.,0.,0.,0. ]
+    velocity_z_out: &VELOCITY_Z_OUT
+      mode: "VELOCITY"
+      values: [ 0.,0.,3.,0.,0.,0. ]
+    velocity_z_out_store: &VELOCITY_Z_OUT_STORE
+      mode: "VELOCITY"
+      values: [ 0.,0.,0.2,0.,0.,0. ]
+    velocity_z_in: &VELOCITY_Z_IN
+      mode: "VELOCITY"
+      values: [ 0.,0.,-1.2,0.,-0.01,0. ]
+    velocity_z_in_quickly: &VELOCITY_Z_IN_QUICKLY
+      mode: "VELOCITY"
+      values: [ 0.,0.,-3.0,0.,-0.01,0. ]
+    velocity_z_in_quickly_with_delay: &VELOCITY_Z_IN_QUICKLY_WITH_DELAY
+      mode: "VELOCITY"
+      values: [ 0.,0.,-3.0,0.,-0.01,0. ]
+      delay: 0.3
+    position_z_out: &POSITION_Z_OUT
+      mode: "POSITION"
+      values: [ 0.,0.,7.,0.,0.,0. ]
+      delay: 0.2
+    position_z_out_little: &POSITION_Z_OUT_LITTLE
+      mode: "POSITION"
+      values: [ 0.,0.,3.,0.,0.,0. ]
+      delay: 0.4
 
-    chassis_move:
-      chassis_backward_30: &CHASSIS_BACKWARD_25
-        frame: base_link
-        position: [ -0.35,0. ]
-        yaw: 0.0
-        chassis_tolerance_position: 0.1
-        chassis_tolerance_angular: 0.3
-        common:
-          timeout: 2.
-      chassis_180: &CHASSIS_180
-        frame: base_link
-        position: [ 0., 0. ]
-        yaw: 2.80
-        chassis_tolerance_position: 0.1
-        chassis_tolerance_angular: 0.2
-        common:
-          timeout: 2.
+  chassis_move:
+    chassis_backward_30: &CHASSIS_BACKWARD_25
+      frame: base_link
+      position: [ -0.35,0. ]
+      yaw: 0.0
+      chassis_tolerance_position: 0.1
+      chassis_tolerance_angular: 0.3
+      common:
+        timeout: 2.
+    chassis_180: &CHASSIS_180
+      frame: base_link
+      position: [ 0., 0. ]
+      yaw: 2.80
+      chassis_tolerance_position: 0.1
+      chassis_tolerance_angular: 0.2
+      common:
+        timeout:
+          2.
 
 steps_list:
   ###################### SMALL_ISLAND #######################
@@ -398,8 +399,6 @@ steps_list:
     - step: "GIMBAL_READY"
       gimbal:
         <<: *SIDE_POS
-      gimbal_lifter:
-        <<:
     - step: "SI_ARM_READY"
       arm:
         joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
@@ -410,6 +409,7 @@ steps_list:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
+  ONE_STONE_SMALL_ISLAND0:
     - step: "SI_GET_STONE"
       arm:
         joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
@@ -438,9 +438,6 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "ORE_LIFTER_UP"
-      ore_lifter:
-
     - step: "STORE_STONE"
       arm:
         joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
@@ -459,6 +456,125 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
 
+  ####FIRST_STONE###
+  TWO_STONE_SMALL_ISLAND:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *SIDE_POS
+    - step: "SI_ARM_READY"
+      arm:
+        joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+  TWO_STONE_SMALL_ISLAND0:
+    - step: "SI_GET_STONE"
+      arm:
+        joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "SI_ARM_LIFT_STONE"
+      arm:
+        joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_TO_STORE_STONE"
+      arm:
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE_STONE"
+      arm:
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "ARM_RESET"
+      arm:
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+  ###SECOND_STONE###
+  TWO_STONE_SMALL_ISLAND00:
+    - step: "SI_ARM_READY"
+      arm:
+        joints: [ *JOINT1_READY_POSITION, *JOINT2_FURTHEST_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+  TWO_STONE_SMALL_ISLAND000:
+    - step: "SI_GET_STONE"
+      arm:
+        joints: [ *JOINT1_READY_POSITION, *JOINT2_FURTHEST_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "SI_ARM_LIFT_STONE"
+      arm:
+        joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_TO_STORE_STONE"
+      arm:
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE_STONE"
+      arm:
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "ARM_RESET"
+      arm:
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
   #################BIG_ISLAND#############
 #  ONE_STONE_BIG_ISLAND：
 #  - gimbal:

From 2445b53557bdc44be2fd91e79d446a52c84b4711 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 23 Mar 2024 21:54:48 +0800
Subject: [PATCH 038/147] change namespace when loading servo params.

---
 engineer_arm_config/launch/servo.launch | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/engineer_arm_config/launch/servo.launch b/engineer_arm_config/launch/servo.launch
index 58ab720..5b32b05 100644
--- a/engineer_arm_config/launch/servo.launch
+++ b/engineer_arm_config/launch/servo.launch
@@ -1,6 +1,6 @@
 <launch>
     <node name="servo_server" pkg="moveit_servo" type="servo_server" output="screen">
         <param name="parameter_ns" type="string" value="engineer_middleware"/>
-        <rosparam ns="engineer_middleware" command="load" file="$(find engineer_arm_config)/config/servo.yaml"/>
+        <rosparam command="load" file="$(find engineer_arm_config)/config/servo.yaml"/>
     </node>
 </launch>

From 23d100a6bfef902194df45111c76a66645b07630 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sun, 24 Mar 2024 12:01:40 +0800
Subject: [PATCH 039/147] add a launch file to start gazebo for engineer.

---
 engineer_middleware/launch/sim.launch | 6 ++++++
 1 file changed, 6 insertions(+)
 create mode 100644 engineer_middleware/launch/sim.launch

diff --git a/engineer_middleware/launch/sim.launch b/engineer_middleware/launch/sim.launch
new file mode 100644
index 0000000..271372d
--- /dev/null
+++ b/engineer_middleware/launch/sim.launch
@@ -0,0 +1,6 @@
+<launch>
+    <include file="$(find rm_gazebo)/launch/empty_world.launch"/>
+    <include file="$(find rm_config)/launch/load_controllers.launch"/>
+    <include file="$(find engineer_middleware)/launch/load.launch"/>
+    <include file="$(find engineer_arm_config)/launch/load_move_group.launch"/>
+</launch>

From a834134b37e9d542e91cdcb690d1230ddee187e8 Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Sun, 24 Mar 2024 16:01:33 +0800
Subject: [PATCH 040/147] Update small island two stone.

---
 engineer_middleware/config/steps_list.yaml | 44 +++++++++++++++++-----
 1 file changed, 35 insertions(+), 9 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 8c295fd..1e4490a 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -50,13 +50,15 @@ common:
       get_ore_position: &JOINT2_GET_ORE_POSITION
                            0.1
       furthest_position: &JOINT2_FURTHEST_POSITION
-                           0.2575
+                           0.232
       exchange_position: &JOINT2_EXCHANGE_POSITION
                           0.07
       lift_stone_position: &JOINT2_LIFT_STONE_POSITION
                             0.085
       store_stone_position: &JOINT2_STORE_STONE_POSITION
                               0.02
+      ready_second_position: &JOINT2_READY_SECOND_POSITION
+                              0.142
 
   joint3:
     mechanical:
@@ -68,8 +70,12 @@ common:
                         -0.275
       ready_position: &JOINT3_READY_POSITION
                         0.098
+      ready_second_position: &JOINT3_READY_SECOND_POSITION
+                        0.03
       get_stone_position: &JOINT3_GET_STONE_POSITION
                             0.034
+      get_second_position: &JOINT3_GET_SECOND_POSITION
+                            -0.018
 
 
   joint4:
@@ -80,6 +86,8 @@ common:
                       0.001
       right_position: &JOINT4_R_POSITION
                         -1.45
+      ready_second_position: &JOINT4_READY_SECOND_POSITION
+                        -0.777
   joint5:
     mechanical:
       mid_position: &JOINT5_MID_POSITION
@@ -96,6 +104,8 @@ common:
                      -1.49
       down_position: &JOINT6_DOWN_POSITION
                        1.569
+      ready_second_position: &JOINT6_READY_SECOND_POSITION
+                       0.694
   joint7:
     mechanical:
       mid_position: &JOINT7_MID_POSITION
@@ -185,8 +195,7 @@ common:
       chassis_tolerance_position: 0.1
       chassis_tolerance_angular: 0.2
       common:
-        timeout:
-          2.
+        timeout: 2.
 
 steps_list:
   ###################### SMALL_ISLAND #######################
@@ -312,7 +321,7 @@ steps_list:
         spacial_shape: SPHERE
         radius: 0.5
         point_resolution: 0.5
-        max_planning_times: 100
+        max_planning_times: 1
         common:
           <<: *QUICKLY
 
@@ -329,9 +338,26 @@ steps_list:
         spacial_shape: SPHERE
         radius: 0.5
         point_resolution: 0.5
-        max_planning_times: 100
+        max_planning_times: 1
         common:
           <<: *NORMALLY
+
+  TEST_ORI:
+    - step: "ori test"
+      arm:
+        frame: exchanger
+        rpy: [ 0., -1.5707963, 0. ]
+        tolerance_orientation: 0.001
+
+  TEST_ORI2:
+    - step: "ori test2"
+      arm:
+        frame: exchanger
+        rpy: [ 0., -1.5707963, 0. ]
+        xyz: [ -0.3, 0.2, -0.2 ]
+        tolerance_position: 0.01
+        tolerance_orientation: 0.001
+
   EXCHANGE_ORI:
     - step: "test new exchange"
       arm:
@@ -344,7 +370,7 @@ steps_list:
         spacial_shape: SPHERE
         radius: 0.5
         point_resolution: 0.5
-        max_planning_times: 100
+        max_planning_times: 1
         common:
           <<: *NORMALLY
   EXCHANGE_STRAIGHT:
@@ -360,7 +386,7 @@ steps_list:
         spacial_shape: SPHERE
         radius: 0.5
         point_resolution: 0.5
-        max_planning_times: 100
+        max_planning_times: 1
         common:
           <<: *NORMALLY
 
@@ -521,7 +547,7 @@ steps_list:
   TWO_STONE_SMALL_ISLAND00:
     - step: "SI_ARM_READY"
       arm:
-        joints: [ *JOINT1_READY_POSITION, *JOINT2_FURTHEST_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_READY_POSITION, *JOINT2_READY_SECOND_POSITION, *JOINT3_READY_SECOND_POSITION, *JOINT4_READY_SECOND_POSITION, *JOINT5_L90_POSITION, *JOINT6_READY_SECOND_POSITION, *JOINT7_R90_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -532,7 +558,7 @@ steps_list:
   TWO_STONE_SMALL_ISLAND000:
     - step: "SI_GET_STONE"
       arm:
-        joints: [ *JOINT1_READY_POSITION, *JOINT2_FURTHEST_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_READY_POSITION, *JOINT2_READY_SECOND_POSITION, *JOINT3_GET_SECOND_POSITION, *JOINT4_READY_SECOND_POSITION, *JOINT5_L90_POSITION, *JOINT6_READY_SECOND_POSITION, *JOINT7_R90_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:

From c9b0be5f473469758697bd0ae9e3518b0bd3d5b1 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sun, 24 Mar 2024 21:37:45 +0800
Subject: [PATCH 041/147] add necessary controllers, remove useless
 controllers.

---
 .../launch/load_controllers.launch              | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/engineer_arm_config/launch/load_controllers.launch b/engineer_arm_config/launch/load_controllers.launch
index 47d0bfb..5bb5ed2 100644
--- a/engineer_arm_config/launch/load_controllers.launch
+++ b/engineer_arm_config/launch/load_controllers.launch
@@ -22,15 +22,16 @@
                 controllers/arm_trajectory_controller
                 controllers/chassis_controller
                 controllers/gpio_controller
-                controllers/pitch_front_controller
-                controllers/pitch_behind_controller
-                controllers/roll_left_controller
-                controllers/roll_right_controller
-                controllers/drag_controller
-                controllers/drag_calibration_controller
-                controllers/pitch_calibration_controller
-                controllers/yaw_calibration_controller
                 controllers/gimbal_controller
+                controllers/ore_bin_rotate_controller
+                controllers/ore_bin_lifter_controller
+                controllers/ore_bin_lifter_calibration_controller
+                controllers/gimbal_lifter_controller
+                controllers/gimbal_lifter_calibration_controller
+                controllers/arm_trajectory_controller
                 "
     />
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="odom_tf_publisher"
+          args="0 0 0 0 0 0 1 map odom"/>
 </launch>

From 25c918ddbd5a8641e6674b81c3f6b8a6ff02b02b Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sun, 24 Mar 2024 21:39:38 +0800
Subject: [PATCH 042/147] Use load_controllers.launch in engineer_arm_config to
 avoid controller conflict with other robots.Add param in launch file to
 select worlds.

---
 engineer_middleware/launch/sim.launch | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/engineer_middleware/launch/sim.launch b/engineer_middleware/launch/sim.launch
index 271372d..4465be7 100644
--- a/engineer_middleware/launch/sim.launch
+++ b/engineer_middleware/launch/sim.launch
@@ -1,6 +1,7 @@
 <launch>
-    <include file="$(find rm_gazebo)/launch/empty_world.launch"/>
-    <include file="$(find rm_config)/launch/load_controllers.launch"/>
+    <arg name="world" default="empty_world" doc="big_resource small_resource"/>
+    <include file="$(find rm_gazebo)/launch/$(arg world).launch"/>
+    <include file="$(find engineer_arm_config)/launch/load_controllers.launch"/>
     <include file="$(find engineer_middleware)/launch/load.launch"/>
     <include file="$(find engineer_arm_config)/launch/load_move_group.launch"/>
 </launch>

From ecce1e852976f3e6cac6488ff9a1be2a7a0247d0 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sun, 24 Mar 2024 23:34:11 +0800
Subject: [PATCH 043/147] Add a config file for controllers in gazebo
 simulation. Use different configure file in different situation.

---
 .../launch/load_controllers.launch            |  10 +-
 .../config/simulation_controllers.yaml        | 125 ++++++++++++++++++
 engineer_middleware/launch/sim.launch         |   4 +-
 3 files changed, 137 insertions(+), 2 deletions(-)
 create mode 100644 engineer_middleware/config/simulation_controllers.yaml

diff --git a/engineer_arm_config/launch/load_controllers.launch b/engineer_arm_config/launch/load_controllers.launch
index 5bb5ed2..169d896 100644
--- a/engineer_arm_config/launch/load_controllers.launch
+++ b/engineer_arm_config/launch/load_controllers.launch
@@ -2,12 +2,20 @@
 <launch>
     <!-- By default, we are not load controllers -->
     <arg name="spawn_controllers" default="true"/>
+    <arg name="use_simulation" default="false"/>
+
+    <group unless="$(arg use_simulation)">
+        <rosparam file="$(find rm_config)/config/rm_controllers/engineer.yaml" command="load"/>
+    </group>
+    <group if="$(arg use_simulation)">
+        <rosparam file="$(find engineer_middleware)/config/simulation_controllers.yaml" command="load"/>
+    </group>
 
-    <rosparam file="$(find rm_config)/config/rm_controllers/engineer.yaml" command="load"/>
 
     <!-- Load joint controller configurations from YAML file to parameter server -->
 
     <!-- Load the controllers -->
+
     <node name="controller_loader" pkg="controller_manager" type="controller_manager" output="screen"
           args="load
                 controllers/robot_state_controller
diff --git a/engineer_middleware/config/simulation_controllers.yaml b/engineer_middleware/config/simulation_controllers.yaml
new file mode 100644
index 0000000..e873175
--- /dev/null
+++ b/engineer_middleware/config/simulation_controllers.yaml
@@ -0,0 +1,125 @@
+controllers:
+  joint_state_controller:
+    type: joint_state_controller/JointStateController
+    publish_rate: 100
+  robot_state_controller:
+    type: robot_state_controller/RobotStateController
+    publish_rate: 100
+
+  arm_trajectory_controller:
+    type: effort_controllers/JointTrajectoryController
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+      - joint7
+    gains:
+      joint1: { p: 20000.0, i: 0, d: 400.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint2: { p: 30000, i: 0, d: 1000.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint3: { p: 10000.0, i: 0, d: 320.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint4: { p: 1000.0, i: 0.0, d: 20, i_clamp_max: 0, i_clamp_min: 0.0, antiwindup: true, publish_state: true }
+      joint5: { p: 350.0, i: 0, d: 50.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint6: { p: 1000.0, i: 0, d: 13., i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint7: { p: 5000., i: 0., d: 10000.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true }
+    constraints:
+      joint1: { trajectory: 0.05, goal: 0.05 }
+      joint2: { trajectory: 0.05, goal: 0.05 }
+      joint3: { trajectory: 0.05, goal: 0.05 }
+      joint4: { trajectory: 0.05, goal: 0.05 }
+      joint5: { trajectory: 0.05, goal: 0.05 }
+      joint6: { trajectory: 0.05, goal: 0.05 }
+      joint7: { trajectory: 0.05, goal: 0.05 }
+
+  chassis_controller:
+    type: rm_chassis_controllers/OmniController
+    # ChassisBase
+    publish_rate: 100
+    enable_odom_tf: true
+    publish_odom_tf: true
+    power:
+      effort_coeff: 2.0
+      vel_coeff: 0.008
+      power_offset: -9.8
+    twist_angular: 0.5233
+    timeout: 0.1
+    pid_follow: { p: 4.0, i: 0, d: 0.02, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true }
+    twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ]
+    max_odom_vel: 10.0
+    # OmniController
+    wheels:
+      left_front:
+        pose: [ 0.213, 0.1945, 0. ]
+        roller_angle: -0.785
+        joint: left_front_wheel_joint
+        <<: &wheel_setting
+          radius: 0.07
+          pid: { p: 0.4, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true }
+      right_front:
+        pose: [ 0.213, -0.1945, 0. ]
+        roller_angle: 0.785
+        joint: right_front_wheel_joint
+        <<: *wheel_setting
+      left_back:
+        pose: [ -0.213, 0.1945, 0. ]
+        roller_angle: 0.785
+        joint: left_back_wheel_joint
+        <<: *wheel_setting
+      right_back:
+        pose: [ -0.213, -0.1945, 0. ]
+        roller_angle: -0.785
+        joint: right_back_wheel_joint
+        <<: *wheel_setting
+
+
+  gimbal_controller:
+    type: rm_gimbal_controllers/Controller
+    yaw:
+      joint: "yaw_joint"
+      pid: { p: 0.5, i: 0.0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true }
+    pitch:
+      joint: "pitch_joint"
+      pid: { p: 0., i: 0.0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true }
+    feedforward:
+      gravity: 0.0
+      enable_gravity_compensation: false
+      mass_origin: [ 0.0,0.0,0.0 ]
+    bullet_solver:
+      resistance_coff_qd_10: 0.45
+      resistance_coff_qd_15: 1.0
+      resistance_coff_qd_16: 0.7
+      resistance_coff_qd_18: 0.55
+      resistance_coff_qd_30: 3.0
+      g: 9.81
+      delay: 0.1
+      dt: 0.001
+      timeout: 0.001
+      publish_rate: 50
+
+  ####################### Position Controllers ######################
+  ore_bin_lifter_controller:
+    type: effort_controllers/JointPositionController
+    joint: ore_bin_lifter_joint
+    pid: { p: 100000., i: 0., d: 5000, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
+
+  ore_bin_rotate_controller:
+    type: effort_controllers/JointPositionController
+    joint: ore_bin_rotate_joint
+    pid: { p: 0.9, i: 0., d: 0.06, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true }
+
+  gimbal_lifter_controller:
+    type: effort_controllers/JointPositionController
+    joint: gimbal_lifter_joint
+    pid: { p: 20000., i: 0., d: 1200, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
+
+  extend_arm_a_controller:
+    type: effort_controllers/JointPositionController
+    joint: extend_arm_a_joint
+    pid: { p: 3., i: 0.1, d: 0.05, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
+
+  extend_arm_b_controller:
+    type: effort_controllers/JointPositionController
+    joint: extend_arm_b_joint
+    pid: { p: 3., i: 0.1, d: 0.05, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
diff --git a/engineer_middleware/launch/sim.launch b/engineer_middleware/launch/sim.launch
index 4465be7..5b7857f 100644
--- a/engineer_middleware/launch/sim.launch
+++ b/engineer_middleware/launch/sim.launch
@@ -1,7 +1,9 @@
 <launch>
     <arg name="world" default="empty_world" doc="big_resource small_resource"/>
     <include file="$(find rm_gazebo)/launch/$(arg world).launch"/>
-    <include file="$(find engineer_arm_config)/launch/load_controllers.launch"/>
+    <include file="$(find engineer_arm_config)/launch/load_controllers.launch">
+        <arg name="use_simulation" value="true"/>
+    </include>
     <include file="$(find engineer_middleware)/launch/load.launch"/>
     <include file="$(find engineer_arm_config)/launch/load_move_group.launch"/>
 </launch>

From fd2ee5e59bb2155d7ee18ca0ce3d10596fee5a35 Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Mon, 25 Mar 2024 00:51:01 +0800
Subject: [PATCH 044/147] Update.

---
 engineer_middleware/config/steps_list.yaml | 26 +++++++++++++++-------
 1 file changed, 18 insertions(+), 8 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 1e4490a..91d8342 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -28,7 +28,7 @@ common:
       down_position: &JOINT1_DOWN_POSITION
                        0.015
       ready_position: &JOINT1_READY_POSITION
-                      0.175
+                      0.165
       up_position: &JOINT1_UP_POSITION
                    0.312
       ready_store: &JOINT1_READY_STORE_POSITION
@@ -76,6 +76,8 @@ common:
                             0.034
       get_second_position: &JOINT3_GET_SECOND_POSITION
                             -0.018
+      store_stone_position: &JOINT3_STORE_STONE_POSITION
+                              0.30
 
 
   joint4:
@@ -101,7 +103,7 @@ common:
       mid_position: &JOINT6_MID_POSITION
                       0.001
       up_position: &JOINT6_UP_POSITION
-                     -1.49
+                     -1.569
       down_position: &JOINT6_DOWN_POSITION
                        1.569
       ready_second_position: &JOINT6_READY_SECOND_POSITION
@@ -459,14 +461,22 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE_STONE"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+  ONE_STONE_SMALL_ISLAND00:
     - step: "STORE_STONE"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_UP_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_DOWN"
+      arm:
+        joints: [ *JOINT1_READY_STORE_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -521,14 +531,14 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE_STONE"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE_STONE"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -579,14 +589,14 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE_STONE"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE_STONE"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:

From 7843d7812740203f9ffa3e489777207f69951e47 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 26 Mar 2024 19:43:01 +0800
Subject: [PATCH 045/147] Adjust default max_planning_times.

---
 engineer_middleware/include/engineer_middleware/motion.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 71b4cd5..c893f49 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -260,7 +260,7 @@ class SpaceEeMotion : public EndEffectorMotion
       k_beta_ = xmlRpcGetDouble(motion["rpy_rectify"], 2);
     }
     point_resolution_ = xmlRpcGetDouble(motion, "point_resolution", 0.01);
-    max_planning_times_ = (int)xmlRpcGetDouble(motion, "max_planning_times", 10);
+    max_planning_times_ = (int)xmlRpcGetDouble(motion, "max_planning_times", 3);
     if (motion.hasMember("spacial_shape"))
     {
       points_.cleanPoints();

From ff247735771b9b88ea9236f136b4b4d2a4db41a7 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 26 Mar 2024 19:54:50 +0800
Subject: [PATCH 046/147] Optimize format.

---
 .../include/engineer_middleware/auto_exchange.h              | 2 +-
 engineer_middleware/include/engineer_middleware/step.h       | 5 +++--
 engineer_middleware/src/middleware.cpp                       | 3 ++-
 3 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/auto_exchange.h b/engineer_middleware/include/engineer_middleware/auto_exchange.h
index ee1b25b..687e4f6 100644
--- a/engineer_middleware/include/engineer_middleware/auto_exchange.h
+++ b/engineer_middleware/include/engineer_middleware/auto_exchange.h
@@ -783,7 +783,7 @@ class UnionMove : public ProgressBase
   }
   void initComputerValue()
   {
-    for (double & servo_scale : servo_scales_)
+    for (double& servo_scale : servo_scales_)
     {
       servo_scale = 0.;
     }
diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h
index 24fdc4e..60fae69 100644
--- a/engineer_middleware/include/engineer_middleware/step.h
+++ b/engineer_middleware/include/engineer_middleware/step.h
@@ -50,7 +50,8 @@ class Step
        moveit::planning_interface::MoveGroupInterface& arm_group, ChassisInterface& chassis_interface,
        ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub,
        ros::Publisher& reversal_pub, ros::Publisher& stone_num_pub, ros::Publisher& planning_result_pub,
-       ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub)
+       ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub,
+       ros::Publisher& gimbal_lift_pub)
     : planning_result_pub_(planning_result_pub), point_cloud_pub_(point_cloud_pub), arm_group_(arm_group)
   {
     ROS_ASSERT(step.hasMember("step"));
@@ -194,7 +195,7 @@ class Step
   MoveitMotionBase* arm_motion_{};
   HandMotion* hand_motion_{};
   JointPositionMotion* end_effector_motion_{};
-  JointPointMotion* ore_rotate_motion_{},* ore_lift_motion_{},* gimbal_lift_motion_{};
+  JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{};
   StoneNumMotion* stone_num_motion_{};
   ChassisMotion* chassis_motion_{};
   GimbalMotion* gimbal_motion_{};
diff --git a/engineer_middleware/src/middleware.cpp b/engineer_middleware/src/middleware.cpp
index f57dee3..701ade3 100644
--- a/engineer_middleware/src/middleware.cpp
+++ b/engineer_middleware/src/middleware.cpp
@@ -72,7 +72,8 @@ Middleware::Middleware(ros::NodeHandle& nh)
       step_queues_.insert(
           std::make_pair(it->first, StepQueue(it->second, scenes_list, tf_, arm_group_, chassis_interface_, hand_pub_,
                                               end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, stone_num_pub_,
-                                              planning_result_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub)));
+                                              planning_result_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_,
+                                              gimbal_lift_pub)));
     }
   }
   else

From d36e0c369a34444fe3cc6e9af0dfed8d9204cf4c Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 26 Mar 2024 19:57:29 +0800
Subject: [PATCH 047/147] Record test steps_list.

---
 engineer_middleware/config/steps_list.yaml | 34 ++++++++++++++++------
 1 file changed, 25 insertions(+), 9 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 60b1cdd..06b24b4 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -184,9 +184,9 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_GRIPPER
+#    - step: "open gripper"
+#      gripper:
+#        <<: *OPEN_GRIPPER
     - step: "ready to get ore"
       arm:
         joints: [ *JOINT1_ONE_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_R90_POSITION]
@@ -298,7 +298,7 @@ steps_list:
         spacial_shape: SPHERE
         radius: 0.5
         point_resolution: 0.5
-        max_planning_times: 100
+        max_planning_times: 1
         common:
           <<: *QUICKLY
 
@@ -313,11 +313,28 @@ steps_list:
         tolerance_position: 0.01
         tolerance_orientation: 0.001
         spacial_shape: SPHERE
-        radius: 0.5
+        radius: 0.3
         point_resolution: 0.5
-        max_planning_times: 100
+        max_planning_times: 1
         common:
           <<: *NORMALLY
+
+  TEST_ORI:
+    - step: "ori test"
+      arm:
+        frame: exchanger
+        rpy: [ 0., -1.5707963, 0. ]
+        tolerance_orientation: 0.001
+
+  TEST_ORI2:
+    - step: "ori test2"
+      arm:
+        frame: exchanger
+        rpy: [ 0., -1.5707963, 0. ]
+        xyz: [ -0.3, 0.2, -0.2 ]
+        tolerance_position: 0.01
+        tolerance_orientation: 0.001
+
   EXCHANGE_ORI:
     - step: "test new exchange"
       arm:
@@ -330,7 +347,7 @@ steps_list:
         spacial_shape: SPHERE
         radius: 0.5
         point_resolution: 0.5
-        max_planning_times: 100
+        max_planning_times: 1
         common:
           <<: *NORMALLY
   EXCHANGE_STRAIGHT:
@@ -346,7 +363,7 @@ steps_list:
         spacial_shape: SPHERE
         radius: 0.5
         point_resolution: 0.5
-        max_planning_times: 100
+        max_planning_times: 1
         common:
           <<: *NORMALLY
 
@@ -379,4 +396,3 @@ steps_list:
 #        max_planning_times: 50
 #        common:
 #          <<: *NORMALLY
-

From 198da3429022a69463ccdf03116cc4feb22bad6f Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Tue, 26 Mar 2024 21:09:02 +0800
Subject: [PATCH 048/147] Update with tested.

---
 engineer_middleware/config/steps_list.yaml | 44 +++++++++-------------
 1 file changed, 18 insertions(+), 26 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 91d8342..26d4695 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -15,7 +15,7 @@ common:
 
   tolerance:
     small_tolerance: &SMALL_TOLERANCE
-      tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1 ]
+      tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.05 ]
     normal_tolerance: &NORMAL_TOLERANCE
       tolerance_joints: [ 0.01, 0.010, 0.015, 0.3, 0.2, 0.15, 0.1 ]
     bigger_tolerance: &BIGGER_TOLERANCE
@@ -28,7 +28,7 @@ common:
       down_position: &JOINT1_DOWN_POSITION
                        0.015
       ready_position: &JOINT1_READY_POSITION
-                      0.165
+                      0.15
       up_position: &JOINT1_UP_POSITION
                    0.312
       ready_store: &JOINT1_READY_STORE_POSITION
@@ -87,7 +87,7 @@ common:
       mid_position: &JOINT4_MID_POSITION
                       0.001
       right_position: &JOINT4_R_POSITION
-                        -1.45
+                        -1.49
       ready_second_position: &JOINT4_READY_SECOND_POSITION
                         -0.777
   joint5:
@@ -115,7 +115,7 @@ common:
       left_90_position: &JOINT7_L90_POSITION
                           -1.59
       right_90_position: &JOINT7_R90_POSITION
-                           1.49
+                           1.57
   gimbal:
     side_pos: &SIDE_POS
       frame: gimbal_lifter
@@ -456,31 +456,23 @@ steps_list:
       arm:
         joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE_STONE"
       arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
-  ONE_STONE_SMALL_ISLAND00:
+          <<: *SMALL_TOLERANCE
     - step: "STORE_STONE"
       arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "JOINT1_DOWN"
-      arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *SMALL_TOLERANCE
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -526,7 +518,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE_STONE"
@@ -535,14 +527,14 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *SMALL_TOLERANCE
     - step: "STORE_STONE"
       arm:
         joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *SMALL_TOLERANCE
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -561,7 +553,7 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *SMALL_TOLERANCE
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
@@ -572,7 +564,7 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *SMALL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
         joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"]
@@ -593,14 +585,14 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *SMALL_TOLERANCE
     - step: "STORE_STONE"
       arm:
         joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *SMALL_TOLERANCE
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER

From b2c44501bac3212a7dfc0dbfb40cffef3d4f8913 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 26 Mar 2024 22:09:04 +0800
Subject: [PATCH 049/147] Fix wrong member name.

---
 engineer_middleware/include/engineer_middleware/step.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h
index 60fae69..e04ff2a 100644
--- a/engineer_middleware/include/engineer_middleware/step.h
+++ b/engineer_middleware/include/engineer_middleware/step.h
@@ -85,7 +85,7 @@ class Step
         if (step["scene_name"] == it->first)
           planning_scene_ = new PlanningScene(it->second, arm_group);
     }
-    if (step.hasMember("ore_rotater"))
+    if (step.hasMember("ore_rotator"))
       ore_rotate_motion_ = new JointPointMotion(step["ore_rotator"], ore_rotate_pub);
     if (step.hasMember("ore_lifter"))
       ore_lift_motion_ = new JointPointMotion(step["ore_lifter"], ore_lift_pub);

From aeffc9fa794855fbee3471591599bf48934fd299 Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Tue, 26 Mar 2024 22:54:27 +0800
Subject: [PATCH 050/147] Update ore_lifter and ore_rotator.

---
 engineer_middleware/config/steps_list.yaml | 51 ++++++++++++++++++++++
 1 file changed, 51 insertions(+)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 26d4695..850daae 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -199,6 +199,18 @@ common:
       common:
         timeout: 2.
 
+  ore_rotator:
+    ready_pos: &READY_POS
+            0.001
+    exchange_pos: &EXCHANGE_POS
+            1.59
+
+  ore_lifter:
+    bin_mid_pos: &BIN_MID_POS
+            0.1
+    bin_down_pos: &BIN_DOWN_POS
+            0.001
+
 steps_list:
   ###################### SMALL_ISLAND #######################
   SMALL_ISLAND_DEMO:
@@ -427,6 +439,12 @@ steps_list:
     - step: "GIMBAL_READY"
       gimbal:
         <<: *SIDE_POS
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_MID_POS
     - step: "SI_ARM_READY"
       arm:
         joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
@@ -467,6 +485,13 @@ steps_list:
         tolerance:
           <<: *SMALL_TOLERANCE
     - step: "STORE_STONE"
+      arm:
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *SMALL_TOLERANCE
+    - step: "JOINT5"
       arm:
         joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
@@ -483,12 +508,21 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "ORE_LIFTER_DOWN"
+      ore_lifter:
+        target: *BIN_DOWN_POS
 
   ####FIRST_STONE###
   TWO_STONE_SMALL_ISLAND:
     - step: "GIMBAL_READY"
       gimbal:
         <<: *SIDE_POS
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_MID_POS
     - step: "SI_ARM_READY"
       arm:
         joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
@@ -529,6 +563,13 @@ steps_list:
         tolerance:
           <<: *SMALL_TOLERANCE
     - step: "STORE_STONE"
+      arm:
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *SMALL_TOLERANCE
+    - step: "JOINT5"
       arm:
         joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
@@ -545,6 +586,9 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "ORE_LIFTER_DOWN"
+      ore_lifter:
+        target: *BIN_DOWN_POS
   ###SECOND_STONE###
   TWO_STONE_SMALL_ISLAND00:
     - step: "SI_ARM_READY"
@@ -587,6 +631,13 @@ steps_list:
         tolerance:
           <<: *SMALL_TOLERANCE
     - step: "STORE_STONE"
+      arm:
+        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *SMALL_TOLERANCE
+    - step: "JOINT5"
       arm:
         joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:

From 460d02020c6c0eb82d9d75fd256deebadf8577df Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Tue, 26 Mar 2024 22:54:37 +0800
Subject: [PATCH 051/147] Update.

---
 engineer_arm_config/config/engineer.srdf               | 2 ++
 engineer_middleware/include/engineer_middleware/step.h | 2 +-
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/engineer_arm_config/config/engineer.srdf b/engineer_arm_config/config/engineer.srdf
index b4dde6c..dd6c64d 100644
--- a/engineer_arm_config/config/engineer.srdf
+++ b/engineer_arm_config/config/engineer.srdf
@@ -50,4 +50,6 @@
         <disable_collisions link1="link6" link2="link7" reason="Never"/>
         <disable_collisions link1="link5" link2="link7" reason="Never"/>
         <disable_collisions link1="link1" link2="camera_link" reason="Never"/>
+        <disable_collisions link1="link7" link2="ore_bin_lifter" reason="Never"/>
+
 </robot>
diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h
index 24fdc4e..f02b4f3 100644
--- a/engineer_middleware/include/engineer_middleware/step.h
+++ b/engineer_middleware/include/engineer_middleware/step.h
@@ -84,7 +84,7 @@ class Step
         if (step["scene_name"] == it->first)
           planning_scene_ = new PlanningScene(it->second, arm_group);
     }
-    if (step.hasMember("ore_rotater"))
+    if (step.hasMember("ore_rotator"))
       ore_rotate_motion_ = new JointPointMotion(step["ore_rotator"], ore_rotate_pub);
     if (step.hasMember("ore_lifter"))
       ore_lift_motion_ = new JointPointMotion(step["ore_lifter"], ore_lift_pub);

From 66be75e03a738f3196e886a7d906cb5f2adc901d Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Wed, 27 Mar 2024 20:51:21 +0800
Subject: [PATCH 052/147] Add get stone from ore bin.

---
 engineer_middleware/config/steps_list.yaml | 89 +++++++++++++++++++++-
 1 file changed, 88 insertions(+), 1 deletion(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 850daae..b657f66 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -33,6 +33,8 @@ common:
                    0.312
       ready_store: &JOINT1_READY_STORE_POSITION
                    0.162
+      bin_get_stone: &JOINT1_BIN_GET_STONE
+                     0.142
     home:
       zero_stone_position: &JOINT1_ZERO_STONE_POSITION
                              0.08
@@ -59,6 +61,8 @@ common:
                               0.02
       ready_second_position: &JOINT2_READY_SECOND_POSITION
                               0.142
+      bin_get_stone: &JOINT2_BIN_GET_STONE
+                            0.087
 
   joint3:
     mechanical:
@@ -78,7 +82,8 @@ common:
                             -0.018
       store_stone_position: &JOINT3_STORE_STONE_POSITION
                               0.30
-
+      bin_get_stone: &JOINT3_BIN_GET_STONE
+                          0.258
 
   joint4:
     mechanical:
@@ -98,6 +103,9 @@ common:
                           -1.54
       right_90_position: &JOINT5_R90_POSITION
                            1.59
+      bin_get_stone: &JOINT5_BIN_GET_STONE
+                           1.679
+
   joint6:
     mechanical:
       mid_position: &JOINT6_MID_POSITION
@@ -108,6 +116,8 @@ common:
                        1.569
       ready_second_position: &JOINT6_READY_SECOND_POSITION
                        0.694
+      bin_get_stone: &JOINT6_BIN_GET_STONE
+                      -1.611
   joint7:
     mechanical:
       mid_position: &JOINT7_MID_POSITION
@@ -206,6 +216,8 @@ common:
             1.59
 
   ore_lifter:
+    bin_up_pos: &BIN_UP_POS
+            0.2
     bin_mid_pos: &BIN_MID_POS
             0.1
     bin_down_pos: &BIN_DOWN_POS
@@ -654,6 +666,81 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+
+  DOWN_STONE_FROM_BIN:
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *EXCHANGE_POS
+    - step: "READY_TO_GET"
+      arm:
+        joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "ORE_LIFTER_UP"
+      ore_lifter:
+        target: *BIN_UP_POS
+    - step: "JOINT2_BACK_GET_STONE"
+      arm:
+        joints: [ "KEEP", *JOINT2_BACK_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT5_LIFT_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  UP_STONE_FROM_BIN:
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *EXCHANGE_POS
+    - step: "READY_TO_GET"
+      arm:
+        joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "JOINT2_BACK_GET_STONE"
+      arm:
+        joints: [ "KEEP", *JOINT2_BACK_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT5_LIFT_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
   #################BIG_ISLAND#############
 #  ONE_STONE_BIG_ISLAND：
 #  - gimbal:

From d19cf68faedfec8f06fac59b4d32f15d27e41a30 Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Thu, 28 Mar 2024 00:28:03 +0800
Subject: [PATCH 053/147] Update with tested.

---
 engineer_middleware/config/steps_list.yaml | 16 +++++++++-------
 1 file changed, 9 insertions(+), 7 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index b657f66..4e2bf1a 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -63,6 +63,8 @@ common:
                               0.142
       bin_get_stone: &JOINT2_BIN_GET_STONE
                             0.087
+      back_bin_position: &JOINT2_BACK_BIN_POSITION
+                             0.001
 
   joint3:
     mechanical:
@@ -671,6 +673,9 @@ steps_list:
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *EXCHANGE_POS
+    - step: "ORE_LIFTER_UP"
+      ore_lifter:
+        target: *BIN_UP_POS
     - step: "READY_TO_GET"
       arm:
         joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ]
@@ -681,14 +686,11 @@ steps_list:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
-    - step: "ORE_LIFTER_UP"
-      ore_lifter:
-        target: *BIN_UP_POS
     - step: "JOINT2_BACK_GET_STONE"
       arm:
-        joints: [ "KEEP", *JOINT2_BACK_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
@@ -722,9 +724,9 @@ steps_list:
         <<: *OPEN_GRIPPER
     - step: "JOINT2_BACK_GET_STONE"
       arm:
-        joints: [ "KEEP", *JOINT2_BACK_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"

From 708299b14a944fef37d10c9990e5288e3da8f660 Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Mon, 1 Apr 2024 21:51:55 +0800
Subject: [PATCH 054/147] Add big island.

---
 engineer_middleware/config/steps_list.yaml | 138 +++++++++++++--------
 1 file changed, 84 insertions(+), 54 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 4e2bf1a..f0a53b9 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -35,6 +35,8 @@ common:
                    0.162
       bin_get_stone: &JOINT1_BIN_GET_STONE
                      0.142
+      big_ready: &JOINT1_BIG_READY
+                     0.006
     home:
       zero_stone_position: &JOINT1_ZERO_STONE_POSITION
                              0.08
@@ -65,6 +67,8 @@ common:
                             0.087
       back_bin_position: &JOINT2_BACK_BIN_POSITION
                              0.001
+      big_ready: &JOINT2_BIG_READY
+                       0.009
 
   joint3:
     mechanical:
@@ -86,6 +90,8 @@ common:
                               0.30
       bin_get_stone: &JOINT3_BIN_GET_STONE
                           0.258
+      big_ready: &JOINT3_BIG_READY
+                     0.071
 
   joint4:
     mechanical:
@@ -107,6 +113,8 @@ common:
                            1.59
       bin_get_stone: &JOINT5_BIN_GET_STONE
                            1.679
+      big_ready: &JOINT5_BIG_READY
+                  -0.017
 
   joint6:
     mechanical:
@@ -120,6 +128,9 @@ common:
                        0.694
       bin_get_stone: &JOINT6_BIN_GET_STONE
                       -1.611
+      big_ready: &JOINT6_BIG_READY
+                       0.014
+
   joint7:
     mechanical:
       mid_position: &JOINT7_MID_POSITION
@@ -128,6 +139,9 @@ common:
                           -1.59
       right_90_position: &JOINT7_R90_POSITION
                            1.57
+      big_ready: &JOINT7_BIG_READY
+                         -0.013
+
   gimbal:
     side_pos: &SIDE_POS
       frame: gimbal_lifter
@@ -744,57 +758,73 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
   #################BIG_ISLAND#############
-#  ONE_STONE_BIG_ISLAND：
-#  - gimbal:
-#      <<: *NORMALLY
-#  - step: "BI_ARM_READY"
-#    arm:
-#      joints: [ ]
-#      common:
-#        <<: *NORMALLY
-#      tolerance:
-#        <<: *NORMAL_TOLERANCE
-#  - step: "OPEN_GRIPPER"
-#    gripper:
-#      <<: *OPEN_GRIPPER
-#  - step: "BI_GET_STONE"
-#    arm:
-#      joints: [ ]
-#      common:
-#        <<: *NORMALLY
-#      tolerance:
-#        <<: *NORMAL_TOLERANCE
-#  - step: "PULL_OUT_STONE"
-#    arm:
-#      joints: [ ]
-#      common:
-#        <<: *NORMALLY
-#      tolerance:
-#        <<: *NORMAL_TOLERANCE
-#  - step: "BI_ARM_UP_STONE"         #########joint6抬起，无需转动joint7#######
-#    arm:
-#      joints: [ ]
-#      common:
-#        <<: *NORMALLY
-#      tolerance:
-#        <<: *NORMAL_TOLERANCE
-#  - step: "ORE_LIFTER_UP"
-#    ore_lifter:
-#
-#  - step: "STORE_STONE"
-#    arm:
-#      joints: [ ]
-#      common:
-#        <<: *NORMALLY
-#      tolerance:
-#        <<: *NORMAL_TOLERANCE
-#  - step: "CLOSE_GRIPPER"
-#    gripper:
-#      <<: *CLOSE_GRIPPER
-#  - step: "ARM_RESET"
-#    arm:
-#      joints: [ ]
-#      common:
-#        <<: *NORMALLY
-#      tolerance:
-#        <<: *NORMAL_TOLERANCE
\ No newline at end of file
+  MID_BIG_ISLAND:
+    - step: "MID_ARM_READY"
+      arm:
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_READY ]
+        common:
+         <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "GET_MID_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", -0.281, "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_MICRO_UP"
+      arm:
+        joints: [ 0.071, "KEEP", "KEEP", "KEEP", "KEEP", -0.119, "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "PULL_OUT_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", 0.188, "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ 0.320, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ARM_UP_STONE"
+      arm:
+        joints: [ "KEEP", 0.142, 0.289, "KEEP", "KEEP", -1.519, "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_TO_STORE"
+      arm:
+        joints: [ 0.155, 0.030, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", 1.617, "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "ARM_RESET"
+      arm:
+        joints: [ 0.006, 0.009, 0.071, -1.495, -0.017, 0.014, -0.013 ]
+      common:
+        <<: *SLOWLY
+      tolerance:
+        <<: *NORMAL_TOLERANCE
\ No newline at end of file

From a401cf01c2d9b36f17b139791f77bb72d7ea260b Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Mon, 1 Apr 2024 22:48:32 +0800
Subject: [PATCH 055/147] Add and modify parameters.

---
 engineer_middleware/config/steps_list.yaml | 162 ++++++++++++---------
 1 file changed, 97 insertions(+), 65 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index f0a53b9..2e84ba0 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -27,16 +27,22 @@ common:
     mechanical:
       down_position: &JOINT1_DOWN_POSITION
                        0.015
-      ready_position: &JOINT1_READY_POSITION
+      small_ready: &JOINT1_SMALL_READY
                       0.15
-      up_position: &JOINT1_UP_POSITION
+      up_pos: &JOINT1_UP_POS
                    0.312
-      ready_store: &JOINT1_READY_STORE_POSITION
+      small_ready_store: &JOINT1_SMALL_READY_STORE
                    0.162
       bin_get_stone: &JOINT1_BIN_GET_STONE
                      0.142
       big_ready: &JOINT1_BIG_READY
                      0.006
+      big_adjust_mid: &JOINT1_BIG_ADJUST_MID
+                      0.071
+      big_up_pos: &JOINT1_BIG_UP_POS
+                      0.320
+      big_ready_store: &JOINT1_BIG_READY_STORE
+                      0.155
     home:
       zero_stone_position: &JOINT1_ZERO_STONE_POSITION
                              0.08
@@ -57,11 +63,11 @@ common:
                            0.232
       exchange_position: &JOINT2_EXCHANGE_POSITION
                           0.07
-      lift_stone_position: &JOINT2_LIFT_STONE_POSITION
+      small_arm_up: &JOINT2_SMALL_ARM_UP
                             0.085
-      store_stone_position: &JOINT2_STORE_STONE_POSITION
+      small_store_stone: &JOINT2_SMALL_STORE_STONE
                               0.02
-      ready_second_position: &JOINT2_READY_SECOND_POSITION
+      small_ready_second: &JOINT2_SMALL_READY_SECOND
                               0.142
       bin_get_stone: &JOINT2_BIN_GET_STONE
                             0.087
@@ -69,6 +75,10 @@ common:
                              0.001
       big_ready: &JOINT2_BIG_READY
                        0.009
+      big_arm_up: &JOINT2_BIG_ARM_UP
+                    0.142
+      big_ready_store: &JOINT2_BIG_READY_STORE
+                      0.030
 
   joint3:
     mechanical:
@@ -78,20 +88,26 @@ common:
                       0.0001
       right_position: &JOINT3_R_POSITION
                         -0.275
-      ready_position: &JOINT3_READY_POSITION
+      small_ready: &JOINT3_SMALL_READY
                         0.098
-      ready_second_position: &JOINT3_READY_SECOND_POSITION
+      small_ready_second: &JOINT3_SMALL_READY_SECOND
                         0.03
-      get_stone_position: &JOINT3_GET_STONE_POSITION
+      small_get_stone: &JOINT3_SMALL_GET_STONE
                             0.034
-      get_second_position: &JOINT3_GET_SECOND_POSITION
+      small_get_second: &JOINT3_SMALL_GET_SECOND
                             -0.018
-      store_stone_position: &JOINT3_STORE_STONE_POSITION
+      small_store_stone: &JOINT3_SMALL_STORE_STONE
                               0.30
       bin_get_stone: &JOINT3_BIN_GET_STONE
                           0.258
       big_ready: &JOINT3_BIG_READY
                      0.071
+      get_big_mid: &JOINT3_GET_BIG_MID
+                      -0.281
+      big_pull_out: &JOINT3_BIG_PULL_OUT
+                     0.188
+      big_arm_up: &JOINT3_BIG_ARM_UP
+                    0.289
 
   joint4:
     mechanical:
@@ -100,8 +116,8 @@ common:
       mid_position: &JOINT4_MID_POSITION
                       0.001
       right_position: &JOINT4_R_POSITION
-                        -1.49
-      ready_second_position: &JOINT4_READY_SECOND_POSITION
+                        -1.495
+      small_ready_second: &JOINT4_SMALL_READY_SECOND
                         -0.777
   joint5:
     mechanical:
@@ -115,6 +131,8 @@ common:
                            1.679
       big_ready: &JOINT5_BIG_READY
                   -0.017
+      big_store: &JOINT5_BIG_STORE
+                  1.617
 
   joint6:
     mechanical:
@@ -124,12 +142,16 @@ common:
                      -1.569
       down_position: &JOINT6_DOWN_POSITION
                        1.569
-      ready_second_position: &JOINT6_READY_SECOND_POSITION
+      small_ready_second: &JOINT6_SMALL_READY_SECOND
                        0.694
       bin_get_stone: &JOINT6_BIN_GET_STONE
                       -1.611
       big_ready: &JOINT6_BIG_READY
                        0.014
+      big_adjust_mid: &JOINT6_BIG_ADJUST_MID
+                       -0.119
+      big_arm_up: &JOINT6_BIG_ARM_UP
+                       -1.519
 
   joint7:
     mechanical:
@@ -139,7 +161,7 @@ common:
                           -1.59
       right_90_position: &JOINT7_R90_POSITION
                            1.57
-      big_ready: &JOINT7_BIG_READY
+      big_pos: &JOINT7_BIG_POS
                          -0.013
 
   gimbal:
@@ -473,9 +495,9 @@ steps_list:
     - step: "ORE_LIFTER_READY"
       ore_lifter:
         target: *BIN_MID_POS
-    - step: "SI_ARM_READY"
+    - step: "SMALL_ARM_READY"
       arm:
-        joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -484,44 +506,44 @@ steps_list:
       gripper:
         <<: *OPEN_GRIPPER
   ONE_STONE_SMALL_ISLAND0:
-    - step: "SI_GET_STONE"
+    - step: "SMALL_GET_STONE"
       arm:
-        joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"]
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "SI_ARM_LIFT_STONE"
+    - step: "SMALL_ARM_UP_STONE"
       arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE_STONE"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *SMALL_TOLERANCE
     - step: "STORE_STONE"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *SMALL_TOLERANCE
-    - step: "JOINT5"
+    - step: "JOINT5_TURN"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -531,7 +553,7 @@ steps_list:
         <<: *CLOSE_GRIPPER
     - step: "ARM_RESET"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -551,9 +573,9 @@ steps_list:
     - step: "ORE_LIFTER_READY"
       ore_lifter:
         target: *BIN_MID_POS
-    - step: "SI_ARM_READY"
+    - step: "SMALL_ARM_READY"
       arm:
-        joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_READY_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -562,44 +584,44 @@ steps_list:
       gripper:
         <<: *OPEN_GRIPPER
   TWO_STONE_SMALL_ISLAND0:
-    - step: "SI_GET_STONE"
+    - step: "SMALL_GET_STONE"
       arm:
-        joints: [ *JOINT1_READY_POSITION, *JOINT2_BACK_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"]
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "SI_ARM_LIFT_STONE"
+    - step: "SMALL_ARM_UP_STONE"
       arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE_STONE"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *SMALL_TOLERANCE
     - step: "STORE_STONE"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *SMALL_TOLERANCE
-    - step: "JOINT5"
+    - step: "JOINT5_TURN"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -609,7 +631,7 @@ steps_list:
         <<: *CLOSE_GRIPPER
     - step: "ARM_RESET"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -619,9 +641,9 @@ steps_list:
         target: *BIN_DOWN_POS
   ###SECOND_STONE###
   TWO_STONE_SMALL_ISLAND00:
-    - step: "SI_ARM_READY"
+    - step: "SMALL_ARM_READY_TWO"
       arm:
-        joints: [ *JOINT1_READY_POSITION, *JOINT2_READY_SECOND_POSITION, *JOINT3_READY_SECOND_POSITION, *JOINT4_READY_SECOND_POSITION, *JOINT5_L90_POSITION, *JOINT6_READY_SECOND_POSITION, *JOINT7_R90_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY_SECOND, *JOINT3_SMALL_READY_SECOND, *JOINT4_SMALL_READY_SECOND, *JOINT5_L90_POSITION, *JOINT6_SMALL_READY_SECOND, *JOINT7_R90_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -630,44 +652,44 @@ steps_list:
       gripper:
         <<: *OPEN_GRIPPER
   TWO_STONE_SMALL_ISLAND000:
-    - step: "SI_GET_STONE"
+    - step: "SMALL_GET_STONE_TWO"
       arm:
-        joints: [ *JOINT1_READY_POSITION, *JOINT2_READY_SECOND_POSITION, *JOINT3_GET_SECOND_POSITION, *JOINT4_READY_SECOND_POSITION, *JOINT5_L90_POSITION, *JOINT6_READY_SECOND_POSITION, *JOINT7_R90_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY_SECOND, *JOINT3_SMALL_GET_SECOND, *JOINT4_SMALL_READY_SECOND, *JOINT5_L90_POSITION, *JOINT6_SMALL_READY_SECOND, *JOINT7_R90_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *SMALL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"]
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "SI_ARM_LIFT_STONE"
+    - step: "SMALL_ARM_LIFT_STONE"
       arm:
-        joints: [ *JOINT1_UP_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_GET_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE_STONE"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_LIFT_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *SMALL_TOLERANCE
     - step: "STORE_STONE"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *SMALL_TOLERANCE
     - step: "JOINT5"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_STORE_STONE_POSITION, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -677,13 +699,13 @@ steps_list:
         <<: *CLOSE_GRIPPER
     - step: "ARM_RESET"
       arm:
-        joints: [ *JOINT1_READY_STORE_POSITION, *JOINT2_STORE_STONE_POSITION, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-  DOWN_STONE_FROM_BIN:
+  GET_DOWN_STONE_BIN:
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *EXCHANGE_POS
@@ -709,7 +731,7 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -722,7 +744,7 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-  UP_STONE_FROM_BIN:
+  GET_UP_STONE_BIN:
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *EXCHANGE_POS
@@ -745,7 +767,7 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_UP_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -757,63 +779,70 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+
   #################BIG_ISLAND#############
   MID_BIG_ISLAND:
     - step: "MID_ARM_READY"
       arm:
-        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_READY ]
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
         common:
          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_MID_POS
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
     - step: "GET_MID_STONE"
       arm:
-        joints: [ "KEEP", "KEEP", -0.281, "KEEP", "KEEP", "KEEP", "KEEP" ]
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "JOINT1_MICRO_UP"
+    - step: "ADJUST_STONE"
       arm:
-        joints: [ 0.071, "KEEP", "KEEP", "KEEP", "KEEP", -0.119, "KEEP" ]
+        joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "PULL_OUT_STONE"
       arm:
-        joints: [ "KEEP", "KEEP", 0.188, "KEEP", "KEEP", "KEEP", "KEEP" ]
+        joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
-        joints: [ 0.320, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ARM_UP_STONE"
       arm:
-        joints: [ "KEEP", 0.142, 0.289, "KEEP", "KEEP", -1.519, "KEEP" ]
+        joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE"
       arm:
-        joints: [ 0.155, 0.030, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE_STONE"
       arm:
-        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", 1.617, "KEEP", "KEEP" ]
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -823,8 +852,11 @@ steps_list:
         <<: *CLOSE_GRIPPER
     - step: "ARM_RESET"
       arm:
-        joints: [ 0.006, 0.009, 0.071, -1.495, -0.017, 0.014, -0.013 ]
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
       common:
         <<: *SLOWLY
       tolerance:
-        <<: *NORMAL_TOLERANCE
\ No newline at end of file
+        <<: *NORMAL_TOLERANCE
+    - step: "ORE_LIFTER_DOWN"
+      ore_lifter:
+        target: *BIN_DOWN_POS
\ No newline at end of file

From 2fd93e26b66f179a7e07064ad114c06656fe7499 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 2 Apr 2024 20:26:53 +0800
Subject: [PATCH 056/147] disable collision check between joint7 and ore bin
 rotator.

---
 engineer_arm_config/config/engineer.srdf | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/engineer_arm_config/config/engineer.srdf b/engineer_arm_config/config/engineer.srdf
index dd6c64d..604c9d1 100644
--- a/engineer_arm_config/config/engineer.srdf
+++ b/engineer_arm_config/config/engineer.srdf
@@ -51,5 +51,5 @@
         <disable_collisions link1="link5" link2="link7" reason="Never"/>
         <disable_collisions link1="link1" link2="camera_link" reason="Never"/>
         <disable_collisions link1="link7" link2="ore_bin_lifter" reason="Never"/>
-
+        <disable_collisions link1="link7" link2="ore_bin_rotator" reason="Never"/>
 </robot>

From 12c6436f2475a34ca8c6682b61a0ce5d82c81f46 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 2 Apr 2024 20:39:28 +0800
Subject: [PATCH 057/147] optimize step lists.

---
 engineer_middleware/config/steps_list.yaml | 270 ++++++++++-----------
 1 file changed, 133 insertions(+), 137 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index bf0fc16..525c465 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -167,13 +167,10 @@ common:
   gimbal:
     side_pos: &SIDE_POS
       frame: gimbal_lifter
-      position: [ 0.03 ,3, -0.02 ]
-    island_pos: &ISLAND_POS
+      position: [ 0.001 , -3, 0. ]
+    front_pos: &FRONT_POS
       frame: gimbal_lifter
-      position: [ 2, 0.001, 0.001 ]
-    ground_pos: &GROUND_POS
-      frame: gimbal_lifter
-      position: [ 2, 0.001, -0.8 ]
+      position: [ 2, 0.001, 0. ]
 
   gimbal_lifter:
     button_pos: &BUTTON_POS
@@ -200,35 +197,6 @@ common:
         <<: *NORMAL_TOLERANCE
 
 
-  reversal:
-    velocity_stop: &VELOCITY_STOP
-      mode: "VELOCITY"
-      values: [ 0.,0.,0.,0.,0.,0. ]
-    velocity_z_out: &VELOCITY_Z_OUT
-      mode: "VELOCITY"
-      values: [ 0.,0.,3.,0.,0.,0. ]
-    velocity_z_out_store: &VELOCITY_Z_OUT_STORE
-      mode: "VELOCITY"
-      values: [ 0.,0.,0.2,0.,0.,0. ]
-    velocity_z_in: &VELOCITY_Z_IN
-      mode: "VELOCITY"
-      values: [ 0.,0.,-1.2,0.,-0.01,0. ]
-    velocity_z_in_quickly: &VELOCITY_Z_IN_QUICKLY
-      mode: "VELOCITY"
-      values: [ 0.,0.,-3.0,0.,-0.01,0. ]
-    velocity_z_in_quickly_with_delay: &VELOCITY_Z_IN_QUICKLY_WITH_DELAY
-      mode: "VELOCITY"
-      values: [ 0.,0.,-3.0,0.,-0.01,0. ]
-      delay: 0.3
-    position_z_out: &POSITION_Z_OUT
-      mode: "POSITION"
-      values: [ 0.,0.,7.,0.,0.,0. ]
-      delay: 0.2
-    position_z_out_little: &POSITION_Z_OUT_LITTLE
-      mode: "POSITION"
-      values: [ 0.,0.,3.,0.,0.,0. ]
-      delay: 0.4
-
   chassis_move:
     chassis_backward_30: &CHASSIS_BACKWARD_25
       frame: base_link
@@ -265,7 +233,7 @@ steps_list:
   EXCHANGE_POS:
     - step: "gimbal look front"
       gimbal:
-        <<: *ISLAND_POS
+        <<: *FRONT_POS
     - step: "move arm"
       arm:
         joints: [*JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
@@ -277,7 +245,7 @@ steps_list:
   GIMBAL_FRONT:
     - step: "gimbal look front"
       gimbal:
-        <<: *ISLAND_POS
+        <<: *FRONT_POS
   GIMBAL_LEFT:
     - step: "gimbal look left"
       gimbal:
@@ -288,7 +256,7 @@ steps_list:
   HOME_ZERO_STONE:
     - step: "gimbal look side"
       gimbal:
-        <<: *ISLAND_POS
+        <<: *FRONT_POS
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -299,7 +267,7 @@ steps_list:
   HOME_ONE_STONE:
     - step: "gimbal look side"
       gimbal:
-        <<: *ISLAND_POS
+        <<: *FRONT_POS
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -310,7 +278,7 @@ steps_list:
   HOME_TWO_STONE:
     - step: "gimbal look side"
       gimbal:
-        <<: *ISLAND_POS
+        <<: *FRONT_POS
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -328,103 +296,7 @@ steps_list:
       gripper:
         <<: *CLOSE_GRIPPER
 
-  ########## EXCHANGE ############
-  OLD_EXCHANGE_SPHERE:
-    - step: "close to a sphere"
-      arm:
-        frame: exchanger
-        xyz: [ -0.4, 0., -0.25 ]
-        rpy: [ 0., 0., 0. ]
-        rpy_rectify: [ 0.01,0.1,0.03 ]
-        drift_dimensions: [ true, true, true, true, true, true ]
-        tolerance_position: 0.1
-        tolerance_orientation: 0.01
-        spacial_shape: SPHERE
-        radius: 0.5
-        point_resolution: 0.5
-        max_planning_times: 1
-        common:
-          <<: *QUICKLY
-
-  EXCHANGE_DOWN:
-    - step: "test new exchange"
-      arm:
-        frame: exchanger
-        is_refer_planning_frame: true
-        xyz: [ -0.3, 0.2, -0.2 ]
-        rpy: [ 0., -1.5707963, 0. ]
-        drift_dimensions: [ true, true, true, true, true, true ]
-        tolerance_position: 0.01
-        tolerance_orientation: 0.001
-        spacial_shape: SPHERE
-        radius: 0.3
-        point_resolution: 0.5
-        max_planning_times: 1
-        common:
-          <<: *NORMALLY
-
-  TEST_ORI:
-    - step: "ori test"
-      arm:
-        frame: exchanger
-        rpy: [ 0., -1.5707963, 0. ]
-        tolerance_orientation: 0.001
-
-  TEST_ORI2:
-    - step: "ori test2"
-      arm:
-        frame: exchanger
-        rpy: [ 0., -1.5707963, 0. ]
-        xyz: [ -0.3, 0.2, -0.2 ]
-        tolerance_position: 0.01
-        tolerance_orientation: 0.001
-
-  EXCHANGE_ORI:
-    - step: "test new exchange"
-      arm:
-        frame: exchanger
-        is_refer_planning_frame: true
-        rpy: [ 0., -1.5707963, 0. ]
-        drift_dimensions: [ true, true, true, true, true, true ]
-        tolerance_position: 0.01
-        tolerance_orientation: 0.001
-        spacial_shape: SPHERE
-        radius: 0.5
-        point_resolution: 0.5
-        max_planning_times: 1
-        common:
-          <<: *NORMALLY
-  EXCHANGE_STRAIGHT:
-    - step: "test new exchange"
-      arm:
-        frame: exchanger
-        is_refer_planning_frame: true
-        xyz: [ -0.3, 0., 0. ]
-        rpy: [ 0., 3.1415926, 0. ]
-        drift_dimensions: [ true, true, true, true, true, true ]
-        tolerance_position: 0.01
-        tolerance_orientation: 0.001
-        spacial_shape: SPHERE
-        radius: 0.5
-        point_resolution: 0.5
-        max_planning_times: 1
-        common:
-          <<: *NORMALLY
-
-  GTESTL:
-    - step: "test"
-      gimbal_lifter:
-        target: *BUTTON_POS
-  GTESTM:
-    - step: "test"
-      gimbal_lifter:
-        target: *MID_POS
-  GTESTH:
-    - step: "test"
-      gimbal_lifter:
-        target: *TALLEST_POS
-
-  ###################SMALL_ISLAND_FROM_TOP################
+  ################### SMALL_ISLAND_FROM_SIDE ################
   ONE_STONE_SMALL_ISLAND:
     - step: "GIMBAL_READY"
       gimbal:
@@ -501,6 +373,9 @@ steps_list:
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
+    - step: "add stone"
+      stone_num:
+        change: "+1"
 
   ####FIRST_STONE###
   TWO_STONE_SMALL_ISLAND:
@@ -579,6 +454,9 @@ steps_list:
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
+    - step: "add stone"
+      stone_num:
+        change: "+1"
   ###SECOND_STONE###
   TWO_STONE_SMALL_ISLAND00:
     - step: "SMALL_ARM_READY_TWO"
@@ -644,8 +522,14 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "add stone"
+      stone_num:
+        change: "+1"
 
   GET_DOWN_STONE_BIN:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *FRONT_POS
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *EXCHANGE_POS
@@ -683,8 +567,21 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "exchange pos"
+      arm:
+        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "remove stone"
+      stone_num:
+        change: "-1"
 
   GET_UP_STONE_BIN:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *FRONT_POS
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *EXCHANGE_POS
@@ -719,6 +616,16 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "exchange pos"
+      arm:
+        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "remove stone"
+      stone_num:
+        change: "-1"
 
   #################BIG_ISLAND#############
   MID_BIG_ISLAND:
@@ -800,3 +707,92 @@ steps_list:
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
+    - step: "add stone"
+      stone_num:
+        change: "+1"
+
+
+  ####################################################  TEST  ##########################################################
+
+  ########## EXCHANGE ############
+  OLD_EXCHANGE_SPHERE:
+    - step: "close to a sphere"
+      arm:
+        frame: exchanger
+        xyz: [ -0.4, 0., -0.25 ]
+        rpy: [ 0., 0., 0. ]
+        rpy_rectify: [ 0.01,0.1,0.03 ]
+        drift_dimensions: [ true, true, true, true, true, true ]
+        tolerance_position: 0.1
+        tolerance_orientation: 0.01
+        spacial_shape: SPHERE
+        radius: 0.5
+        point_resolution: 0.5
+        max_planning_times: 1
+        common:
+          <<: *QUICKLY
+
+  EXCHANGE_DOWN:
+    - step: "test new exchange"
+      arm:
+        frame: exchanger
+        is_refer_planning_frame: true
+        xyz: [ -0.3, 0.2, -0.2 ]
+        rpy: [ 0., -1.5707963, 0. ]
+        drift_dimensions: [ true, true, true, true, true, true ]
+        tolerance_position: 0.01
+        tolerance_orientation: 0.001
+        spacial_shape: SPHERE
+        radius: 0.3
+        point_resolution: 0.5
+        max_planning_times: 1
+        common:
+          <<: *NORMALLY
+
+  TEST_ORI:
+    - step: "ori test"
+      arm:
+        frame: exchanger
+        rpy: [ 0., -1.5707963, 0. ]
+        tolerance_orientation: 0.001
+
+  TEST_ORI2:
+    - step: "ori test2"
+      arm:
+        frame: exchanger
+        rpy: [ 0., -1.5707963, 0. ]
+        xyz: [ -0.3, 0.2, -0.2 ]
+        tolerance_position: 0.01
+        tolerance_orientation: 0.001
+
+  EXCHANGE_ORI:
+    - step: "test new exchange"
+      arm:
+        frame: exchanger
+        is_refer_planning_frame: true
+        rpy: [ 0., -1.5707963, 0. ]
+        drift_dimensions: [ true, true, true, true, true, true ]
+        tolerance_position: 0.01
+        tolerance_orientation: 0.001
+        spacial_shape: SPHERE
+        radius: 0.5
+        point_resolution: 0.5
+        max_planning_times: 1
+        common:
+          <<: *NORMALLY
+  EXCHANGE_STRAIGHT:
+    - step: "test new exchange"
+      arm:
+        frame: exchanger
+        is_refer_planning_frame: true
+        xyz: [ -0.3, 0., 0. ]
+        rpy: [ 0., 3.1415926, 0. ]
+        drift_dimensions: [ true, true, true, true, true, true ]
+        tolerance_position: 0.01
+        tolerance_orientation: 0.001
+        spacial_shape: SPHERE
+        radius: 0.5
+        point_resolution: 0.5
+        max_planning_times: 1
+        common:
+          <<: *NORMALLY

From fb6bc3bafa200a4dfa46410c72dfbe019e8289d2 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 8 Apr 2024 22:52:22 +0800
Subject: [PATCH 058/147] update steps_list.yaml.

---
 engineer_middleware/config/steps_list.yaml | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 525c465..75478c9 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -166,10 +166,10 @@ common:
 
   gimbal:
     side_pos: &SIDE_POS
-      frame: gimbal_lifter
+      frame: pitch
       position: [ 0.001 , -3, 0. ]
     front_pos: &FRONT_POS
-      frame: gimbal_lifter
+      frame: pitch
       position: [ 2, 0.001, 0. ]
 
   gimbal_lifter:
@@ -254,7 +254,7 @@ steps_list:
 
   ############################ HOME ##########################
   HOME_ZERO_STONE:
-    - step: "gimbal look side"
+    - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
     - step: "close gripper"
@@ -265,7 +265,7 @@ steps_list:
         <<: *HOME_1
         variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
   HOME_ONE_STONE:
-    - step: "gimbal look side"
+    - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
     - step: "close gripper"
@@ -276,7 +276,7 @@ steps_list:
         <<: *HOME_1
         variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ]
   HOME_TWO_STONE:
-    - step: "gimbal look side"
+    - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
     - step: "close gripper"
@@ -376,6 +376,9 @@ steps_list:
     - step: "add stone"
       stone_num:
         change: "+1"
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *SIDE_POS
 
   ####FIRST_STONE###
   TWO_STONE_SMALL_ISLAND:

From 2382cd4d33b96391d5b0f72c9b079ff5cab72b71 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 9 Apr 2024 23:54:21 +0800
Subject: [PATCH 059/147] Add support of extend arms in middleware.

---
 .../launch/load_controllers.launch            |  4 +++
 .../config/simulation_controllers.yaml        |  8 ++---
 engineer_middleware/config/steps_list.yaml    | 29 +++++++++++++++++++
 .../include/engineer_middleware/middleware.h  |  3 +-
 .../include/engineer_middleware/step.h        | 13 +++++++--
 .../include/engineer_middleware/step_queue.h  |  5 ++--
 engineer_middleware/src/middleware.cpp        |  6 ++--
 7 files changed, 57 insertions(+), 11 deletions(-)

diff --git a/engineer_arm_config/launch/load_controllers.launch b/engineer_arm_config/launch/load_controllers.launch
index 169d896..69275b2 100644
--- a/engineer_arm_config/launch/load_controllers.launch
+++ b/engineer_arm_config/launch/load_controllers.launch
@@ -32,6 +32,10 @@
                 controllers/gpio_controller
                 controllers/gimbal_controller
                 controllers/ore_bin_rotate_controller
+                controllers/extend_arm_front_controller
+                controllers/extend_arm_back_controller
+                controllers/extend_arm_front_calibration_controller
+                controllers/extend_arm_back_calibration_controller
                 controllers/ore_bin_lifter_controller
                 controllers/ore_bin_lifter_calibration_controller
                 controllers/gimbal_lifter_controller
diff --git a/engineer_middleware/config/simulation_controllers.yaml b/engineer_middleware/config/simulation_controllers.yaml
index e873175..6271b5b 100644
--- a/engineer_middleware/config/simulation_controllers.yaml
+++ b/engineer_middleware/config/simulation_controllers.yaml
@@ -114,12 +114,12 @@ controllers:
     joint: gimbal_lifter_joint
     pid: { p: 20000., i: 0., d: 1200, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
 
-  extend_arm_a_controller:
+  extend_arm_front_controller:
     type: effort_controllers/JointPositionController
-    joint: extend_arm_a_joint
+    joint: extend_arm_front_joint
     pid: { p: 3., i: 0.1, d: 0.05, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
 
-  extend_arm_b_controller:
+  extend_arm_back_controller:
     type: effort_controllers/JointPositionController
-    joint: extend_arm_b_joint
+    joint: extend_arm_back_joint
     pid: { p: 3., i: 0.1, d: 0.05, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 75478c9..de130d3 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -799,3 +799,32 @@ steps_list:
         max_planning_times: 1
         common:
           <<: *NORMALLY
+  TEST11:
+    - step: "test"
+      extend_arm_front:
+        target: 1
+    - step: "test"
+      extend_arm_back:
+        target: 1
+
+  TEST10:
+    - step: "test"
+      extend_arm_front:
+        target: 1
+    - step: "test"
+      extend_arm_back:
+        target: 0
+  TEST01:
+    - step: "test"
+      extend_arm_front:
+        target: 0
+    - step: "test"
+      extend_arm_back:
+        target: 1
+  TEST00:
+    - step: "test"
+      extend_arm_front:
+        target: 0
+    - step: "test"
+      extend_arm_back:
+        target: 0
diff --git a/engineer_middleware/include/engineer_middleware/middleware.h b/engineer_middleware/include/engineer_middleware/middleware.h
index 58bcb2d..cc9b809 100644
--- a/engineer_middleware/include/engineer_middleware/middleware.h
+++ b/engineer_middleware/include/engineer_middleware/middleware.h
@@ -82,7 +82,8 @@ class Middleware
   moveit::planning_interface::MoveGroupInterface arm_group_;
   ChassisInterface chassis_interface_;
   ros::Publisher hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, planning_result_pub_,
-      stone_num_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub;
+      stone_num_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, extend_arm_f_pub_,
+      extend_arm_b_pub_;
   std::unordered_map<std::string, StepQueue> step_queues_;
   tf2_ros::Buffer tf_;
   tf2_ros::TransformListener tf_listener_;
diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h
index e04ff2a..b94ea72 100644
--- a/engineer_middleware/include/engineer_middleware/step.h
+++ b/engineer_middleware/include/engineer_middleware/step.h
@@ -51,7 +51,7 @@ class Step
        ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub,
        ros::Publisher& reversal_pub, ros::Publisher& stone_num_pub, ros::Publisher& planning_result_pub,
        ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub,
-       ros::Publisher& gimbal_lift_pub)
+       ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub)
     : planning_result_pub_(planning_result_pub), point_cloud_pub_(point_cloud_pub), arm_group_(arm_group)
   {
     ROS_ASSERT(step.hasMember("step"));
@@ -91,6 +91,10 @@ class Step
       ore_lift_motion_ = new JointPointMotion(step["ore_lifter"], ore_lift_pub);
     if (step.hasMember("gimbal_lifter"))
       gimbal_lift_motion_ = new JointPointMotion(step["gimbal_lifter"], gimbal_lift_pub);
+    if (step.hasMember("extend_arm_front"))
+      extend_arm_front_motion_ = new JointPointMotion(step["extend_arm_front"], extend_arm_f_pub);
+    if (step.hasMember("extend_arm_back"))
+      extend_arm_back_motion_ = new JointPointMotion(step["extend_arm_back"], extend_arm_b_pub);
   }
   bool move()
   {
@@ -128,6 +132,10 @@ class Step
       success &= ore_rotate_motion_->move();
     if (gimbal_lift_motion_)
       success &= gimbal_lift_motion_->move();
+    if (extend_arm_back_motion_)
+      success &= extend_arm_back_motion_->move();
+    if (extend_arm_front_motion_)
+      success &= extend_arm_front_motion_->move();
     return success;
   }
   void stop()
@@ -195,7 +203,8 @@ class Step
   MoveitMotionBase* arm_motion_{};
   HandMotion* hand_motion_{};
   JointPositionMotion* end_effector_motion_{};
-  JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{};
+  JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{}, *extend_arm_front_motion_{},
+      *extend_arm_back_motion_{};
   StoneNumMotion* stone_num_motion_{};
   ChassisMotion* chassis_motion_{};
   GimbalMotion* gimbal_motion_{};
diff --git a/engineer_middleware/include/engineer_middleware/step_queue.h b/engineer_middleware/include/engineer_middleware/step_queue.h
index 2c6b63c..d2fa912 100644
--- a/engineer_middleware/include/engineer_middleware/step_queue.h
+++ b/engineer_middleware/include/engineer_middleware/step_queue.h
@@ -56,14 +56,15 @@ class StepQueue
             ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& stone_num_pub,
             ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub, ros::Publisher& reversal_pub,
             ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub,
-            ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub)
+            ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub,
+            ros::Publisher& extend_arm_b_pub)
     : chassis_interface_(chassis_interface)
   {
     ROS_ASSERT(steps.getType() == XmlRpc::XmlRpcValue::TypeArray);
     for (int i = 0; i < steps.size(); ++i)
       queue_.emplace_back(steps[i], scenes, tf, arm_group, chassis_interface, hand_pub, end_effector_pub, stone_num_pub,
                           gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, point_cloud_pub, ore_rotate_pub,
-                          ore_lift_pub, gimbal_lift_pub);
+                          ore_lift_pub, gimbal_lift_pub, extend_arm_f_pub, extend_arm_b_pub);
   }
   bool run(actionlib::SimpleActionServer<rm_msgs::EngineerAction>& as)
   {
diff --git a/engineer_middleware/src/middleware.cpp b/engineer_middleware/src/middleware.cpp
index 701ade3..3ef4368 100644
--- a/engineer_middleware/src/middleware.cpp
+++ b/engineer_middleware/src/middleware.cpp
@@ -55,7 +55,9 @@ Middleware::Middleware(ros::NodeHandle& nh)
   , point_cloud_pub_(nh.advertise<sensor_msgs::PointCloud2>("/cloud", 100))
   , ore_rotate_pub_(nh.advertise<std_msgs::Float64>("/controllers/ore_bin_rotate_controller/command", 10))
   , ore_lift_pub_(nh.advertise<std_msgs::Float64>("/controllers/ore_bin_lifter_controller/command", 10))
-  , gimbal_lift_pub(nh.advertise<std_msgs::Float64>("/controllers/gimbal_lifter_controller/command", 10))
+  , gimbal_lift_pub_(nh.advertise<std_msgs::Float64>("/controllers/gimbal_lifter_controller/command", 10))
+  , extend_arm_f_pub_(nh.advertise<std_msgs::Float64>("/controllers/extend_arm_front_controller/command", 10))
+  , extend_arm_b_pub_(nh.advertise<std_msgs::Float64>("/controllers/extend_arm_back_controller/command", 10))
   , tf_listener_(tf_)
   , is_middleware_control_(false)
 {
@@ -73,7 +75,7 @@ Middleware::Middleware(ros::NodeHandle& nh)
           std::make_pair(it->first, StepQueue(it->second, scenes_list, tf_, arm_group_, chassis_interface_, hand_pub_,
                                               end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, stone_num_pub_,
                                               planning_result_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_,
-                                              gimbal_lift_pub)));
+                                              gimbal_lift_pub_, extend_arm_f_pub_, extend_arm_b_pub_)));
     }
   }
   else

From d0ca289e1d166e72f01370f1892c2a23252bfcb5 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 10 Apr 2024 11:51:42 +0800
Subject: [PATCH 060/147] Add extend arm and gimbal lifter motion.

---
 engineer_middleware/config/steps_list.yaml | 56 ++++++++++++++++++++--
 1 file changed, 51 insertions(+), 5 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index de130d3..325a608 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -166,15 +166,20 @@ common:
 
   gimbal:
     side_pos: &SIDE_POS
-      frame: pitch
+      frame: gimbal_lifter
       position: [ 0.001 , -3, 0. ]
     front_pos: &FRONT_POS
-      frame: pitch
+      frame: gimbal_lifter
       position: [ 2, 0.001, 0. ]
+    small_island_pos: &SMALL_ISLAND_POS
+      frame: gimbal_lifter
+      position: [ 1, 1, 0.]
 
   gimbal_lifter:
     button_pos: &BUTTON_POS
-      0.01
+      0.05
+    small_island: &LIFTER_SMALL_ISLAND
+      0.06
     mid_pos: &MID_POS
       0.1
     tallest_pos: &TALLEST_POS
@@ -228,6 +233,11 @@ common:
             0.1
     bin_down_pos: &BIN_DOWN_POS
             0.001
+  extend_arm:
+    close: &EX_CLOSE
+          0.0
+    open: &EX_OPEN
+          2.6
 
 steps_list:
   EXCHANGE_POS:
@@ -257,6 +267,15 @@ steps_list:
     - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
+    - step: "gimbal lifter down"
+      gimbal_lifter:
+        target: *BUTTON_POS
+    - step: "close ex arm"
+      extend_arm_front:
+        target: *EX_CLOSE
+    - step: "close ex arm"
+      extend_arm_back:
+        target: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -268,6 +287,15 @@ steps_list:
     - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
+    - step: "gimbal lifter down"
+      gimbal_lifter:
+        target: *MID_POS
+    - step: "close ex arm"
+      extend_arm_front:
+        target: *EX_CLOSE
+    - step: "close ex arm"
+      extend_arm_back:
+        target: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -279,6 +307,15 @@ steps_list:
     - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
+    - step: "gimbal lifter down"
+      gimbal_lifter:
+        target: *MID_POS
+    - step: "close ex arm"
+      extend_arm_front:
+        target: *EX_CLOSE
+    - step: "close ex arm"
+      extend_arm_back:
+        target: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -300,7 +337,7 @@ steps_list:
   ONE_STONE_SMALL_ISLAND:
     - step: "GIMBAL_READY"
       gimbal:
-        <<: *SIDE_POS
+        <<: *SMALL_ISLAND_POS
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *READY_POS
@@ -384,7 +421,7 @@ steps_list:
   TWO_STONE_SMALL_ISLAND:
     - step: "GIMBAL_READY"
       gimbal:
-        <<: *SIDE_POS
+        <<: *SMALL_ISLAND_POS
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *READY_POS
@@ -632,6 +669,12 @@ steps_list:
 
   #################BIG_ISLAND#############
   MID_BIG_ISLAND:
+    - step: "open ex arm"
+      extend_arm_front:
+        target: *EX_OPEN
+    - step: "open ex arm"
+      extend_arm_back:
+        target: *EX_OPEN
     - step: "MID_ARM_READY"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -700,6 +743,9 @@ steps_list:
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
+    - step: "move arm out"
+      arm:
+        joints: ["KEEP", ]
     - step: "ARM_RESET"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]

From c0c16fcc8174756a8f8b1fafbe85b04935afb6ac Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 11 Apr 2024 04:52:13 +0800
Subject: [PATCH 061/147] Fix steps list bug.

---
 engineer_middleware/config/steps_list.yaml | 169 ++++++++++++++-------
 1 file changed, 112 insertions(+), 57 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 325a608..169b022 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -26,7 +26,7 @@ common:
   joint1:
     mechanical:
       down_position: &JOINT1_DOWN_POSITION
-                       0.015
+                       0.02
       small_ready: &JOINT1_SMALL_READY
                       0.15
       up_pos: &JOINT1_UP_POS
@@ -45,7 +45,7 @@ common:
                       0.155
     home:
       zero_stone_position: &JOINT1_ZERO_STONE_POSITION
-                             0.08
+                             0.01
       one_stone_position: &JOINT1_ONE_STONE_POSITION
                             0.156
       two_stone_position: &JOINT1_TWO_STONE_POSITION
@@ -85,11 +85,13 @@ common:
       left_position: &JOINT3_L_POSITION
                        0.28
       mid_position: &JOINT3_MID_POSITION
-                      0.0001
+                      0.04
       right_position: &JOINT3_R_POSITION
                         -0.275
+      home: &JOINT3_HOME_POS
+        0.08
       small_ready: &JOINT3_SMALL_READY
-                        0.098
+                        0.208
       small_ready_second: &JOINT3_SMALL_READY_SECOND
                         0.03
       small_get_stone: &JOINT3_SMALL_GET_STONE
@@ -122,7 +124,7 @@ common:
   joint5:
     mechanical:
       mid_position: &JOINT5_MID_POSITION
-                      0.001
+                      0.011
       left_90_position: &JOINT5_L90_POSITION
                           -1.54
       right_90_position: &JOINT5_R90_POSITION
@@ -156,7 +158,7 @@ common:
   joint7:
     mechanical:
       mid_position: &JOINT7_MID_POSITION
-                      0.001
+                      0.044
       left_90_position: &JOINT7_L90_POSITION
                           -1.59
       right_90_position: &JOINT7_R90_POSITION
@@ -173,15 +175,19 @@ common:
       position: [ 2, 0.001, 0. ]
     small_island_pos: &SMALL_ISLAND_POS
       frame: gimbal_lifter
-      position: [ 1, 1, 0.]
+      position: [ 1, -1, 0.]
 
   gimbal_lifter:
     button_pos: &BUTTON_POS
-      0.05
+      0.07
     small_island: &LIFTER_SMALL_ISLAND
-      0.06
+      0.1
     mid_pos: &MID_POS
       0.1
+    big_island: &LIFTER_BIG_ISLAND
+      0.185
+    two_stone_pos: &TWO_STONE_POS
+      0.163
     tallest_pos: &TALLEST_POS
       0.28
 
@@ -195,7 +201,7 @@ common:
 
   arm:
     home: &HOME_1
-      joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+      joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
@@ -203,9 +209,9 @@ common:
 
 
   chassis_move:
-    chassis_backward_30: &CHASSIS_BACKWARD_25
+    chassis_left_315: &CHASSIS_LEFT_15
       frame: base_link
-      position: [ -0.35,0. ]
+      position: [ 0., 15. ]
       yaw: 0.0
       chassis_tolerance_position: 0.1
       chassis_tolerance_angular: 0.3
@@ -246,11 +252,14 @@ steps_list:
         <<: *FRONT_POS
     - step: "move arm"
       arm:
-        joints: [*JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
+        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "gimbal lifter tallest"
+      gimbal_lifter:
+        target: *TALLEST_POS
 
   GIMBAL_FRONT:
     - step: "gimbal look front"
@@ -279,6 +288,12 @@ steps_list:
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_UP"
+      ore_lifter:
+        target: *BIN_DOWN_POS
     - step: "home with no stone"
       arm:
         <<: *HOME_1
@@ -299,10 +314,16 @@ steps_list:
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_UP"
+      ore_lifter:
+        target: *BIN_DOWN_POS
     - step: "home with one stone"
       arm:
         <<: *HOME_1
-        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ]
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
   HOME_TWO_STONE:
     - step: "gimbal look front"
       gimbal:
@@ -319,10 +340,16 @@ steps_list:
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_UP"
+      ore_lifter:
+        target: *BIN_DOWN_POS
     - step: "home with two stone"
       arm:
         <<: *HOME_1
-        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
 
   OPEN_GRIPPER:
     - step: "open gripper"
@@ -346,15 +373,23 @@ steps_list:
         target: *BIN_MID_POS
     - step: "SMALL_ARM_READY"
       arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
   ONE_STONE_SMALL_ISLAND0:
+    - step: "SMALL_ARM_READY"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "SMALL_GET_STONE"
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
@@ -407,6 +442,13 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
@@ -415,7 +457,7 @@ steps_list:
         change: "+1"
     - step: "GIMBAL_READY"
       gimbal:
-        <<: *SIDE_POS
+        <<: *FRONT_POS
 
   ####FIRST_STONE###
   TWO_STONE_SMALL_ISLAND:
@@ -430,7 +472,7 @@ steps_list:
         target: *BIN_MID_POS
     - step: "SMALL_ARM_READY"
       arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -439,6 +481,13 @@ steps_list:
       gripper:
         <<: *OPEN_GRIPPER
   TWO_STONE_SMALL_ISLAND0:
+    - step: "SMALL_ARM_READY"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "SMALL_GET_STONE"
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
@@ -562,6 +611,13 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "add stone"
       stone_num:
         change: "+1"
@@ -609,11 +665,14 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "exchange pos"
       arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "gimbal lifter tallest"
+      gimbal_lifter:
+        target: *TALLEST_POS
     - step: "remove stone"
       stone_num:
         change: "-1"
@@ -658,17 +717,23 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "exchange pos"
       arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "gimbal lifter tallest"
+      gimbal_lifter:
+        target: *TALLEST_POS
     - step: "remove stone"
       stone_num:
         change: "-1"
 
   #################BIG_ISLAND#############
   MID_BIG_ISLAND:
+    - step: "gimbal ready"
+      gimbal:
+        <<: *SMALL_ISLAND_POS
     - step: "open ex arm"
       extend_arm_front:
         target: *EX_OPEN
@@ -688,9 +753,13 @@ steps_list:
     - step: "ORE_LIFTER_READY"
       ore_lifter:
         target: *BIN_MID_POS
+    - step: "gimbal lifter"
+      gimbal_lifter:
+        target: *LIFTER_BIG_ISLAND
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
+  MID_BIG_ISLAND0:
     - step: "GET_MID_STONE"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -712,6 +781,9 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "chassis move left"
+      chassis:
+        <<: *CHASSIS_LEFT_15
     - step: "JOINT1_UP"
       arm:
         joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
@@ -745,21 +817,33 @@ steps_list:
         <<: *CLOSE_GRIPPER
     - step: "move arm out"
       arm:
-        joints: ["KEEP", ]
-    - step: "ARM_RESET"
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "home"
       arm:
-        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
-      common:
-        <<: *SLOWLY
-      tolerance:
-        <<: *NORMAL_TOLERANCE
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
     - step: "add stone"
       stone_num:
         change: "+1"
-
+    - step: "open ex arm"
+      extend_arm_front:
+        target: *EX_CLOSE
+    - step: "open ex arm"
+      extend_arm_back:
+        target: *EX_CLOSE
+    - step: "gimbal front"
+      gimbal:
+        <<: *FRONT_POS
 
   ####################################################  TEST  ##########################################################
 
@@ -845,32 +929,3 @@ steps_list:
         max_planning_times: 1
         common:
           <<: *NORMALLY
-  TEST11:
-    - step: "test"
-      extend_arm_front:
-        target: 1
-    - step: "test"
-      extend_arm_back:
-        target: 1
-
-  TEST10:
-    - step: "test"
-      extend_arm_front:
-        target: 1
-    - step: "test"
-      extend_arm_back:
-        target: 0
-  TEST01:
-    - step: "test"
-      extend_arm_front:
-        target: 0
-    - step: "test"
-      extend_arm_back:
-        target: 1
-  TEST00:
-    - step: "test"
-      extend_arm_front:
-        target: 0
-    - step: "test"
-      extend_arm_back:
-        target: 0

From fedc09ff2e4b543ee76ea6fc9580583b3752c79e Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sun, 14 Apr 2024 15:13:39 +0800
Subject: [PATCH 062/147] Replace KDL solver with TRAC-IK solver.

---
 engineer_arm_config/config/kinematics.yaml | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/engineer_arm_config/config/kinematics.yaml b/engineer_arm_config/config/kinematics.yaml
index a4b51af..b298b37 100644
--- a/engineer_arm_config/config/kinematics.yaml
+++ b/engineer_arm_config/config/kinematics.yaml
@@ -1,4 +1,3 @@
 engineer_arm:
-  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
-  kinematics_solver_search_resolution: 0.005
+  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
   kinematics_solver_timeout: 0.005

From 1e78179e1996ba1eec86d8d9c03f96928248b9ed Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sun, 14 Apr 2024 15:13:59 +0800
Subject: [PATCH 063/147] Record steps list.

---
 engineer_middleware/config/steps_list.yaml | 167 ++++++++++++++++++---
 1 file changed, 145 insertions(+), 22 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 169b022..d485ff8 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -28,17 +28,17 @@ common:
       down_position: &JOINT1_DOWN_POSITION
                        0.02
       small_ready: &JOINT1_SMALL_READY
-                      0.15
+                      0.16
       up_pos: &JOINT1_UP_POS
                    0.312
       small_ready_store: &JOINT1_SMALL_READY_STORE
-                   0.162
+                   0.17
       bin_get_stone: &JOINT1_BIN_GET_STONE
                      0.142
       big_ready: &JOINT1_BIG_READY
                      0.006
       big_adjust_mid: &JOINT1_BIG_ADJUST_MID
-                      0.071
+                      0.08
       big_up_pos: &JOINT1_BIG_UP_POS
                       0.320
       big_ready_store: &JOINT1_BIG_READY_STORE
@@ -78,7 +78,7 @@ common:
       big_arm_up: &JOINT2_BIG_ARM_UP
                     0.142
       big_ready_store: &JOINT2_BIG_READY_STORE
-                      0.030
+                      0.020
 
   joint3:
     mechanical:
@@ -95,21 +95,25 @@ common:
       small_ready_second: &JOINT3_SMALL_READY_SECOND
                         0.03
       small_get_stone: &JOINT3_SMALL_GET_STONE
-                            0.034
+                            0.047
+      small_get_stone2: &JOINT3_SMALL_GET_STONE2
+                            0.056
       small_get_second: &JOINT3_SMALL_GET_SECOND
                             -0.018
       small_store_stone: &JOINT3_SMALL_STORE_STONE
-                              0.30
+                              0.291
       bin_get_stone: &JOINT3_BIN_GET_STONE
                           0.258
       big_ready: &JOINT3_BIG_READY
                      0.071
       get_big_mid: &JOINT3_GET_BIG_MID
                       -0.281
+      get_big_side: &JOINT3_GET_BIG_SIDE
+                     -0.199
       big_pull_out: &JOINT3_BIG_PULL_OUT
                      0.188
       big_arm_up: &JOINT3_BIG_ARM_UP
-                    0.289
+                    0.291
 
   joint4:
     mechanical:
@@ -162,7 +166,7 @@ common:
       left_90_position: &JOINT7_L90_POSITION
                           -1.59
       right_90_position: &JOINT7_R90_POSITION
-                           1.57
+                           1.62
       big_pos: &JOINT7_BIG_POS
                          -0.013
 
@@ -183,13 +187,13 @@ common:
     small_island: &LIFTER_SMALL_ISLAND
       0.1
     mid_pos: &MID_POS
-      0.1
+      0.14
     big_island: &LIFTER_BIG_ISLAND
-      0.185
+      0.260
     two_stone_pos: &TWO_STONE_POS
       0.163
     tallest_pos: &TALLEST_POS
-      0.28
+      0.394
 
   gripper:
     open_gripper: &OPEN_GRIPPER
@@ -397,16 +401,16 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "JOINT1_UP"
+    - step: "JOINT1_UP AND BACK"
       arm:
-        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"]
+        joints: [ *JOINT1_UP_POS, "KEEP", *JOINT3_SMALL_GET_STONE2, "KEEP", "KEEP", "KEEP", "KEEP"]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "SMALL_ARM_UP_STONE"
       arm:
-        joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE2, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -554,7 +558,7 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *SMALL_TOLERANCE
+          <<: *NORMAL_TOLERANCE
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
@@ -781,9 +785,9 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "chassis move left"
-      chassis:
-        <<: *CHASSIS_LEFT_15
+#    - step: "chassis move left"
+#      chassis:
+#        <<: *CHASSIS_LEFT_15
     - step: "JOINT1_UP"
       arm:
         joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
@@ -835,12 +839,116 @@ steps_list:
     - step: "add stone"
       stone_num:
         change: "+1"
+    - step: "gimbal front"
+      gimbal:
+        <<: *FRONT_POS
+
+
+  SIDE_BIG_ISLAND:
+    - step: "gimbal ready"
+      gimbal:
+        <<: *SMALL_ISLAND_POS
     - step: "open ex arm"
       extend_arm_front:
-        target: *EX_CLOSE
-    - step: "open ex arm"
-      extend_arm_back:
-        target: *EX_CLOSE
+        target: *EX_OPEN
+#    - step: "open ex arm"
+#      extend_arm_back:
+#        target: *EX_OPEN
+    - step: "MID_ARM_READY"
+      arm:
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_MID_POS
+    - step: "gimbal lifter"
+      gimbal_lifter:
+        target: *LIFTER_BIG_ISLAND
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+  SIDE_BIG_ISLAND0:
+    - step: "GET_MID_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ADJUST_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "PULL_OUT_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    #    - step: "chassis move left"
+    #      chassis:
+    #        <<: *CHASSIS_LEFT_15
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ARM_UP_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_TO_STORE"
+      arm:
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "move arm out"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ORE_LIFTER_DOWN"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "add stone"
+      stone_num:
+        change: "+1"
     - step: "gimbal front"
       gimbal:
         <<: *FRONT_POS
@@ -929,3 +1037,18 @@ steps_list:
         max_planning_times: 1
         common:
           <<: *NORMALLY
+
+  GIMBAL_DOWN:
+    - step: "gimbal lowest"
+      gimbal_lifter:
+        target: *BUTTON_POS
+
+  GIMBAL_MID:
+    - step: "gimbal mid"
+      gimbal_lifter:
+        target: *LIFTER_BIG_ISLAND
+
+  GIMBAL_TALL:
+    - step: "gimbal tall"
+      gimbal_lifter:
+        target: *TALLEST_POS

From c81a6efe13f929715e9b8f20d2780d41433a931b Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sun, 14 Apr 2024 15:51:59 +0800
Subject: [PATCH 064/147] Disable collision check between joint3 and base_link.

---
 engineer_arm_config/config/engineer.srdf | 1 +
 1 file changed, 1 insertion(+)

diff --git a/engineer_arm_config/config/engineer.srdf b/engineer_arm_config/config/engineer.srdf
index 604c9d1..c49a334 100644
--- a/engineer_arm_config/config/engineer.srdf
+++ b/engineer_arm_config/config/engineer.srdf
@@ -34,6 +34,7 @@
         <disable_collisions link1="base_link" link2="left_front_wheel" reason="Adjacent"/>
         <disable_collisions link1="base_link" link2="link1" reason="Adjacent"/>
         <disable_collisions link1="base_link" link2="link2" reason="Default"/>
+        <disable_collisions link1="base_link" link2="link3" reason="Never"/>
         <disable_collisions link1="base_link" link2="link5" reason="Default"/>
         <disable_collisions link1="base_link" link2="link6" reason="Default"/>
         <disable_collisions link1="gimbal_base" link2="link1" reason="Never"/>

From 5bbc02ece3fd22f7e3667ed001ceac16366ed952 Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Sun, 14 Apr 2024 18:57:26 +0800
Subject: [PATCH 065/147] Merge extend_arm in step_list and add has_stone
 logic.

---
 engineer_middleware/config/engineer.yaml      |   6 +-
 engineer_middleware/config/steps_list.yaml    | 613 +++++++++++++++---
 .../include/engineer_middleware/motion.h      |  38 ++
 .../include/engineer_middleware/step.h        |  16 +-
 4 files changed, 585 insertions(+), 88 deletions(-)

diff --git a/engineer_middleware/config/engineer.yaml b/engineer_middleware/config/engineer.yaml
index c63f929..73c761e 100644
--- a/engineer_middleware/config/engineer.yaml
+++ b/engineer_middleware/config/engineer.yaml
@@ -1,9 +1,9 @@
 chassis:
   x:
-    pid: { p: 0.0, i: 0.0, d: 0.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+    pid: { p: 4.5, i: 0.0, d: 0.2, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
   y:
-    pid: { p: 0.0, i: 0.0, d: 0.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+    pid: { p: 4.5, i: 0.0, d: 0.2, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
   yaw:
-    pid: { p: 3., i: 0.0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+    pid: { p: 5., i: 0.0, d: 0.05, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
   yaw_start_threshold: 0.05
   max_vel: 0.1
diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 325a608..ef72681 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -26,7 +26,7 @@ common:
   joint1:
     mechanical:
       down_position: &JOINT1_DOWN_POSITION
-                       0.015
+                       0.02
       small_ready: &JOINT1_SMALL_READY
                       0.15
       up_pos: &JOINT1_UP_POS
@@ -45,7 +45,7 @@ common:
                       0.155
     home:
       zero_stone_position: &JOINT1_ZERO_STONE_POSITION
-                             0.08
+                             0.01
       one_stone_position: &JOINT1_ONE_STONE_POSITION
                             0.156
       two_stone_position: &JOINT1_TWO_STONE_POSITION
@@ -78,18 +78,20 @@ common:
       big_arm_up: &JOINT2_BIG_ARM_UP
                     0.142
       big_ready_store: &JOINT2_BIG_READY_STORE
-                      0.030
+                      0.020
 
   joint3:
     mechanical:
       left_position: &JOINT3_L_POSITION
                        0.28
       mid_position: &JOINT3_MID_POSITION
-                      0.0001
+                      0.04
       right_position: &JOINT3_R_POSITION
                         -0.275
+      home: &JOINT3_HOME_POS
+        0.08
       small_ready: &JOINT3_SMALL_READY
-                        0.098
+                        0.208
       small_ready_second: &JOINT3_SMALL_READY_SECOND
                         0.03
       small_get_stone: &JOINT3_SMALL_GET_STONE
@@ -104,10 +106,12 @@ common:
                      0.071
       get_big_mid: &JOINT3_GET_BIG_MID
                       -0.281
+      get_big_side: &JOINT3_GET_BIG_SIDE
+                     -0.199
       big_pull_out: &JOINT3_BIG_PULL_OUT
                      0.188
       big_arm_up: &JOINT3_BIG_ARM_UP
-                    0.289
+                    0.291
 
   joint4:
     mechanical:
@@ -122,7 +126,7 @@ common:
   joint5:
     mechanical:
       mid_position: &JOINT5_MID_POSITION
-                      0.001
+                      0.011
       left_90_position: &JOINT5_L90_POSITION
                           -1.54
       right_90_position: &JOINT5_R90_POSITION
@@ -156,7 +160,7 @@ common:
   joint7:
     mechanical:
       mid_position: &JOINT7_MID_POSITION
-                      0.001
+                      0.044
       left_90_position: &JOINT7_L90_POSITION
                           -1.59
       right_90_position: &JOINT7_R90_POSITION
@@ -173,17 +177,21 @@ common:
       position: [ 2, 0.001, 0. ]
     small_island_pos: &SMALL_ISLAND_POS
       frame: gimbal_lifter
-      position: [ 1, 1, 0.]
+      position: [ 1, -1, 0.]
 
   gimbal_lifter:
     button_pos: &BUTTON_POS
-      0.05
+      0.07
     small_island: &LIFTER_SMALL_ISLAND
-      0.06
-    mid_pos: &MID_POS
       0.1
+    mid_pos: &MID_POS
+      0.14
+    big_island: &LIFTER_BIG_ISLAND
+      0.260
+    two_stone_pos: &TWO_STONE_POS
+      0.163
     tallest_pos: &TALLEST_POS
-      0.28
+      0.394
 
   gripper:
     open_gripper: &OPEN_GRIPPER
@@ -195,7 +203,7 @@ common:
 
   arm:
     home: &HOME_1
-      joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_MID_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+      joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
@@ -203,9 +211,9 @@ common:
 
 
   chassis_move:
-    chassis_backward_30: &CHASSIS_BACKWARD_25
+    chassis_left_315: &CHASSIS_LEFT_15
       frame: base_link
-      position: [ -0.35,0. ]
+      position: [ 0., 15. ]
       yaw: 0.0
       chassis_tolerance_position: 0.1
       chassis_tolerance_angular: 0.3
@@ -246,11 +254,14 @@ steps_list:
         <<: *FRONT_POS
     - step: "move arm"
       arm:
-        joints: [*JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
+        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "gimbal lifter tallest"
+      gimbal_lifter:
+        target: *TALLEST_POS
 
   GIMBAL_FRONT:
     - step: "gimbal look front"
@@ -272,13 +283,17 @@ steps_list:
         target: *BUTTON_POS
     - step: "close ex arm"
       extend_arm_front:
-        target: *EX_CLOSE
-    - step: "close ex arm"
-      extend_arm_back:
-        target: *EX_CLOSE
+        front: *EX_CLOSE
+        back: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_UP"
+      ore_lifter:
+        target: *BIN_DOWN_POS
     - step: "home with no stone"
       arm:
         <<: *HOME_1
@@ -292,17 +307,21 @@ steps_list:
         target: *MID_POS
     - step: "close ex arm"
       extend_arm_front:
-        target: *EX_CLOSE
-    - step: "close ex arm"
-      extend_arm_back:
-        target: *EX_CLOSE
+        front: *EX_CLOSE
+        back: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_UP"
+      ore_lifter:
+        target: *BIN_DOWN_POS
     - step: "home with one stone"
       arm:
         <<: *HOME_1
-        variable: [ *JOINT1_ONE_STONE_POSITION, 0, 0, 0, 0, 0 ]
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
   HOME_TWO_STONE:
     - step: "gimbal look front"
       gimbal:
@@ -312,17 +331,21 @@ steps_list:
         target: *MID_POS
     - step: "close ex arm"
       extend_arm_front:
-        target: *EX_CLOSE
-    - step: "close ex arm"
-      extend_arm_back:
-        target: *EX_CLOSE
+        front: *EX_CLOSE
+        back: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_UP"
+      ore_lifter:
+        target: *BIN_DOWN_POS
     - step: "home with two stone"
       arm:
         <<: *HOME_1
-        variable: [ *JOINT1_TWO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
 
   OPEN_GRIPPER:
     - step: "open gripper"
@@ -334,7 +357,8 @@ steps_list:
         <<: *CLOSE_GRIPPER
 
   ################### SMALL_ISLAND_FROM_SIDE ################
-  ONE_STONE_SMALL_ISLAND:
+  ########ONE_STONE_ONCE#########
+  HAS0_ONE_STONE_SMALL_ISLAND:
     - step: "GIMBAL_READY"
       gimbal:
         <<: *SMALL_ISLAND_POS
@@ -346,15 +370,23 @@ steps_list:
         target: *BIN_MID_POS
     - step: "SMALL_ARM_READY"
       arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
-  ONE_STONE_SMALL_ISLAND0:
+  HAS0_ONE_STONE_SMALL_ISLAND0:
+    - step: "SMALL_ARM_READY"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "SMALL_GET_STONE"
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
@@ -407,6 +439,13 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
@@ -415,8 +454,103 @@ steps_list:
         change: "+1"
     - step: "GIMBAL_READY"
       gimbal:
-        <<: *SIDE_POS
+        <<: *FRONT_POS
+
+  HAS1_ONE_STONE_SMALL_ISLAND:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *SMALL_ISLAND_POS
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "SMALL_ARM_READY"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
 
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+  ONE_STONE_SMALL_ISLAND0:
+    - step: "SMALL_ARM_READY"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "SMALL_GET_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "SMALL_ARM_UP_STONE"
+      arm:
+        joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_TO_STORE_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *SMALL_TOLERANCE
+    - step: "STORE_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *SMALL_TOLERANCE
+    - step: "JOINT5_TURN"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *SMALL_TOLERANCE
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "ARM_RESET"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "add stone"
+      stone_num:
+        change: "+1"
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *FRONT_POS
+  ##########TWO_STONE_ONCE###########
   ####FIRST_STONE###
   TWO_STONE_SMALL_ISLAND:
     - step: "GIMBAL_READY"
@@ -430,7 +564,7 @@ steps_list:
         target: *BIN_MID_POS
     - step: "SMALL_ARM_READY"
       arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -439,6 +573,13 @@ steps_list:
       gripper:
         <<: *OPEN_GRIPPER
   TWO_STONE_SMALL_ISLAND0:
+    - step: "SMALL_ARM_READY"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "SMALL_GET_STONE"
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
@@ -505,7 +646,7 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *SMALL_TOLERANCE
+          <<: *NORMAL_TOLERANCE
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
@@ -562,6 +703,13 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "add stone"
       stone_num:
         change: "+1"
@@ -609,11 +757,14 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "exchange pos"
       arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "gimbal lifter tallest"
+      gimbal_lifter:
+        target: *TALLEST_POS
     - step: "remove stone"
       stone_num:
         change: "-1"
@@ -658,23 +809,28 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "exchange pos"
       arm:
-        joints: [ *JOINT1_TWO_STONE_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_L_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "gimbal lifter tallest"
+      gimbal_lifter:
+        target: *TALLEST_POS
     - step: "remove stone"
       stone_num:
         change: "-1"
 
   #################BIG_ISLAND#############
-  MID_BIG_ISLAND:
-    - step: "open ex arm"
-      extend_arm_front:
-        target: *EX_OPEN
+  ######MID_STONE#######
+  HAS0_MID_BIG_ISLAND:
+    - step: "gimbal ready"
+      gimbal:
+        <<: *SMALL_ISLAND_POS
     - step: "open ex arm"
-      extend_arm_back:
-        target: *EX_OPEN
+      extend_arm:
+        front: *EX_OPEN
+        back: *EX_OPEN
     - step: "MID_ARM_READY"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -688,9 +844,13 @@ steps_list:
     - step: "ORE_LIFTER_READY"
       ore_lifter:
         target: *BIN_MID_POS
+    - step: "gimbal lifter"
+      gimbal_lifter:
+        target: *LIFTER_BIG_ISLAND
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
+  HAS0_MID_BIG_ISLAND0:
     - step: "GET_MID_STONE"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -712,6 +872,9 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+#    - step: "chassis move left"
+#      chassis:
+#        <<: *CHASSIS_LEFT_15
     - step: "JOINT1_UP"
       arm:
         joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
@@ -745,21 +908,340 @@ steps_list:
         <<: *CLOSE_GRIPPER
     - step: "move arm out"
       arm:
-        joints: ["KEEP", ]
-    - step: "ARM_RESET"
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ORE_LIFTER_DOWN"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "add stone"
+      stone_num:
+        change: "+1"
+    - step: "gimbal front"
+      gimbal:
+        <<: *FRONT_POS
+
+  HAS1_MID_BIG_ISLAND:
+    - step: "gimbal ready"
+      gimbal:
+        <<: *SMALL_ISLAND_POS
+    - step: "open ex arm"
+      extend_arm:
+        front: *EX_OPEN
+        back: *EX_OPEN
+    - step: "MID_ARM_READY"
+      arm:
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "gimbal lifter"
+      gimbal_lifter:
+        target: *LIFTER_BIG_ISLAND
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+  HAS1_MID_BIG_ISLAND0:
+    - step: "GET_MID_STONE"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
-      common:
-        <<: *SLOWLY
-      tolerance:
-        <<: *NORMAL_TOLERANCE
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ADJUST_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "PULL_OUT_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    #    - step: "chassis move left"
+    #      chassis:
+    #        <<: *CHASSIS_LEFT_15
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ARM_UP_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_TO_STORE"
+      arm:
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "move arm out"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "add stone"
+      stone_num:
+        change: "+1"
+    - step: "gimbal front"
+      gimbal:
+        <<: *FRONT_POS
+  ########SIDE_STONE#########
+  HAS0_SIDE_BIG_ISLAND:
+    - step: "gimbal ready"
+      gimbal:
+        <<: *SMALL_ISLAND_POS
+    - step: "open ex arm"
+      extend_arm:
+        front: *EX_OPEN
+    - step: "MID_ARM_READY"
+      arm:
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_MID_POS
+    - step: "gimbal lifter"
+      gimbal_lifter:
+        target: *LIFTER_BIG_ISLAND
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+  HAS0_SIDE_BIG_ISLAND0:
+    - step: "GET_MID_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ADJUST_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "PULL_OUT_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    #    - step: "chassis move left"
+    #      chassis:
+    #        <<: *CHASSIS_LEFT_15
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ARM_UP_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_TO_STORE"
+      arm:
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "move arm out"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
     - step: "add stone"
       stone_num:
         change: "+1"
+    - step: "gimbal front"
+      gimbal:
+        <<: *FRONT_POS
 
+  HAS1_SIDE_BIG_ISLAND:
+    - step: "gimbal ready"
+      gimbal:
+        <<: *SMALL_ISLAND_POS
+    - step: "open ex arm"
+      extend_arm:
+        front: *EX_OPEN
+    - step: "MID_ARM_READY"
+      arm:
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "gimbal lifter"
+      gimbal_lifter:
+        target: *LIFTER_BIG_ISLAND
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+  HAS1_SIDE_BIG_ISLAND0:
+    - step: "GET_MID_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ADJUST_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "PULL_OUT_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    #    - step: "chassis move left"
+    #      chassis:
+    #        <<: *CHASSIS_LEFT_15
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ARM_UP_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_TO_STORE"
+      arm:
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "move arm out"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "add stone"
+      stone_num:
+        change: "+1"
+    - step: "gimbal front"
+      gimbal:
+        <<: *FRONT_POS
 
   ####################################################  TEST  ##########################################################
 
@@ -845,32 +1327,5 @@ steps_list:
         max_planning_times: 1
         common:
           <<: *NORMALLY
-  TEST11:
-    - step: "test"
-      extend_arm_front:
-        target: 1
-    - step: "test"
-      extend_arm_back:
-        target: 1
 
-  TEST10:
-    - step: "test"
-      extend_arm_front:
-        target: 1
-    - step: "test"
-      extend_arm_back:
-        target: 0
-  TEST01:
-    - step: "test"
-      extend_arm_front:
-        target: 0
-    - step: "test"
-      extend_arm_back:
-        target: 1
-  TEST00:
-    - step: "test"
-      extend_arm_front:
-        target: 0
-    - step: "test"
-      extend_arm_back:
-        target: 0
+
diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index c893f49..4a1870b 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -720,4 +720,42 @@ class JointPointMotion : public PublishMotion<std_msgs::Float64>
   double target_;
 };
 
+class ExtendFrontMotion : public PublishMotion<std_msgs::Float64>
+{
+public:
+  ExtendFrontMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface)
+    : PublishMotion<std_msgs::Float64>(motion, interface)
+  {
+    ROS_ASSERT(motion.hasMember("front"));
+    target_ = xmlRpcGetDouble(motion, "front", 0.0);
+  }
+  bool move() override
+  {
+    msg_.data = target_;
+    return PublishMotion::move();
+  }
+
+private:
+  double target_;
+};
+
+class ExtendBackMotion : public PublishMotion<std_msgs::Float64>
+{
+public:
+  ExtendBackMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface)
+    : PublishMotion<std_msgs::Float64>(motion, interface)
+  {
+    ROS_ASSERT(motion.hasMember("back"));
+    target_ = xmlRpcGetDouble(motion, "back", 0.0);
+  }
+  bool move() override
+  {
+    msg_.data = target_;
+    return PublishMotion::move();
+  }
+
+private:
+  double target_;
+};
+
 };  // namespace engineer_middleware
diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h
index b94ea72..00499f9 100644
--- a/engineer_middleware/include/engineer_middleware/step.h
+++ b/engineer_middleware/include/engineer_middleware/step.h
@@ -91,10 +91,13 @@ class Step
       ore_lift_motion_ = new JointPointMotion(step["ore_lifter"], ore_lift_pub);
     if (step.hasMember("gimbal_lifter"))
       gimbal_lift_motion_ = new JointPointMotion(step["gimbal_lifter"], gimbal_lift_pub);
-    if (step.hasMember("extend_arm_front"))
-      extend_arm_front_motion_ = new JointPointMotion(step["extend_arm_front"], extend_arm_f_pub);
-    if (step.hasMember("extend_arm_back"))
-      extend_arm_back_motion_ = new JointPointMotion(step["extend_arm_back"], extend_arm_b_pub);
+    if (step.hasMember("extend_arm"))
+    {
+      if (step["extend_arm"].hasMember("front"))
+        extend_arm_front_motion_ = new ExtendFrontMotion(step["extend_arm"], extend_arm_f_pub);
+      if (step["extend_arm"].hasMember("back"))
+        extend_arm_back_motion_ = new ExtendBackMotion(step["extend_arm"], extend_arm_b_pub);
+    }
   }
   bool move()
   {
@@ -203,8 +206,9 @@ class Step
   MoveitMotionBase* arm_motion_{};
   HandMotion* hand_motion_{};
   JointPositionMotion* end_effector_motion_{};
-  JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{}, *extend_arm_front_motion_{},
-      *extend_arm_back_motion_{};
+  JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{};
+  ExtendFrontMotion *extend_arm_front_motion_{};
+  ExtendBackMotion *extend_arm_back_motion_{};
   StoneNumMotion* stone_num_motion_{};
   ChassisMotion* chassis_motion_{};
   GimbalMotion* gimbal_motion_{};

From 10ea5834a57add4c91b393e36e17c0822c71547f Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sun, 14 Apr 2024 22:38:40 +0800
Subject: [PATCH 066/147] Remove drift dimensions.

---
 .../include/engineer_middleware/motion.h      | 33 +++++--------------
 1 file changed, 9 insertions(+), 24 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index c893f49..6369257 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -236,16 +236,8 @@ class SpaceEeMotion : public EndEffectorMotion
                 tf2_ros::Buffer& tf)
     : EndEffectorMotion(motion, interface, tf)
   {
-    drift_dimensions_.resize(6, true);
     radius_ = xmlRpcGetDouble(motion, "radius", 0.1);
     link7_length_ = xmlRpcGetDouble(motion, "link7_length", 0.);
-    if (motion.hasMember("drift_dimensions"))
-    {
-      for (int i = 0; i < (int)drift_dimensions_.size(); ++i)
-      {
-        drift_dimensions_[i] = motion["drift_dimensions"][i];
-      }
-    }
     if (motion.hasMember("basics_length"))
     {
       ROS_ASSERT(motion["basics_length"].getType() == XmlRpc::XmlRpcValue::TypeArray);
@@ -298,9 +290,6 @@ class SpaceEeMotion : public EndEffectorMotion
             exchange2base = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
             quatToRPY(exchange2base.transform.rotation, roll_temp, pitch_temp, yaw_temp);
             quatToRPY(target_.pose.orientation, roll, pitch, yaw);
-            roll = drift_dimensions_[3] ? roll : (roll - roll_temp);
-            pitch = drift_dimensions_[4] ? pitch : (pitch - pitch_temp);
-            yaw = drift_dimensions_[5] ? yaw : (yaw - yaw_temp);
 
             tf2::Quaternion tmp_tf_quaternion;
             tmp_tf_quaternion.setRPY(roll, pitch, yaw);
@@ -317,9 +306,9 @@ class SpaceEeMotion : public EndEffectorMotion
             temp_target.pose.orientation = quat_tf;
             tf2::doTransform(temp_target.pose, final_target_.pose,
                              tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0)));
-            final_target_.pose.position.x = drift_dimensions_[0] ? final_target_.pose.position.x : 0.;
-            final_target_.pose.position.y = drift_dimensions_[1] ? final_target_.pose.position.y : 0.;
-            final_target_.pose.position.z = drift_dimensions_[2] ? final_target_.pose.position.z : 0.;
+            final_target_.pose.position.x = final_target_.pose.position.x;
+            final_target_.pose.position.y = final_target_.pose.position.y;
+            final_target_.pose.position.z = final_target_.pose.position.z;
             final_target_.header.frame_id = interface_.getPlanningFrame();
           }
           catch (tf2::TransformException& ex)
@@ -340,19 +329,16 @@ class SpaceEeMotion : public EndEffectorMotion
             target_.pose.position.z = points_.getPoints()[i].z;
             quatToRPY(base2exchange.transform.rotation, roll, pitch, yaw);
             quatToRPY(target_.pose.orientation, roll_temp, pitch_temp, yaw_temp);
-            roll = drift_dimensions_[3] ? (roll + roll_temp) : 0.;
-            pitch = drift_dimensions_[4] ? (pitch + pitch_temp) : 0.;
-            yaw = drift_dimensions_[5] ? (yaw + yaw_temp) : 0.;
+            roll = roll + roll_temp;
+            pitch = pitch + pitch_temp;
+            yaw = yaw + yaw_temp;
             tf2::Quaternion tf_quaternion;
             tf_quaternion.setRPY(roll, pitch, yaw);
             geometry_msgs::Quaternion quat_tf = tf2::toMsg(tf_quaternion);
 
-            final_target_.pose.position.x =
-                drift_dimensions_[0] ? base2exchange.transform.translation.x + target_.pose.position.x : 0.;
-            final_target_.pose.position.y =
-                drift_dimensions_[1] ? base2exchange.transform.translation.y + target_.pose.position.y : 0.;
-            final_target_.pose.position.z =
-                drift_dimensions_[2] ? base2exchange.transform.translation.z + target_.pose.position.z : 0.;
+            final_target_.pose.position.x = base2exchange.transform.translation.x + target_.pose.position.x;
+            final_target_.pose.position.y = base2exchange.transform.translation.y + target_.pose.position.y;
+            final_target_.pose.position.z = base2exchange.transform.translation.z + target_.pose.position.z;
             final_target_.pose.orientation = quat_tf;
             final_target_.header.frame_id = interface_.getPlanningFrame();
           }
@@ -389,7 +375,6 @@ class SpaceEeMotion : public EndEffectorMotion
                 tolerance_orientation_);
   }
   bool is_refer_planning_frame_;
-  std::vector<bool> drift_dimensions_;
   geometry_msgs::PoseStamped final_target_;
   int max_planning_times_{};
   double radius_, point_resolution_, x_length_, y_length_, z_length_, k_theta_, k_beta_, k_x_, link7_length_;

From 89e681edb6d387369c679f38551f7c1a8be4ca47 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 17 Apr 2024 22:41:39 +0800
Subject: [PATCH 067/147] Remove points.

---
 engineer_arm_config/launch/moveit.rviz        | 177 +++++++--
 engineer_middleware/config/steps_list.yaml    | 357 ++++++++++++++++--
 .../include/engineer_middleware/motion.h      | 167 +++-----
 3 files changed, 521 insertions(+), 180 deletions(-)

diff --git a/engineer_arm_config/launch/moveit.rviz b/engineer_arm_config/launch/moveit.rviz
index ab46ed2..36d800a 100644
--- a/engineer_arm_config/launch/moveit.rviz
+++ b/engineer_arm_config/launch/moveit.rviz
@@ -84,16 +84,21 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-            Value: true
           camera_optical_frame:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          drag_link:
+          extend_arm_back:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          gimbal_base:
+            Value: true
+          extend_arm_front:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          gimbal_lifter:
             Alpha: 1
             Show Axes: false
             Show Trail: false
@@ -143,23 +148,26 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
-          mimic_drag_link:
+          mimic_link1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          pitch:
+            Value: true
+          ore_bin_lifter:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          pitch_behind_link:
+          ore_bin_rotator:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          pitch_front_link:
+            Value: true
+          pitch:
             Alpha: 1
             Show Axes: false
             Show Trail: false
+            Value: true
           right_back_wheel:
             Alpha: 1
             Show Axes: false
@@ -170,19 +178,11 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
-          roll_left_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          roll_right_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
           tools_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          tools_link6:
+          tools_link_st:
             Alpha: 1
             Show Axes: false
             Show Trail: false
@@ -246,16 +246,21 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-            Value: true
           camera_optical_frame:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          drag_link:
+          extend_arm_back:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          gimbal_base:
+            Value: true
+          extend_arm_front:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          gimbal_lifter:
             Alpha: 1
             Show Axes: false
             Show Trail: false
@@ -305,23 +310,26 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
-          mimic_drag_link:
+          mimic_link1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          pitch:
+            Value: true
+          ore_bin_lifter:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          pitch_behind_link:
+          ore_bin_rotator:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          pitch_front_link:
+            Value: true
+          pitch:
             Alpha: 1
             Show Axes: false
             Show Trail: false
+            Value: true
           right_back_wheel:
             Alpha: 1
             Show Axes: false
@@ -332,19 +340,11 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
-          roll_left_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          roll_right_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
           tools_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          tools_link6:
+          tools_link_st:
             Alpha: 1
             Show Axes: false
             Show Trail: false
@@ -359,12 +359,68 @@ Visualization Manager:
       Value: true
       Velocity_Scaling_Factor: 0.1
     - Class: rviz/TF
-      Enabled: false
+      Enabled: true
       Filter (blacklist): ""
       Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: false
+        base_link:
+          Value: true
+        camera_link:
+          Value: true
+        camera_optical_frame:
+          Value: true
+        exchanger:
+          Value: true
+        extend_arm_back:
+          Value: true
+        extend_arm_front:
+          Value: true
+        gimbal_lifter:
+          Value: true
+        left_back_wheel:
+          Value: true
+        left_front_wheel:
+          Value: true
+        link1:
+          Value: true
+        link2:
+          Value: true
+        link3:
+          Value: true
+        link4:
+          Value: true
+        link5:
+          Value: true
+        link6:
+          Value: true
+        link7:
+          Value: true
+        map:
+          Value: true
+        mimic_link1:
+          Value: true
+        odom:
+          Value: true
+        ore_bin_lifter:
+          Value: true
+        ore_bin_rotator:
+          Value: true
+        pitch:
+          Value: true
+        real_world:
+          Value: true
+        right_back_wheel:
+          Value: true
+        right_front_wheel:
+          Value: true
+        tools_link:
+          Value: true
+        tools_link_st:
+          Value: true
+        yaw:
+          Value: true
       Marker Alpha: 1
       Marker Scale: 1
       Name: TF
@@ -372,9 +428,52 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        {}
+        map:
+          exchanger:
+            {}
+          odom:
+            base_link:
+              extend_arm_back:
+                {}
+              extend_arm_front:
+                {}
+              gimbal_lifter:
+                camera_link:
+                  {}
+                camera_optical_frame:
+                  {}
+                pitch:
+                  {}
+                yaw:
+                  {}
+              left_back_wheel:
+                {}
+              left_front_wheel:
+                {}
+              link1:
+                link2:
+                  link3:
+                    link4:
+                      link5:
+                        link6:
+                          link7:
+                            tools_link:
+                              {}
+                            tools_link_st:
+                              {}
+              mimic_link1:
+                {}
+              ore_bin_rotator:
+                ore_bin_lifter:
+                  {}
+              real_world:
+                {}
+              right_back_wheel:
+                {}
+              right_front_wheel:
+                {}
       Update Interval: 0
-      Value: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -391,7 +490,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.8833990097045898
+      Distance: 3.3441882133483887
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -407,9 +506,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5947970747947693
+      Pitch: 0.20979738235473633
       Target Frame: base_link
-      Yaw: 1.4263750314712524
+      Yaw: 3.1727137565612793
     Saved: ~
 Window Geometry:
   Displays:
@@ -428,4 +527,4 @@ Window Geometry:
     collapsed: false
   Width: 1665
   X: 757
-  Y: 84
+  Y: 27
diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index d485ff8..018f06c 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -187,13 +187,14 @@ common:
     small_island: &LIFTER_SMALL_ISLAND
       0.1
     mid_pos: &MID_POS
-      0.14
+      0.1
     big_island: &LIFTER_BIG_ISLAND
-      0.260
+
+      0.14
     two_stone_pos: &TWO_STONE_POS
       0.163
     tallest_pos: &TALLEST_POS
-      0.394
+      0.28
 
   gripper:
     open_gripper: &OPEN_GRIPPER
@@ -285,10 +286,8 @@ steps_list:
         target: *BUTTON_POS
     - step: "close ex arm"
       extend_arm_front:
-        target: *EX_CLOSE
-    - step: "close ex arm"
-      extend_arm_back:
-        target: *EX_CLOSE
+        front: *EX_CLOSE
+        back: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -311,10 +310,8 @@ steps_list:
         target: *MID_POS
     - step: "close ex arm"
       extend_arm_front:
-        target: *EX_CLOSE
-    - step: "close ex arm"
-      extend_arm_back:
-        target: *EX_CLOSE
+        front: *EX_CLOSE
+        back: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -337,10 +334,8 @@ steps_list:
         target: *MID_POS
     - step: "close ex arm"
       extend_arm_front:
-        target: *EX_CLOSE
-    - step: "close ex arm"
-      extend_arm_back:
-        target: *EX_CLOSE
+        front: *EX_CLOSE
+        back: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -365,7 +360,8 @@ steps_list:
         <<: *CLOSE_GRIPPER
 
   ################### SMALL_ISLAND_FROM_SIDE ################
-  ONE_STONE_SMALL_ISLAND:
+  ########ONE_STONE_ONCE#########
+  HAS0_ONE_STONE_SMALL_ISLAND:
     - step: "GIMBAL_READY"
       gimbal:
         <<: *SMALL_ISLAND_POS
@@ -386,7 +382,7 @@ steps_list:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
-  ONE_STONE_SMALL_ISLAND0:
+  HAS0_ONE_STONE_SMALL_ISLAND0:
     - step: "SMALL_ARM_READY"
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
@@ -463,6 +459,101 @@ steps_list:
       gimbal:
         <<: *FRONT_POS
 
+  HAS1_ONE_STONE_SMALL_ISLAND:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *SMALL_ISLAND_POS
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "SMALL_ARM_READY"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+  ONE_STONE_SMALL_ISLAND0:
+    - step: "SMALL_ARM_READY"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "SMALL_GET_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP AND BACK"
+      arm:
+        joints: [ *JOINT1_UP_POS, "KEEP", *JOINT3_SMALL_GET_STONE2, "KEEP", "KEEP", "KEEP", "KEEP"]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "SMALL_ARM_UP_STONE"
+      arm:
+        joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE2, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_TO_STORE_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *SMALL_TOLERANCE
+    - step: "STORE_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *SMALL_TOLERANCE
+    - step: "JOINT5_TURN"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *SMALL_TOLERANCE
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "ARM_RESET"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "add stone"
+      stone_num:
+        change: "+1"
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *FRONT_POS
+  ##########TWO_STONE_ONCE###########
   ####FIRST_STONE###
   TWO_STONE_SMALL_ISLAND:
     - step: "GIMBAL_READY"
@@ -734,16 +825,15 @@ steps_list:
         change: "-1"
 
   #################BIG_ISLAND#############
-  MID_BIG_ISLAND:
+  ######MID_STONE#######
+  HAS0_MID_BIG_ISLAND:
     - step: "gimbal ready"
       gimbal:
         <<: *SMALL_ISLAND_POS
     - step: "open ex arm"
-      extend_arm_front:
-        target: *EX_OPEN
-    - step: "open ex arm"
-      extend_arm_back:
-        target: *EX_OPEN
+      extend_arm:
+        front: *EX_OPEN
+        back: *EX_OPEN
     - step: "MID_ARM_READY"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -763,7 +853,7 @@ steps_list:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
-  MID_BIG_ISLAND0:
+  HAS0_MID_BIG_ISLAND0:
     - step: "GET_MID_STONE"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -843,17 +933,117 @@ steps_list:
       gimbal:
         <<: *FRONT_POS
 
-
-  SIDE_BIG_ISLAND:
+  HAS1_MID_BIG_ISLAND:
     - step: "gimbal ready"
       gimbal:
         <<: *SMALL_ISLAND_POS
     - step: "open ex arm"
-      extend_arm_front:
-        target: *EX_OPEN
-#    - step: "open ex arm"
-#      extend_arm_back:
-#        target: *EX_OPEN
+      extend_arm:
+        front: *EX_OPEN
+        back: *EX_OPEN
+    - step: "MID_ARM_READY"
+      arm:
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "gimbal lifter"
+      gimbal_lifter:
+        target: *LIFTER_BIG_ISLAND
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+  HAS1_MID_BIG_ISLAND0:
+    - step: "GET_MID_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ADJUST_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "PULL_OUT_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    #    - step: "chassis move left"
+    #      chassis:
+    #        <<: *CHASSIS_LEFT_15
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ARM_UP_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_TO_STORE"
+      arm:
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "move arm out"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "add stone"
+      stone_num:
+        change: "+1"
+    - step: "gimbal front"
+      gimbal:
+        <<: *FRONT_POS
+  ########SIDE_STONE#########
+  HAS0_SIDE_BIG_ISLAND:
+    - step: "gimbal ready"
+      gimbal:
+        <<: *SMALL_ISLAND_POS
+    - step: "open ex arm"
+      extend_arm:
+        front: *EX_OPEN
     - step: "MID_ARM_READY"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -873,7 +1063,7 @@ steps_list:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
-  SIDE_BIG_ISLAND0:
+  HAS0_SIDE_BIG_ISLAND0:
     - step: "GET_MID_STONE"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -953,6 +1143,109 @@ steps_list:
       gimbal:
         <<: *FRONT_POS
 
+  HAS1_SIDE_BIG_ISLAND:
+    - step: "gimbal ready"
+      gimbal:
+        <<: *SMALL_ISLAND_POS
+    - step: "open ex arm"
+      extend_arm:
+        front: *EX_OPEN
+    - step: "MID_ARM_READY"
+      arm:
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "gimbal lifter"
+      gimbal_lifter:
+        target: *LIFTER_BIG_ISLAND
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+  HAS1_SIDE_BIG_ISLAND0:
+    - step: "GET_MID_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ADJUST_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "PULL_OUT_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    #    - step: "chassis move left"
+    #      chassis:
+    #        <<: *CHASSIS_LEFT_15
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ARM_UP_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_TO_STORE"
+      arm:
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE_STONE"
+      arm:
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "move arm out"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "add stone"
+      stone_num:
+        change: "+1"
+    - step: "gimbal front"
+      gimbal:
+        <<: *FRONT_POS
+
   ####################################################  TEST  ##########################################################
 
   ########## EXCHANGE ############
diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 6369257..a84783a 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -48,7 +48,6 @@
 #include <std_msgs/Int32.h>
 #include <std_msgs/String.h>
 #include <engineer_middleware/chassis_interface.h>
-#include <engineer_middleware/points.h>
 
 namespace engineer_middleware
 {
@@ -113,17 +112,12 @@ class MoveitMotionBase : public MotionBase<moveit::planning_interface::MoveGroup
   {
     return msg_;
   }
-  sensor_msgs::PointCloud2 getPointCloud2()
-  {
-    return points_.getPointCloud2();
-  }
 
 protected:
   virtual bool isReachGoal() = 0;
   double speed_, accel_;
   int countdown_{};
   std_msgs::Int32 msg_;
-  Points points_;
 };
 
 class EndEffectorMotion : public MoveitMotionBase
@@ -236,36 +230,7 @@ class SpaceEeMotion : public EndEffectorMotion
                 tf2_ros::Buffer& tf)
     : EndEffectorMotion(motion, interface, tf)
   {
-    radius_ = xmlRpcGetDouble(motion, "radius", 0.1);
-    link7_length_ = xmlRpcGetDouble(motion, "link7_length", 0.);
-    if (motion.hasMember("basics_length"))
-    {
-      ROS_ASSERT(motion["basics_length"].getType() == XmlRpc::XmlRpcValue::TypeArray);
-      x_length_ = xmlRpcGetDouble(motion["basics_length"], 0);
-      y_length_ = xmlRpcGetDouble(motion["basics_length"], 1);
-      z_length_ = xmlRpcGetDouble(motion["basics_length"], 2);
-    }
-    if (motion.hasMember("rpy_rectify"))
-    {
-      k_x_ = xmlRpcGetDouble(motion["rpy_rectify"], 0);
-      k_theta_ = xmlRpcGetDouble(motion["rpy_rectify"], 1);
-      k_beta_ = xmlRpcGetDouble(motion["rpy_rectify"], 2);
-    }
-    point_resolution_ = xmlRpcGetDouble(motion, "point_resolution", 0.01);
     max_planning_times_ = (int)xmlRpcGetDouble(motion, "max_planning_times", 3);
-    if (motion.hasMember("spacial_shape"))
-    {
-      points_.cleanPoints();
-      if (motion["spacial_shape"] == "SPHERE")
-        points_.setValue(Points::SPHERE, target_.pose.position.x, target_.pose.position.y, target_.pose.position.z,
-                         radius_, point_resolution_);
-      else if (motion["spacial_shape"] == "BASICS")
-        points_.setValue(Points::BASICS, target_.pose.position.x, target_.pose.position.y, target_.pose.position.z,
-                         x_length_, y_length_, z_length_, point_resolution_);
-      else
-        ROS_ERROR("NO SUCH SHAPE");
-      points_.generateGeometryPoints();
-    }
     if (motion.hasMember("is_refer_planning_frame"))
       is_refer_planning_frame_ = motion["is_refer_planning_frame"];
     else
@@ -273,87 +238,72 @@ class SpaceEeMotion : public EndEffectorMotion
   }
   bool move() override
   {
-    points_.cleanPoints();
-    points_.generateGeometryPoints();
     MoveitMotionBase::move();
-    int move_times = (int)points_.getPoints().size();
-    for (int i = 0; i < move_times && i < max_planning_times_; ++i)
+    if (!target_.header.frame_id.empty())
     {
-      if (!target_.header.frame_id.empty())
+      if (!is_refer_planning_frame_)
       {
-        if (!is_refer_planning_frame_)
+        try
         {
-          try
-          {
-            double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
-            geometry_msgs::TransformStamped exchange2base;
-            exchange2base = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
-            quatToRPY(exchange2base.transform.rotation, roll_temp, pitch_temp, yaw_temp);
-            quatToRPY(target_.pose.orientation, roll, pitch, yaw);
-
-            tf2::Quaternion tmp_tf_quaternion;
-            tmp_tf_quaternion.setRPY(roll, pitch, yaw);
-            geometry_msgs::Quaternion quat_tf = tf2::toMsg(tmp_tf_quaternion);
-            quatToRPY(quat_tf, roll, pitch, yaw);
-            points_.rectifyForRPY(pitch_temp, yaw_temp, k_x_, k_theta_, k_beta_);
-            if (link7_length_)
-              points_.rectifyForLink7(pitch_temp, link7_length_);
-            target_.pose.position.x = points_.getPoints()[i].x;
-            target_.pose.position.y = points_.getPoints()[i].y;
-            target_.pose.position.z = points_.getPoints()[i].z;
-            geometry_msgs::PoseStamped temp_target;
-            temp_target.pose.position = target_.pose.position;
-            temp_target.pose.orientation = quat_tf;
-            tf2::doTransform(temp_target.pose, final_target_.pose,
-                             tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0)));
-            final_target_.pose.position.x = final_target_.pose.position.x;
-            final_target_.pose.position.y = final_target_.pose.position.y;
-            final_target_.pose.position.z = final_target_.pose.position.z;
-            final_target_.header.frame_id = interface_.getPlanningFrame();
-          }
-          catch (tf2::TransformException& ex)
-          {
-            ROS_WARN("%s", ex.what());
-            return false;
-          }
+          double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
+          geometry_msgs::TransformStamped exchange2base;
+          exchange2base = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
+          quatToRPY(exchange2base.transform.rotation, roll_temp, pitch_temp, yaw_temp);
+          quatToRPY(target_.pose.orientation, roll, pitch, yaw);
+
+          tf2::Quaternion tmp_tf_quaternion;
+          tmp_tf_quaternion.setRPY(roll, pitch, yaw);
+          geometry_msgs::Quaternion quat_tf = tf2::toMsg(tmp_tf_quaternion);
+          quatToRPY(quat_tf, roll, pitch, yaw);
+          geometry_msgs::PoseStamped temp_target;
+          temp_target.pose.position = target_.pose.position;
+          temp_target.pose.orientation = quat_tf;
+          tf2::doTransform(temp_target.pose, final_target_.pose,
+                           tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0)));
+          final_target_.pose.position.x = final_target_.pose.position.x;
+          final_target_.pose.position.y = final_target_.pose.position.y;
+          final_target_.pose.position.z = final_target_.pose.position.z;
+          final_target_.header.frame_id = interface_.getPlanningFrame();
         }
-        else
+        catch (tf2::TransformException& ex)
+        {
+          ROS_WARN("%s", ex.what());
+          return false;
+        }
+      }
+      else
+      {
+        try
+        {
+          double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
+          geometry_msgs::TransformStamped base2exchange;
+          base2exchange = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
+          quatToRPY(base2exchange.transform.rotation, roll, pitch, yaw);
+          quatToRPY(target_.pose.orientation, roll_temp, pitch_temp, yaw_temp);
+          roll = roll + roll_temp;
+          pitch = pitch + pitch_temp;
+          yaw = yaw + yaw_temp;
+          tf2::Quaternion tf_quaternion;
+          tf_quaternion.setRPY(roll, pitch, yaw);
+          geometry_msgs::Quaternion quat_tf = tf2::toMsg(tf_quaternion);
+
+          final_target_.pose.position.x = base2exchange.transform.translation.x + target_.pose.position.x;
+          final_target_.pose.position.y = base2exchange.transform.translation.y + target_.pose.position.y;
+          final_target_.pose.position.z = base2exchange.transform.translation.z + target_.pose.position.z;
+          final_target_.pose.orientation = quat_tf;
+          final_target_.header.frame_id = interface_.getPlanningFrame();
+        }
+        catch (tf2::TransformException& ex)
         {
-          try
-          {
-            double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
-            geometry_msgs::TransformStamped base2exchange;
-            base2exchange = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
-            target_.pose.position.x = points_.getPoints()[i].x;
-            target_.pose.position.y = points_.getPoints()[i].y;
-            target_.pose.position.z = points_.getPoints()[i].z;
-            quatToRPY(base2exchange.transform.rotation, roll, pitch, yaw);
-            quatToRPY(target_.pose.orientation, roll_temp, pitch_temp, yaw_temp);
-            roll = roll + roll_temp;
-            pitch = pitch + pitch_temp;
-            yaw = yaw + yaw_temp;
-            tf2::Quaternion tf_quaternion;
-            tf_quaternion.setRPY(roll, pitch, yaw);
-            geometry_msgs::Quaternion quat_tf = tf2::toMsg(tf_quaternion);
-
-            final_target_.pose.position.x = base2exchange.transform.translation.x + target_.pose.position.x;
-            final_target_.pose.position.y = base2exchange.transform.translation.y + target_.pose.position.y;
-            final_target_.pose.position.z = base2exchange.transform.translation.z + target_.pose.position.z;
-            final_target_.pose.orientation = quat_tf;
-            final_target_.header.frame_id = interface_.getPlanningFrame();
-          }
-          catch (tf2::TransformException& ex)
-          {
-            ROS_WARN("%s", ex.what());
-            return false;
-          }
+          ROS_WARN("%s", ex.what());
+          return false;
         }
-        interface_.setPoseTarget(final_target_);
-        moveit::planning_interface::MoveGroupInterface::Plan plan;
-        msg_.data = interface_.plan(plan).val;
-        if (msg_.data == 1)
-          return interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
       }
+      interface_.setPoseTarget(final_target_);
+      moveit::planning_interface::MoveGroupInterface::Plan plan;
+      msg_.data = interface_.plan(plan).val;
+      if (msg_.data == 1)
+        return interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
     }
     return false;
   }
@@ -377,7 +327,6 @@ class SpaceEeMotion : public EndEffectorMotion
   bool is_refer_planning_frame_;
   geometry_msgs::PoseStamped final_target_;
   int max_planning_times_{};
-  double radius_, point_resolution_, x_length_, y_length_, z_length_, k_theta_, k_beta_, k_x_, link7_length_;
 };
 
 class JointMotion : public MoveitMotionBase

From ca90f99e6a6c0640e4c1b9bc9949e1a801391e08 Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Wed, 17 Apr 2024 22:46:26 +0800
Subject: [PATCH 068/147] Update extend_arm logic.

---
 .../include/engineer_middleware/motion.h      | 30 +++++--------------
 .../include/engineer_middleware/step.h        |  7 ++---
 2 files changed, 10 insertions(+), 27 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 4a1870b..d5e535d 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -720,33 +720,17 @@ class JointPointMotion : public PublishMotion<std_msgs::Float64>
   double target_;
 };
 
-class ExtendFrontMotion : public PublishMotion<std_msgs::Float64>
+class ExtendMotion : public PublishMotion<std_msgs::Float64>
 {
 public:
-  ExtendFrontMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface)
+  ExtendMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface, bool is_front)
     : PublishMotion<std_msgs::Float64>(motion, interface)
   {
-    ROS_ASSERT(motion.hasMember("front"));
-    target_ = xmlRpcGetDouble(motion, "front", 0.0);
-  }
-  bool move() override
-  {
-    msg_.data = target_;
-    return PublishMotion::move();
-  }
-
-private:
-  double target_;
-};
-
-class ExtendBackMotion : public PublishMotion<std_msgs::Float64>
-{
-public:
-  ExtendBackMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface)
-    : PublishMotion<std_msgs::Float64>(motion, interface)
-  {
-    ROS_ASSERT(motion.hasMember("back"));
-    target_ = xmlRpcGetDouble(motion, "back", 0.0);
+    ROS_ASSERT(motion.hasMember("front") || motion.hasMember("back"));
+    if (is_front)
+      target_ = xmlRpcGetDouble(motion, "front", 0.0);
+    else
+      target_ = xmlRpcGetDouble(motion, "back", 0.0);
   }
   bool move() override
   {
diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h
index 00499f9..253c47c 100644
--- a/engineer_middleware/include/engineer_middleware/step.h
+++ b/engineer_middleware/include/engineer_middleware/step.h
@@ -94,9 +94,9 @@ class Step
     if (step.hasMember("extend_arm"))
     {
       if (step["extend_arm"].hasMember("front"))
-        extend_arm_front_motion_ = new ExtendFrontMotion(step["extend_arm"], extend_arm_f_pub);
+        extend_arm_front_motion_ = new ExtendMotion(step["extend_arm"], extend_arm_f_pub, true);
       if (step["extend_arm"].hasMember("back"))
-        extend_arm_back_motion_ = new ExtendBackMotion(step["extend_arm"], extend_arm_b_pub);
+        extend_arm_back_motion_ = new ExtendMotion(step["extend_arm"], extend_arm_b_pub, false);
     }
   }
   bool move()
@@ -207,8 +207,7 @@ class Step
   HandMotion* hand_motion_{};
   JointPositionMotion* end_effector_motion_{};
   JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{};
-  ExtendFrontMotion *extend_arm_front_motion_{};
-  ExtendBackMotion *extend_arm_back_motion_{};
+  ExtendMotion *extend_arm_front_motion_{}, *extend_arm_back_motion_{};
   StoneNumMotion* stone_num_motion_{};
   ChassisMotion* chassis_motion_{};
   GimbalMotion* gimbal_motion_{};

From a528da8d994be57b64facb276ac91340473a132e Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 17 Apr 2024 23:30:55 +0800
Subject: [PATCH 069/147] Remove points and make space ee motion available.

---
 engineer_arm_config/launch/moveit.rviz        | 62 ++++++++--------
 engineer_middleware/config/steps_list.yaml    | 27 -------
 .../include/engineer_middleware/middleware.h  |  3 +-
 .../include/engineer_middleware/motion.h      | 72 +++++++++++--------
 .../include/engineer_middleware/step.h        | 11 +--
 .../include/engineer_middleware/step_queue.h  |  9 ++-
 engineer_middleware/src/middleware.cpp        |  5 +-
 7 files changed, 84 insertions(+), 105 deletions(-)

diff --git a/engineer_arm_config/launch/moveit.rviz b/engineer_arm_config/launch/moveit.rviz
index 36d800a..bd3c630 100644
--- a/engineer_arm_config/launch/moveit.rviz
+++ b/engineer_arm_config/launch/moveit.rviz
@@ -368,59 +368,59 @@ Visualization Manager:
         base_link:
           Value: true
         camera_link:
-          Value: true
+          Value: false
         camera_optical_frame:
-          Value: true
+          Value: false
         exchanger:
           Value: true
         extend_arm_back:
-          Value: true
+          Value: false
         extend_arm_front:
-          Value: true
+          Value: false
         gimbal_lifter:
-          Value: true
+          Value: false
         left_back_wheel:
-          Value: true
+          Value: false
         left_front_wheel:
-          Value: true
+          Value: false
         link1:
-          Value: true
+          Value: false
         link2:
-          Value: true
+          Value: false
         link3:
-          Value: true
+          Value: false
         link4:
-          Value: true
+          Value: false
         link5:
-          Value: true
+          Value: false
         link6:
-          Value: true
+          Value: false
         link7:
           Value: true
         map:
-          Value: true
+          Value: false
         mimic_link1:
-          Value: true
+          Value: false
         odom:
-          Value: true
+          Value: false
         ore_bin_lifter:
-          Value: true
+          Value: false
         ore_bin_rotator:
-          Value: true
+          Value: false
         pitch:
-          Value: true
+          Value: false
         real_world:
-          Value: true
+          Value: false
         right_back_wheel:
-          Value: true
+          Value: false
         right_front_wheel:
-          Value: true
+          Value: false
         tools_link:
-          Value: true
+          Value: false
         tools_link_st:
-          Value: true
+          Value: false
         yaw:
-          Value: true
+          Value: false
       Marker Alpha: 1
       Marker Scale: 1
       Name: TF
@@ -490,7 +490,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 3.3441882133483887
+      Distance: 2.504984140396118
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -498,17 +498,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: 0.7602222561836243
-        Y: -0.15494105219841003
-        Z: 0.9399657249450684
+        X: 0.23874804377555847
+        Y: 0.04174364358186722
+        Z: 0.6224369406700134
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.20979738235473633
+      Pitch: 0.5297974348068237
       Target Frame: base_link
-      Yaw: 3.1727137565612793
+      Yaw: 2.0677237510681152
     Saved: ~
 Window Geometry:
   Displays:
diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 018f06c..f87bd8e 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -284,10 +284,6 @@ steps_list:
     - step: "gimbal lifter down"
       gimbal_lifter:
         target: *BUTTON_POS
-    - step: "close ex arm"
-      extend_arm_front:
-        front: *EX_CLOSE
-        back: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -308,10 +304,6 @@ steps_list:
     - step: "gimbal lifter down"
       gimbal_lifter:
         target: *MID_POS
-    - step: "close ex arm"
-      extend_arm_front:
-        front: *EX_CLOSE
-        back: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -332,10 +324,6 @@ steps_list:
     - step: "gimbal lifter down"
       gimbal_lifter:
         target: *MID_POS
-    - step: "close ex arm"
-      extend_arm_front:
-        front: *EX_CLOSE
-        back: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -830,10 +818,6 @@ steps_list:
     - step: "gimbal ready"
       gimbal:
         <<: *SMALL_ISLAND_POS
-    - step: "open ex arm"
-      extend_arm:
-        front: *EX_OPEN
-        back: *EX_OPEN
     - step: "MID_ARM_READY"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -937,10 +921,6 @@ steps_list:
     - step: "gimbal ready"
       gimbal:
         <<: *SMALL_ISLAND_POS
-    - step: "open ex arm"
-      extend_arm:
-        front: *EX_OPEN
-        back: *EX_OPEN
     - step: "MID_ARM_READY"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -1041,9 +1021,6 @@ steps_list:
     - step: "gimbal ready"
       gimbal:
         <<: *SMALL_ISLAND_POS
-    - step: "open ex arm"
-      extend_arm:
-        front: *EX_OPEN
     - step: "MID_ARM_READY"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -1147,9 +1124,6 @@ steps_list:
     - step: "gimbal ready"
       gimbal:
         <<: *SMALL_ISLAND_POS
-    - step: "open ex arm"
-      extend_arm:
-        front: *EX_OPEN
     - step: "MID_ARM_READY"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -1321,7 +1295,6 @@ steps_list:
         is_refer_planning_frame: true
         xyz: [ -0.3, 0., 0. ]
         rpy: [ 0., 3.1415926, 0. ]
-        drift_dimensions: [ true, true, true, true, true, true ]
         tolerance_position: 0.01
         tolerance_orientation: 0.001
         spacial_shape: SPHERE
diff --git a/engineer_middleware/include/engineer_middleware/middleware.h b/engineer_middleware/include/engineer_middleware/middleware.h
index cc9b809..439f8f3 100644
--- a/engineer_middleware/include/engineer_middleware/middleware.h
+++ b/engineer_middleware/include/engineer_middleware/middleware.h
@@ -82,8 +82,7 @@ class Middleware
   moveit::planning_interface::MoveGroupInterface arm_group_;
   ChassisInterface chassis_interface_;
   ros::Publisher hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, planning_result_pub_,
-      stone_num_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, extend_arm_f_pub_,
-      extend_arm_b_pub_;
+      stone_num_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, extend_arm_f_pub_, extend_arm_b_pub_;
   std::unordered_map<std::string, StepQueue> step_queues_;
   tf2_ros::Buffer tf_;
   tf2_ros::TransformListener tf_listener_;
diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index a84783a..74403bb 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -230,7 +230,6 @@ class SpaceEeMotion : public EndEffectorMotion
                 tf2_ros::Buffer& tf)
     : EndEffectorMotion(motion, interface, tf)
   {
-    max_planning_times_ = (int)xmlRpcGetDouble(motion, "max_planning_times", 3);
     if (motion.hasMember("is_refer_planning_frame"))
       is_refer_planning_frame_ = motion["is_refer_planning_frame"];
     else
@@ -243,33 +242,40 @@ class SpaceEeMotion : public EndEffectorMotion
     {
       if (!is_refer_planning_frame_)
       {
-        try
-        {
-          double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
-          geometry_msgs::TransformStamped exchange2base;
-          exchange2base = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
-          quatToRPY(exchange2base.transform.rotation, roll_temp, pitch_temp, yaw_temp);
-          quatToRPY(target_.pose.orientation, roll, pitch, yaw);
-
-          tf2::Quaternion tmp_tf_quaternion;
-          tmp_tf_quaternion.setRPY(roll, pitch, yaw);
-          geometry_msgs::Quaternion quat_tf = tf2::toMsg(tmp_tf_quaternion);
-          quatToRPY(quat_tf, roll, pitch, yaw);
-          geometry_msgs::PoseStamped temp_target;
-          temp_target.pose.position = target_.pose.position;
-          temp_target.pose.orientation = quat_tf;
-          tf2::doTransform(temp_target.pose, final_target_.pose,
-                           tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0)));
-          final_target_.pose.position.x = final_target_.pose.position.x;
-          final_target_.pose.position.y = final_target_.pose.position.y;
-          final_target_.pose.position.z = final_target_.pose.position.z;
-          final_target_.header.frame_id = interface_.getPlanningFrame();
-        }
-        catch (tf2::TransformException& ex)
-        {
-          ROS_WARN("%s", ex.what());
-          return false;
-        }
+        //            try
+        //            {
+        //              double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
+        //              geometry_msgs::TransformStamped exchange2base;
+        //              exchange2base = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
+        //              quatToRPY(exchange2base.transform.rotation, roll_temp, pitch_temp, yaw_temp);
+        //              quatToRPY(target_.pose.orientation, roll, pitch, yaw);
+        //
+        //              tf2::Quaternion tmp_tf_quaternion;
+        //              tmp_tf_quaternion.setRPY(roll, pitch, yaw);
+        //              geometry_msgs::Quaternion quat_tf = tf2::toMsg(tmp_tf_quaternion);
+        //              quatToRPY(quat_tf, roll, pitch, yaw);
+        //              points_.rectifyForRPY(pitch_temp, yaw_temp, k_x_, k_theta_, k_beta_);
+        //              if (link7_length_)
+        //                points_.rectifyForLink7(pitch_temp, link7_length_);
+        //              target_.pose.position.x = points_.getPoints()[i].x;
+        //              target_.pose.position.y = points_.getPoints()[i].y;
+        //              target_.pose.position.z = points_.getPoints()[i].z;
+        //              geometry_msgs::PoseStamped temp_target;
+        //              temp_target.pose.position = target_.pose.position;
+        //              temp_target.pose.orientation = quat_tf;
+        //              tf2::doTransform(temp_target.pose, final_target_.pose,
+        //                               tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id,
+        //                               ros::Time(0)));
+        //              final_target_.pose.position.x = final_target_.pose.position.x;
+        //              final_target_.pose.position.y = final_target_.pose.position.y;
+        //              final_target_.pose.position.z = final_target_.pose.position.z;
+        //              final_target_.header.frame_id = interface_.getPlanningFrame();
+        //            }
+        //            catch (tf2::TransformException& ex)
+        //            {
+        //              ROS_WARN("%s", ex.what());
+        //              return false;
+        //            }
       }
       else
       {
@@ -278,11 +284,17 @@ class SpaceEeMotion : public EndEffectorMotion
           double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
           geometry_msgs::TransformStamped base2exchange;
           base2exchange = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
+          target_.pose.position.x = target_.pose.position.x;
+          target_.pose.position.y = target_.pose.position.y;
+          target_.pose.position.z = target_.pose.position.z;
           quatToRPY(base2exchange.transform.rotation, roll, pitch, yaw);
+          ROS_INFO_STREAM("base2exchange roll: " << roll);
           quatToRPY(target_.pose.orientation, roll_temp, pitch_temp, yaw_temp);
+          ROS_INFO_STREAM("target roll: " << roll_temp);
           roll = roll + roll_temp;
           pitch = pitch + pitch_temp;
           yaw = yaw + yaw_temp;
+          ROS_INFO_STREAM("roll added: " << roll);
           tf2::Quaternion tf_quaternion;
           tf_quaternion.setRPY(roll, pitch, yaw);
           geometry_msgs::Quaternion quat_tf = tf2::toMsg(tf_quaternion);
@@ -292,6 +304,9 @@ class SpaceEeMotion : public EndEffectorMotion
           final_target_.pose.position.z = base2exchange.transform.translation.z + target_.pose.position.z;
           final_target_.pose.orientation = quat_tf;
           final_target_.header.frame_id = interface_.getPlanningFrame();
+          double rolll, pitchh, yaww;
+          quatToRPY(final_target_.pose.orientation, rolll, pitchh, yaww);
+          ROS_INFO_STREAM("transformed roll: " << rolll);
         }
         catch (tf2::TransformException& ex)
         {
@@ -326,7 +341,6 @@ class SpaceEeMotion : public EndEffectorMotion
   }
   bool is_refer_planning_frame_;
   geometry_msgs::PoseStamped final_target_;
-  int max_planning_times_{};
 };
 
 class JointMotion : public MoveitMotionBase
diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h
index b94ea72..f21ff9e 100644
--- a/engineer_middleware/include/engineer_middleware/step.h
+++ b/engineer_middleware/include/engineer_middleware/step.h
@@ -50,9 +50,9 @@ class Step
        moveit::planning_interface::MoveGroupInterface& arm_group, ChassisInterface& chassis_interface,
        ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub,
        ros::Publisher& reversal_pub, ros::Publisher& stone_num_pub, ros::Publisher& planning_result_pub,
-       ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub,
-       ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub)
-    : planning_result_pub_(planning_result_pub), point_cloud_pub_(point_cloud_pub), arm_group_(arm_group)
+       ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub,
+       ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub)
+    : planning_result_pub_(planning_result_pub), arm_group_(arm_group)
   {
     ROS_ASSERT(step.hasMember("step"));
     step_name_ = static_cast<std::string>(step["step"]);
@@ -102,11 +102,6 @@ class Step
     if (arm_motion_)
     {
       success &= arm_motion_->move();
-      if (!arm_motion_->getPointCloud2().data.empty())
-      {
-        sensor_msgs::PointCloud2 point_cloud2 = arm_motion_->getPointCloud2();
-        point_cloud_pub_.publish(point_cloud2);
-      }
       std_msgs::Int32 msg = arm_motion_->getPlanningResult();
       planning_result_pub_.publish(msg);
     }
diff --git a/engineer_middleware/include/engineer_middleware/step_queue.h b/engineer_middleware/include/engineer_middleware/step_queue.h
index d2fa912..c020932 100644
--- a/engineer_middleware/include/engineer_middleware/step_queue.h
+++ b/engineer_middleware/include/engineer_middleware/step_queue.h
@@ -55,16 +55,15 @@ class StepQueue
             moveit::planning_interface::MoveGroupInterface& arm_group, ChassisInterface& chassis_interface,
             ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& stone_num_pub,
             ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub, ros::Publisher& reversal_pub,
-            ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub,
-            ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub,
-            ros::Publisher& extend_arm_b_pub)
+            ros::Publisher& planning_result_pub, ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub,
+            ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub)
     : chassis_interface_(chassis_interface)
   {
     ROS_ASSERT(steps.getType() == XmlRpc::XmlRpcValue::TypeArray);
     for (int i = 0; i < steps.size(); ++i)
       queue_.emplace_back(steps[i], scenes, tf, arm_group, chassis_interface, hand_pub, end_effector_pub, stone_num_pub,
-                          gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, point_cloud_pub, ore_rotate_pub,
-                          ore_lift_pub, gimbal_lift_pub, extend_arm_f_pub, extend_arm_b_pub);
+                          gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, ore_rotate_pub, ore_lift_pub,
+                          gimbal_lift_pub, extend_arm_f_pub, extend_arm_b_pub);
   }
   bool run(actionlib::SimpleActionServer<rm_msgs::EngineerAction>& as)
   {
diff --git a/engineer_middleware/src/middleware.cpp b/engineer_middleware/src/middleware.cpp
index 3ef4368..a27b7a1 100644
--- a/engineer_middleware/src/middleware.cpp
+++ b/engineer_middleware/src/middleware.cpp
@@ -52,7 +52,6 @@ Middleware::Middleware(ros::NodeHandle& nh)
   , reversal_pub_(nh.advertise<rm_msgs::MultiDofCmd>("/controllers/multi_dof_controller/command", 10))
   , planning_result_pub_(nh.advertise<std_msgs::Int32>("/planning_result", 10))
   , stone_num_pub_(nh.advertise<std_msgs::String>("/stone_num", 10))
-  , point_cloud_pub_(nh.advertise<sensor_msgs::PointCloud2>("/cloud", 100))
   , ore_rotate_pub_(nh.advertise<std_msgs::Float64>("/controllers/ore_bin_rotate_controller/command", 10))
   , ore_lift_pub_(nh.advertise<std_msgs::Float64>("/controllers/ore_bin_lifter_controller/command", 10))
   , gimbal_lift_pub_(nh.advertise<std_msgs::Float64>("/controllers/gimbal_lifter_controller/command", 10))
@@ -74,8 +73,8 @@ Middleware::Middleware(ros::NodeHandle& nh)
       step_queues_.insert(
           std::make_pair(it->first, StepQueue(it->second, scenes_list, tf_, arm_group_, chassis_interface_, hand_pub_,
                                               end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, stone_num_pub_,
-                                              planning_result_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_,
-                                              gimbal_lift_pub_, extend_arm_f_pub_, extend_arm_b_pub_)));
+                                              planning_result_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_,
+                                              extend_arm_f_pub_, extend_arm_b_pub_)));
     }
   }
   else

From 9ddcfb05024e55fe922581bda86afee929606858 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 18 Apr 2024 22:27:56 +0800
Subject: [PATCH 070/147] record.

---
 engineer_arm_config/launch/moveit.rviz        | 102 ++++++++++--------
 engineer_middleware/config/steps_list.yaml    |   3 -
 .../include/engineer_middleware/motion.h      |  36 +++++--
 3 files changed, 85 insertions(+), 56 deletions(-)

diff --git a/engineer_arm_config/launch/moveit.rviz b/engineer_arm_config/launch/moveit.rviz
index bd3c630..a0a4838 100644
--- a/engineer_arm_config/launch/moveit.rviz
+++ b/engineer_arm_config/launch/moveit.rviz
@@ -428,50 +428,60 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        map:
-          exchanger:
+        base_link:
+          {}
+        exchanger:
+          {}
+        extend_arm_back:
+          {}
+        extend_arm_front:
+          {}
+        gimbal_lifter:
+          camera_link:
+            {}
+          camera_optical_frame:
+            {}
+        left_back_wheel:
+          {}
+        left_front_wheel:
+          {}
+        link1:
+          {}
+        link2:
+          {}
+        link3:
+          {}
+        link4:
+          {}
+        link5:
+          {}
+        link6:
+          {}
+        link7:
+          tools_link:
             {}
-          odom:
-            base_link:
-              extend_arm_back:
-                {}
-              extend_arm_front:
-                {}
-              gimbal_lifter:
-                camera_link:
-                  {}
-                camera_optical_frame:
-                  {}
-                pitch:
-                  {}
-                yaw:
-                  {}
-              left_back_wheel:
-                {}
-              left_front_wheel:
-                {}
-              link1:
-                link2:
-                  link3:
-                    link4:
-                      link5:
-                        link6:
-                          link7:
-                            tools_link:
-                              {}
-                            tools_link_st:
-                              {}
-              mimic_link1:
-                {}
-              ore_bin_rotator:
-                ore_bin_lifter:
-                  {}
-              real_world:
-                {}
-              right_back_wheel:
-                {}
-              right_front_wheel:
-                {}
+          tools_link_st:
+            {}
+        map:
+          {}
+        mimic_link1:
+          {}
+        odom:
+          {}
+        ore_bin_lifter:
+          {}
+        ore_bin_rotator:
+          {}
+        pitch:
+          {}
+        real_world:
+          {}
+        right_back_wheel:
+          {}
+        right_front_wheel:
+          {}
+        yaw:
+          {}
       Update Interval: 0
       Value: true
   Enabled: true
@@ -490,7 +500,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2.504984140396118
+      Distance: 1.4108070135116577
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -506,9 +516,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5297974348068237
+      Pitch: 0.48979732394218445
       Target Frame: base_link
-      Yaw: 2.0677237510681152
+      Yaw: 3.2827558517456055
     Saved: ~
 Window Geometry:
   Displays:
diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index f87bd8e..49318cc 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -1250,9 +1250,6 @@ steps_list:
         drift_dimensions: [ true, true, true, true, true, true ]
         tolerance_position: 0.01
         tolerance_orientation: 0.001
-        spacial_shape: SPHERE
-        radius: 0.3
-        point_resolution: 0.5
         max_planning_times: 1
         common:
           <<: *NORMALLY
diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 74403bb..8aef38c 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -291,18 +291,39 @@ class SpaceEeMotion : public EndEffectorMotion
           ROS_INFO_STREAM("base2exchange roll: " << roll);
           quatToRPY(target_.pose.orientation, roll_temp, pitch_temp, yaw_temp);
           ROS_INFO_STREAM("target roll: " << roll_temp);
-          roll = roll + roll_temp;
-          pitch = pitch + pitch_temp;
-          yaw = yaw + yaw_temp;
-          ROS_INFO_STREAM("roll added: " << roll);
+          quat_base2exchange_.setW(base2exchange.transform.rotation.w);
+          quat_base2exchange_.setX(base2exchange.transform.rotation.x);
+          quat_base2exchange_.setY(base2exchange.transform.rotation.y);
+          quat_base2exchange_.setZ(base2exchange.transform.rotation.z);
+          ROS_INFO_STREAM("base2exchange quat w: " << quat_base2exchange_.w());
+          ROS_INFO_STREAM("base2exchange quat x: " << quat_base2exchange_.x());
+          ROS_INFO_STREAM("base2exchange quat y: " << quat_base2exchange_.y());
+          ROS_INFO_STREAM("base2exchange quat z: " << quat_base2exchange_.z());
+
+          quat_target_.setW(target_.pose.orientation.w);
+          quat_target_.setX(target_.pose.orientation.x);
+          quat_target_.setY(target_.pose.orientation.y);
+          quat_target_.setZ(target_.pose.orientation.z);
+          ROS_INFO_STREAM("target quat w: " << quat_target_.w());
+          ROS_INFO_STREAM("target quat x: " << quat_target_.x());
+          ROS_INFO_STREAM("target quat y: " << quat_target_.y());
+          ROS_INFO_STREAM("target quat z: " << quat_target_.z());
+
           tf2::Quaternion tf_quaternion;
-          tf_quaternion.setRPY(roll, pitch, yaw);
-          geometry_msgs::Quaternion quat_tf = tf2::toMsg(tf_quaternion);
+          tf_quaternion = quat_target_ * quat_base2exchange_;
 
           final_target_.pose.position.x = base2exchange.transform.translation.x + target_.pose.position.x;
           final_target_.pose.position.y = base2exchange.transform.translation.y + target_.pose.position.y;
           final_target_.pose.position.z = base2exchange.transform.translation.z + target_.pose.position.z;
-          final_target_.pose.orientation = quat_tf;
+          final_target_.pose.orientation.w = tf_quaternion.w();
+          final_target_.pose.orientation.x = tf_quaternion.x();
+          final_target_.pose.orientation.y = tf_quaternion.y();
+          final_target_.pose.orientation.z = tf_quaternion.z();
+          ROS_INFO_STREAM("final target w: " << final_target_.pose.orientation.w);
+          ROS_INFO_STREAM("final target x: " << final_target_.pose.orientation.x);
+          ROS_INFO_STREAM("final target y: " << final_target_.pose.orientation.y);
+          ROS_INFO_STREAM("final target z: " << final_target_.pose.orientation.z);
+
           final_target_.header.frame_id = interface_.getPlanningFrame();
           double rolll, pitchh, yaww;
           quatToRPY(final_target_.pose.orientation, rolll, pitchh, yaww);
@@ -341,6 +362,7 @@ class SpaceEeMotion : public EndEffectorMotion
   }
   bool is_refer_planning_frame_;
   geometry_msgs::PoseStamped final_target_;
+  tf2::Quaternion quat_base2exchange_, quat_target_;
 };
 
 class JointMotion : public MoveitMotionBase

From 86c5ecce73d0e5c115adb7967a345442e4a84177 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 20 Apr 2024 17:10:00 +0800
Subject: [PATCH 071/147] modify quaternion multiplication order.

---
 engineer_middleware/config/steps_list.yaml    | 20 +++----------------
 .../include/engineer_middleware/motion.h      |  2 +-
 2 files changed, 4 insertions(+), 18 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 49318cc..d236696 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -1245,7 +1245,7 @@ steps_list:
       arm:
         frame: exchanger
         is_refer_planning_frame: true
-        xyz: [ -0.3, 0.2, -0.2 ]
+        xyz: [ -0.2, 0., 0. ]
         rpy: [ 0., -1.5707963, 0. ]
         drift_dimensions: [ true, true, true, true, true, true ]
         tolerance_position: 0.01
@@ -1270,27 +1270,13 @@ steps_list:
         tolerance_position: 0.01
         tolerance_orientation: 0.001
 
-  EXCHANGE_ORI:
-    - step: "test new exchange"
-      arm:
-        frame: exchanger
-        is_refer_planning_frame: true
-        rpy: [ 0., -1.5707963, 0. ]
-        drift_dimensions: [ true, true, true, true, true, true ]
-        tolerance_position: 0.01
-        tolerance_orientation: 0.001
-        spacial_shape: SPHERE
-        radius: 0.5
-        point_resolution: 0.5
-        max_planning_times: 1
-        common:
-          <<: *NORMALLY
+
   EXCHANGE_STRAIGHT:
     - step: "test new exchange"
       arm:
         frame: exchanger
         is_refer_planning_frame: true
-        xyz: [ -0.3, 0., 0. ]
+        xyz: [ -0.2, 0., 0. ]
         rpy: [ 0., 3.1415926, 0. ]
         tolerance_position: 0.01
         tolerance_orientation: 0.001
diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 8aef38c..dd5b48f 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -310,7 +310,7 @@ class SpaceEeMotion : public EndEffectorMotion
           ROS_INFO_STREAM("target quat z: " << quat_target_.z());
 
           tf2::Quaternion tf_quaternion;
-          tf_quaternion = quat_target_ * quat_base2exchange_;
+          tf_quaternion = quat_base2exchange_ * quat_target_;
 
           final_target_.pose.position.x = base2exchange.transform.translation.x + target_.pose.position.x;
           final_target_.pose.position.y = base2exchange.transform.translation.y + target_.pose.position.y;

From 9fd7f4195d1cad7159a126b96bf30e7ed85d1c53 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 20 Apr 2024 17:17:31 +0800
Subject: [PATCH 072/147] fix format.

---
 engineer_middleware/config/steps_list.yaml | 1 -
 1 file changed, 1 deletion(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 427219b..d3d1a0a 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -1349,4 +1349,3 @@ steps_list:
     - step: "gimbal tall"
       gimbal_lifter:
         target: *TALLEST_POS
-

From 44ecbf84d2e2fbc5ff6eb3bfddf99fd742621263 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 20 Apr 2024 17:17:53 +0800
Subject: [PATCH 073/147] fix format.

---
 engineer_middleware/config/steps_list.yaml | 1 -
 1 file changed, 1 deletion(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 427219b..d3d1a0a 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -1349,4 +1349,3 @@ steps_list:
     - step: "gimbal tall"
       gimbal_lifter:
         target: *TALLEST_POS
-

From 11e44202501b32700069feb76e3ce58cb796a164 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 20 Apr 2024 20:22:08 +0800
Subject: [PATCH 074/147] Available auto exchange.

---
 .../include/engineer_middleware/motion.h           | 14 --------------
 1 file changed, 14 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index dd5b48f..e237922 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -288,26 +288,16 @@ class SpaceEeMotion : public EndEffectorMotion
           target_.pose.position.y = target_.pose.position.y;
           target_.pose.position.z = target_.pose.position.z;
           quatToRPY(base2exchange.transform.rotation, roll, pitch, yaw);
-          ROS_INFO_STREAM("base2exchange roll: " << roll);
           quatToRPY(target_.pose.orientation, roll_temp, pitch_temp, yaw_temp);
-          ROS_INFO_STREAM("target roll: " << roll_temp);
           quat_base2exchange_.setW(base2exchange.transform.rotation.w);
           quat_base2exchange_.setX(base2exchange.transform.rotation.x);
           quat_base2exchange_.setY(base2exchange.transform.rotation.y);
           quat_base2exchange_.setZ(base2exchange.transform.rotation.z);
-          ROS_INFO_STREAM("base2exchange quat w: " << quat_base2exchange_.w());
-          ROS_INFO_STREAM("base2exchange quat x: " << quat_base2exchange_.x());
-          ROS_INFO_STREAM("base2exchange quat y: " << quat_base2exchange_.y());
-          ROS_INFO_STREAM("base2exchange quat z: " << quat_base2exchange_.z());
 
           quat_target_.setW(target_.pose.orientation.w);
           quat_target_.setX(target_.pose.orientation.x);
           quat_target_.setY(target_.pose.orientation.y);
           quat_target_.setZ(target_.pose.orientation.z);
-          ROS_INFO_STREAM("target quat w: " << quat_target_.w());
-          ROS_INFO_STREAM("target quat x: " << quat_target_.x());
-          ROS_INFO_STREAM("target quat y: " << quat_target_.y());
-          ROS_INFO_STREAM("target quat z: " << quat_target_.z());
 
           tf2::Quaternion tf_quaternion;
           tf_quaternion = quat_base2exchange_ * quat_target_;
@@ -319,10 +309,6 @@ class SpaceEeMotion : public EndEffectorMotion
           final_target_.pose.orientation.x = tf_quaternion.x();
           final_target_.pose.orientation.y = tf_quaternion.y();
           final_target_.pose.orientation.z = tf_quaternion.z();
-          ROS_INFO_STREAM("final target w: " << final_target_.pose.orientation.w);
-          ROS_INFO_STREAM("final target x: " << final_target_.pose.orientation.x);
-          ROS_INFO_STREAM("final target y: " << final_target_.pose.orientation.y);
-          ROS_INFO_STREAM("final target z: " << final_target_.pose.orientation.z);
 
           final_target_.header.frame_id = interface_.getPlanningFrame();
           double rolll, pitchh, yaww;

From 016e7bedf3eefac59503b294b9c2e8863f8f0972 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sun, 21 Apr 2024 18:39:20 +0800
Subject: [PATCH 075/147] Add points.

---
 .../include/engineer_middleware/middleware.h  |   3 +-
 .../include/engineer_middleware/motion.h      | 182 ++++++++++--------
 .../include/engineer_middleware/step.h        |  11 +-
 .../include/engineer_middleware/step_queue.h  |   9 +-
 engineer_middleware/src/middleware.cpp        |   5 +-
 5 files changed, 124 insertions(+), 86 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/middleware.h b/engineer_middleware/include/engineer_middleware/middleware.h
index 439f8f3..cc9b809 100644
--- a/engineer_middleware/include/engineer_middleware/middleware.h
+++ b/engineer_middleware/include/engineer_middleware/middleware.h
@@ -82,7 +82,8 @@ class Middleware
   moveit::planning_interface::MoveGroupInterface arm_group_;
   ChassisInterface chassis_interface_;
   ros::Publisher hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, planning_result_pub_,
-      stone_num_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, extend_arm_f_pub_, extend_arm_b_pub_;
+      stone_num_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, extend_arm_f_pub_,
+      extend_arm_b_pub_;
   std::unordered_map<std::string, StepQueue> step_queues_;
   tf2_ros::Buffer tf_;
   tf2_ros::TransformListener tf_listener_;
diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index c8038e1..44cd7f9 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -48,6 +48,7 @@
 #include <std_msgs/Int32.h>
 #include <std_msgs/String.h>
 #include <engineer_middleware/chassis_interface.h>
+#include <engineer_middleware/points.h>
 
 namespace engineer_middleware
 {
@@ -112,12 +113,17 @@ class MoveitMotionBase : public MotionBase<moveit::planning_interface::MoveGroup
   {
     return msg_;
   }
+  sensor_msgs::PointCloud2 getPointCloud2()
+  {
+    return points_.getPointCloud2();
+  }
 
 protected:
   virtual bool isReachGoal() = 0;
   double speed_, accel_;
   int countdown_{};
   std_msgs::Int32 msg_;
+  Points points_;
 };
 
 class EndEffectorMotion : public MoveitMotionBase
@@ -230,95 +236,117 @@ class SpaceEeMotion : public EndEffectorMotion
                 tf2_ros::Buffer& tf)
     : EndEffectorMotion(motion, interface, tf)
   {
+    point_resolution_ = xmlRpcGetDouble(motion, "point_resolution", 0.01);
+    radius_ = xmlRpcGetDouble(motion, "radius", 0.1);
+    max_planning_times_ = (int)xmlRpcGetDouble(motion, "max_planning_times", 3);
     if (motion.hasMember("is_refer_planning_frame"))
       is_refer_planning_frame_ = motion["is_refer_planning_frame"];
     else
       is_refer_planning_frame_ = false;
+    if (motion.hasMember("spacial_shape"))
+    {
+      points_.cleanPoints();
+      if (motion["spacial_shape"] == "SPHERE")
+        points_.setValue(Points::SPHERE, target_.pose.position.x, target_.pose.position.y, target_.pose.position.z,
+                         radius_, point_resolution_);
+      else if (motion["spacial_shape"] == "BASICS")
+        points_.setValue(Points::BASICS, target_.pose.position.x, target_.pose.position.y, target_.pose.position.z,
+                         x_length_, y_length_, z_length_, point_resolution_);
+      else
+        ROS_ERROR("NO SUCH SHAPE");
+      points_.generateGeometryPoints();
+    }
   }
   bool move() override
   {
+    points_.cleanPoints();
+    points_.generateGeometryPoints();
     MoveitMotionBase::move();
-    if (!target_.header.frame_id.empty())
+    int move_times = (int)points_.getPoints().size();
+    for (int i = 0; i < move_times && i < max_planning_times_; ++i)
     {
-      if (!is_refer_planning_frame_)
+      if (!target_.header.frame_id.empty())
       {
-        //            try
-        //            {
-        //              double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
-        //              geometry_msgs::TransformStamped exchange2base;
-        //              exchange2base = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
-        //              quatToRPY(exchange2base.transform.rotation, roll_temp, pitch_temp, yaw_temp);
-        //              quatToRPY(target_.pose.orientation, roll, pitch, yaw);
-        //
-        //              tf2::Quaternion tmp_tf_quaternion;
-        //              tmp_tf_quaternion.setRPY(roll, pitch, yaw);
-        //              geometry_msgs::Quaternion quat_tf = tf2::toMsg(tmp_tf_quaternion);
-        //              quatToRPY(quat_tf, roll, pitch, yaw);
-        //              points_.rectifyForRPY(pitch_temp, yaw_temp, k_x_, k_theta_, k_beta_);
-        //              if (link7_length_)
-        //                points_.rectifyForLink7(pitch_temp, link7_length_);
-        //              target_.pose.position.x = points_.getPoints()[i].x;
-        //              target_.pose.position.y = points_.getPoints()[i].y;
-        //              target_.pose.position.z = points_.getPoints()[i].z;
-        //              geometry_msgs::PoseStamped temp_target;
-        //              temp_target.pose.position = target_.pose.position;
-        //              temp_target.pose.orientation = quat_tf;
-        //              tf2::doTransform(temp_target.pose, final_target_.pose,
-        //                               tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id,
-        //                               ros::Time(0)));
-        //              final_target_.pose.position.x = final_target_.pose.position.x;
-        //              final_target_.pose.position.y = final_target_.pose.position.y;
-        //              final_target_.pose.position.z = final_target_.pose.position.z;
-        //              final_target_.header.frame_id = interface_.getPlanningFrame();
-        //            }
-        //            catch (tf2::TransformException& ex)
-        //            {
-        //              ROS_WARN("%s", ex.what());
-        //              return false;
-        //            }
-      }
-      else
-      {
-        try
+        if (!is_refer_planning_frame_)
         {
-          double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
-          geometry_msgs::TransformStamped base2exchange;
-          base2exchange = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
-          target_.pose.position.x = target_.pose.position.x;
-          target_.pose.position.y = target_.pose.position.y;
-          target_.pose.position.z = target_.pose.position.z;
-          quatToRPY(base2exchange.transform.rotation, roll, pitch, yaw);
-          quatToRPY(target_.pose.orientation, roll_temp, pitch_temp, yaw_temp);
-          quat_base2exchange_.setW(base2exchange.transform.rotation.w);
-          quat_base2exchange_.setX(base2exchange.transform.rotation.x);
-          quat_base2exchange_.setY(base2exchange.transform.rotation.y);
-          quat_base2exchange_.setZ(base2exchange.transform.rotation.z);
-
-          quat_target_.setW(target_.pose.orientation.w);
-          quat_target_.setX(target_.pose.orientation.x);
-          quat_target_.setY(target_.pose.orientation.y);
-          quat_target_.setZ(target_.pose.orientation.z);
-
-          tf2::Quaternion tf_quaternion;
-          tf_quaternion = quat_base2exchange_ * quat_target_;
-
-          final_target_.pose.position.x = base2exchange.transform.translation.x + target_.pose.position.x;
-          final_target_.pose.position.y = base2exchange.transform.translation.y + target_.pose.position.y;
-          final_target_.pose.position.z = base2exchange.transform.translation.z + target_.pose.position.z;
-          final_target_.pose.orientation.w = tf_quaternion.w();
-          final_target_.pose.orientation.x = tf_quaternion.x();
-          final_target_.pose.orientation.y = tf_quaternion.y();
-          final_target_.pose.orientation.z = tf_quaternion.z();
-
-          final_target_.header.frame_id = interface_.getPlanningFrame();
-          double rolll, pitchh, yaww;
-          quatToRPY(final_target_.pose.orientation, rolll, pitchh, yaww);
-          ROS_INFO_STREAM("transformed roll: " << rolll);
+          //            try
+          //            {
+          //              double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
+          //              geometry_msgs::TransformStamped exchange2base;
+          //              exchange2base = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
+          //              quatToRPY(exchange2base.transform.rotation, roll_temp, pitch_temp, yaw_temp);
+          //              quatToRPY(target_.pose.orientation, roll, pitch, yaw);
+          //
+          //              tf2::Quaternion tmp_tf_quaternion;
+          //              tmp_tf_quaternion.setRPY(roll, pitch, yaw);
+          //              geometry_msgs::Quaternion quat_tf = tf2::toMsg(tmp_tf_quaternion);
+          //              quatToRPY(quat_tf, roll, pitch, yaw);
+          //              points_.rectifyForRPY(pitch_temp, yaw_temp, k_x_, k_theta_, k_beta_);
+          //              if (link7_length_)
+          //                points_.rectifyForLink7(pitch_temp, link7_length_);
+          //              target_.pose.position.x = points_.getPoints()[i].x;
+          //              target_.pose.position.y = points_.getPoints()[i].y;
+          //              target_.pose.position.z = points_.getPoints()[i].z;
+          //              geometry_msgs::PoseStamped temp_target;
+          //              temp_target.pose.position = target_.pose.position;
+          //              temp_target.pose.orientation = quat_tf;
+          //              tf2::doTransform(temp_target.pose, final_target_.pose,
+          //                               tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id,
+          //                               ros::Time(0)));
+          //              final_target_.pose.position.x = final_target_.pose.position.x;
+          //              final_target_.pose.position.y = final_target_.pose.position.y;
+          //              final_target_.pose.position.z = final_target_.pose.position.z;
+          //              final_target_.header.frame_id = interface_.getPlanningFrame();
+          //            }
+          //            catch (tf2::TransformException& ex)
+          //            {
+          //              ROS_WARN("%s", ex.what());
+          //              return false;
+          //            }
         }
-        catch (tf2::TransformException& ex)
+        else
         {
-          ROS_WARN("%s", ex.what());
-          return false;
+          try
+          {
+            double roll, pitch, yaw, roll_temp, pitch_temp, yaw_temp;
+            geometry_msgs::TransformStamped base2exchange;
+            base2exchange = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
+            target_.pose.position.x = points_.getPoints()[i].x;
+            target_.pose.position.y = points_.getPoints()[i].y;
+            target_.pose.position.z = points_.getPoints()[i].z;
+            quatToRPY(base2exchange.transform.rotation, roll, pitch, yaw);
+            quatToRPY(target_.pose.orientation, roll_temp, pitch_temp, yaw_temp);
+            quat_base2exchange_.setW(base2exchange.transform.rotation.w);
+            quat_base2exchange_.setX(base2exchange.transform.rotation.x);
+            quat_base2exchange_.setY(base2exchange.transform.rotation.y);
+            quat_base2exchange_.setZ(base2exchange.transform.rotation.z);
+
+            quat_target_.setW(target_.pose.orientation.w);
+            quat_target_.setX(target_.pose.orientation.x);
+            quat_target_.setY(target_.pose.orientation.y);
+            quat_target_.setZ(target_.pose.orientation.z);
+
+            tf2::Quaternion tf_quaternion;
+            tf_quaternion = quat_base2exchange_ * quat_target_;
+
+            final_target_.pose.position.x = base2exchange.transform.translation.x + target_.pose.position.x;
+            final_target_.pose.position.y = base2exchange.transform.translation.y + target_.pose.position.y;
+            final_target_.pose.position.z = base2exchange.transform.translation.z + target_.pose.position.z;
+            final_target_.pose.orientation.w = tf_quaternion.w();
+            final_target_.pose.orientation.x = tf_quaternion.x();
+            final_target_.pose.orientation.y = tf_quaternion.y();
+            final_target_.pose.orientation.z = tf_quaternion.z();
+
+            final_target_.header.frame_id = interface_.getPlanningFrame();
+            double rolll, pitchh, yaww;
+            quatToRPY(final_target_.pose.orientation, rolll, pitchh, yaww);
+            ROS_INFO_STREAM("transformed roll: " << rolll);
+          }
+          catch (tf2::TransformException& ex)
+          {
+            ROS_WARN("%s", ex.what());
+            return false;
+          }
         }
       }
       interface_.setPoseTarget(final_target_);
@@ -349,6 +377,8 @@ class SpaceEeMotion : public EndEffectorMotion
   bool is_refer_planning_frame_;
   geometry_msgs::PoseStamped final_target_;
   tf2::Quaternion quat_base2exchange_, quat_target_;
+  int max_planning_times_{};
+  double radius_, point_resolution_, x_length_, y_length_, z_length_;
 };
 
 class JointMotion : public MoveitMotionBase
diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h
index 886cc6d..253c47c 100644
--- a/engineer_middleware/include/engineer_middleware/step.h
+++ b/engineer_middleware/include/engineer_middleware/step.h
@@ -50,9 +50,9 @@ class Step
        moveit::planning_interface::MoveGroupInterface& arm_group, ChassisInterface& chassis_interface,
        ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub,
        ros::Publisher& reversal_pub, ros::Publisher& stone_num_pub, ros::Publisher& planning_result_pub,
-       ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub,
-       ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub)
-    : planning_result_pub_(planning_result_pub), arm_group_(arm_group)
+       ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub,
+       ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub)
+    : planning_result_pub_(planning_result_pub), point_cloud_pub_(point_cloud_pub), arm_group_(arm_group)
   {
     ROS_ASSERT(step.hasMember("step"));
     step_name_ = static_cast<std::string>(step["step"]);
@@ -105,6 +105,11 @@ class Step
     if (arm_motion_)
     {
       success &= arm_motion_->move();
+      if (!arm_motion_->getPointCloud2().data.empty())
+      {
+        sensor_msgs::PointCloud2 point_cloud2 = arm_motion_->getPointCloud2();
+        point_cloud_pub_.publish(point_cloud2);
+      }
       std_msgs::Int32 msg = arm_motion_->getPlanningResult();
       planning_result_pub_.publish(msg);
     }
diff --git a/engineer_middleware/include/engineer_middleware/step_queue.h b/engineer_middleware/include/engineer_middleware/step_queue.h
index c020932..d2fa912 100644
--- a/engineer_middleware/include/engineer_middleware/step_queue.h
+++ b/engineer_middleware/include/engineer_middleware/step_queue.h
@@ -55,15 +55,16 @@ class StepQueue
             moveit::planning_interface::MoveGroupInterface& arm_group, ChassisInterface& chassis_interface,
             ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& stone_num_pub,
             ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub, ros::Publisher& reversal_pub,
-            ros::Publisher& planning_result_pub, ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub,
-            ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub)
+            ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub,
+            ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub,
+            ros::Publisher& extend_arm_b_pub)
     : chassis_interface_(chassis_interface)
   {
     ROS_ASSERT(steps.getType() == XmlRpc::XmlRpcValue::TypeArray);
     for (int i = 0; i < steps.size(); ++i)
       queue_.emplace_back(steps[i], scenes, tf, arm_group, chassis_interface, hand_pub, end_effector_pub, stone_num_pub,
-                          gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, ore_rotate_pub, ore_lift_pub,
-                          gimbal_lift_pub, extend_arm_f_pub, extend_arm_b_pub);
+                          gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, point_cloud_pub, ore_rotate_pub,
+                          ore_lift_pub, gimbal_lift_pub, extend_arm_f_pub, extend_arm_b_pub);
   }
   bool run(actionlib::SimpleActionServer<rm_msgs::EngineerAction>& as)
   {
diff --git a/engineer_middleware/src/middleware.cpp b/engineer_middleware/src/middleware.cpp
index a27b7a1..3ef4368 100644
--- a/engineer_middleware/src/middleware.cpp
+++ b/engineer_middleware/src/middleware.cpp
@@ -52,6 +52,7 @@ Middleware::Middleware(ros::NodeHandle& nh)
   , reversal_pub_(nh.advertise<rm_msgs::MultiDofCmd>("/controllers/multi_dof_controller/command", 10))
   , planning_result_pub_(nh.advertise<std_msgs::Int32>("/planning_result", 10))
   , stone_num_pub_(nh.advertise<std_msgs::String>("/stone_num", 10))
+  , point_cloud_pub_(nh.advertise<sensor_msgs::PointCloud2>("/cloud", 100))
   , ore_rotate_pub_(nh.advertise<std_msgs::Float64>("/controllers/ore_bin_rotate_controller/command", 10))
   , ore_lift_pub_(nh.advertise<std_msgs::Float64>("/controllers/ore_bin_lifter_controller/command", 10))
   , gimbal_lift_pub_(nh.advertise<std_msgs::Float64>("/controllers/gimbal_lifter_controller/command", 10))
@@ -73,8 +74,8 @@ Middleware::Middleware(ros::NodeHandle& nh)
       step_queues_.insert(
           std::make_pair(it->first, StepQueue(it->second, scenes_list, tf_, arm_group_, chassis_interface_, hand_pub_,
                                               end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, stone_num_pub_,
-                                              planning_result_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_,
-                                              extend_arm_f_pub_, extend_arm_b_pub_)));
+                                              planning_result_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_,
+                                              gimbal_lift_pub_, extend_arm_f_pub_, extend_arm_b_pub_)));
     }
   }
   else

From 6e22eea2b041f6bbec50e0c3c0fd262edb37026c Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sun, 21 Apr 2024 18:39:45 +0800
Subject: [PATCH 076/147] Update steps list.

---
 engineer_middleware/config/steps_list.yaml | 522 ++++++++++++++-------
 1 file changed, 365 insertions(+), 157 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index d236696..96a1678 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -28,11 +28,15 @@ common:
       down_position: &JOINT1_DOWN_POSITION
                        0.02
       small_ready: &JOINT1_SMALL_READY
-                      0.16
+                      0.475
+      small_down: &JOINT1_SMALL_DOWN
+                  0.31
+      small_ready_store: &JOINT1_SMALL_READY_STORE
+                           0.249
+      small_ready_store_third: &JOINT1_SMALL_READY_STORE_THIRD
+                         0.387
       up_pos: &JOINT1_UP_POS
                    0.312
-      small_ready_store: &JOINT1_SMALL_READY_STORE
-                   0.17
       bin_get_stone: &JOINT1_BIN_GET_STONE
                      0.142
       big_ready: &JOINT1_BIG_READY
@@ -63,12 +67,8 @@ common:
                            0.232
       exchange_position: &JOINT2_EXCHANGE_POSITION
                           0.07
-      small_arm_up: &JOINT2_SMALL_ARM_UP
-                            0.085
-      small_store_stone: &JOINT2_SMALL_STORE_STONE
-                              0.02
-      small_ready_second: &JOINT2_SMALL_READY_SECOND
-                              0.142
+      small_ready: &JOINT2_SMALL_READY
+                      0.025
       bin_get_stone: &JOINT2_BIN_GET_STONE
                             0.087
       back_bin_position: &JOINT2_BACK_BIN_POSITION
@@ -90,18 +90,14 @@ common:
                         -0.275
       home: &JOINT3_HOME_POS
         0.08
-      small_ready: &JOINT3_SMALL_READY
-                        0.208
-      small_ready_second: &JOINT3_SMALL_READY_SECOND
-                        0.03
-      small_get_stone: &JOINT3_SMALL_GET_STONE
-                            0.047
-      small_get_stone2: &JOINT3_SMALL_GET_STONE2
-                            0.056
-      small_get_second: &JOINT3_SMALL_GET_SECOND
-                            -0.018
-      small_store_stone: &JOINT3_SMALL_STORE_STONE
-                              0.291
+      small_ready_mid: &JOINT3_SMALL_READY_MID
+                        0.037
+      small_ready_left: &JOINT3_SMALL_READY_LEFT
+                        0.308
+      small_ready_right: &JOINT3_SMALL_READY_RIGHT
+                        -0.233
+      small_turn: &JOINT3_SMALL_TURN
+                   0.257
       bin_get_stone: &JOINT3_BIN_GET_STONE
                           0.258
       big_ready: &JOINT3_BIG_READY
@@ -123,8 +119,10 @@ common:
                       0.001
       right_position: &JOINT4_R_POSITION
                         -1.495
-      small_ready_second: &JOINT4_SMALL_READY_SECOND
-                        -0.777
+      small_ready: &JOINT4_SMALL_READY
+                        0.054
+      small_turn: &JOINT4_SMALL_TURN
+                  -1.497
   joint5:
     mechanical:
       mid_position: &JOINT5_MID_POSITION
@@ -139,6 +137,12 @@ common:
                   -0.017
       big_store: &JOINT5_BIG_STORE
                   1.617
+      small_ready: &JOINT5_SMALL_READY
+                  -3.128
+      small_up_stone: &JOINT5_SMALL_UP_STONE
+                    -0.001
+      small_ready_store: &JOINT5_SMALL_READY_STORE
+                    1.615
 
   joint6:
     mechanical:
@@ -148,8 +152,12 @@ common:
                      -1.569
       down_position: &JOINT6_DOWN_POSITION
                        1.569
-      small_ready_second: &JOINT6_SMALL_READY_SECOND
-                       0.694
+      small_ready: &JOINT6_SMALL_READY
+                       -1.607
+      small_back: &JOINT6_SMALL_BACK
+                   -1.987
+      small_ready_store: &JOINT6_SMALL_READY_STORE
+                   -1.633
       bin_get_stone: &JOINT6_BIN_GET_STONE
                       -1.611
       big_ready: &JOINT6_BIG_READY
@@ -169,6 +177,10 @@ common:
                            1.62
       big_pos: &JOINT7_BIG_POS
                          -0.013
+      small_ready: &JOINT7_SMALL_READY
+                         0.012
+      small_ready_store: &JOINT7_SMALL_READY_STORE
+                          1.722
 
   gimbal:
     side_pos: &SIDE_POS
@@ -187,14 +199,13 @@ common:
     small_island: &LIFTER_SMALL_ISLAND
       0.1
     mid_pos: &MID_POS
-      0.1
-    big_island: &LIFTER_BIG_ISLAND
-
       0.14
+    big_island: &LIFTER_BIG_ISLAND
+      0.260
     two_stone_pos: &TWO_STONE_POS
       0.163
     tallest_pos: &TALLEST_POS
-      0.28
+      0.394
 
   gripper:
     open_gripper: &OPEN_GRIPPER
@@ -284,6 +295,10 @@ steps_list:
     - step: "gimbal lifter down"
       gimbal_lifter:
         target: *BUTTON_POS
+    - step: "close ex arm"
+      extend_arm_front:
+        front: *EX_CLOSE
+        back: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -304,6 +319,10 @@ steps_list:
     - step: "gimbal lifter down"
       gimbal_lifter:
         target: *MID_POS
+    - step: "close ex arm"
+      extend_arm_front:
+        front: *EX_CLOSE
+        back: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -324,6 +343,10 @@ steps_list:
     - step: "gimbal lifter down"
       gimbal_lifter:
         target: *MID_POS
+    - step: "close ex arm"
+      extend_arm_front:
+        front: *EX_CLOSE
+        back: *EX_CLOSE
     - step: "close gripper"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -347,215 +370,284 @@ steps_list:
       gripper:
         <<: *CLOSE_GRIPPER
 
-  ################### SMALL_ISLAND_FROM_SIDE ################
+  ################### SMALL_ISLAND_FROM_UP ################
   ########ONE_STONE_ONCE#########
-  HAS0_ONE_STONE_SMALL_ISLAND:
+  ONE_STONE_SMALL_ISLAND:
     - step: "GIMBAL_READY"
       gimbal:
-        <<: *SMALL_ISLAND_POS
+        <<: *FRONT_POS
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *READY_POS
     - step: "ORE_LIFTER_READY"
       ore_lifter:
         target: *BIN_MID_POS
-    - step: "SMALL_ARM_READY"
+    - step: "ARM_READY"
       arm:
-        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
-  HAS0_ONE_STONE_SMALL_ISLAND0:
-    - step: "SMALL_ARM_READY"
+  ONE_STONE_SMALL_ISLAND0:
+    - step: "GET_STONE"
       arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "SMALL_GET_STONE"
+    - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "JOINT1_UP AND BACK"
+    - step: "ARM_UP_STONE"
       arm:
-        joints: [ *JOINT1_UP_POS, "KEEP", *JOINT3_SMALL_GET_STONE2, "KEEP", "KEEP", "KEEP", "KEEP"]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "SMALL_ARM_UP_STONE"
+    - step: "JOINT6_BACK"
       arm:
-        joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE2, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "READY_TO_STORE_STONE"
+    - step: "TURN_STONE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *SMALL_TOLERANCE
-    - step: "STORE_STONE"
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
-          <<: *SMALL_TOLERANCE
-    - step: "JOINT5_TURN"
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
-          <<: *SMALL_TOLERANCE
+          <<: *NORMAL_TOLERANCE
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "ARM_RESET"
+    - step: "MOVE_ARM_OUT"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ORE_LIFTER_DOWN"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "ADD_STONE"
+      stone_num:
+        change: "+1"
+
+  #######TWO_STONE_ONCE#########
+    ######FIRST_STONE########
+  TWO_STONE_SMALL_ISLAND:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_MID_POS
+    - step: "ARM_READY"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "home"
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+  TWO_STONE_SMALL_ISLAND0:
+    - step: "GET_STONE"
       arm:
-        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ARM_UP_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT6_BACK"
+      arm:
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "TURN_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_STORE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "MOVE_ARM_OUT"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
-    - step: "add stone"
+    - step: "ADD_STONE"
       stone_num:
         change: "+1"
-    - step: "GIMBAL_READY"
-      gimbal:
-        <<: *FRONT_POS
 
-  HAS1_ONE_STONE_SMALL_ISLAND:
+    #####SECOND_STONE########
+#  TWO_STONE_SMALL_ISLAND00:
     - step: "GIMBAL_READY"
       gimbal:
-        <<: *SMALL_ISLAND_POS
+        <<: *FRONT_POS
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *READY_POS
     - step: "ORE_LIFTER_READY"
       ore_lifter:
         target: *BIN_DOWN_POS
-    - step: "SMALL_ARM_READY"
+    - step: "ARM_READY"
       arm:
-        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
-  ONE_STONE_SMALL_ISLAND0:
-    - step: "SMALL_ARM_READY"
+#  TWO_STONE_SMALL_ISLAND000:
+    - step: "GET_STONE"
       arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "SMALL_GET_STONE"
+    - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "JOINT1_UP AND BACK"
+    - step: "ARM_UP_STONE"
       arm:
-        joints: [ *JOINT1_UP_POS, "KEEP", *JOINT3_SMALL_GET_STONE2, "KEEP", "KEEP", "KEEP", "KEEP"]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "SMALL_ARM_UP_STONE"
+    - step: "JOINT6_BACK"
       arm:
-        joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE2, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "READY_TO_STORE_STONE"
+    - step: "TURN_STONE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *SMALL_TOLERANCE
-    - step: "STORE_STONE"
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
-          <<: *SMALL_TOLERANCE
-    - step: "JOINT5_TURN"
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
-          <<: *SMALL_TOLERANCE
+          <<: *NORMAL_TOLERANCE
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "ARM_RESET"
+    - step: "MOVE_ARM_OUT"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "home"
-      arm:
-        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
-        common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "add stone"
+    - step: "ADD_STONE"
       stone_num:
         change: "+1"
+
+  ######THREE_STONE_ONCE(yi jian san lian)#########
+    ######FIRST_STONE########
+  THREE_STONE_SMALL_ISLAND:
     - step: "GIMBAL_READY"
       gimbal:
         <<: *FRONT_POS
-  ##########TWO_STONE_ONCE###########
-  ####FIRST_STONE###
-  TWO_STONE_SMALL_ISLAND:
-    - step: "GIMBAL_READY"
-      gimbal:
-        <<: *SMALL_ISLAND_POS
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *READY_POS
     - step: "ORE_LIFTER_READY"
       ore_lifter:
         target: *BIN_MID_POS
-    - step: "SMALL_ARM_READY"
+    - step: "ARM_READY"
       arm:
-        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -563,77 +655,87 @@ steps_list:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
-  TWO_STONE_SMALL_ISLAND0:
-    - step: "SMALL_ARM_READY"
+  THREE_STONE_SMALL_ISLAND0:
+    - step: "GET_STONE"
       arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_READY, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "SMALL_GET_STONE"
+    - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_BACK_POSITION, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "JOINT1_UP"
+    - step: "ARM_UP_STONE"
       arm:
-        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "SMALL_ARM_UP_STONE"
+    - step: "JOINT6_BACK"
       arm:
-        joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "READY_TO_STORE_STONE"
+    - step: "TURN_STONE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *SMALL_TOLERANCE
-    - step: "STORE_STONE"
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
-          <<: *SMALL_TOLERANCE
-    - step: "JOINT5_TURN"
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
-          <<: *SMALL_TOLERANCE
+          <<: *NORMAL_TOLERANCE
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "ARM_RESET"
+    - step: "MOVE_ARM_OUT"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
-    - step: "add stone"
+    - step: "ADD_STONE"
       stone_num:
         change: "+1"
-  ###SECOND_STONE###
-  TWO_STONE_SMALL_ISLAND00:
-    - step: "SMALL_ARM_READY_TWO"
+
+    #####SECOND_STONE########
+#  THREE_STONE_SMALL_ISLAND00:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "ARM_READY"
       arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY_SECOND, *JOINT3_SMALL_READY_SECOND, *JOINT4_SMALL_READY_SECOND, *JOINT5_L90_POSITION, *JOINT6_SMALL_READY_SECOND, *JOINT7_R90_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -641,70 +743,155 @@ steps_list:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
-  TWO_STONE_SMALL_ISLAND000:
-    - step: "SMALL_GET_STONE_TWO"
+#  THREE_STONE_SMALL_ISLAND000:
+    - step: "GET_STONE"
       arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY_SECOND, *JOINT3_SMALL_GET_SECOND, *JOINT4_SMALL_READY_SECOND, *JOINT5_L90_POSITION, *JOINT6_SMALL_READY_SECOND, *JOINT7_R90_POSITION ]
+        joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *SMALL_TOLERANCE
+          <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP"]
+        joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "SMALL_ARM_LIFT_STONE"
+    - step: "ARM_UP_STONE"
       arm:
-        joints: [ *JOINT1_UP_POS, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_GET_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "READY_TO_STORE_STONE"
+    - step: "JOINT6_BACK"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_ARM_UP, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *SMALL_TOLERANCE
-    - step: "STORE_STONE"
+          <<: *NORMAL_TOLERANCE
+    - step: "TURN_STONE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *SMALL_TOLERANCE
-    - step: "JOINT5"
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_SMALL_STORE_STONE, *JOINT4_R_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *SMALL_TOLERANCE
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "ARM_RESET"
+    - step: "MOVE_ARM_OUT"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ADD_STONE"
+      stone_num:
+        change: "+1"
+    #######THIRD_STONE#########
+#  THREE_STONE_SMALL_ISLAND0000:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "ARM_READY"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE_STONE, *JOINT3_L_POSITION, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_MID_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_RIGHT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "home"
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+#  THREE_STONE_SMALL_ISLAND00000:
+    - step: "GET_STONE"
       arm:
-        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_RIGHT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "add stone"
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ARM_UP_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_RIGHT, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT6_BACK"
+      arm:
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "TURN_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_STORE(THIRD_STONE)"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+#    - step: "CLOSE_GRIPPER"
+#      gripper:
+#        <<: *CLOSE_GRIPPER
+#    - step: "MOVE_ARM_OUT"
+#      arm:
+#        joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+#        common:
+#          <<: *SLOWLY
+#        tolerance:
+#          <<: *NORMAL_TOLERANCE
+    - step: "ADD_STONE"
       stone_num:
         change: "+1"
 
+  ######GET STONE FROM BIN#########
   GET_DOWN_STONE_BIN:
     - step: "GIMBAL_READY"
       gimbal:
@@ -818,6 +1005,10 @@ steps_list:
     - step: "gimbal ready"
       gimbal:
         <<: *SMALL_ISLAND_POS
+    - step: "open ex arm"
+      extend_arm:
+        front: *EX_OPEN
+        back: *EX_OPEN
     - step: "MID_ARM_READY"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -917,10 +1108,16 @@ steps_list:
       gimbal:
         <<: *FRONT_POS
 
+
   HAS1_MID_BIG_ISLAND:
     - step: "gimbal ready"
       gimbal:
         <<: *SMALL_ISLAND_POS
+    - step: "open ex arm"
+
+      extend_arm:
+        front: *EX_OPEN
+        back: *EX_OPEN
     - step: "MID_ARM_READY"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -1021,6 +1218,10 @@ steps_list:
     - step: "gimbal ready"
       gimbal:
         <<: *SMALL_ISLAND_POS
+    - step: "open ex arm"
+      extend_arm:
+        front: *EX_OPEN
+
     - step: "MID_ARM_READY"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -1040,6 +1241,7 @@ steps_list:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
+
   HAS0_SIDE_BIG_ISLAND0:
     - step: "GET_MID_STONE"
       arm:
@@ -1124,6 +1326,9 @@ steps_list:
     - step: "gimbal ready"
       gimbal:
         <<: *SMALL_ISLAND_POS
+    - step: "open ex arm"
+      extend_arm:
+        front: *EX_OPEN
     - step: "MID_ARM_READY"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -1250,6 +1455,9 @@ steps_list:
         drift_dimensions: [ true, true, true, true, true, true ]
         tolerance_position: 0.01
         tolerance_orientation: 0.001
+        spacial_shape: SPHERE
+        radius: 0.3
+        point_resolution: 0.5
         max_planning_times: 1
         common:
           <<: *NORMALLY
@@ -1276,7 +1484,7 @@ steps_list:
       arm:
         frame: exchanger
         is_refer_planning_frame: true
-        xyz: [ -0.2, 0., 0. ]
+        xyz: [ -0.3, 0., 0. ]
         rpy: [ 0., 3.1415926, 0. ]
         tolerance_position: 0.01
         tolerance_orientation: 0.001

From f32d12f95404fd31b599b02336ea8debd81810b0 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 24 Apr 2024 01:05:54 +0800
Subject: [PATCH 077/147] Record.

---
 engineer_middleware/config/steps_list.yaml | 410 +++++----------------
 1 file changed, 90 insertions(+), 320 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 96a1678..a84a101 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -17,7 +17,7 @@ common:
     small_tolerance: &SMALL_TOLERANCE
       tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.05 ]
     normal_tolerance: &NORMAL_TOLERANCE
-      tolerance_joints: [ 0.01, 0.010, 0.015, 0.3, 0.2, 0.15, 0.1 ]
+      tolerance_joints: [ 0.01, 0.010, 0.015, 0.3, 0.2, 0.15, 0.07 ]
     bigger_tolerance: &BIGGER_TOLERANCE
       tolerance_joints: [ 0.005, 0.010, 0.01, 0.3, 0.2, 0.2 ]
     max_tolerance: &MAX_TOLERANCE
@@ -28,11 +28,11 @@ common:
       down_position: &JOINT1_DOWN_POSITION
                        0.02
       small_ready: &JOINT1_SMALL_READY
-                      0.475
+                      0.480
       small_down: &JOINT1_SMALL_DOWN
                   0.31
       small_ready_store: &JOINT1_SMALL_READY_STORE
-                           0.249
+                           0.254
       small_ready_store_third: &JOINT1_SMALL_READY_STORE_THIRD
                          0.387
       up_pos: &JOINT1_UP_POS
@@ -46,7 +46,7 @@ common:
       big_up_pos: &JOINT1_BIG_UP_POS
                       0.320
       big_ready_store: &JOINT1_BIG_READY_STORE
-                      0.155
+                      0.233
     home:
       zero_stone_position: &JOINT1_ZERO_STONE_POSITION
                              0.01
@@ -68,30 +68,30 @@ common:
       exchange_position: &JOINT2_EXCHANGE_POSITION
                           0.07
       small_ready: &JOINT2_SMALL_READY
-                      0.025
+                      0.034
       bin_get_stone: &JOINT2_BIN_GET_STONE
                             0.087
       back_bin_position: &JOINT2_BACK_BIN_POSITION
                              0.001
       big_ready: &JOINT2_BIG_READY
-                       0.009
+                       0.03
       big_arm_up: &JOINT2_BIG_ARM_UP
                     0.142
       big_ready_store: &JOINT2_BIG_READY_STORE
-                      0.020
+                      0.03
 
   joint3:
     mechanical:
       left_position: &JOINT3_L_POSITION
-                       0.28
+                       0.308
       mid_position: &JOINT3_MID_POSITION
                       0.04
       right_position: &JOINT3_R_POSITION
-                        -0.275
+                        -0.278
       home: &JOINT3_HOME_POS
         0.08
       small_ready_mid: &JOINT3_SMALL_READY_MID
-                        0.037
+                        0.032
       small_ready_left: &JOINT3_SMALL_READY_LEFT
                         0.308
       small_ready_right: &JOINT3_SMALL_READY_RIGHT
@@ -103,13 +103,13 @@ common:
       big_ready: &JOINT3_BIG_READY
                      0.071
       get_big_mid: &JOINT3_GET_BIG_MID
-                      -0.281
+                      -0.292
       get_big_side: &JOINT3_GET_BIG_SIDE
                      -0.199
       big_pull_out: &JOINT3_BIG_PULL_OUT
                      0.188
       big_arm_up: &JOINT3_BIG_ARM_UP
-                    0.291
+                    0.267
 
   joint4:
     mechanical:
@@ -118,11 +118,13 @@ common:
       mid_position: &JOINT4_MID_POSITION
                       0.001
       right_position: &JOINT4_R_POSITION
-                        -1.495
+                        -1.52
       small_ready: &JOINT4_SMALL_READY
                         0.054
       small_turn: &JOINT4_SMALL_TURN
                   -1.497
+      big_store: &JOINT4_BIG_STORE
+                   -1.525
   joint5:
     mechanical:
       mid_position: &JOINT5_MID_POSITION
@@ -135,13 +137,15 @@ common:
                            1.679
       big_ready: &JOINT5_BIG_READY
                   -0.017
+      big_ready_store: &JOINT5_BIG_READY_STORE
+                 -0.001
       big_store: &JOINT5_BIG_STORE
-                  1.617
+                  1.49
       small_ready: &JOINT5_SMALL_READY
                   -3.128
       small_up_stone: &JOINT5_SMALL_UP_STONE
                     -0.001
-      small_ready_store: &JOINT5_SMALL_READY_STORE
+      small_store: &JOINT5_SMALL_STORE
                     1.615
 
   joint6:
@@ -166,6 +170,8 @@ common:
                        -0.119
       big_arm_up: &JOINT6_BIG_ARM_UP
                        -1.519
+      big_ready_store: &JOINT6_BIG_READY_STORE
+                      -1.561
 
   joint7:
     mechanical:
@@ -177,6 +183,8 @@ common:
                            1.62
       big_pos: &JOINT7_BIG_POS
                          -0.013
+      big_ready_store: &JOINT7_BIG_READY_STORE
+                          -1.522
       small_ready: &JOINT7_SMALL_READY
                          0.012
       small_ready_store: &JOINT7_SMALL_READY_STORE
@@ -195,17 +203,17 @@ common:
 
   gimbal_lifter:
     button_pos: &BUTTON_POS
-      0.07
+      0.05
     small_island: &LIFTER_SMALL_ISLAND
-      0.1
+      0.05
     mid_pos: &MID_POS
       0.14
     big_island: &LIFTER_BIG_ISLAND
       0.260
     two_stone_pos: &TWO_STONE_POS
-      0.163
+      0.05
     tallest_pos: &TALLEST_POS
-      0.394
+      0.28
 
   gripper:
     open_gripper: &OPEN_GRIPPER
@@ -216,15 +224,15 @@ common:
       state: false
 
   arm:
-    home: &HOME_1
-      joints: [ "VARIABLE", *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+    home: &HOME
+      joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
 
 
-  chassis_move:
+  chassis:
     chassis_left_315: &CHASSIS_LEFT_15
       frame: base_link
       position: [ 0., 15. ]
@@ -288,7 +296,7 @@ steps_list:
   ############ MID #############
 
   ############################ HOME ##########################
-  HOME_ZERO_STONE:
+  HOME:
     - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
@@ -296,55 +304,7 @@ steps_list:
       gimbal_lifter:
         target: *BUTTON_POS
     - step: "close ex arm"
-      extend_arm_front:
-        front: *EX_CLOSE
-        back: *EX_CLOSE
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "ORE_ROTATOR_READY"
-      ore_rotator:
-        target: *READY_POS
-    - step: "ORE_LIFTER_UP"
-      ore_lifter:
-        target: *BIN_DOWN_POS
-    - step: "home with no stone"
-      arm:
-        <<: *HOME_1
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
-  HOME_ONE_STONE:
-    - step: "gimbal look front"
-      gimbal:
-        <<: *FRONT_POS
-    - step: "gimbal lifter down"
-      gimbal_lifter:
-        target: *MID_POS
-    - step: "close ex arm"
-      extend_arm_front:
-        front: *EX_CLOSE
-        back: *EX_CLOSE
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "ORE_ROTATOR_READY"
-      ore_rotator:
-        target: *READY_POS
-    - step: "ORE_LIFTER_UP"
-      ore_lifter:
-        target: *BIN_DOWN_POS
-    - step: "home with one stone"
-      arm:
-        <<: *HOME_1
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
-  HOME_TWO_STONE:
-    - step: "gimbal look front"
-      gimbal:
-        <<: *FRONT_POS
-    - step: "gimbal lifter down"
-      gimbal_lifter:
-        target: *MID_POS
-    - step: "close ex arm"
-      extend_arm_front:
+      extend_arm:
         front: *EX_CLOSE
         back: *EX_CLOSE
     - step: "close gripper"
@@ -356,10 +316,9 @@ steps_list:
     - step: "ORE_LIFTER_UP"
       ore_lifter:
         target: *BIN_DOWN_POS
-    - step: "home with two stone"
+    - step: "home"
       arm:
-        <<: *HOME_1
-        variable: [ *JOINT1_ZERO_STONE_POSITION, 0, 0, 0, 0, 0 ]
+        <<: *HOME
 
   OPEN_GRIPPER:
     - step: "open gripper"
@@ -437,11 +396,12 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+#  ONE_STONE_SMALL_ISLAND00:
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -452,14 +412,17 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        <<: *HOME
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
     - step: "ADD_STONE"
       stone_num:
-        change: "+1"
+        change: "SILVER"
 
-  #######TWO_STONE_ONCE#########
+  ########################## TWO_STONE_ONCE ########################
     ######FIRST_STONE########
   TWO_STONE_SMALL_ISLAND:
     - step: "GIMBAL_READY"
@@ -481,6 +444,9 @@ steps_list:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
+    - step: "gimbal lifter ready"
+      gimbal_lifter:
+        target: *BUTTON_POS
   TWO_STONE_SMALL_ISLAND0:
     - step: "GET_STONE"
       arm:
@@ -526,7 +492,7 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -546,7 +512,7 @@ steps_list:
         target: *BIN_DOWN_POS
     - step: "ADD_STONE"
       stone_num:
-        change: "+1"
+        change: "SILVER"
 
     #####SECOND_STONE########
 #  TWO_STONE_SMALL_ISLAND00:
@@ -614,7 +580,7 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -629,9 +595,12 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "home"
+      arm:
+        <<: *HOME
     - step: "ADD_STONE"
       stone_num:
-        change: "+1"
+        change: "SILVER"
 
   ######THREE_STONE_ONCE(yi jian san lian)#########
     ######FIRST_STONE########
@@ -700,7 +669,7 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -720,7 +689,7 @@ steps_list:
         target: *BIN_DOWN_POS
     - step: "ADD_STONE"
       stone_num:
-        change: "+1"
+        change: "SILVER"
 
     #####SECOND_STONE########
 #  THREE_STONE_SMALL_ISLAND00:
@@ -788,7 +757,7 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -805,7 +774,7 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "ADD_STONE"
       stone_num:
-        change: "+1"
+        change: "SILVER"
     #######THIRD_STONE#########
 #  THREE_STONE_SMALL_ISLAND0000:
     - step: "GIMBAL_READY"
@@ -872,7 +841,7 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_READY_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -889,7 +858,7 @@ steps_list:
 #          <<: *NORMAL_TOLERANCE
     - step: "ADD_STONE"
       stone_num:
-        change: "+1"
+        change: "SILVER"
 
   ######GET STONE FROM BIN#########
   GET_DOWN_STONE_BIN:
@@ -1001,7 +970,7 @@ steps_list:
 
   #################BIG_ISLAND#############
   ######MID_STONE#######
-  HAS0_MID_BIG_ISLAND:
+  MID_BIG_ISLAND:
     - step: "gimbal ready"
       gimbal:
         <<: *SMALL_ISLAND_POS
@@ -1028,7 +997,7 @@ steps_list:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
-  HAS0_MID_BIG_ISLAND0:
+  MID_BIG_ISLAND0:
     - step: "GET_MID_STONE"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -1062,25 +1031,26 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "ARM_UP_STONE"
       arm:
-        joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE"
       arm:
-        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE_STONE"
       arm:
-        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+#  MID_BIG_ISLAND00:
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -1093,128 +1063,19 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "home"
       arm:
-        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
+        <<: *HOME
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
     - step: "add stone"
       stone_num:
-        change: "+1"
+        change: "GOLD"
     - step: "gimbal front"
       gimbal:
         <<: *FRONT_POS
 
-
-  HAS1_MID_BIG_ISLAND:
-    - step: "gimbal ready"
-      gimbal:
-        <<: *SMALL_ISLAND_POS
-    - step: "open ex arm"
-
-      extend_arm:
-        front: *EX_OPEN
-        back: *EX_OPEN
-    - step: "MID_ARM_READY"
-      arm:
-        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "ORE_ROTATOR_READY"
-      ore_rotator:
-        target: *READY_POS
-    - step: "ORE_LIFTER_READY"
-      ore_lifter:
-        target: *BIN_DOWN_POS
-    - step: "gimbal lifter"
-      gimbal_lifter:
-        target: *LIFTER_BIG_ISLAND
-    - step: "OPEN_GRIPPER"
-      gripper:
-        <<: *OPEN_GRIPPER
-  HAS1_MID_BIG_ISLAND0:
-    - step: "GET_MID_STONE"
-      arm:
-        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "ADJUST_STONE"
-      arm:
-        joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "PULL_OUT_STONE"
-      arm:
-        joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    #    - step: "chassis move left"
-    #      chassis:
-    #        <<: *CHASSIS_LEFT_15
-    - step: "JOINT1_UP"
-      arm:
-        joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "ARM_UP_STONE"
-      arm:
-        joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "READY_TO_STORE"
-      arm:
-        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "STORE_STONE"
-      arm:
-        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "CLOSE_GRIPPER"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "move arm out"
-      arm:
-        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "home"
-      arm:
-        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "add stone"
-      stone_num:
-        change: "+1"
-    - step: "gimbal front"
-      gimbal:
-        <<: *FRONT_POS
   ########SIDE_STONE#########
-  HAS0_SIDE_BIG_ISLAND:
+  SIDE_BIG_ISLAND:
     - step: "gimbal ready"
       gimbal:
         <<: *SMALL_ISLAND_POS
@@ -1242,7 +1103,7 @@ steps_list:
       gripper:
         <<: *OPEN_GRIPPER
 
-  HAS0_SIDE_BIG_ISLAND0:
+  SIDE_BIG_ISLAND0:
     - step: "GET_MID_STONE"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -1276,21 +1137,21 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "ARM_UP_STONE"
       arm:
-        joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE"
       arm:
-        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE_STONE"
       arm:
-        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -1307,120 +1168,13 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "home"
       arm:
-        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
+        <<: *HOME
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
     - step: "add stone"
       stone_num:
-        change: "+1"
-    - step: "gimbal front"
-      gimbal:
-        <<: *FRONT_POS
-
-  HAS1_SIDE_BIG_ISLAND:
-    - step: "gimbal ready"
-      gimbal:
-        <<: *SMALL_ISLAND_POS
-    - step: "open ex arm"
-      extend_arm:
-        front: *EX_OPEN
-    - step: "MID_ARM_READY"
-      arm:
-        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "ORE_ROTATOR_READY"
-      ore_rotator:
-        target: *READY_POS
-    - step: "ORE_LIFTER_READY"
-      ore_lifter:
-        target: *BIN_DOWN_POS
-    - step: "gimbal lifter"
-      gimbal_lifter:
-        target: *LIFTER_BIG_ISLAND
-    - step: "OPEN_GRIPPER"
-      gripper:
-        <<: *OPEN_GRIPPER
-  HAS1_SIDE_BIG_ISLAND0:
-    - step: "GET_MID_STONE"
-      arm:
-        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "ADJUST_STONE"
-      arm:
-        joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "PULL_OUT_STONE"
-      arm:
-        joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    #    - step: "chassis move left"
-    #      chassis:
-    #        <<: *CHASSIS_LEFT_15
-    - step: "JOINT1_UP"
-      arm:
-        joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "ARM_UP_STONE"
-      arm:
-        joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "READY_TO_STORE"
-      arm:
-        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "STORE_STONE"
-      arm:
-        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_R_POSITION, *JOINT5_BIG_STORE, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
-        common:
-          <<: *SLOWLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "CLOSE_GRIPPER"
-      gripper:
-        <<: *CLOSE_GRIPPER
-    - step: "move arm out"
-      arm:
-        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "home"
-      arm:
-        joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "add stone"
-      stone_num:
-        change: "+1"
+        change: "GOLD"
     - step: "gimbal front"
       gimbal:
         <<: *FRONT_POS
@@ -1450,7 +1204,7 @@ steps_list:
       arm:
         frame: exchanger
         is_refer_planning_frame: true
-        xyz: [ -0.2, 0., 0. ]
+        xyz: [ -0.3, 0.2, -0.2 ]
         rpy: [ 0., -1.5707963, 0. ]
         drift_dimensions: [ true, true, true, true, true, true ]
         tolerance_position: 0.01
@@ -1478,7 +1232,21 @@ steps_list:
         tolerance_position: 0.01
         tolerance_orientation: 0.001
 
-
+  EXCHANGE_ORI:
+    - step: "test new exchange"
+      arm:
+        frame: exchanger
+        is_refer_planning_frame: true
+        rpy: [ 0., -1.5707963, 0. ]
+        drift_dimensions: [ true, true, true, true, true, true ]
+        tolerance_position: 0.01
+        tolerance_orientation: 0.001
+        spacial_shape: SPHERE
+        radius: 0.5
+        point_resolution: 0.5
+        max_planning_times: 1
+        common:
+          <<: *NORMALLY
   EXCHANGE_STRAIGHT:
     - step: "test new exchange"
       arm:
@@ -1486,6 +1254,7 @@ steps_list:
         is_refer_planning_frame: true
         xyz: [ -0.3, 0., 0. ]
         rpy: [ 0., 3.1415926, 0. ]
+        drift_dimensions: [ true, true, true, true, true, true ]
         tolerance_position: 0.01
         tolerance_orientation: 0.001
         spacial_shape: SPHERE
@@ -1495,6 +1264,7 @@ steps_list:
         common:
           <<: *NORMALLY
 
+
   GIMBAL_DOWN:
     - step: "gimbal lowest"
       gimbal_lifter:

From 3ed5c5a8e5b8fd9c0a7a3bb542936eb4d8d10f02 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Fri, 26 Apr 2024 01:54:19 +0800
Subject: [PATCH 078/147] Change servo command frame.

---
 engineer_arm_config/config/servo.yaml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/engineer_arm_config/config/servo.yaml b/engineer_arm_config/config/servo.yaml
index a987de8..298263e 100644
--- a/engineer_arm_config/config/servo.yaml
+++ b/engineer_arm_config/config/servo.yaml
@@ -32,8 +32,8 @@ move_group_name: engineer_arm  # Often 'manipulator' or 'arm'
 planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'world'
 
 ## Other frames
-ee_frame_name: tools_link  # The name of the end effector link, used to return the EE pose
-robot_link_command_frame: tools_link  # commands must be given in the frame of a robot link. Usually either the base or end effector
+ee_frame_name: link7  # The name of the end effector link, used to return the EE pose
+robot_link_command_frame: link7  # commands must be given in the frame of a robot link. Usually either the base or end effector
 
 ## Stopping behaviour
 incoming_command_timeout: 0.1  # Stop servoing if X seconds elapse without a new command

From 9f492fcd9ec141f1be1a9ade818cb0e04e518010 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Fri, 26 Apr 2024 01:55:15 +0800
Subject: [PATCH 079/147] update step lists.

---
 engineer_middleware/config/steps_list.yaml | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index a84a101..ff0f46b 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -103,7 +103,7 @@ common:
       big_ready: &JOINT3_BIG_READY
                      0.071
       get_big_mid: &JOINT3_GET_BIG_MID
-                      -0.292
+                      -0.295
       get_big_side: &JOINT3_GET_BIG_SIDE
                      -0.199
       big_pull_out: &JOINT3_BIG_PULL_OUT
@@ -118,7 +118,7 @@ common:
       mid_position: &JOINT4_MID_POSITION
                       0.001
       right_position: &JOINT4_R_POSITION
-                        -1.52
+                        -1.554
       small_ready: &JOINT4_SMALL_READY
                         0.054
       small_turn: &JOINT4_SMALL_TURN
@@ -209,7 +209,7 @@ common:
     mid_pos: &MID_POS
       0.14
     big_island: &LIFTER_BIG_ISLAND
-      0.260
+      0.230
     two_stone_pos: &TWO_STONE_POS
       0.05
     tallest_pos: &TALLEST_POS

From 25fcf7a7fc945e1c11298563bf7d112bd1c7853c Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 27 Apr 2024 23:26:44 +0800
Subject: [PATCH 080/147] Add moveit_exchange.h.

---
 engineer_middleware/CMakeLists.txt            |   3 +-
 .../engineer_middleware/moveit_exchange.h     | 104 ++++++++++++++++++
 2 files changed, 106 insertions(+), 1 deletion(-)
 create mode 100644 engineer_middleware/include/engineer_middleware/moveit_exchange.h

diff --git a/engineer_middleware/CMakeLists.txt b/engineer_middleware/CMakeLists.txt
index 4c94c69..b6764bb 100644
--- a/engineer_middleware/CMakeLists.txt
+++ b/engineer_middleware/CMakeLists.txt
@@ -69,7 +69,8 @@ include_directories(
 
 add_executable(${PROJECT_NAME}
         src/middleware.cpp
-        src/engineer_middleware.cpp include/engineer_middleware/auto_exchange.h include/engineer_middleware/auto_exchange.h)
+        src/engineer_middleware.cpp include/engineer_middleware/auto_exchange.h include/engineer_middleware/auto_exchange.h
+        include/engineer_middleware/moveit_exchange.h)
 
 add_dependencies(${PROJECT_NAME}
         ${catkin_EXPORTED_TARGETS}
diff --git a/engineer_middleware/include/engineer_middleware/moveit_exchange.h b/engineer_middleware/include/engineer_middleware/moveit_exchange.h
new file mode 100644
index 0000000..d110df9
--- /dev/null
+++ b/engineer_middleware/include/engineer_middleware/moveit_exchange.h
@@ -0,0 +1,104 @@
+//
+// Created by cch on 24-4-27.
+//
+
+#pragma once
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <engineer_middleware/motion.h>
+
+namespace moveit_exchange
+{
+enum States
+{
+  FIND,
+  CHASSIS_APPROACH,
+  CHASSIS_POMPENSATE,
+  SPHERE,
+  LINE,
+  REACH
+};
+
+class SingleDirectionMove
+{
+public:
+  std::string name;
+  double tolerance{}, start_vel{}, offset_refer_exchanger{}, max_vel{}, error{}, pid_value{};
+  control_toolbox::Pid pid;
+  void init(XmlRpc::XmlRpcValue& config, std::string config_name, ros::NodeHandle& nh)
+  {
+    name = config_name;
+    error = 1e10;
+    max_vel = config.hasMember("max_vel") ? (double)config["max_vel"] : 1e10;
+    start_vel = config.hasMember("start_vel") ? (double)config["start_vel"] : 0.;
+    tolerance = config.hasMember("tolerance") ? (double)config["tolerance"] : 1e10;
+    offset_refer_exchanger = config.hasMember("offset_refer_exchanger") ? (double)config["offset_refer_exchanger"] : 0.;
+    ros::NodeHandle pid_config = ros::NodeHandle(nh, name);
+    pid.init(ros::NodeHandle(pid_config, "pid"));
+  }
+  bool isFinish() const
+  {
+    return abs(error) <= tolerance;
+  }
+  double computerVel(ros::Duration dt)
+  {
+    double vel = start_vel + abs(pid.computeCommand(error, dt));
+    int direction = (error > 0) ? 1 : -1;
+    return abs(vel) >= max_vel ? direction * max_vel : direction * vel;
+  }
+  void getPidValue(ros::Duration dt)
+  {
+    double vel = start_vel + abs(pid.computeCommand(error, dt));
+    int direction = (error > 0) ? 1 : -1;
+    pid_value = abs(vel) >= max_vel ? direction * max_vel : direction * vel;
+  }
+};
+
+class ChassisMotion
+{
+public:
+  enum AdjustStates
+  {
+    SET_GOAL,
+    ADJUST_X,
+    ADJUST_Y,
+    ADJUST_YAW,
+    FINISH
+  };
+  ChassisMotion(XmlRpc::XmlRpcValue& config, tf2_ros::Buffer& tf_buffer, ros::NodeHandle& nh)
+    : tf_buffer_(tf_buffer), nh_(nh)
+  {
+    state_ = SET_GOAL;
+    last_state_ = state_;
+    x_.init(config["x"], "x", nh);
+    y_.init(config["y"], "y", nh);
+    yaw_.init(config["yaw"], "yaw", nh);
+  }
+
+private:
+  void initComputeValue()
+  {
+    chassis_vel_cmd_.linear.x = 0;
+    chassis_vel_cmd_.linear.y = 0;
+    chassis_vel_cmd_.linear.z = 0;
+    chassis_vel_cmd_.angular.x = 0;
+    chassis_vel_cmd_.angular.y = 0;
+    chassis_vel_cmd_.angular.z = 0;
+  }
+
+  AdjustStates state_, last_state_;
+  tf2_ros::Buffer& tf_buffer_;
+  ros::NodeHandle nh_{};
+  SingleDirectionMove x_, y_, yaw_;
+  geometry_msgs::Twist chassis_vel_cmd_{};
+};
+
+class FindExchanger
+{
+};
+
+class MoveitExchange
+{
+};
+
+}  // namespace moveit_exchange

From d50b6991109522bf1aa38c8f270dc8766b5f01cf Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 29 Apr 2024 22:48:56 +0800
Subject: [PATCH 081/147] Adjust chassis motion pid.

---
 engineer_middleware/config/engineer.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/engineer_middleware/config/engineer.yaml b/engineer_middleware/config/engineer.yaml
index 73c761e..fb1caf4 100644
--- a/engineer_middleware/config/engineer.yaml
+++ b/engineer_middleware/config/engineer.yaml
@@ -4,6 +4,6 @@ chassis:
   y:
     pid: { p: 4.5, i: 0.0, d: 0.2, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
   yaw:
-    pid: { p: 5., i: 0.0, d: 0.05, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+    pid: { p: 5., i: 0.0, d: 0.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
   yaw_start_threshold: 0.05
   max_vel: 0.1

From c678b18b80730de4f998a07a7ad7671fd74a67c6 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 29 Apr 2024 23:49:22 +0800
Subject: [PATCH 082/147] change home step into 2 stages.

---
 engineer_middleware/config/steps_list.yaml | 248 +++++++++++++++++----
 1 file changed, 201 insertions(+), 47 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index ff0f46b..6e97f02 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -46,7 +46,7 @@ common:
       big_up_pos: &JOINT1_BIG_UP_POS
                       0.320
       big_ready_store: &JOINT1_BIG_READY_STORE
-                      0.233
+                      0.133
     home:
       zero_stone_position: &JOINT1_ZERO_STONE_POSITION
                              0.01
@@ -60,15 +60,17 @@ common:
   joint2:
     mechanical:
       back_position: &JOINT2_BACK_POSITION
-                       0.01
+                       0.047
       get_ore_position: &JOINT2_GET_ORE_POSITION
                            0.1
       furthest_position: &JOINT2_FURTHEST_POSITION
                            0.232
       exchange_position: &JOINT2_EXCHANGE_POSITION
-                          0.07
+                          0.105
       small_ready: &JOINT2_SMALL_READY
-                      0.034
+                      0.029
+      small_store: &JOINT2_SMALL_STORE
+                      0.044
       bin_get_stone: &JOINT2_BIN_GET_STONE
                             0.087
       back_bin_position: &JOINT2_BACK_BIN_POSITION
@@ -78,7 +80,7 @@ common:
       big_arm_up: &JOINT2_BIG_ARM_UP
                     0.142
       big_ready_store: &JOINT2_BIG_READY_STORE
-                      0.03
+                      0.055
 
   joint3:
     mechanical:
@@ -109,7 +111,9 @@ common:
       big_pull_out: &JOINT3_BIG_PULL_OUT
                      0.188
       big_arm_up: &JOINT3_BIG_ARM_UP
-                    0.267
+                    0.285
+      exchange: &JOINT3_EXCHANGE
+                  0.081
 
   joint4:
     mechanical:
@@ -125,6 +129,8 @@ common:
                   -1.497
       big_store: &JOINT4_BIG_STORE
                    -1.525
+      exchange: &JOINT4_EXCHANGE
+                  -0.086
   joint5:
     mechanical:
       mid_position: &JOINT5_MID_POSITION
@@ -147,6 +153,8 @@ common:
                     -0.001
       small_store: &JOINT5_SMALL_STORE
                     1.615
+      exchange: &JOINT5_EXCHANGE
+                1.805
 
   joint6:
     mechanical:
@@ -172,13 +180,15 @@ common:
                        -1.519
       big_ready_store: &JOINT6_BIG_READY_STORE
                       -1.561
+      exchange: &JOINT6_EXCHANGE
+                0.139
 
   joint7:
     mechanical:
       mid_position: &JOINT7_MID_POSITION
                       0.044
       left_90_position: &JOINT7_L90_POSITION
-                          -1.59
+                          -1.56
       right_90_position: &JOINT7_R90_POSITION
                            1.62
       big_pos: &JOINT7_BIG_POS
@@ -189,32 +199,33 @@ common:
                          0.012
       small_ready_store: &JOINT7_SMALL_READY_STORE
                           1.722
+      exchange: &JOINT7_EXCHANGE
+                  -1.771
 
   gimbal:
     side_pos: &SIDE_POS
       frame: gimbal_lifter
-      position: [ 0.001 , -3, 0. ]
+      position: [ 0.001 , -3., 0. ]
     front_pos: &FRONT_POS
       frame: gimbal_lifter
-      position: [ 2, 0.001, 0. ]
+      position: [ 2., 0.001, 0. ]
     small_island_pos: &SMALL_ISLAND_POS
       frame: gimbal_lifter
-      position: [ 1, -1, 0.]
+      position: [ 1., -1., 0.]
 
   gimbal_lifter:
     button_pos: &BUTTON_POS
       0.05
-    small_island: &LIFTER_SMALL_ISLAND
-      0.05
     mid_pos: &MID_POS
       0.14
-    big_island: &LIFTER_BIG_ISLAND
-      0.230
     two_stone_pos: &TWO_STONE_POS
       0.05
     tallest_pos: &TALLEST_POS
       0.28
-
+    big_island: &LIFTER_BIG_ISLAND
+                  0.230
+    small_island: &LIFTER_SMALL_ISLAND
+                    0.05
   gripper:
     open_gripper: &OPEN_GRIPPER
       pin: 0
@@ -224,12 +235,20 @@ common:
       state: false
 
   arm:
+    pre_home: &PRE_HOME
+      joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, "KEEP", *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+      common:
+        <<: *NORMALLY
+      tolerance:
+        <<: *SMALL_TOLERANCE
+
     home: &HOME
       joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
-        <<: *NORMAL_TOLERANCE
+        <<: *SMALL_TOLERANCE
+
 
 
   chassis:
@@ -249,6 +268,22 @@ common:
       chassis_tolerance_angular: 0.2
       common:
         timeout: 2.
+    chassis_90: &CHASSIS_TURN
+      frame: base_link
+      position: [ 0., 0. ]
+      yaw: -1.40
+      chassis_tolerance_position: 0.1
+      chassis_tolerance_angular: 0.2
+      common:
+        timeout: 2.
+    chassis_back: &CHASSIS_BACK
+      frame: base_link
+      position: [ -0.4, 0. ]
+      yaw: 0.0
+      chassis_tolerance_position: 0.1
+      chassis_tolerance_angular: 0.2
+      common:
+        timeout: 2.
 
   ore_rotator:
     ready_pos: &READY_POS
@@ -276,7 +311,7 @@ steps_list:
         <<: *FRONT_POS
     - step: "move arm"
       arm:
-        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
+        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_EXCHANGE, *JOINT6_EXCHANGE, *JOINT7_EXCHANGE]
         common:
           <<: *NORMALLY
         tolerance:
@@ -316,6 +351,9 @@ steps_list:
     - step: "ORE_LIFTER_UP"
       ore_lifter:
         target: *BIN_DOWN_POS
+    - step: "pre_home"
+      arm:
+        <<: *PRE_HOME
     - step: "home"
       arm:
         <<: *HOME
@@ -351,6 +389,9 @@ steps_list:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
+    - step: "gimbal lifter ready"
+      gimbal_lifter:
+        target: *LIFTER_SMALL_ISLAND
   ONE_STONE_SMALL_ISLAND0:
     - step: "GET_STONE"
       arm:
@@ -389,14 +430,14 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "READY_STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -412,6 +453,15 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "CHASSIS_BACK"
+      chassis:
+        <<: *CHASSIS_BACK
+    - step: "CHASSIS_TURN"
+      chassis:
+        <<: *CHASSIS_TURN
+    - step: "pre_home"
+      arm:
+        <<: *PRE_HOME
     - step: "home"
       arm:
         <<: *HOME
@@ -446,7 +496,7 @@ steps_list:
         <<: *OPEN_GRIPPER
     - step: "gimbal lifter ready"
       gimbal_lifter:
-        target: *BUTTON_POS
+        target: *LIFTER_SMALL_ISLAND
   TWO_STONE_SMALL_ISLAND0:
     - step: "GET_STONE"
       arm:
@@ -485,14 +535,14 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "READY_STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -573,14 +623,14 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "READY_STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -595,6 +645,15 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "CHASSIS_BACK"
+      chassis:
+        <<: *CHASSIS_BACK
+    - step: "CHASSIS_TURN"
+      chassis:
+        <<: *CHASSIS_TURN
+    - step: "pre_home"
+      arm:
+        <<: *PRE_HOME
     - step: "home"
       arm:
         <<: *HOME
@@ -662,14 +721,14 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "READY_STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -750,14 +809,14 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "READY_STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -834,14 +893,14 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "READY_STORE(THIRD_STONE)"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
-        joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -902,13 +961,30 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "exchange pos"
+    - step: "exchange pos1"
       arm:
-        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "exchange pos2"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_MID_POSITION, *JOINT6_EXCHANGE, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "exchange pos2"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_EXCHANGE, *JOINT6_EXCHANGE, *JOINT7_EXCHANGE]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ORE_LIFTER_DOWN"
+      ore_lifter:
+        target: *BIN_DOWN_POS
     - step: "gimbal lifter tallest"
       gimbal_lifter:
         target: *TALLEST_POS
@@ -954,13 +1030,30 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "exchange pos"
+    - step: "exchange pos1"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "exchange pos2"
       arm:
-        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_MID_POSITION, *JOINT6_EXCHANGE, *JOINT7_MID_POSITION ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "exchange pos2"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_EXCHANGE, *JOINT6_EXCHANGE, *JOINT7_EXCHANGE ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ORE_LIFTER_DOWN"
+      ore_lifter:
+        target: *BIN_DOWN_POS
     - step: "gimbal lifter tallest"
       gimbal_lifter:
         target: *TALLEST_POS
@@ -987,10 +1080,10 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
-        target: *READY_POS
-    - step: "ORE_LIFTER_READY"
-      ore_lifter:
-        target: *BIN_MID_POS
+        target: *EXCHANGE_POS
+#    - step: "ORE_LIFTER_READY"
+#      ore_lifter:
+#        target: *BIN_MID_POS
     - step: "gimbal lifter"
       gimbal_lifter:
         target: *LIFTER_BIG_ISLAND
@@ -1038,14 +1131,14 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE"
       arm:
-        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ]
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE_STONE"
       arm:
-        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ]
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -1061,12 +1154,22 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "pre_home"
+      arm:
+        <<: *PRE_HOME
     - step: "home"
       arm:
         <<: *HOME
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
+    - step: "ORE_ROTATOR_RESET"
+      ore_rotator:
+        target: *READY_POS
+    - step: "EX_ARM_RESET"
+      extend_arm:
+        front: *EX_CLOSE
+        back: *EX_CLOSE
     - step: "add stone"
       stone_num:
         change: "GOLD"
@@ -1082,7 +1185,7 @@ steps_list:
     - step: "open ex arm"
       extend_arm:
         front: *EX_OPEN
-
+        back: *EX_CLOSE
     - step: "MID_ARM_READY"
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
@@ -1092,10 +1195,10 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
-        target: *READY_POS
-    - step: "ORE_LIFTER_READY"
-      ore_lifter:
-        target: *BIN_MID_POS
+        target: *EXCHANGE_POS
+#    - step: "ORE_LIFTER_READY"
+#      ore_lifter:
+#        target: *BIN_MID_POS
     - step: "gimbal lifter"
       gimbal_lifter:
         target: *LIFTER_BIG_ISLAND
@@ -1144,14 +1247,14 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE"
       arm:
-        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ]
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE_STONE"
       arm:
-        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_READY_STORE ]
+        joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ]
         common:
           <<: *SLOWLY
         tolerance:
@@ -1166,18 +1269,59 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "pre_home"
+      arm:
+        <<: *PRE_HOME
     - step: "home"
       arm:
         <<: *HOME
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
+    - step: "ORE_ROTATOR_RESET"
+      ore_rotator:
+        target: *READY_POS
+    - step: "EX_ARM_RESET"
+      extend_arm:
+        front: *EX_CLOSE
+        back: *EX_CLOSE
     - step: "add stone"
       stone_num:
         change: "GOLD"
     - step: "gimbal front"
       gimbal:
         <<: *FRONT_POS
+  ##########GIMBAL_LIFTER##########
+  ZERO_STONE_GIMBAL_LIFTER:
+    - step: "gimbal lifter zero stone pos"
+      gimbal_lifter:
+        target: *BUTTON_POS
+  ONE_STONE_GIMBAL_LIFTER:
+    - step: "gimbal lifter one stone pos"
+      gimbal_lifter:
+        target: *MID_POS
+  TWO_STONE_GIMBAL_LIFTER:
+    - step: "gimbal lifter zero stone pos"
+      gimbal_lifter:
+        target: *TWO_STONE_POS
+  TALLEST_GIMBAL_LIFTER:
+    - step: "gimbal lifter tallest pos"
+      gimbal_lifter:
+        target: *TALLEST_POS
+
+  ##########ORE_LIFTER###############
+  ORE_LIFTER_DOWN:
+    - step: "ore lifter down"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+  ORE_LIFTER_MID:
+    - step: "ore lifter mid"
+      ore_lifter:
+        target: *BIN_MID_POS
+  ORE_LIFTER_UP:
+    - step: "ore lifter up"
+      ore_lifter:
+        target: *BIN_UP_POS
 
   ####################################################  TEST  ##########################################################
 
@@ -1279,3 +1423,13 @@ steps_list:
     - step: "gimbal tall"
       gimbal_lifter:
         target: *TALLEST_POS
+
+  CHASSIS_TEST1:
+    - step: "chassis turn"
+      chassis:
+        <<: *CHASSIS_TURN
+
+  CHASSIS_TEST2:
+    - step: "chassis back"
+      chassis:
+        <<: *CHASSIS_BACK

From a316fe8cd146dca8b95fbbd7391439c511d4f931 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 30 Apr 2024 01:33:20 +0800
Subject: [PATCH 083/147] change servo direction

---
 engineer_arm_config/config/servo.yaml | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/engineer_arm_config/config/servo.yaml b/engineer_arm_config/config/servo.yaml
index 298263e..70b7b47 100644
--- a/engineer_arm_config/config/servo.yaml
+++ b/engineer_arm_config/config/servo.yaml
@@ -7,8 +7,8 @@ use_gazebo: true # Whether the robot is started in a Gazebo simulation environme
 command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
 scale:
   # Scale parameters are only used if command_in_type=="unitless"
-  linear: 0.6  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
-  rotational: 0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
+  linear: 0.4  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
+  rotational: 0.2 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
   # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
   joint: 0.005
 low_pass_filter_coeff: 1.  # Larger --> trust the filtered data more, trust the measurements less.
@@ -32,8 +32,8 @@ move_group_name: engineer_arm  # Often 'manipulator' or 'arm'
 planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'world'
 
 ## Other frames
-ee_frame_name: link7  # The name of the end effector link, used to return the EE pose
-robot_link_command_frame: link7  # commands must be given in the frame of a robot link. Usually either the base or end effector
+ee_frame_name: tools_link  # The name of the end effector link, used to return the EE pose
+robot_link_command_frame: tools_link  # commands must be given in the frame of a robot link. Usually either the base or end effector
 
 ## Stopping behaviour
 incoming_command_timeout: 0.1  # Stop servoing if X seconds elapse without a new command

From 8eca8f67a290274e5503f56c0d213b1f490e2139 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 30 Apr 2024 01:34:21 +0800
Subject: [PATCH 084/147] record.

---
 engineer_middleware/config/steps_list.yaml | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 6e97f02..7969fea 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -131,6 +131,10 @@ common:
                    -1.525
       exchange: &JOINT4_EXCHANGE
                   -0.086
+      pre_home: &JOINT4_PRE_HOME
+                  -0.91
+
+
   joint5:
     mechanical:
       mid_position: &JOINT5_MID_POSITION
@@ -236,7 +240,7 @@ common:
 
   arm:
     pre_home: &PRE_HOME
-      joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, "KEEP", *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+      joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_PRE_HOME, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
       common:
         <<: *NORMALLY
       tolerance:
@@ -311,7 +315,7 @@ steps_list:
         <<: *FRONT_POS
     - step: "move arm"
       arm:
-        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_EXCHANGE, *JOINT6_EXCHANGE, *JOINT7_EXCHANGE]
+        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
         common:
           <<: *NORMALLY
         tolerance:

From d21e50027671943c50d2b253950655bb4d8fd5f3 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 1 May 2024 22:08:07 +0800
Subject: [PATCH 085/147] record.

---
 .../engineer_middleware/moveit_exchange.h     | 98 ++++++++++++++++++-
 1 file changed, 94 insertions(+), 4 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/moveit_exchange.h b/engineer_middleware/include/engineer_middleware/moveit_exchange.h
index d110df9..7cca356 100644
--- a/engineer_middleware/include/engineer_middleware/moveit_exchange.h
+++ b/engineer_middleware/include/engineer_middleware/moveit_exchange.h
@@ -74,6 +74,60 @@ class ChassisMotion
     y_.init(config["y"], "y", nh);
     yaw_.init(config["yaw"], "yaw", nh);
   }
+  void init()
+  {
+    state_ = SET_GOAL;
+    initComputeValue();
+  }
+  void stateMachine()
+  {
+    switch (state_)
+    {
+      case SET_GOAL:
+      {
+        setGoal();
+        state_ = ADJUST_X;
+      }
+      break;
+      case ADJUST_X:
+      {
+        computeChassisVel();
+        chassis_vel_cmd_.linear.y = 0.;
+        chassis_vel_cmd_.angular.z = 0.;
+        if (x_.isFinish())
+          state_ = re_adjusted_ ? FINISH : ADJUST_Y;
+      }
+      break;
+      case ADJUST_Y:
+      {
+        computeChassisVel();
+        chassis_vel_cmd_.linear.x = 0.;
+        chassis_vel_cmd_.angular.z = 0.;
+        if (y_.isFinish())
+          state_ = ADJUST_YAW;
+      }
+      break;
+      case ADJUST_YAW:
+      {
+        computeChassisVel();
+        chassis_vel_cmd_.linear.x = 0.;
+        chassis_vel_cmd_.linear.y = 0.;
+        if (yaw_.isFinish())
+        {
+          state_ = SET_GOAL;
+          re_adjusted_ = true;
+        }
+      }
+      break;
+      case FINISH:
+      {
+        is_finish_ = true;
+        re_adjusted_ = false;
+        initComputeValue();
+      }
+      break;
+    }
+  }
 
 private:
   void initComputeValue()
@@ -85,16 +139,52 @@ class ChassisMotion
     chassis_vel_cmd_.angular.y = 0;
     chassis_vel_cmd_.angular.z = 0;
   }
+  void computeChassisVel()
+  {
+    geometry_msgs::TransformStamped current;
+    current = tf_buffer_.lookupTransform("map", "base_link", ros::Time(0));
+    x_.error = chassis_target_.pose.position.x - current.transform.translation.x;
+    y_.error = chassis_target_.pose.position.y - current.transform.translation.y;
+    double roll, pitch, yaw_current, yaw_goal;
+    quatToRPY(current.transform.rotation, roll, pitch, yaw_current);
+    quatToRPY(chassis_target_.pose.orientation, roll, pitch, yaw_goal);
+    yaw_.error = angles::shortest_angular_distance(yaw_current, yaw_goal);
 
+    ros::Duration dt = ros::Time::now() - last_time_;
+    chassis_vel_cmd_.linear.x = x_.computerVel(dt);
+    chassis_vel_cmd_.linear.y = y_.computerVel(dt);
+    chassis_vel_cmd_.angular.z = yaw_.computerVel(dt);
+
+    last_time_ = ros::Time::now();
+  }
+  void setGoal()
+  {
+    geometry_msgs::TransformStamped base2exchange;
+    double roll, pitch, yaw;
+    base2exchange = tf_buffer_.lookupTransform("base_link", "exchanger", ros::Time(0));
+    quatToRPY(base2exchange.transform.rotation, roll, pitch, yaw);
+
+    double goal_x = base2exchange.transform.translation.x - x_.offset_refer_exchanger;
+    double goal_y = base2exchange.transform.translation.y - y_.offset_refer_exchanger;
+    double goal_yaw = yaw * yaw_.offset_refer_exchanger;
+    chassis_original_target_.pose.position.x = goal_x;
+    chassis_original_target_.pose.position.y = goal_y;
+
+    tf2::Quaternion quat_tf;
+    quat_tf.setRPY(0, 0, goal_yaw);
+    geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf);
+    chassis_original_target_.pose.orientation = quat_msg;
+    chassis_target_ = chassis_original_target_;
+    tf2::doTransform(chassis_target_, chassis_target_, tf_buffer_.lookupTransform("map", "base_link", ros::Time(0)));
+  }
+  bool re_adjusted_{ false }, is_finish_{ false };
   AdjustStates state_, last_state_;
   tf2_ros::Buffer& tf_buffer_;
   ros::NodeHandle nh_{};
+  ros::Time last_time_;
   SingleDirectionMove x_, y_, yaw_;
   geometry_msgs::Twist chassis_vel_cmd_{};
-};
-
-class FindExchanger
-{
+  geometry_msgs::PoseStamped chassis_target_{}, chassis_original_target_{};
 };
 
 class MoveitExchange

From afc93b8fd154bdf50c6c59b4339137c52789db84 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 2 May 2024 17:00:00 +0800
Subject: [PATCH 086/147] add ChassisTargetMotion,pass arm2base from
 JointMotion to ChassisTargetMotion.

---
 engineer_middleware/config/steps_list.yaml    | 185 +++++++++++++++---
 .../include/engineer_middleware/motion.h      | 178 ++++++++++++-----
 .../include/engineer_middleware/points.h      |   6 +
 .../include/engineer_middleware/step.h        |  13 +-
 4 files changed, 307 insertions(+), 75 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 7969fea..46a66f5 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -28,7 +28,7 @@ common:
       down_position: &JOINT1_DOWN_POSITION
                        0.02
       small_ready: &JOINT1_SMALL_READY
-                      0.480
+                      0.487
       small_down: &JOINT1_SMALL_DOWN
                   0.31
       small_ready_store: &JOINT1_SMALL_READY_STORE
@@ -37,8 +37,10 @@ common:
                          0.387
       up_pos: &JOINT1_UP_POS
                    0.312
-      bin_get_stone: &JOINT1_BIN_GET_STONE
-                     0.142
+      bin_get_stone_left: &JOINT1_BIN_GET_STONE_LEFT
+                     0.232
+      bin_get_stone_right: &JOINT1_BIN_GET_STONE_RIGHT
+                            0.144
       big_ready: &JOINT1_BIG_READY
                      0.006
       big_adjust_mid: &JOINT1_BIG_ADJUST_MID
@@ -101,11 +103,11 @@ common:
       small_turn: &JOINT3_SMALL_TURN
                    0.257
       bin_get_stone: &JOINT3_BIN_GET_STONE
-                          0.258
+                          0.252
       big_ready: &JOINT3_BIG_READY
                      0.071
       get_big_mid: &JOINT3_GET_BIG_MID
-                      -0.295
+                      -0.299
       get_big_side: &JOINT3_GET_BIG_SIDE
                      -0.199
       big_pull_out: &JOINT3_BIG_PULL_OUT
@@ -120,7 +122,7 @@ common:
       left_position: &JOINT4_L_POSITION
                        1.00
       mid_position: &JOINT4_MID_POSITION
-                      0.001
+                      0.021
       right_position: &JOINT4_R_POSITION
                         -1.554
       small_ready: &JOINT4_SMALL_READY
@@ -152,7 +154,7 @@ common:
       big_store: &JOINT5_BIG_STORE
                   1.49
       small_ready: &JOINT5_SMALL_READY
-                  -3.128
+                  -3.20
       small_up_stone: &JOINT5_SMALL_UP_STONE
                     -0.001
       small_store: &JOINT5_SMALL_STORE
@@ -205,6 +207,10 @@ common:
                           1.722
       exchange: &JOINT7_EXCHANGE
                   -1.771
+      bin_get_left: &JOINT7_BIN_GET_LEFT
+                  -1.526
+      bin_get_right: &JOINT7_BIN_GET_RIGHT
+                  1.594
 
   gimbal:
     side_pos: &SIDE_POS
@@ -254,6 +260,16 @@ common:
         <<: *SMALL_TOLERANCE
 
 
+  chassis_target:
+    test: &TEST
+      frame: base_link
+      chassis_tolerance_position: 0.01
+      chassis_tolerance_angular: 0.01
+      offset: [0., 0.]
+      yaw_scale: 1
+      target_frame: exchanger
+      common:
+        timeout: 2.
 
   chassis:
     chassis_left_315: &CHASSIS_LEFT_15
@@ -302,6 +318,10 @@ common:
             0.1
     bin_down_pos: &BIN_DOWN_POS
             0.001
+    bin_get_up: &BIN_GET_UP
+      0.033
+    bin_get_down: &BIN_GET_DOWN
+      0.225
   extend_arm:
     close: &EX_CLOSE
           0.0
@@ -309,6 +329,11 @@ common:
           2.6
 
 steps_list:
+  TEST:
+    - step: "test"
+      chassis_target:
+        <<: *TEST
+
   EXCHANGE_POS:
     - step: "gimbal look front"
       gimbal:
@@ -924,7 +949,7 @@ steps_list:
         change: "SILVER"
 
   ######GET STONE FROM BIN#########
-  GET_DOWN_STONE_BIN:
+  GET_DOWN_STONE_LEFT:
     - step: "GIMBAL_READY"
       gimbal:
         <<: *FRONT_POS
@@ -933,10 +958,10 @@ steps_list:
         target: *EXCHANGE_POS
     - step: "ORE_LIFTER_UP"
       ore_lifter:
-        target: *BIN_UP_POS
+        target: *BIN_GET_DOWN
     - step: "READY_TO_GET"
       arm:
-        joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -965,25 +990,69 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "exchange pos1"
+    - step: "exchange pos"
       arm:
-        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "exchange pos2"
+    - step: "ORE_LIFTER_DOWN"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "gimbal lifter tallest"
+      gimbal_lifter:
+        target: *TALLEST_POS
+    - step: "remove stone"
+      stone_num:
+        change: "-1"
+
+  GET_DOWN_STONE_RIGHT:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *EXCHANGE_POS
+    - step: "ORE_LIFTER_UP"
+      ore_lifter:
+        target: *BIN_GET_DOWN
+    - step: "READY_TO_GET"
       arm:
-        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_MID_POSITION, *JOINT6_EXCHANGE, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "exchange pos2"
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "JOINT2_BACK_GET_STONE"
       arm:
-        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_EXCHANGE, *JOINT6_EXCHANGE, *JOINT7_EXCHANGE]
+        joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *QUICKLY
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT5_LIFT_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "exchange pos"
+      arm:
+        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
+        common:
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ORE_LIFTER_DOWN"
@@ -996,16 +1065,19 @@ steps_list:
       stone_num:
         change: "-1"
 
-  GET_UP_STONE_BIN:
+  GET_UP_STONE_LEFT:
     - step: "GIMBAL_READY"
       gimbal:
         <<: *FRONT_POS
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *EXCHANGE_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_GET_UP
     - step: "READY_TO_GET"
       arm:
-        joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1034,25 +1106,69 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "exchange pos1"
+    - step: "exchange pos"
       arm:
-        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "exchange pos2"
+    - step: "ORE_LIFTER_DOWN"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "gimbal lifter tallest"
+      gimbal_lifter:
+        target: *TALLEST_POS
+    - step: "remove stone"
+      stone_num:
+        change: "-1"
+
+  GET_UP_STONE_RIGHT:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *EXCHANGE_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_GET_UP
+    - step: "READY_TO_GET"
       arm:
-        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_MID_POSITION, *JOINT6_EXCHANGE, *JOINT7_MID_POSITION ]
+        joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "exchange pos2"
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "JOINT2_BACK_GET_STONE"
       arm:
-        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_EXCHANGE, *JOINT4_EXCHANGE, *JOINT5_EXCHANGE, *JOINT6_EXCHANGE, *JOINT7_EXCHANGE ]
+        joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *QUICKLY
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT5_LIFT_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "exchange pos"
+      arm:
+        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
+        common:
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ORE_LIFTER_DOWN"
@@ -1328,6 +1444,23 @@ steps_list:
         target: *BIN_UP_POS
 
   ####################################################  TEST  ##########################################################
+  ARM_BACK_TEST:
+    - step: "arm back test"
+      arm:
+        joints: [ "KEEP", *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, "KEEP", "KEEP", "KEEP", "KEEP" ]
+        record_arm2base: true
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  CHASSIS_TARGET_TEST1:
+    - step: "test"
+      chassis:
+        frame: "base_link"
+        offset: [ 0., 0. ]
+        yaw_scale: 0.9
+        target_frame: "exchanger"
 
   ########## EXCHANGE ############
   OLD_EXCHANGE_SPHERE:
diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 44cd7f9..f216dcc 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -338,9 +338,6 @@ class SpaceEeMotion : public EndEffectorMotion
             final_target_.pose.orientation.z = tf_quaternion.z();
 
             final_target_.header.frame_id = interface_.getPlanningFrame();
-            double rolll, pitchh, yaww;
-            quatToRPY(final_target_.pose.orientation, rolll, pitchh, yaww);
-            ROS_INFO_STREAM("transformed roll: " << rolll);
           }
           catch (tf2::TransformException& ex)
           {
@@ -384,8 +381,9 @@ class SpaceEeMotion : public EndEffectorMotion
 class JointMotion : public MoveitMotionBase
 {
 public:
-  JointMotion(XmlRpc::XmlRpcValue& motion, moveit::planning_interface::MoveGroupInterface& interface)
-    : MoveitMotionBase(motion, interface)
+  JointMotion(XmlRpc::XmlRpcValue& motion, moveit::planning_interface::MoveGroupInterface& interface,
+              tf2_ros::Buffer& tf_buffer)
+    : MoveitMotionBase(motion, interface), tf_buffer_(tf_buffer)
   {
     if (motion.hasMember("joints"))
     {
@@ -408,9 +406,26 @@ class JointMotion : public MoveitMotionBase
       for (int i = 0; i < motion["tolerance"]["tolerance_joints"].size(); ++i)
         tolerance_joints_.push_back(xmlRpcGetDouble(motion["tolerance"]["tolerance_joints"], i));
     }
+    if (motion.hasMember("record_arm2base"))
+      record_arm2base_ = bool(motion["record_arm2base"]);
   }
   bool move() override
   {
+    if (record_arm2base_)
+    {
+      try
+      {
+        arm2base.header.frame_id = "base_link";
+        arm2base.header.stamp = ros::Time::now();
+        arm2base.child_frame_id = "chassis_target";
+        arm2base = tf_buffer_.lookupTransform("base_link", "joint4", ros::Time(0));
+      }
+      catch (tf2::TransformException& ex)
+      {
+        ROS_WARN("%s", ex.what());
+        return false;
+      }
+    }
     final_target_.clear();
     if (target_.empty())
       return false;
@@ -431,6 +446,7 @@ class JointMotion : public MoveitMotionBase
     msg_.data = interface_.plan(plan).val;
     return (interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
   }
+  static geometry_msgs::TransformStamped arm2base;
 
 private:
   bool isReachGoal() override
@@ -446,6 +462,8 @@ class JointMotion : public MoveitMotionBase
     return flag;
   }
   std::vector<double> target_, final_target_, tolerance_joints_;
+  bool record_arm2base_{ false };
+  tf2_ros::Buffer& tf_buffer_;
 };
 
 template <class MsgType>
@@ -589,49 +607,6 @@ class GimbalMotion : public PublishMotion<rm_msgs::GimbalCmd>
   }
 };
 
-class ChassisMotion : public MotionBase<ChassisInterface>
-{
-public:
-  ChassisMotion(XmlRpc::XmlRpcValue& motion, ChassisInterface& interface)
-    : MotionBase<ChassisInterface>(motion, interface)
-  {
-    chassis_tolerance_position_ = xmlRpcGetDouble(motion, "chassis_tolerance_position", 0.01);
-    chassis_tolerance_angular_ = xmlRpcGetDouble(motion, "chassis_tolerance_angular", 0.01);
-    if (motion.hasMember("frame"))
-      target_.header.frame_id = std::string(motion["frame"]);
-    if (motion.hasMember("position"))
-    {
-      target_.pose.position.x = xmlRpcGetDouble(motion["position"], 0);
-      target_.pose.position.y = xmlRpcGetDouble(motion["position"], 1);
-    }
-    if (motion.hasMember("yaw"))
-    {
-      tf2::Quaternion quat_tf;
-      quat_tf.setRPY(0, 0, motion["yaw"]);
-      geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf);
-      target_.pose.orientation = quat_msg;
-    }
-  }
-  bool move() override
-  {
-    interface_.setGoal(target_);
-    return true;
-  }
-  bool isFinish() override
-  {
-    return interface_.getErrorPos() < chassis_tolerance_position_ &&
-           interface_.getErrorYaw() < chassis_tolerance_angular_;
-  }
-  void stop() override
-  {
-    interface_.stop();
-  }
-
-private:
-  geometry_msgs::PoseStamped target_;
-  double chassis_tolerance_position_, chassis_tolerance_angular_;
-};
-
 class ReversalMotion : public PublishMotion<rm_msgs::MultiDofCmd>
 {
 public:
@@ -728,4 +703,111 @@ class ExtendMotion : public PublishMotion<std_msgs::Float64>
   double target_;
 };
 
+class ChassisMotion : public MotionBase<ChassisInterface>
+{
+public:
+  ChassisMotion(XmlRpc::XmlRpcValue& motion, ChassisInterface& interface)
+    : MotionBase<ChassisInterface>(motion, interface)
+  {
+    chassis_tolerance_position_ = xmlRpcGetDouble(motion, "chassis_tolerance_position", 0.01);
+    chassis_tolerance_angular_ = xmlRpcGetDouble(motion, "chassis_tolerance_angular", 0.01);
+    if (motion.hasMember("frame"))
+      target_.header.frame_id = std::string(motion["frame"]);
+    if (motion.hasMember("position"))
+    {
+      target_.pose.position.x = xmlRpcGetDouble(motion["position"], 0);
+      target_.pose.position.y = xmlRpcGetDouble(motion["position"], 1);
+    }
+    if (motion.hasMember("yaw"))
+    {
+      tf2::Quaternion quat_tf;
+      quat_tf.setRPY(0, 0, motion["yaw"]);
+      geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf);
+      target_.pose.orientation = quat_msg;
+    }
+  }
+  bool move() override
+  {
+    interface_.setGoal(target_);
+    return true;
+  }
+  bool isFinish() override
+  {
+    return interface_.getErrorPos() < chassis_tolerance_position_ &&
+           interface_.getErrorYaw() < chassis_tolerance_angular_;
+  }
+  void stop() override
+  {
+    interface_.stop();
+  }
+
+protected:
+  geometry_msgs::PoseStamped target_;
+  double chassis_tolerance_position_, chassis_tolerance_angular_;
+};
+
+class ChassisTargetMotion : public ChassisMotion
+{
+public:
+  ChassisTargetMotion(XmlRpc::XmlRpcValue& motion, ChassisInterface& interface, tf2_ros::Buffer& tf_buffer)
+    : ChassisMotion(motion, interface), tf_buffer_(tf_buffer)
+  {
+    x_offset_ = xmlRpcGetDouble(motion["offset"], 0);
+    y_offset_ = xmlRpcGetDouble(motion["offset"], 1);
+    yaw_scale_ = xmlRpcGetDouble(motion, "yaw_scale", 1);
+    move_target_ = std::string(motion["target_frame"]);
+  }
+  bool move() override
+  {
+    if (move_target_ == "arm")
+    {
+      try
+      {
+        double roll, pitch, yaw;
+        target_.pose.position.x = JointMotion::arm2base.transform.translation.x;
+        target_.pose.position.y = JointMotion::arm2base.transform.translation.y;
+        quatToRPY(JointMotion::arm2base.transform.rotation, roll, pitch, yaw);
+        tf2::Quaternion quat_tf;
+        quat_tf.setRPY(0, 0, yaw * yaw_scale_);
+        geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf);
+        target_.pose.orientation = quat_msg;
+      }
+      catch (tf2::TransformException& ex)
+      {
+        ROS_WARN("%s", ex.what());
+        return false;
+      }
+    }
+    else
+    {
+      try
+      {
+        double roll, pitch, yaw;
+        geometry_msgs::TransformStamped target2base_link;
+        target2base_link = tf_buffer_.lookupTransform("base_link", move_target_, ros::Time(0));
+        target_.pose.position.x = target2base_link.transform.translation.x;
+        target_.pose.position.y = target2base_link.transform.translation.y;
+        quatToRPY(target2base_link.transform.rotation, roll, pitch, yaw);
+        tf2::Quaternion quat_tf;
+        quat_tf.setRPY(0, 0, yaw * yaw_scale_);
+        geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf);
+        target_.pose.orientation = quat_msg;
+      }
+      catch (tf2::TransformException& ex)
+      {
+        ROS_WARN("%s", ex.what());
+        return false;
+      }
+    }
+
+    interface_.setGoal(target_);
+    return true;
+  }
+
+private:
+  double x_offset_, y_offset_, yaw_scale_;
+  std::string move_target_;
+  tf2_ros::Buffer& tf_buffer_;
+};
+
 };  // namespace engineer_middleware
diff --git a/engineer_middleware/include/engineer_middleware/points.h b/engineer_middleware/include/engineer_middleware/points.h
index c0b69eb..d179ba8 100644
--- a/engineer_middleware/include/engineer_middleware/points.h
+++ b/engineer_middleware/include/engineer_middleware/points.h
@@ -57,6 +57,12 @@ class Points
       points_final_.push_back(p);
     }
   }
+  void generatePointsInLine(double center_x, double center_y, double center_z, double length, double roll, double pitch,
+                            double yaw, double point_resolution)
+  {
+    points_final_.clear();
+    std::vector<Point> points;
+  }
   sensor_msgs::PointCloud2 getPointCloud2()
   {
     sensor_msgs::PointCloud2 cloud2;
diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h
index 253c47c..3f5024d 100644
--- a/engineer_middleware/include/engineer_middleware/step.h
+++ b/engineer_middleware/include/engineer_middleware/step.h
@@ -59,7 +59,7 @@ class Step
     if (step.hasMember("arm"))
     {
       if (step["arm"].hasMember("joints"))
-        arm_motion_ = new JointMotion(step["arm"], arm_group);
+        arm_motion_ = new JointMotion(step["arm"], arm_group, tf);
       else if (step["arm"].hasMember("spacial_shape"))
         arm_motion_ = new SpaceEeMotion(step["arm"], arm_group, tf);
       else
@@ -98,6 +98,8 @@ class Step
       if (step["extend_arm"].hasMember("back"))
         extend_arm_back_motion_ = new ExtendMotion(step["extend_arm"], extend_arm_b_pub, false);
     }
+    if (step.hasMember("chassis_target"))
+      chassis_target_motion_ = new ChassisTargetMotion(step["chassis_target"], chassis_interface, tf);
   }
   bool move()
   {
@@ -139,6 +141,8 @@ class Step
       success &= extend_arm_back_motion_->move();
     if (extend_arm_front_motion_)
       success &= extend_arm_front_motion_->move();
+    if (chassis_target_motion_)
+      success &= chassis_target_motion_->move();
     return success;
   }
   void stop()
@@ -149,6 +153,8 @@ class Step
       hand_motion_->stop();
     if (chassis_motion_)
       chassis_motion_->stop();
+    if (chassis_target_motion_)
+      chassis_target_motion_->stop();
   }
 
   void deleteScene()
@@ -176,6 +182,8 @@ class Step
       success &= gimbal_motion_->isFinish();
     if (reversal_motion_)
       success &= reversal_motion_->isFinish();
+    if (chassis_target_motion_)
+      success &= chassis_target_motion_->isFinish();
     return success;
   }
   bool checkTimeout(ros::Duration period)
@@ -191,6 +199,8 @@ class Step
       success &= chassis_motion_->checkTimeout(period);
     if (gimbal_motion_)
       success &= gimbal_motion_->checkTimeout(period);
+    if (chassis_target_motion_)
+      success &= chassis_target_motion_->checkTimeout(period);
     return success;
   }
 
@@ -216,6 +226,7 @@ class Step
   PlanningScene* planning_scene_{};
   moveit::planning_interface::PlanningSceneInterface planning_scene_interface_;
   moveit::planning_interface::MoveGroupInterface& arm_group_;
+  ChassisTargetMotion* chassis_target_motion_{};
 };
 
 }  // namespace engineer_middleware

From d14681c7184d6456f6d3f8cde110a706ff96e2bc Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 4 May 2024 01:57:40 +0800
Subject: [PATCH 087/147] fix ore lifter don't fall when an ore is in bin.

---
 engineer_middleware/config/steps_list.yaml | 131 +++++++++++++++++++--
 1 file changed, 118 insertions(+), 13 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 46a66f5..32aa9b0 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -329,11 +329,6 @@ common:
           2.6
 
 steps_list:
-  TEST:
-    - step: "test"
-      chassis_target:
-        <<: *TEST
-
   EXCHANGE_POS:
     - step: "gimbal look front"
       gimbal:
@@ -361,6 +356,12 @@ steps_list:
 
   ############################ HOME ##########################
   HOME:
+    - step: "close gripper"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "pre_home"
+      arm:
+        <<: *PRE_HOME
     - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
@@ -371,18 +372,12 @@ steps_list:
       extend_arm:
         front: *EX_CLOSE
         back: *EX_CLOSE
-    - step: "close gripper"
-      gripper:
-        <<: *CLOSE_GRIPPER
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *READY_POS
     - step: "ORE_LIFTER_UP"
       ore_lifter:
         target: *BIN_DOWN_POS
-    - step: "pre_home"
-      arm:
-        <<: *PRE_HOME
     - step: "home"
       arm:
         <<: *HOME
@@ -398,7 +393,7 @@ steps_list:
 
   ################### SMALL_ISLAND_FROM_UP ################
   ########ONE_STONE_ONCE#########
-  ONE_STONE_SMALL_ISLAND:
+  0_TAKE_ONE_STONE_SMALL_ISLAND:
     - step: "GIMBAL_READY"
       gimbal:
         <<: *FRONT_POS
@@ -421,7 +416,7 @@ steps_list:
     - step: "gimbal lifter ready"
       gimbal_lifter:
         target: *LIFTER_SMALL_ISLAND
-  ONE_STONE_SMALL_ISLAND0:
+  0_TAKE_ONE_STONE_SMALL_ISLAND0:
     - step: "GET_STONE"
       arm:
         joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
@@ -500,6 +495,108 @@ steps_list:
     - step: "ADD_STONE"
       stone_num:
         change: "SILVER"
+  1_TAKE_ONE_STONE_SMALL_ISLAND:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "ARM_READY"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "gimbal lifter ready"
+      gimbal_lifter:
+        target: *LIFTER_SMALL_ISLAND
+  1_TAKE_ONE_STONE_SMALL_ISLAND0:
+    - step: "GET_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ARM_UP_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT6_BACK"
+      arm:
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "TURN_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_STORE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "STORE"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    #  ONE_STONE_SMALL_ISLAND00:
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
+    - step: "MOVE_ARM_OUT"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *SLOWLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "CHASSIS_BACK"
+      chassis:
+        <<: *CHASSIS_BACK
+    - step: "CHASSIS_TURN"
+      chassis:
+        <<: *CHASSIS_TURN
+    - step: "pre_home"
+      arm:
+        <<: *PRE_HOME
+    - step: "home"
+      arm:
+        <<: *HOME
+    - step: "ORE_LIFTER_DOWN"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "ADD_STONE"
+      stone_num:
+        change: "SILVER"
 
   ########################## TWO_STONE_ONCE ########################
     ######FIRST_STONE########
@@ -1454,6 +1551,14 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
 
+  CHASSIS_TARGET_TEST2:
+    - step: "test"
+      chassis:
+        frame: "base_link"
+        offset: [0., 0.]
+        yaw_scale: 0.9
+        target_frame: "arm"
+
   CHASSIS_TARGET_TEST1:
     - step: "test"
       chassis:

From 411a94600ad32169ba7657118ef4d601de63e42a Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 4 May 2024 02:00:10 +0800
Subject: [PATCH 088/147] Add arm back which just move back joint 2 and 3.

---
 .../include/engineer_middleware/motion.h      | 28 +++++++++----------
 engineer_middleware/src/middleware.cpp        |  1 +
 2 files changed, 15 insertions(+), 14 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index f216dcc..85fcac2 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -409,6 +409,7 @@ class JointMotion : public MoveitMotionBase
     if (motion.hasMember("record_arm2base"))
       record_arm2base_ = bool(motion["record_arm2base"]);
   }
+  static geometry_msgs::TransformStamped arm2base;
   bool move() override
   {
     if (record_arm2base_)
@@ -418,7 +419,9 @@ class JointMotion : public MoveitMotionBase
         arm2base.header.frame_id = "base_link";
         arm2base.header.stamp = ros::Time::now();
         arm2base.child_frame_id = "chassis_target";
-        arm2base = tf_buffer_.lookupTransform("base_link", "joint4", ros::Time(0));
+
+        arm2base = tf_buffer_.lookupTransform("base_link", "link4", ros::Time(0));
+        ROS_INFO_STREAM("X is: " << arm2base.transform.translation.x << "Y is: " << arm2base.transform.translation.y);
       }
       catch (tf2::TransformException& ex)
       {
@@ -446,7 +449,6 @@ class JointMotion : public MoveitMotionBase
     msg_.data = interface_.plan(plan).val;
     return (interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
   }
-  static geometry_msgs::TransformStamped arm2base;
 
 private:
   bool isReachGoal() override
@@ -763,12 +765,10 @@ class ChassisTargetMotion : public ChassisMotion
     {
       try
       {
-        double roll, pitch, yaw;
-        target_.pose.position.x = JointMotion::arm2base.transform.translation.x;
-        target_.pose.position.y = JointMotion::arm2base.transform.translation.y;
-        quatToRPY(JointMotion::arm2base.transform.rotation, roll, pitch, yaw);
+        target_.pose.position.x = JointMotion::arm2base.transform.translation.x + x_offset_;
+        target_.pose.position.y = JointMotion::arm2base.transform.translation.y + y_offset_;
         tf2::Quaternion quat_tf;
-        quat_tf.setRPY(0, 0, yaw * yaw_scale_);
+        quat_tf.setRPY(0, 0, 0);
         geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf);
         target_.pose.orientation = quat_msg;
       }
@@ -783,11 +783,11 @@ class ChassisTargetMotion : public ChassisMotion
       try
       {
         double roll, pitch, yaw;
-        geometry_msgs::TransformStamped target2base_link;
-        target2base_link = tf_buffer_.lookupTransform("base_link", move_target_, ros::Time(0));
-        target_.pose.position.x = target2base_link.transform.translation.x;
-        target_.pose.position.y = target2base_link.transform.translation.y;
-        quatToRPY(target2base_link.transform.rotation, roll, pitch, yaw);
+        geometry_msgs::TransformStamped base2target;
+        base2target = tf_buffer_.lookupTransform("base_link", move_target_, ros::Time(0));
+        target_.pose.position.x = base2target.transform.translation.x + x_offset_;
+        target_.pose.position.y = base2target.transform.translation.y + y_offset_;
+        quatToRPY(base2target.transform.rotation, roll, pitch, yaw);
         tf2::Quaternion quat_tf;
         quat_tf.setRPY(0, 0, yaw * yaw_scale_);
         geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf);
@@ -805,8 +805,8 @@ class ChassisTargetMotion : public ChassisMotion
   }
 
 private:
-  double x_offset_, y_offset_, yaw_scale_;
-  std::string move_target_;
+  double x_offset_{}, y_offset_{}, yaw_scale_{};
+  std::string move_target_{};
   tf2_ros::Buffer& tf_buffer_;
 };
 
diff --git a/engineer_middleware/src/middleware.cpp b/engineer_middleware/src/middleware.cpp
index 3ef4368..686e9f2 100644
--- a/engineer_middleware/src/middleware.cpp
+++ b/engineer_middleware/src/middleware.cpp
@@ -82,5 +82,6 @@ Middleware::Middleware(ros::NodeHandle& nh)
     ROS_ERROR("no steps list define in yaml");
   as_.start();
 }
+geometry_msgs::TransformStamped engineer_middleware::JointMotion::arm2base;
 
 }  // namespace engineer_middleware

From 9c8e59b8a187dd0412b4986871fb074ca73d4158 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 4 May 2024 16:05:49 +0800
Subject: [PATCH 089/147] Add chassis motion to compensate arm.

---
 .../include/engineer_middleware/motion.h      | 34 ++++++++++++++++---
 1 file changed, 29 insertions(+), 5 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 85fcac2..12de930 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -754,6 +754,10 @@ class ChassisTargetMotion : public ChassisMotion
   ChassisTargetMotion(XmlRpc::XmlRpcValue& motion, ChassisInterface& interface, tf2_ros::Buffer& tf_buffer)
     : ChassisMotion(motion, interface), tf_buffer_(tf_buffer)
   {
+    chassis_tolerance_position_ = xmlRpcGetDouble(motion, "chassis_tolerance_position", 0.01);
+    chassis_tolerance_angular_ = xmlRpcGetDouble(motion, "chassis_tolerance_angular", 0.01);
+    if (motion.hasMember("frame"))
+      target_.header.frame_id = std::string(motion["frame"]);
     x_offset_ = xmlRpcGetDouble(motion["offset"], 0);
     y_offset_ = xmlRpcGetDouble(motion["offset"], 1);
     yaw_scale_ = xmlRpcGetDouble(motion, "yaw_scale", 1);
@@ -763,14 +767,21 @@ class ChassisTargetMotion : public ChassisMotion
   {
     if (move_target_ == "arm")
     {
+      ROS_INFO_STREAM("TARGET IS ARM");
       try
       {
-        target_.pose.position.x = JointMotion::arm2base.transform.translation.x + x_offset_;
-        target_.pose.position.y = JointMotion::arm2base.transform.translation.y + y_offset_;
+        geometry_msgs::TransformStamped arm2base_now = tf_buffer_.lookupTransform("base_link", "link4", ros::Time(0));
+
+        target_.pose.position.x =
+            JointMotion::arm2base.transform.translation.x - arm2base_now.transform.translation.x + x_offset_;
+        target_.pose.position.y =
+            JointMotion::arm2base.transform.translation.y - arm2base_now.transform.translation.y + y_offset_;
         tf2::Quaternion quat_tf;
         quat_tf.setRPY(0, 0, 0);
         geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf);
         target_.pose.orientation = quat_msg;
+        interface_.setGoal(target_);
+        return true;
       }
       catch (tf2::TransformException& ex)
       {
@@ -780,6 +791,7 @@ class ChassisTargetMotion : public ChassisMotion
     }
     else
     {
+      ROS_INFO_STREAM("TARGET IS " << move_target_);
       try
       {
         double roll, pitch, yaw;
@@ -787,11 +799,17 @@ class ChassisTargetMotion : public ChassisMotion
         base2target = tf_buffer_.lookupTransform("base_link", move_target_, ros::Time(0));
         target_.pose.position.x = base2target.transform.translation.x + x_offset_;
         target_.pose.position.y = base2target.transform.translation.y + y_offset_;
+        ROS_INFO_STREAM("base2target x: " << base2target.transform.translation.x);
+        ROS_INFO_STREAM("base2target y: " << base2target.transform.translation.y);
+        ROS_INFO_STREAM("target x: " << target_.pose.position.x);
+        ROS_INFO_STREAM("target y: " << target_.pose.position.y);
         quatToRPY(base2target.transform.rotation, roll, pitch, yaw);
         tf2::Quaternion quat_tf;
         quat_tf.setRPY(0, 0, yaw * yaw_scale_);
         geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf);
         target_.pose.orientation = quat_msg;
+        interface_.setGoal(target_);
+        return true;
       }
       catch (tf2::TransformException& ex)
       {
@@ -799,9 +817,15 @@ class ChassisTargetMotion : public ChassisMotion
         return false;
       }
     }
-
-    interface_.setGoal(target_);
-    return true;
+  }
+  bool isFinish() override
+  {
+    return interface_.getErrorPos() < chassis_tolerance_position_ &&
+           interface_.getErrorYaw() < chassis_tolerance_angular_;
+  }
+  void stop() override
+  {
+    interface_.stop();
   }
 
 private:

From 43528482a676f9534b2e8c5613ae59dfb286c4fd Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 4 May 2024 16:07:35 +0800
Subject: [PATCH 090/147] update sim.launch.

---
 engineer_middleware/launch/sim.launch | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/engineer_middleware/launch/sim.launch b/engineer_middleware/launch/sim.launch
index 5b7857f..394f0ff 100644
--- a/engineer_middleware/launch/sim.launch
+++ b/engineer_middleware/launch/sim.launch
@@ -1,9 +1,10 @@
 <launch>
     <arg name="world" default="empty_world" doc="big_resource small_resource"/>
     <include file="$(find rm_gazebo)/launch/$(arg world).launch"/>
+    <include file="$(find engineer_middleware)/launch/load.launch"/>
+    <include file="$(find engineer_arm_config)/launch/load_move_group.launch"/>
+    <include file="$(find rm_config)/launch/manual.launch"/>
     <include file="$(find engineer_arm_config)/launch/load_controllers.launch">
         <arg name="use_simulation" value="true"/>
     </include>
-    <include file="$(find engineer_middleware)/launch/load.launch"/>
-    <include file="$(find engineer_arm_config)/launch/load_move_group.launch"/>
 </launch>

From 047f146856ad4f1d7a7ef7af641f41d24d38e787 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sun, 5 May 2024 00:02:35 +0800
Subject: [PATCH 091/147] record.

---
 engineer_middleware/config/steps_list.yaml | 148 +++++++++++----------
 1 file changed, 75 insertions(+), 73 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 32aa9b0..3cee057 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -76,7 +76,7 @@ common:
       bin_get_stone: &JOINT2_BIN_GET_STONE
                             0.087
       back_bin_position: &JOINT2_BACK_BIN_POSITION
-                             0.001
+                             0.05
       big_ready: &JOINT2_BIG_READY
                        0.03
       big_arm_up: &JOINT2_BIG_ARM_UP
@@ -1540,6 +1540,21 @@ steps_list:
       ore_lifter:
         target: *BIN_UP_POS
 
+  GIMBAL_DOWN:
+    - step: "gimbal lowest"
+      gimbal_lifter:
+        target: *BUTTON_POS
+
+  GIMBAL_MID:
+    - step: "gimbal mid"
+      gimbal_lifter:
+        target: *LIFTER_BIG_ISLAND
+
+  GIMBAL_TALL:
+    - step: "gimbal tall"
+      gimbal_lifter:
+        target: *TALLEST_POS
+
   ####################################################  TEST  ##########################################################
   ARM_BACK_TEST:
     - step: "arm back test"
@@ -1550,41 +1565,41 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-  CHASSIS_TARGET_TEST2:
+  ARM_BACK_TEST1:
     - step: "test"
-      chassis:
-        frame: "base_link"
-        offset: [0., 0.]
+      chassis_target:
+        frame: base_link
+        position: [ 0., 0. ]
+        yaw: 0.
+        offset: [ 0., 0. ]
         yaw_scale: 0.9
-        target_frame: "arm"
+        target_frame: arm
+        common:
+          timeout: 2.
+
+
+#    - step: "test2"
+#      chassis:
+#        frame: base_link
+#        position: [ 0., 0. ]
+#        yaw: -1.40
+#        chassis_tolerance_position: 0.1
+#        chassis_tolerance_angular: 0.2
+#        common:
+#          timeout: 2.
+
 
   CHASSIS_TARGET_TEST1:
     - step: "test"
-      chassis:
-        frame: "base_link"
+      chassis_target:
+        frame: base_link
         offset: [ 0., 0. ]
         yaw_scale: 0.9
-        target_frame: "exchanger"
-
-  ########## EXCHANGE ############
-  OLD_EXCHANGE_SPHERE:
-    - step: "close to a sphere"
-      arm:
-        frame: exchanger
-        xyz: [ -0.4, 0., -0.25 ]
-        rpy: [ 0., 0., 0. ]
-        rpy_rectify: [ 0.01,0.1,0.03 ]
-        drift_dimensions: [ true, true, true, true, true, true ]
-        tolerance_position: 0.1
-        tolerance_orientation: 0.01
-        spacial_shape: SPHERE
-        radius: 0.5
-        point_resolution: 0.5
-        max_planning_times: 1
+        target_frame: exchanger
         common:
-          <<: *QUICKLY
+          timeout: 2.
 
+  ########## EXCHANGE ############
   EXCHANGE_DOWN:
     - step: "test new exchange"
       arm:
@@ -1602,28 +1617,13 @@ steps_list:
         common:
           <<: *NORMALLY
 
-  TEST_ORI:
-    - step: "ori test"
-      arm:
-        frame: exchanger
-        rpy: [ 0., -1.5707963, 0. ]
-        tolerance_orientation: 0.001
-
-  TEST_ORI2:
-    - step: "ori test2"
-      arm:
-        frame: exchanger
-        rpy: [ 0., -1.5707963, 0. ]
-        xyz: [ -0.3, 0.2, -0.2 ]
-        tolerance_position: 0.01
-        tolerance_orientation: 0.001
-
-  EXCHANGE_ORI:
+  EXCHANGE_STRAIGHT:
     - step: "test new exchange"
       arm:
         frame: exchanger
         is_refer_planning_frame: true
-        rpy: [ 0., -1.5707963, 0. ]
+        xyz: [ -0.3, 0., 0. ]
+        rpy: [ 0., 3.1415926, 0. ]
         drift_dimensions: [ true, true, true, true, true, true ]
         tolerance_position: 0.01
         tolerance_orientation: 0.001
@@ -1633,8 +1633,17 @@ steps_list:
         max_planning_times: 1
         common:
           <<: *NORMALLY
-  EXCHANGE_STRAIGHT:
-    - step: "test new exchange"
+
+  AUTO_TEST:
+    - step: "chassis approach"
+      chassis_target:
+        frame: base_link
+        offset: [ 0., 0. ]
+        yaw_scale: 0.9
+        target_frame: exchanger
+        common:
+          timeout: 2.
+    - step: "auto sphere exchange"
       arm:
         frame: exchanger
         is_refer_planning_frame: true
@@ -1649,29 +1658,22 @@ steps_list:
         max_planning_times: 1
         common:
           <<: *NORMALLY
-
-
-  GIMBAL_DOWN:
-    - step: "gimbal lowest"
-      gimbal_lifter:
-        target: *BUTTON_POS
-
-  GIMBAL_MID:
-    - step: "gimbal mid"
-      gimbal_lifter:
-        target: *LIFTER_BIG_ISLAND
-
-  GIMBAL_TALL:
-    - step: "gimbal tall"
-      gimbal_lifter:
-        target: *TALLEST_POS
-
-  CHASSIS_TEST1:
-    - step: "chassis turn"
-      chassis:
-        <<: *CHASSIS_TURN
-
-  CHASSIS_TEST2:
-    - step: "chassis back"
-      chassis:
-        <<: *CHASSIS_BACK
+    - step: "arm back"
+      arm:
+        joints: [ "KEEP", *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, "KEEP", "KEEP", "KEEP", "KEEP" ]
+        record_arm2base: true
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+  AUTO_TEST0:
+    - step: "chassis compensation"
+      chassis_target:
+        frame: base_link
+        position: [ 0., 0. ]
+        yaw: 0.
+        offset: [ 0., 0. ]
+        yaw_scale: 0.9
+        target_frame: arm
+        common:
+          timeout: 2.

From acc1fa6cd493aa1ada4faa5dd7c83a62266dd23b Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 8 May 2024 02:06:37 +0800
Subject: [PATCH 092/147] record steps list.

---
 engineer_middleware/config/steps_list.yaml | 327 +++++++++++++++++----
 1 file changed, 262 insertions(+), 65 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 3cee057..1678a00 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -19,9 +19,9 @@ common:
     normal_tolerance: &NORMAL_TOLERANCE
       tolerance_joints: [ 0.01, 0.010, 0.015, 0.3, 0.2, 0.15, 0.07 ]
     bigger_tolerance: &BIGGER_TOLERANCE
-      tolerance_joints: [ 0.005, 0.010, 0.01, 0.3, 0.2, 0.2 ]
+      tolerance_joints: [ 0.015, 0.015, 0.02, 0.3, 0.2, 0.2, 0.2 ]
     max_tolerance: &MAX_TOLERANCE
-      tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 ]
+      tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3]
 
   joint1:
     mechanical:
@@ -49,6 +49,8 @@ common:
                       0.320
       big_ready_store: &JOINT1_BIG_READY_STORE
                       0.133
+      bin_get_stone: &JOINT1_BIN_GET_STONE
+        0.1668
     home:
       zero_stone_position: &JOINT1_ZERO_STONE_POSITION
                              0.01
@@ -74,9 +76,11 @@ common:
       small_store: &JOINT2_SMALL_STORE
                       0.044
       bin_get_stone: &JOINT2_BIN_GET_STONE
-                            0.087
+                            0.13
       back_bin_position: &JOINT2_BACK_BIN_POSITION
                              0.05
+      avoid_collision: &JOINT2_AVOID_COLLISION
+                        0.1
       big_ready: &JOINT2_BIG_READY
                        0.03
       big_arm_up: &JOINT2_BIG_ARM_UP
@@ -104,6 +108,8 @@ common:
                    0.257
       bin_get_stone: &JOINT3_BIN_GET_STONE
                           0.252
+      bin_get_lv4_stone: &JOINT3_BIN_GET_LV4_STONE
+                           0.31
       big_ready: &JOINT3_BIG_READY
                      0.071
       get_big_mid: &JOINT3_GET_BIG_MID
@@ -213,16 +219,20 @@ common:
                   1.594
 
   gimbal:
-    side_pos: &SIDE_POS
+    right_pos: &RIGHT_POS
       frame: gimbal_lifter
       position: [ 0.001 , -3., 0. ]
+    left_pos: &LEFT_POS
+      frame: gimbal_lifter
+      position: [ 0.001, 3., 0. ]
     front_pos: &FRONT_POS
       frame: gimbal_lifter
       position: [ 2., 0.001, 0. ]
-    small_island_pos: &SMALL_ISLAND_POS
+    small_island_pos: &BIG_ISLAND_POS
       frame: gimbal_lifter
       position: [ 1., -1., 0.]
 
+
   gimbal_lifter:
     button_pos: &BUTTON_POS
       0.05
@@ -248,9 +258,9 @@ common:
     pre_home: &PRE_HOME
       joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_PRE_HOME, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
       common:
-        <<: *NORMALLY
+        <<: *QUICKLY
       tolerance:
-        <<: *SMALL_TOLERANCE
+        <<: *NORMAL_TOLERANCE
 
     home: &HOME
       joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
@@ -314,6 +324,8 @@ common:
   ore_lifter:
     bin_up_pos: &BIN_UP_POS
             0.2
+    small_up_pos: &BIN_SMALL_UP_POS
+      0.03
     bin_mid_pos: &BIN_MID_POS
             0.1
     bin_down_pos: &BIN_DOWN_POS
@@ -344,14 +356,6 @@ steps_list:
       gimbal_lifter:
         target: *TALLEST_POS
 
-  GIMBAL_FRONT:
-    - step: "gimbal look front"
-      gimbal:
-        <<: *FRONT_POS
-  GIMBAL_LEFT:
-    - step: "gimbal look left"
-      gimbal:
-        <<: *SIDE_POS
   ############ MID #############
 
   ############################ HOME ##########################
@@ -407,7 +411,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "OPEN_GRIPPER"
@@ -442,28 +446,28 @@ steps_list:
       arm:
         joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "TURN_STONE"
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_STORE"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 #  ONE_STONE_SMALL_ISLAND00:
@@ -474,7 +478,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *SLOWLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "CHASSIS_BACK"
@@ -509,7 +513,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "OPEN_GRIPPER"
@@ -544,28 +548,28 @@ steps_list:
       arm:
         joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "TURN_STONE"
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_STORE"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     #  ONE_STONE_SMALL_ISLAND00:
@@ -576,7 +580,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *SLOWLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "CHASSIS_BACK"
@@ -614,7 +618,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "OPEN_GRIPPER"
@@ -642,35 +646,35 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT6_BACK"
       arm:
         joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "TURN_STONE"
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_STORE"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "CLOSE_GRIPPER"
@@ -680,7 +684,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *SLOWLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ORE_LIFTER_DOWN"
@@ -705,7 +709,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "OPEN_GRIPPER"
@@ -730,35 +734,35 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT6_BACK"
       arm:
         joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "TURN_STONE"
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_STORE"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "CLOSE_GRIPPER"
@@ -768,7 +772,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *SLOWLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "CHASSIS_BACK"
@@ -1046,6 +1050,137 @@ steps_list:
         change: "SILVER"
 
   ######GET STONE FROM BIN#########
+
+  GET_DOWN_STONE_BIN:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *EXCHANGE_POS
+    - step: "ORE_LIFTER_UP"
+      ore_lifter:
+        target: *BIN_UP_POS
+    - step: "JOINT2 move out"
+      arm:
+        joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_TO_GET"
+      arm:
+        joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_LV4_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "JOINT2_BACK_GET_STONE"
+      arm:
+        joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT5_LIFT_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "exchange pos"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ORE_LIFTER_DOWN"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "gimbal lifter tallest"
+      gimbal_lifter:
+        target: *TALLEST_POS
+    - step: "remove stone"
+      stone_num:
+        change: "-1"
+
+  GET_UP_STONE_BIN:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *EXCHANGE_POS
+    - step: "roe lifter ready"
+      ore_lifter:
+        target: *BIN_SMALL_UP_POS
+    - step: "JOINT2 move out"
+      arm:
+        joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "READY_TO_GET"
+      arm:
+        joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_LV4_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "JOINT2_BACK_GET_STONE"
+      arm:
+        joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT5_LIFT_STONE"
+      arm:
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "exchange pos"
+      arm:
+        joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ORE_LIFTER_DOWN"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "gimbal lifter tallest"
+      gimbal_lifter:
+        target: *TALLEST_POS
+    - step: "remove stone"
+      stone_num:
+        change: "-1"
+
   GET_DOWN_STONE_LEFT:
     - step: "GIMBAL_READY"
       gimbal:
@@ -1056,11 +1191,18 @@ steps_list:
     - step: "ORE_LIFTER_UP"
       ore_lifter:
         target: *BIN_GET_DOWN
+    - step: "JOINT2 move out"
+      arm:
+        joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "READY_TO_GET"
       arm:
         joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "OPEN_GRIPPER"
@@ -1070,7 +1212,7 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
@@ -1114,11 +1256,18 @@ steps_list:
     - step: "ORE_LIFTER_UP"
       ore_lifter:
         target: *BIN_GET_DOWN
+    - step: "JOINT2 move out"
+      arm:
+        joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "READY_TO_GET"
       arm:
         joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "OPEN_GRIPPER"
@@ -1128,7 +1277,7 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
@@ -1172,11 +1321,18 @@ steps_list:
     - step: "ORE_LIFTER_READY"
       ore_lifter:
         target: *BIN_GET_UP
+    - step: "JOINT2 move out"
+      arm:
+        joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "READY_TO_GET"
       arm:
         joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "OPEN_GRIPPER"
@@ -1186,7 +1342,7 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
@@ -1230,11 +1386,18 @@ steps_list:
     - step: "ORE_LIFTER_READY"
       ore_lifter:
         target: *BIN_GET_UP
+    - step: "JOINT2 move out"
+      arm:
+        joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "READY_TO_GET"
       arm:
         joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "OPEN_GRIPPER"
@@ -1244,7 +1407,7 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
@@ -1283,7 +1446,7 @@ steps_list:
   MID_BIG_ISLAND:
     - step: "gimbal ready"
       gimbal:
-        <<: *SMALL_ISLAND_POS
+        <<: *BIG_ISLAND_POS
     - step: "open ex arm"
       extend_arm:
         front: *EX_OPEN
@@ -1292,7 +1455,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
         common:
-         <<: *SLOWLY
+         <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ORE_ROTATOR_READY"
@@ -1312,7 +1475,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ADJUST_STONE"
@@ -1336,28 +1499,28 @@ steps_list:
       arm:
         joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ARM_UP_STONE"
       arm:
         joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE"
       arm:
         joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE_STONE"
       arm:
         joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 #  MID_BIG_ISLAND00:
@@ -1368,7 +1531,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "pre_home"
@@ -1398,7 +1561,7 @@ steps_list:
   SIDE_BIG_ISLAND:
     - step: "gimbal ready"
       gimbal:
-        <<: *SMALL_ISLAND_POS
+        <<: *BIG_ISLAND_POS
     - step: "open ex arm"
       extend_arm:
         front: *EX_OPEN
@@ -1407,7 +1570,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
         common:
-          <<: *SLOWLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ORE_ROTATOR_READY"
@@ -1428,7 +1591,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ADJUST_STONE"
@@ -1452,28 +1615,28 @@ steps_list:
       arm:
         joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ARM_UP_STONE"
       arm:
         joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_STORE"
       arm:
         joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE_STONE"
       arm:
         joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ]
         common:
-          <<: *SLOWLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "CLOSE_GRIPPER"
@@ -1483,7 +1646,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "pre_home"
@@ -1508,6 +1671,11 @@ steps_list:
     - step: "gimbal front"
       gimbal:
         <<: *FRONT_POS
+
+  ################## STORE #################
+
+
+
   ##########GIMBAL_LIFTER##########
   ZERO_STONE_GIMBAL_LIFTER:
     - step: "gimbal lifter zero stone pos"
@@ -1540,6 +1708,8 @@ steps_list:
       ore_lifter:
         target: *BIN_UP_POS
 
+  ################ GIMBAL LIFTER ###############
+
   GIMBAL_DOWN:
     - step: "gimbal lowest"
       gimbal_lifter:
@@ -1555,6 +1725,33 @@ steps_list:
       gimbal_lifter:
         target: *TALLEST_POS
 
+  ############## ORE ROTATOR ###############
+  ORE_ROTATOR_INIT:
+    - step: "ore rotator init pos"
+      ore_rotator:
+        target: *READY_POS
+
+  ORE_ROTATOR_EXCHANGE:
+    - step: "ore rotator exchange pos"
+      ore_rotator:
+        target: *EXCHANGE_POS
+
+  ########### GIMBAL ##########
+
+  GIMBAL_FRONT:
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
+  GIMBAL_LEFT:
+    - step: "gimbal look left"
+      gimbal:
+        <<: *LEFT_POS
+  GIMBAL_RIGHT:
+    - step: "gimbal look right"
+      gimbal:
+        <<: *RIGHT_POS
+
+
   ####################################################  TEST  ##########################################################
   ARM_BACK_TEST:
     - step: "arm back test"

From 3c829836ec246cb8c159719b91056835b189c8ab Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 8 May 2024 23:57:17 +0800
Subject: [PATCH 093/147] record.

---
 engineer_middleware/config/steps_list.yaml | 29 ++++++++++++----------
 1 file changed, 16 insertions(+), 13 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 1678a00..b42d2a0 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -51,6 +51,8 @@ common:
                       0.133
       bin_get_stone: &JOINT1_BIN_GET_STONE
         0.1668
+      exchange_pos: &JOINT1_EXCHANGE_POS
+        0.1
     home:
       zero_stone_position: &JOINT1_ZERO_STONE_POSITION
                              0.01
@@ -258,7 +260,7 @@ common:
     pre_home: &PRE_HOME
       joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_PRE_HOME, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
       common:
-        <<: *QUICKLY
+        <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
 
@@ -334,6 +336,7 @@ common:
       0.033
     bin_get_down: &BIN_GET_DOWN
       0.225
+
   extend_arm:
     close: &EX_CLOSE
           0.0
@@ -347,7 +350,7 @@ steps_list:
         <<: *FRONT_POS
     - step: "move arm"
       arm:
-        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
+        joints: [*JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
         common:
           <<: *NORMALLY
         tolerance:
@@ -478,7 +481,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "CHASSIS_BACK"
@@ -580,7 +583,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "CHASSIS_BACK"
@@ -684,7 +687,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ORE_LIFTER_DOWN"
@@ -772,7 +775,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "CHASSIS_BACK"
@@ -1103,7 +1106,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ORE_LIFTER_DOWN"
@@ -1123,9 +1126,9 @@ steps_list:
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *EXCHANGE_POS
-    - step: "roe lifter ready"
-      ore_lifter:
-        target: *BIN_SMALL_UP_POS
+#    - step: "roe lifter ready"
+#      ore_lifter:
+#        target: *BIN_SMALL_UP_POS
     - step: "JOINT2 move out"
       arm:
         joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
@@ -1168,7 +1171,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ORE_LIFTER_DOWN"
@@ -1260,7 +1263,7 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_GET"
@@ -1646,7 +1649,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "pre_home"

From 51f0fb3c95738d126d74764d179967d380b73dc3 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 11 May 2024 17:53:19 +0800
Subject: [PATCH 094/147] record steps list.

---
 engineer_middleware/config/steps_list.yaml | 39 +++++++++++++---------
 1 file changed, 23 insertions(+), 16 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index b42d2a0..1562acf 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -38,9 +38,9 @@ common:
       up_pos: &JOINT1_UP_POS
                    0.312
       bin_get_stone_left: &JOINT1_BIN_GET_STONE_LEFT
-                     0.232
+                     0.2
       bin_get_stone_right: &JOINT1_BIN_GET_STONE_RIGHT
-                            0.144
+                            0.1131
       big_ready: &JOINT1_BIG_READY
                      0.006
       big_adjust_mid: &JOINT1_BIG_ADJUST_MID
@@ -50,7 +50,7 @@ common:
       big_ready_store: &JOINT1_BIG_READY_STORE
                       0.133
       bin_get_stone: &JOINT1_BIN_GET_STONE
-        0.1668
+        0.143
       exchange_pos: &JOINT1_EXCHANGE_POS
         0.1
     home:
@@ -80,7 +80,7 @@ common:
       bin_get_stone: &JOINT2_BIN_GET_STONE
                             0.13
       back_bin_position: &JOINT2_BACK_BIN_POSITION
-                             0.05
+                             0.07
       avoid_collision: &JOINT2_AVOID_COLLISION
                         0.1
       big_ready: &JOINT2_BIG_READY
@@ -154,7 +154,7 @@ common:
       right_90_position: &JOINT5_R90_POSITION
                            1.59
       bin_get_stone: &JOINT5_BIN_GET_STONE
-                           1.679
+                           1.603
       big_ready: &JOINT5_BIG_READY
                   -0.017
       big_ready_store: &JOINT5_BIG_READY_STORE
@@ -216,9 +216,9 @@ common:
       exchange: &JOINT7_EXCHANGE
                   -1.771
       bin_get_left: &JOINT7_BIN_GET_LEFT
-                  -1.526
+                  -1.5774
       bin_get_right: &JOINT7_BIN_GET_RIGHT
-                  1.594
+                  1.68
 
   gimbal:
     right_pos: &RIGHT_POS
@@ -321,13 +321,13 @@ common:
     ready_pos: &READY_POS
             0.001
     exchange_pos: &EXCHANGE_POS
-            1.59
+            1.6
 
   ore_lifter:
     bin_up_pos: &BIN_UP_POS
-            0.2
+            0.22
     small_up_pos: &BIN_SMALL_UP_POS
-      0.03
+      0.01
     bin_mid_pos: &BIN_MID_POS
             0.1
     bin_down_pos: &BIN_DOWN_POS
@@ -1078,6 +1078,7 @@ steps_list:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+  GET_DOWN_STONE_BIN0:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
@@ -1085,7 +1086,7 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
@@ -1126,9 +1127,9 @@ steps_list:
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *EXCHANGE_POS
-#    - step: "roe lifter ready"
-#      ore_lifter:
-#        target: *BIN_SMALL_UP_POS
+    - step: "roe lifter ready"
+      ore_lifter:
+        target: *BIN_SMALL_UP_POS
     - step: "JOINT2 move out"
       arm:
         joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
@@ -1143,6 +1144,7 @@ steps_list:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+  GET_UP_STONE_BIN0:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
@@ -1153,6 +1155,7 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+  GET_UP_STONE_BIN00:
     - step: "JOINT1_UP"
       arm:
         joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
@@ -1208,6 +1211,7 @@ steps_list:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+  GET_DOWN_STONE_LEFT0:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
@@ -1215,7 +1219,7 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
@@ -1273,6 +1277,7 @@ steps_list:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+  GET_DOWN_STONE_RIGHT0:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
@@ -1280,7 +1285,7 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
@@ -1338,6 +1343,7 @@ steps_list:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+  GET_UP_STONE_LEFT0:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
@@ -1403,6 +1409,7 @@ steps_list:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+  GET_UP_STONE_RIGHT0:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER

From ab06274d79ae316291ea1d30f310cee38cb8a180 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 13 May 2024 02:40:57 +0800
Subject: [PATCH 095/147] adjust max vel.

---
 engineer_middleware/config/engineer.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/engineer_middleware/config/engineer.yaml b/engineer_middleware/config/engineer.yaml
index fb1caf4..24d6f86 100644
--- a/engineer_middleware/config/engineer.yaml
+++ b/engineer_middleware/config/engineer.yaml
@@ -6,4 +6,4 @@ chassis:
   yaw:
     pid: { p: 5., i: 0.0, d: 0.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
   yaw_start_threshold: 0.05
-  max_vel: 0.1
+  max_vel: 10

From 5c4f322408e9174738a58ced0eea179a460c640d Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 13 May 2024 02:41:36 +0800
Subject: [PATCH 096/147] adjust motion order.

---
 engineer_middleware/config/steps_list.yaml | 209 ++++++++-------------
 1 file changed, 79 insertions(+), 130 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 1562acf..a667e61 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -15,11 +15,11 @@ common:
 
   tolerance:
     small_tolerance: &SMALL_TOLERANCE
-      tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.05 ]
+      tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.03 ]
     normal_tolerance: &NORMAL_TOLERANCE
-      tolerance_joints: [ 0.01, 0.010, 0.015, 0.3, 0.2, 0.15, 0.07 ]
+      tolerance_joints: [ 0.01, 0.010, 0.02, 0.3, 0.3, 0.15, 0.05 ]
     bigger_tolerance: &BIGGER_TOLERANCE
-      tolerance_joints: [ 0.015, 0.015, 0.02, 0.3, 0.2, 0.2, 0.2 ]
+      tolerance_joints: [ 0.015, 0.015, 0.02, 0.3, 0.2, 0.2, 0.1 ]
     max_tolerance: &MAX_TOLERANCE
       tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3]
 
@@ -30,7 +30,7 @@ common:
       small_ready: &JOINT1_SMALL_READY
                       0.487
       small_down: &JOINT1_SMALL_DOWN
-                  0.31
+                  0.3677
       small_ready_store: &JOINT1_SMALL_READY_STORE
                            0.254
       small_ready_store_third: &JOINT1_SMALL_READY_STORE_THIRD
@@ -50,7 +50,7 @@ common:
       big_ready_store: &JOINT1_BIG_READY_STORE
                       0.133
       bin_get_stone: &JOINT1_BIN_GET_STONE
-        0.143
+        0.1521
       exchange_pos: &JOINT1_EXCHANGE_POS
         0.1
     home:
@@ -76,11 +76,11 @@ common:
       small_ready: &JOINT2_SMALL_READY
                       0.029
       small_store: &JOINT2_SMALL_STORE
-                      0.044
+                      0.064040
       bin_get_stone: &JOINT2_BIN_GET_STONE
                             0.13
       back_bin_position: &JOINT2_BACK_BIN_POSITION
-                             0.07
+                             0.06651
       avoid_collision: &JOINT2_AVOID_COLLISION
                         0.1
       big_ready: &JOINT2_BIG_READY
@@ -109,7 +109,7 @@ common:
       small_turn: &JOINT3_SMALL_TURN
                    0.257
       bin_get_stone: &JOINT3_BIN_GET_STONE
-                          0.252
+                          0.2608
       bin_get_lv4_stone: &JOINT3_BIN_GET_LV4_STONE
                            0.31
       big_ready: &JOINT3_BIG_READY
@@ -154,7 +154,7 @@ common:
       right_90_position: &JOINT5_R90_POSITION
                            1.59
       bin_get_stone: &JOINT5_BIN_GET_STONE
-                           1.603
+                           1.5798
       big_ready: &JOINT5_BIG_READY
                   -0.017
       big_ready_store: &JOINT5_BIG_READY_STORE
@@ -185,7 +185,7 @@ common:
       small_ready_store: &JOINT6_SMALL_READY_STORE
                    -1.633
       bin_get_stone: &JOINT6_BIN_GET_STONE
-                      -1.611
+                      -1.5936
       big_ready: &JOINT6_BIG_READY
                        0.014
       big_adjust_mid: &JOINT6_BIG_ADJUST_MID
@@ -200,7 +200,7 @@ common:
   joint7:
     mechanical:
       mid_position: &JOINT7_MID_POSITION
-                      0.044
+                      -0.098300
       left_90_position: &JOINT7_L90_POSITION
                           -1.56
       right_90_position: &JOINT7_R90_POSITION
@@ -215,10 +215,12 @@ common:
                           1.722
       exchange: &JOINT7_EXCHANGE
                   -1.771
+      lv4_ore: &JOINT7_LV4_ORE
+                 -0.098300
       bin_get_left: &JOINT7_BIN_GET_LEFT
-                  -1.5774
+                  -1.6973
       bin_get_right: &JOINT7_BIN_GET_RIGHT
-                  1.68
+                  1.554
 
   gimbal:
     right_pos: &RIGHT_POS
@@ -292,6 +294,7 @@ common:
       chassis_tolerance_angular: 0.3
       common:
         timeout: 2.
+
     chassis_180: &CHASSIS_180
       frame: base_link
       position: [ 0., 0. ]
@@ -300,6 +303,7 @@ common:
       chassis_tolerance_angular: 0.2
       common:
         timeout: 2.
+
     chassis_90: &CHASSIS_TURN
       frame: base_link
       position: [ 0., 0. ]
@@ -308,6 +312,7 @@ common:
       chassis_tolerance_angular: 0.2
       common:
         timeout: 2.
+
     chassis_back: &CHASSIS_BACK
       frame: base_link
       position: [ -0.4, 0. ]
@@ -679,7 +684,7 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -698,7 +703,6 @@ steps_list:
         change: "SILVER"
 
     #####SECOND_STONE########
-#  TWO_STONE_SMALL_ISLAND00:
     - step: "GIMBAL_READY"
       gimbal:
         <<: *FRONT_POS
@@ -718,7 +722,6 @@ steps_list:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
-#  TWO_STONE_SMALL_ISLAND000:
     - step: "GET_STONE"
       arm:
         joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
@@ -767,32 +770,33 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
-    - step: "MOVE_ARM_OUT"
-      arm:
-        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
+    - step: "ADD_STONE"
+      stone_num:
+        change: "SILVER"
     - step: "CHASSIS_BACK"
       chassis:
         <<: *CHASSIS_BACK
     - step: "CHASSIS_TURN"
       chassis:
         <<: *CHASSIS_TURN
+    - step: "MOVE_ARM_OUT"
+      arm:
+        joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
     - step: "pre_home"
       arm:
         <<: *PRE_HOME
     - step: "home"
       arm:
         <<: *HOME
-    - step: "ADD_STONE"
-      stone_num:
-        change: "SILVER"
+
 
   ######THREE_STONE_ONCE(yi jian san lian)#########
     ######FIRST_STONE########
@@ -1068,7 +1072,7 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_GET"
@@ -1078,7 +1082,6 @@ steps_list:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-  GET_DOWN_STONE_BIN0:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
@@ -1091,14 +1094,7 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "JOINT5_LIFT_STONE"
-      arm:
-        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
         common:
           <<: *QUICKLY
         tolerance:
@@ -1110,15 +1106,16 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "remove stone"
+      stone_num:
+        change: "-1"
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
     - step: "gimbal lifter tallest"
       gimbal_lifter:
         target: *TALLEST_POS
-    - step: "remove stone"
-      stone_num:
-        change: "-1"
+
 
   GET_UP_STONE_BIN:
     - step: "GIMBAL_READY"
@@ -1134,17 +1131,16 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_GET"
       arm:
         joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_LV4_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-  GET_UP_STONE_BIN0:
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
@@ -1155,17 +1151,9 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-  GET_UP_STONE_BIN00:
     - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "JOINT5_LIFT_STONE"
-      arm:
-        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
         common:
           <<: *QUICKLY
         tolerance:
@@ -1177,15 +1165,16 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "remove stone"
+      stone_num:
+        change: "-1"
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
     - step: "gimbal lifter tallest"
       gimbal_lifter:
         target: *TALLEST_POS
-    - step: "remove stone"
-      stone_num:
-        change: "-1"
+
 
   GET_DOWN_STONE_LEFT:
     - step: "GIMBAL_READY"
@@ -1201,17 +1190,16 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_GET"
       arm:
         joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
-  GET_DOWN_STONE_LEFT0:
+          <<: *BIGGER_TOLERANCE
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
@@ -1224,18 +1212,14 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "JOINT5_LIFT_STONE"
-      arm:
-        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "remove stone"
+      stone_num:
+        change: "-1"
     - step: "exchange pos"
       arm:
         joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
@@ -1249,9 +1233,7 @@ steps_list:
     - step: "gimbal lifter tallest"
       gimbal_lifter:
         target: *TALLEST_POS
-    - step: "remove stone"
-      stone_num:
-        change: "-1"
+
 
   GET_DOWN_STONE_RIGHT:
     - step: "GIMBAL_READY"
@@ -1267,17 +1249,16 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_GET"
       arm:
         joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
-  GET_DOWN_STONE_RIGHT0:
+          <<: *BIGGER_TOLERANCE
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
@@ -1290,14 +1271,7 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "JOINT5_LIFT_STONE"
-      arm:
-        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1309,15 +1283,16 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "remove stone"
+      stone_num:
+        change: "-1"
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
     - step: "gimbal lifter tallest"
       gimbal_lifter:
         target: *TALLEST_POS
-    - step: "remove stone"
-      stone_num:
-        change: "-1"
+
 
   GET_UP_STONE_LEFT:
     - step: "GIMBAL_READY"
@@ -1333,17 +1308,16 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_GET"
       arm:
         joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
-  GET_UP_STONE_LEFT0:
+          <<: *BIGGER_TOLERANCE
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
@@ -1351,19 +1325,12 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "JOINT5_LIFT_STONE"
-      arm:
-        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1375,15 +1342,16 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "remove stone"
+      stone_num:
+        change: "-1"
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
     - step: "gimbal lifter tallest"
       gimbal_lifter:
         target: *TALLEST_POS
-    - step: "remove stone"
-      stone_num:
-        change: "-1"
+
 
   GET_UP_STONE_RIGHT:
     - step: "GIMBAL_READY"
@@ -1399,17 +1367,16 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_TO_GET"
       arm:
         joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
-  GET_UP_STONE_RIGHT0:
+          <<: *BIGGER_TOLERANCE
     - step: "OPEN_GRIPPER"
       gripper:
         <<: *OPEN_GRIPPER
@@ -1417,21 +1384,14 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "JOINT5_LIFT_STONE"
-      arm:
-        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "exchange pos"
@@ -1441,15 +1401,16 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "remove stone"
+      stone_num:
+        change: "-1"
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
     - step: "gimbal lifter tallest"
       gimbal_lifter:
         target: *TALLEST_POS
-    - step: "remove stone"
-      stone_num:
-        change: "-1"
+
 
   #################BIG_ISLAND#############
   ######MID_STONE#######
@@ -1471,9 +1432,6 @@ steps_list:
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *EXCHANGE_POS
-#    - step: "ORE_LIFTER_READY"
-#      ore_lifter:
-#        target: *BIN_MID_POS
     - step: "gimbal lifter"
       gimbal_lifter:
         target: *LIFTER_BIG_ISLAND
@@ -1502,9 +1460,6 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-#    - step: "chassis move left"
-#      chassis:
-#        <<: *CHASSIS_LEFT_15
     - step: "JOINT1_UP"
       arm:
         joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
@@ -1586,9 +1541,6 @@ steps_list:
     - step: "ORE_ROTATOR_READY"
       ore_rotator:
         target: *EXCHANGE_POS
-#    - step: "ORE_LIFTER_READY"
-#      ore_lifter:
-#        target: *BIN_MID_POS
     - step: "gimbal lifter"
       gimbal_lifter:
         target: *LIFTER_BIG_ISLAND
@@ -1618,9 +1570,6 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    #    - step: "chassis move left"
-    #      chassis:
-    #        <<: *CHASSIS_LEFT_15
     - step: "JOINT1_UP"
       arm:
         joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]

From e548a091abe51c44a0939d03f175cf5542607b34 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 13 May 2024 21:46:13 +0800
Subject: [PATCH 097/147] adjust tolerance.

---
 engineer_middleware/config/steps_list.yaml | 46 +++++++++++++++-------
 1 file changed, 31 insertions(+), 15 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index a667e61..74a8a9f 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -477,8 +477,7 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
-#  ONE_STONE_SMALL_ISLAND00:
+          <<: *BIGGER_TOLERANCE
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -488,7 +487,7 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "CHASSIS_BACK"
       chassis:
         <<: *CHASSIS_BACK
@@ -579,8 +578,7 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
-    #  ONE_STONE_SMALL_ISLAND00:
+          <<: *BIGGER_TOLERANCE
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -777,12 +775,6 @@ steps_list:
     - step: "ADD_STONE"
       stone_num:
         change: "SILVER"
-    - step: "CHASSIS_BACK"
-      chassis:
-        <<: *CHASSIS_BACK
-    - step: "CHASSIS_TURN"
-      chassis:
-        <<: *CHASSIS_TURN
     - step: "MOVE_ARM_OUT"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
@@ -790,6 +782,12 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
+    - step: "CHASSIS_BACK"
+      chassis:
+        <<: *CHASSIS_BACK
+    - step: "CHASSIS_TURN"
+      chassis:
+        <<: *CHASSIS_TURN
     - step: "pre_home"
       arm:
         <<: *PRE_HOME
@@ -1424,7 +1422,7 @@ steps_list:
         back: *EX_OPEN
     - step: "MID_ARM_READY"
       arm:
-        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ]
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY ]
         common:
          <<: *QUICKLY
         tolerance:
@@ -1460,6 +1458,9 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "left"
+      chassis:
+        <<: *CHASSIS_LEFT_15
     - step: "JOINT1_UP"
       arm:
         joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
@@ -1492,6 +1493,9 @@ steps_list:
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
+    - step: "add stone"
+      stone_num:
+        change: "GOLD"
     - step: "move arm out"
       arm:
         joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
@@ -1499,6 +1503,12 @@ steps_list:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "chassis left"
+      chassis:
+        <<: *CHASSIS_LEFT_15
+    - step: "chassis turn"
+      chassis:
+        <<: *CHASSIS_180
     - step: "pre_home"
       arm:
         <<: *PRE_HOME
@@ -1515,9 +1525,6 @@ steps_list:
       extend_arm:
         front: *EX_CLOSE
         back: *EX_CLOSE
-    - step: "add stone"
-      stone_num:
-        change: "GOLD"
     - step: "gimbal front"
       gimbal:
         <<: *FRONT_POS
@@ -1570,6 +1577,9 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "left"
+      chassis:
+        <<: *CHASSIS_LEFT_15
     - step: "JOINT1_UP"
       arm:
         joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
@@ -1617,6 +1627,12 @@ steps_list:
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
+    - step: "chassis left"
+      chassis:
+        <<: *CHASSIS_LEFT_15
+    - step: "chassis turn"
+      chassis:
+        <<: *CHASSIS_180
     - step: "ORE_ROTATOR_RESET"
       ore_rotator:
         target: *READY_POS

From 9d0a14f0569b2a9798bfed9ee492decbc641fed9 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 15 May 2024 01:05:35 +0800
Subject: [PATCH 098/147] adjust tolerance.

---
 engineer_middleware/config/steps_list.yaml | 202 +++++++++++++++------
 1 file changed, 150 insertions(+), 52 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 74a8a9f..a290985 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -17,9 +17,9 @@ common:
     small_tolerance: &SMALL_TOLERANCE
       tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.03 ]
     normal_tolerance: &NORMAL_TOLERANCE
-      tolerance_joints: [ 0.01, 0.010, 0.02, 0.3, 0.3, 0.15, 0.05 ]
+      tolerance_joints: [ 0.01, 0.010, 0.02, 0.35, 0.3, 0.15, 0.05 ]
     bigger_tolerance: &BIGGER_TOLERANCE
-      tolerance_joints: [ 0.015, 0.015, 0.02, 0.3, 0.2, 0.2, 0.1 ]
+      tolerance_joints: [ 0.015, 0.015, 0.02, 0.35, 0.2, 0.2, 0.1 ]
     max_tolerance: &MAX_TOLERANCE
       tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3]
 
@@ -33,6 +33,8 @@ common:
                   0.3677
       small_ready_store: &JOINT1_SMALL_READY_STORE
                            0.254
+      small_store_down: &JOINT1_SMALL_STORE_DOWN
+                          0.1962
       small_ready_store_third: &JOINT1_SMALL_READY_STORE_THIRD
                          0.387
       up_pos: &JOINT1_UP_POS
@@ -76,7 +78,7 @@ common:
       small_ready: &JOINT2_SMALL_READY
                       0.029
       small_store: &JOINT2_SMALL_STORE
-                      0.064040
+                      0.060290
       bin_get_stone: &JOINT2_BIN_GET_STONE
                             0.13
       back_bin_position: &JOINT2_BACK_BIN_POSITION
@@ -102,6 +104,8 @@ common:
         0.08
       small_ready_mid: &JOINT3_SMALL_READY_MID
                         0.032
+      take_one: &JOINT3_TAKE_ONE
+                  -0.052850
       small_ready_left: &JOINT3_SMALL_READY_LEFT
                         0.308
       small_ready_right: &JOINT3_SMALL_READY_RIGHT
@@ -135,6 +139,8 @@ common:
                         -1.554
       small_ready: &JOINT4_SMALL_READY
                         0.054
+      to: &JOINT4_TAKE_ONE
+        0.828930
       small_turn: &JOINT4_SMALL_TURN
                   -1.497
       big_store: &JOINT4_BIG_STORE
@@ -166,7 +172,7 @@ common:
       small_up_stone: &JOINT5_SMALL_UP_STONE
                     -0.001
       small_store: &JOINT5_SMALL_STORE
-                    1.615
+                    1.4662
       exchange: &JOINT5_EXCHANGE
                 1.805
 
@@ -219,6 +225,8 @@ common:
                  -0.098300
       bin_get_left: &JOINT7_BIN_GET_LEFT
                   -1.6973
+      to: &JOINT7_TAKE_ONE
+        -0.844500
       bin_get_right: &JOINT7_BIN_GET_RIGHT
                   1.554
 
@@ -363,6 +371,34 @@ steps_list:
     - step: "gimbal lifter tallest"
       gimbal_lifter:
         target: *TALLEST_POS
+  EXCHANGE_R:
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "move arm to exchange pos"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_BIN_GET_LEFT ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "gimbal lifter tallest"
+      gimbal_lifter:
+        target: *TALLEST_POS
+  EXCHANGE_L:
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "move arm to exchange pos"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_BIN_GET_RIGHT ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "gimbal lifter tallest"
+      gimbal_lifter:
+        target: *TALLEST_POS
 
   ############ MID #############
 
@@ -404,6 +440,74 @@ steps_list:
         <<: *CLOSE_GRIPPER
 
   ################### SMALL_ISLAND_FROM_UP ################
+
+  ##### TAKE ONE USE ONE #####
+
+  TAKE_ONE_USE_ONE:
+    - step: "GIMBAL_READY"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "ORE_ROTATOR_READY"
+      ore_rotator:
+        target: *READY_POS
+    - step: "ORE_LIFTER_READY"
+      ore_lifter:
+        target: *BIN_DOWN_POS
+    - step: "ready to get ore"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_TAKE_ONE, *JOINT4_TAKE_ONE, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_TAKE_ONE ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "gimbal lifter ready"
+      gimbal_lifter:
+        target: *LIFTER_SMALL_ISLAND
+  TAKE_ONE_USE_ONE0:
+    - step: "GET_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_TAKE_ONE, *JOINT4_TAKE_ONE, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_TAKE_ONE ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT1_UP"
+      arm:
+        joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "CHASSIS_BACK"
+      chassis:
+        <<: *CHASSIS_BACK
+    - step: "CHASSIS_TURN"
+      chassis:
+        <<: *CHASSIS_TURN
+    - step: "gimbal lifter ready"
+      gimbal_lifter:
+        target: *TALLEST_POS
+    - step: "ROTATE_STONE"
+      arm:
+        joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_TAKE_ONE, *JOINT4_TAKE_ONE, *JOINT5_SMALL_READY, *JOINT6_MID_POSITION, *JOINT7_TAKE_ONE ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "move arm to exchange pos"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+
+
+
   ########ONE_STONE_ONCE#########
   0_TAKE_ONE_STONE_SMALL_ISLAND:
     - step: "GIMBAL_READY"
@@ -414,7 +518,7 @@ steps_list:
         target: *READY_POS
     - step: "ORE_LIFTER_READY"
       ore_lifter:
-        target: *BIN_MID_POS
+        target: *BIN_DOWN_POS
     - step: "ARM_READY"
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
@@ -588,7 +692,7 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "CHASSIS_BACK"
       chassis:
         <<: *CHASSIS_BACK
@@ -619,7 +723,7 @@ steps_list:
         target: *READY_POS
     - step: "ORE_LIFTER_READY"
       ore_lifter:
-        target: *BIN_MID_POS
+        target: *BIN_DOWN_POS
     - step: "ARM_READY"
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
@@ -666,14 +770,14 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "READY_STORE"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
@@ -683,6 +787,9 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
+    - step: "ADD_STONE"
+      stone_num:
+        change: "SILVER"
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -696,9 +803,7 @@ steps_list:
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
-    - step: "ADD_STONE"
-      stone_num:
-        change: "SILVER"
+
 
     #####SECOND_STONE########
     - step: "GIMBAL_READY"
@@ -759,7 +864,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
@@ -769,12 +874,12 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
-    - step: "CLOSE_GRIPPER"
-      gripper:
-        <<: *CLOSE_GRIPPER
     - step: "ADD_STONE"
       stone_num:
         change: "SILVER"
+    - step: "CLOSE_GRIPPER"
+      gripper:
+        <<: *CLOSE_GRIPPER
     - step: "MOVE_ARM_OUT"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
@@ -1422,7 +1527,7 @@ steps_list:
         back: *EX_OPEN
     - step: "MID_ARM_READY"
       arm:
-        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY ]
+        joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY , *JOINT7_BIG_POS]
         common:
          <<: *QUICKLY
         tolerance:
@@ -1443,7 +1548,7 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "ADJUST_STONE"
       arm:
         joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ]
@@ -1458,9 +1563,10 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "left"
-      chassis:
-        <<: *CHASSIS_LEFT_15
+    - step: "gimbal front"
+      gimbal:
+        <<: *FRONT_POS
+  MID_BIG_ISLAND00:
     - step: "JOINT1_UP"
       arm:
         joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
@@ -1489,7 +1595,6 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-#  MID_BIG_ISLAND00:
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -1500,21 +1605,9 @@ steps_list:
       arm:
         joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "chassis left"
-      chassis:
-        <<: *CHASSIS_LEFT_15
-    - step: "chassis turn"
-      chassis:
-        <<: *CHASSIS_180
-    - step: "pre_home"
-      arm:
-        <<: *PRE_HOME
-    - step: "home"
-      arm:
-        <<: *HOME
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
@@ -1525,6 +1618,13 @@ steps_list:
       extend_arm:
         front: *EX_CLOSE
         back: *EX_CLOSE
+    - step: "pre_home"
+      arm:
+        <<: *PRE_HOME
+    - step: "home"
+      arm:
+        <<: *HOME
+
     - step: "gimbal front"
       gimbal:
         <<: *FRONT_POS
@@ -1562,7 +1662,7 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "ADJUST_STONE"
       arm:
         joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ]
@@ -1577,9 +1677,10 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "left"
-      chassis:
-        <<: *CHASSIS_LEFT_15
+    - step: "gimbal front"
+      gimbal:
+        <<: *FRONT_POS
+  SIDE_BIG_ISLAND00:
     - step: "JOINT1_UP"
       arm:
         joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
@@ -1608,6 +1709,9 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "add stone"
+      stone_num:
+        change: "GOLD"
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER
@@ -1617,7 +1721,10 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
+    - step: "gimbal front"
+      gimbal:
+        <<: *FRONT_POS
     - step: "pre_home"
       arm:
         <<: *PRE_HOME
@@ -1627,12 +1734,6 @@ steps_list:
     - step: "ORE_LIFTER_DOWN"
       ore_lifter:
         target: *BIN_DOWN_POS
-    - step: "chassis left"
-      chassis:
-        <<: *CHASSIS_LEFT_15
-    - step: "chassis turn"
-      chassis:
-        <<: *CHASSIS_180
     - step: "ORE_ROTATOR_RESET"
       ore_rotator:
         target: *READY_POS
@@ -1640,12 +1741,9 @@ steps_list:
       extend_arm:
         front: *EX_CLOSE
         back: *EX_CLOSE
-    - step: "add stone"
-      stone_num:
-        change: "GOLD"
-    - step: "gimbal front"
-      gimbal:
-        <<: *FRONT_POS
+
+
+
 
   ################## STORE #################
 

From ec83643c7ddd8df91aa4bd1e022858f9dcd1c83c Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 16 May 2024 18:04:20 +0800
Subject: [PATCH 099/147] output joints with too big error.

---
 engineer_middleware/include/engineer_middleware/motion.h | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 12de930..3ad3ff1 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -455,11 +455,14 @@ class JointMotion : public MoveitMotionBase
   {
     std::vector<double> current = interface_.getCurrentJointValues();
     double error = 0.;
-    bool flag = 1;
+    bool flag = 1, joint_reached = 0;
     for (int i = 0; i < (int)final_target_.size(); ++i)
     {
       error = std::abs(final_target_[i] - current[i]);
-      flag &= (error < tolerance_joints_[i]);
+      joint_reached = (error < tolerance_joints_[i]);
+      if (!joint_reached)
+        ROS_INFO_STREAM("Joint" << i + 1 << " didn't reach configured tolerance range,error: " << error);
+      flag &= joint_reached;
     }
     return flag;
   }

From 5002c34a023b66452150ada764a4f6483e3c4564 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 16 May 2024 18:04:35 +0800
Subject: [PATCH 100/147] modify store ore position.

---
 engineer_middleware/config/steps_list.yaml | 62 ++++++++++++++--------
 1 file changed, 39 insertions(+), 23 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index a290985..9eebf9e 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -17,7 +17,7 @@ common:
     small_tolerance: &SMALL_TOLERANCE
       tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.03 ]
     normal_tolerance: &NORMAL_TOLERANCE
-      tolerance_joints: [ 0.01, 0.010, 0.02, 0.35, 0.3, 0.15, 0.05 ]
+      tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.05 ]
     bigger_tolerance: &BIGGER_TOLERANCE
       tolerance_joints: [ 0.015, 0.015, 0.02, 0.35, 0.2, 0.2, 0.1 ]
     max_tolerance: &MAX_TOLERANCE
@@ -30,7 +30,7 @@ common:
       small_ready: &JOINT1_SMALL_READY
                       0.487
       small_down: &JOINT1_SMALL_DOWN
-                  0.3677
+                  0.34
       small_ready_store: &JOINT1_SMALL_READY_STORE
                            0.254
       small_store_down: &JOINT1_SMALL_STORE_DOWN
@@ -44,9 +44,9 @@ common:
       bin_get_stone_right: &JOINT1_BIN_GET_STONE_RIGHT
                             0.1131
       big_ready: &JOINT1_BIG_READY
-                     0.006
+                     0.005
       big_adjust_mid: &JOINT1_BIG_ADJUST_MID
-                      0.08
+                      0.079
       big_up_pos: &JOINT1_BIG_UP_POS
                       0.320
       big_ready_store: &JOINT1_BIG_READY_STORE
@@ -90,7 +90,7 @@ common:
       big_arm_up: &JOINT2_BIG_ARM_UP
                     0.142
       big_ready_store: &JOINT2_BIG_READY_STORE
-                      0.055
+                      0.0739
 
   joint3:
     mechanical:
@@ -103,7 +103,7 @@ common:
       home: &JOINT3_HOME_POS
         0.08
       small_ready_mid: &JOINT3_SMALL_READY_MID
-                        0.032
+                        0.0402
       take_one: &JOINT3_TAKE_ONE
                   -0.052850
       small_ready_left: &JOINT3_SMALL_READY_LEFT
@@ -125,7 +125,7 @@ common:
       big_pull_out: &JOINT3_BIG_PULL_OUT
                      0.188
       big_arm_up: &JOINT3_BIG_ARM_UP
-                    0.285
+                    0.308
       exchange: &JOINT3_EXCHANGE
                   0.081
 
@@ -160,7 +160,7 @@ common:
       right_90_position: &JOINT5_R90_POSITION
                            1.59
       bin_get_stone: &JOINT5_BIN_GET_STONE
-                           1.5798
+                           1.63912
       big_ready: &JOINT5_BIG_READY
                   -0.017
       big_ready_store: &JOINT5_BIG_READY_STORE
@@ -168,7 +168,7 @@ common:
       big_store: &JOINT5_BIG_STORE
                   1.49
       small_ready: &JOINT5_SMALL_READY
-                  -3.20
+                  -3.1267
       small_up_stone: &JOINT5_SMALL_UP_STONE
                     -0.001
       small_store: &JOINT5_SMALL_STORE
@@ -724,6 +724,12 @@ steps_list:
     - step: "ORE_LIFTER_READY"
       ore_lifter:
         target: *BIN_DOWN_POS
+    - step: "OPEN_GRIPPER"
+      gripper:
+        <<: *OPEN_GRIPPER
+    - step: "gimbal lifter ready"
+      gimbal_lifter:
+        target: *LIFTER_SMALL_ISLAND
     - step: "ARM_READY"
       arm:
         joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
@@ -731,12 +737,7 @@ steps_list:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "OPEN_GRIPPER"
-      gripper:
-        <<: *OPEN_GRIPPER
-    - step: "gimbal lifter ready"
-      gimbal_lifter:
-        target: *LIFTER_SMALL_ISLAND
+
   TWO_STONE_SMALL_ISLAND0:
     - step: "GET_STONE"
       arm:
@@ -1251,16 +1252,23 @@ steps_list:
       arm:
         joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "JOINT5_UP"
+      arm:
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "exchange pos"
       arm:
         joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
@@ -1315,7 +1323,14 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
-        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
+        joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "JOINT5_UP"
+      arm:
+        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1549,13 +1564,14 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
+  MID_BIG_ISLAND00:
     - step: "ADJUST_STONE"
       arm:
         joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ]
         common:
           <<: *SLOWLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "PULL_OUT_STONE"
       arm:
         joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ]
@@ -1566,7 +1582,7 @@ steps_list:
     - step: "gimbal front"
       gimbal:
         <<: *FRONT_POS
-  MID_BIG_ISLAND00:
+  MID_BIG_ISLAND000:
     - step: "JOINT1_UP"
       arm:
         joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
@@ -1702,6 +1718,9 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "add stone"
+      stone_num:
+        change: "GOLD"
     - step: "STORE_STONE"
       arm:
         joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ]
@@ -1709,9 +1728,6 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "add stone"
-      stone_num:
-        change: "GOLD"
     - step: "CLOSE_GRIPPER"
       gripper:
         <<: *CLOSE_GRIPPER

From c2530b0d6df57b27b94507c6704d3898ecaebd0f Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Fri, 17 May 2024 22:56:02 +0800
Subject: [PATCH 101/147] roll back joint7 position for small island ore.

---
 engineer_middleware/config/steps_list.yaml | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 9eebf9e..6f97576 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -115,7 +115,7 @@ common:
       bin_get_stone: &JOINT3_BIN_GET_STONE
                           0.2608
       bin_get_lv4_stone: &JOINT3_BIN_GET_LV4_STONE
-                           0.31
+                           0.294414
       big_ready: &JOINT3_BIG_READY
                      0.071
       get_big_mid: &JOINT3_GET_BIG_MID
@@ -1640,7 +1640,6 @@ steps_list:
     - step: "home"
       arm:
         <<: *HOME
-
     - step: "gimbal front"
       gimbal:
         <<: *FRONT_POS

From 09aa3ecceda40219cb921f00976cd190170ca4c6 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 18 May 2024 07:42:04 +0800
Subject: [PATCH 102/147] simplify small island motion.

---
 engineer_middleware/config/steps_list.yaml | 74 +++-------------------
 1 file changed, 9 insertions(+), 65 deletions(-)

diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/steps_list.yaml
index 6f97576..cd2f7a9 100644
--- a/engineer_middleware/config/steps_list.yaml
+++ b/engineer_middleware/config/steps_list.yaml
@@ -17,7 +17,7 @@ common:
     small_tolerance: &SMALL_TOLERANCE
       tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.03 ]
     normal_tolerance: &NORMAL_TOLERANCE
-      tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.05 ]
+      tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.07 ]
     bigger_tolerance: &BIGGER_TOLERANCE
       tolerance_joints: [ 0.015, 0.015, 0.02, 0.35, 0.2, 0.2, 0.1 ]
     max_tolerance: &MAX_TOLERANCE
@@ -554,20 +554,6 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "JOINT6_BACK"
-      arm:
-        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "TURN_STONE"
-      arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
     - step: "READY_STORE"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
@@ -655,20 +641,6 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "JOINT6_BACK"
-      arm:
-        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "TURN_STONE"
-      arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
     - step: "READY_STORE"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
@@ -743,14 +715,14 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
         joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ARM_UP_STONE"
@@ -760,25 +732,11 @@ steps_list:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "JOINT6_BACK"
-      arm:
-        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "TURN_STONE"
-      arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
     - step: "READY_STORE"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
@@ -798,7 +756,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ORE_LIFTER_DOWN"
@@ -830,14 +788,14 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "JOINT1_UP"
       arm:
         joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ARM_UP_STONE"
@@ -847,25 +805,11 @@ steps_list:
           <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "JOINT6_BACK"
-      arm:
-        joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "TURN_STONE"
-      arm:
-        joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ]
-        common:
-          <<: *QUICKLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
     - step: "READY_STORE"
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "STORE"
@@ -885,7 +829,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "CHASSIS_BACK"

From 0d32e53f69f6eb7460a85d09fc32b3e3034f80f4 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 3 Jun 2024 22:11:03 +0800
Subject: [PATCH 103/147] add engineer2_arm_connfig, modify launch files to
 select different config files.

---
 engineer2_arm_config/.setup_assistant         |  11 +
 engineer2_arm_config/CMakeLists.txt           |  10 +
 .../config/chomp_planning.yaml                |  18 +
 engineer2_arm_config/config/engineer2.srdf    |  55 ++
 .../config/fake_controllers.yaml              |  13 +
 engineer2_arm_config/config/joint_limits.yaml |  40 ++
 engineer2_arm_config/config/kinematics.yaml   |   3 +
 .../config/ompl_planning.yaml                 | 158 +++++
 .../config/ros_controllers.yaml               |  31 +
 engineer2_arm_config/config/sensors_3d.yaml   |   2 +
 engineer2_arm_config/config/servo.yaml        |  68 +++
 .../launch/chomp_planning_pipeline.launch.xml |  23 +
 ...ineer_moveit_controller_manager.launch.xml |  10 +
 .../engineer_moveit_sensor_manager.launch.xml |   3 +
 .../fake_moveit_controller_manager.launch.xml |   9 +
 .../launch/load_controllers.launch            |  49 ++
 .../launch/load_move_group.launch             |  18 +
 engineer2_arm_config/launch/move_group.launch |  65 +++
 engineer2_arm_config/launch/moveit.rviz       | 540 ++++++++++++++++++
 .../ompl-chomp_planning_pipeline.launch.xml   |  20 +
 .../launch/ompl_planning_pipeline.launch.xml  |  25 +
 ...otion_planner_planning_pipeline.launch.xml |  15 +
 .../launch/planning_context.launch            |  25 +
 .../launch/planning_pipeline.launch.xml       |  10 +
 engineer2_arm_config/launch/rviz.launch       |  14 +
 engineer2_arm_config/launch/servo.launch      |   6 +
 .../launch/trajectory_execution.launch.xml    |  21 +
 engineer2_arm_config/package.xml              |  27 +
 engineer_middleware/config/engineer2.yaml     |   9 +
 .../config/engineer2_steps_list.yaml          |   0
 ...eps_list.yaml => engineer_steps_list.yaml} |   0
 engineer_middleware/launch/load.launch        |   5 +-
 engineer_middleware/launch/sim.launch         |   7 +-
 33 files changed, 1305 insertions(+), 5 deletions(-)
 create mode 100644 engineer2_arm_config/.setup_assistant
 create mode 100644 engineer2_arm_config/CMakeLists.txt
 create mode 100644 engineer2_arm_config/config/chomp_planning.yaml
 create mode 100644 engineer2_arm_config/config/engineer2.srdf
 create mode 100644 engineer2_arm_config/config/fake_controllers.yaml
 create mode 100644 engineer2_arm_config/config/joint_limits.yaml
 create mode 100644 engineer2_arm_config/config/kinematics.yaml
 create mode 100644 engineer2_arm_config/config/ompl_planning.yaml
 create mode 100644 engineer2_arm_config/config/ros_controllers.yaml
 create mode 100644 engineer2_arm_config/config/sensors_3d.yaml
 create mode 100644 engineer2_arm_config/config/servo.yaml
 create mode 100644 engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml
 create mode 100644 engineer2_arm_config/launch/engineer_moveit_controller_manager.launch.xml
 create mode 100644 engineer2_arm_config/launch/engineer_moveit_sensor_manager.launch.xml
 create mode 100644 engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml
 create mode 100644 engineer2_arm_config/launch/load_controllers.launch
 create mode 100644 engineer2_arm_config/launch/load_move_group.launch
 create mode 100644 engineer2_arm_config/launch/move_group.launch
 create mode 100644 engineer2_arm_config/launch/moveit.rviz
 create mode 100644 engineer2_arm_config/launch/ompl-chomp_planning_pipeline.launch.xml
 create mode 100644 engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml
 create mode 100644 engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
 create mode 100644 engineer2_arm_config/launch/planning_context.launch
 create mode 100644 engineer2_arm_config/launch/planning_pipeline.launch.xml
 create mode 100644 engineer2_arm_config/launch/rviz.launch
 create mode 100644 engineer2_arm_config/launch/servo.launch
 create mode 100644 engineer2_arm_config/launch/trajectory_execution.launch.xml
 create mode 100644 engineer2_arm_config/package.xml
 create mode 100644 engineer_middleware/config/engineer2.yaml
 create mode 100644 engineer_middleware/config/engineer2_steps_list.yaml
 rename engineer_middleware/config/{steps_list.yaml => engineer_steps_list.yaml} (100%)

diff --git a/engineer2_arm_config/.setup_assistant b/engineer2_arm_config/.setup_assistant
new file mode 100644
index 0000000..197025b
--- /dev/null
+++ b/engineer2_arm_config/.setup_assistant
@@ -0,0 +1,11 @@
+moveit_setup_assistant_config:
+  URDF:
+    package: rm_description
+    relative_path: urdf/engineer2/engineer2.urdf.xacro
+    xacro_args: ""
+  SRDF:
+    relative_path: config/engineer2.srdf
+  CONFIG:
+    author_name: QiayuanLiao
+    author_email: liaoqiayuan@gmail.com
+    generated_timestamp: 1617294211
diff --git a/engineer2_arm_config/CMakeLists.txt b/engineer2_arm_config/CMakeLists.txt
new file mode 100644
index 0000000..ad88885
--- /dev/null
+++ b/engineer2_arm_config/CMakeLists.txt
@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 3.1.3)
+project(engineer2_arm_config)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+        PATTERN "setup_assistant.launch" EXCLUDE)
+install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/engineer2_arm_config/config/chomp_planning.yaml b/engineer2_arm_config/config/chomp_planning.yaml
new file mode 100644
index 0000000..0831fda
--- /dev/null
+++ b/engineer2_arm_config/config/chomp_planning.yaml
@@ -0,0 +1,18 @@
+planning_time_limit: 10.0
+max_iterations: 200
+max_iterations_after_collision_free: 5
+smoothness_cost_weight: 0.1
+obstacle_cost_weight: 1.0
+learning_rate: 0.01
+smoothness_cost_velocity: 0.0
+smoothness_cost_acceleration: 1.0
+smoothness_cost_jerk: 0.0
+ridge_factor: 0.01
+use_pseudo_inverse: false
+pseudo_inverse_ridge_factor: 1e-4
+joint_update_limit: 0.1
+collision_clearance: 0.2
+collision_threshold: 0.07
+use_stochastic_descent: true
+enable_failure_recovery: true
+max_recovery_attempts: 15
diff --git a/engineer2_arm_config/config/engineer2.srdf b/engineer2_arm_config/config/engineer2.srdf
new file mode 100644
index 0000000..a9012bb
--- /dev/null
+++ b/engineer2_arm_config/config/engineer2.srdf
@@ -0,0 +1,55 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="engineer">
+    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="engineer_arm">
+        <joint name="joint1"/>
+        <joint name="joint2"/>
+        <joint name="joint3"/>
+        <joint name="joint4"/>
+        <joint name="joint5"/>
+        <joint name="joint6"/>
+    </group>
+    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+    <group_state name="home" group="engineer_arm">
+        <joint name="joint1" value="0.16"/>
+        <joint name="joint2" value="0"/>
+        <joint name="joint3" value="0"/>
+        <joint name="joint4" value="0"/>
+        <joint name="joint5" value="1.92225"/>
+        <joint name="joint6" value="0"/>
+        <joint name="joint7" value="0"/>
+    </group_state>
+        <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+        <disable_collisions link1="base_link" link2="gimbal_base" reason="Adjacent"/>
+        <disable_collisions link1="base_link" link2="left_back_wheel" reason="Adjacent"/>
+        <disable_collisions link1="base_link" link2="left_front_wheel" reason="Adjacent"/>
+        <disable_collisions link1="base_link" link2="link1" reason="Adjacent"/>
+        <disable_collisions link1="base_link" link2="link2" reason="Default"/>
+        <disable_collisions link1="base_link" link2="link3" reason="Never"/>
+        <disable_collisions link1="base_link" link2="link5" reason="Default"/>
+        <disable_collisions link1="base_link" link2="link6" reason="Default"/>
+        <disable_collisions link1="gimbal_base" link2="link1" reason="Never"/>
+        <disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
+        <disable_collisions link1="link1" link2="link3" reason="Never"/>
+        <disable_collisions link1="link1" link2="link4" reason="Never"/>
+        <disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
+        <disable_collisions link1="link2" link2="link4" reason="Never"/>
+        <disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
+        <disable_collisions link1="link3" link2="link5" reason="Default"/>
+        <disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
+        <disable_collisions link1="link4" link2="link6" reason="Never"/>
+        <disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
+        <disable_collisions link1="link6" link2="link7" reason="Never"/>
+        <disable_collisions link1="link5" link2="link7" reason="Never"/>
+        <disable_collisions link1="link1" link2="camera_link" reason="Never"/>
+        <disable_collisions link1="link7" link2="ore_bin_lifter" reason="Never"/>
+        <disable_collisions link1="link7" link2="ore_bin_rotator" reason="Never"/>
+</robot>
diff --git a/engineer2_arm_config/config/fake_controllers.yaml b/engineer2_arm_config/config/fake_controllers.yaml
new file mode 100644
index 0000000..c51e46f
--- /dev/null
+++ b/engineer2_arm_config/config/fake_controllers.yaml
@@ -0,0 +1,13 @@
+controller_list:
+  - name: fake_engineer_arm_controller
+    type: $(arg fake_execution_type)
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+initial:  # Define initial robot poses per group
+#  - group: engineer_arm
+#    pose: home
diff --git a/engineer2_arm_config/config/joint_limits.yaml b/engineer2_arm_config/config/joint_limits.yaml
new file mode 100644
index 0000000..f0cf0d3
--- /dev/null
+++ b/engineer2_arm_config/config/joint_limits.yaml
@@ -0,0 +1,40 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+
+# For beginners, we downscale velocity and acceleration limits.
+# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+default_velocity_scaling_factor: 0.1
+default_acceleration_scaling_factor: 0.1
+
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  joint1:
+    has_velocity_limits: true
+    max_velocity: 1.80
+    has_acceleration_limits: true
+    max_acceleration: 3.5
+  joint2:
+    has_velocity_limits: true
+    max_velocity: 2.75
+    has_acceleration_limits: true
+    max_acceleration: 6
+  joint3:
+    has_velocity_limits: true
+    max_velocity: 1.80
+    has_acceleration_limits: true
+    max_acceleration: 5
+  joint4:
+    has_velocity_limits: true
+    max_velocity: 19
+    has_acceleration_limits: true
+    max_acceleration: 50
+  joint5:
+    has_velocity_limits: true
+    max_velocity: 15
+    has_acceleration_limits: true
+    max_acceleration: 20
+  joint6:
+    has_velocity_limits: true
+    max_velocity: 50
+    has_acceleration_limits: true
+    max_acceleration: 90
diff --git a/engineer2_arm_config/config/kinematics.yaml b/engineer2_arm_config/config/kinematics.yaml
new file mode 100644
index 0000000..b298b37
--- /dev/null
+++ b/engineer2_arm_config/config/kinematics.yaml
@@ -0,0 +1,3 @@
+engineer_arm:
+  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+  kinematics_solver_timeout: 0.005
diff --git a/engineer2_arm_config/config/ompl_planning.yaml b/engineer2_arm_config/config/ompl_planning.yaml
new file mode 100644
index 0000000..8e69e09
--- /dev/null
+++ b/engineer2_arm_config/config/ompl_planning.yaml
@@ -0,0 +1,158 @@
+planner_configs:
+  AnytimePathShortening:
+    type: geometric::AnytimePathShortening
+    shortcut: true  # Attempt to shortcut all new solution paths
+    hybridize: true  # Compute hybrid solution trajectories
+    max_hybrid_paths: 40  # Number of hybrid paths generated per iteration
+    num_planners: 20  # The number of default planners to use for planning
+    planners: ""  # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]"
+  SBL:
+    type: geometric::SBL
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  EST:
+    type: geometric::EST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+  LBKPIECE:
+    type: geometric::LBKPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  BKPIECE:
+    type: geometric::BKPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  KPIECE:
+    type: geometric::KPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
+    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  RRT:
+    type: geometric::RRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+  RRTConnect:
+    type: geometric::RRTConnect
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  RRTstar:
+    type: geometric::RRTstar
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
+  TRRT:
+    type: geometric::TRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    max_states_failed: 10  # when to start increasing temp. default: 10
+    temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
+    min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
+    init_temperature: 10e-6  # initial temperature. default: 10e-6
+    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+    frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
+  PRM:
+    type: geometric::PRM
+    max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
+  PRMstar:
+    type: geometric::PRMstar
+  FMT:
+    type: geometric::FMT
+    num_samples: 1000  # number of states that the planner should sample. default: 1000
+    radius_multiplier: 1.1  # multiplier used for the nearest neighbors search radius. default: 1.1
+    nearest_k: 1  # use Knearest strategy. default: 1
+    cache_cc: 1  # use collision checking cache. default: 1
+    heuristics: 0  # activate cost to go heuristics. default: 0
+    extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
+  BFMT:
+    type: geometric::BFMT
+    num_samples: 1000  # number of states that the planner should sample. default: 1000
+    radius_multiplier: 1.0  # multiplier used for the nearest neighbors search radius. default: 1.0
+    nearest_k: 1  # use the Knearest strategy. default: 1
+    balanced: 0  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
+    optimality: 1  # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
+    heuristics: 1  # activates cost to go heuristics. default: 1
+    cache_cc: 1  # use the collision checking cache. default: 1
+    extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
+  PDST:
+    type: geometric::PDST
+  STRIDE:
+    type: geometric::STRIDE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    use_projected_distance: 0  # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
+    degree: 16  # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
+    max_degree: 18  # max degree of a node in the GNAT. default: 12
+    min_degree: 12  # min degree of a node in the GNAT. default: 12
+    max_pts_per_leaf: 6  # max points per leaf in the GNAT. default: 6
+    estimated_dimension: 0.0  # estimated dimension of the free space. default: 0.0
+    min_valid_path_fraction: 0.2  # Accept partially valid moves above fraction. default: 0.2
+  BiTRRT:
+    type: geometric::BiTRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
+    init_temperature: 100  # initial temperature. default: 100
+    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+    frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
+  LBTRRT:
+    type: geometric::LBTRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    epsilon: 0.4  # optimality approximation factor. default: 0.4
+  BiEST:
+    type: geometric::BiEST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  ProjEST:
+    type: geometric::ProjEST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+  LazyPRM:
+    type: geometric::LazyPRM
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  LazyPRMstar:
+    type: geometric::LazyPRMstar
+  SPARS:
+    type: geometric::SPARS
+    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
+    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
+    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
+    max_failures: 1000  # maximum consecutive failure limit. default: 1000
+  SPARStwo:
+    type: geometric::SPARStwo
+    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
+    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
+    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
+    max_failures: 5000  # maximum consecutive failure limit. default: 5000
+engineer_arm:
+  default_planner_config: RRTConnect
+  planner_configs:
+    - AnytimePathShortening
+    - SBL
+    - EST
+    - LBKPIECE
+    - BKPIECE
+    - KPIECE
+    - RRT
+    - RRTConnect
+    - RRTstar
+    - TRRT
+    - PRM
+    - PRMstar
+    - FMT
+    - BFMT
+    - PDST
+    - STRIDE
+    - BiTRRT
+    - LBTRRT
+    - BiEST
+    - ProjEST
+    - LazyPRM
+    - LazyPRMstar
+    - SPARS
+    - SPARStwo
+  projection_evaluator: joints(joint1,joint2)
+  longest_valid_segment_fraction: 0.005
diff --git a/engineer2_arm_config/config/ros_controllers.yaml b/engineer2_arm_config/config/ros_controllers.yaml
new file mode 100644
index 0000000..a4837dc
--- /dev/null
+++ b/engineer2_arm_config/config/ros_controllers.yaml
@@ -0,0 +1,31 @@
+# Simulation settings for using moveit_sim_controllers
+moveit_sim_hw_interface:
+  joint_model_group: engineer_arm
+  joint_model_group_pose: home
+# Settings for ros_control_boilerplate control loop
+generic_hw_control_loop:
+  loop_hz: 300
+  cycle_time_error_threshold: 0.01
+# Settings for ros_control hardware interface
+hardware_interface:
+  joints:
+    - joint1
+    - joint2
+    - joint3
+    - joint4
+    - joint5
+    - joint6
+  sim_control_mode: 1  # 0: position, 1: velocity
+
+controller_list:
+  - name: controllers/arm_trajectory_controller
+    action_ns: follow_joint_trajectory
+    default: True
+    type: FollowJointTrajectory
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
diff --git a/engineer2_arm_config/config/sensors_3d.yaml b/engineer2_arm_config/config/sensors_3d.yaml
new file mode 100644
index 0000000..add9f47
--- /dev/null
+++ b/engineer2_arm_config/config/sensors_3d.yaml
@@ -0,0 +1,2 @@
+sensors:
+  - { }
diff --git a/engineer2_arm_config/config/servo.yaml b/engineer2_arm_config/config/servo.yaml
new file mode 100644
index 0000000..76270c2
--- /dev/null
+++ b/engineer2_arm_config/config/servo.yaml
@@ -0,0 +1,68 @@
+###############################################
+# Modify all parameters related to servoing here
+###############################################
+use_gazebo: true # Whether the robot is started in a Gazebo simulation environment
+
+## Properties of incoming commands
+command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+scale:
+  # Scale parameters are only used if command_in_type=="unitless"
+  linear: 0.4  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
+  rotational: 0.2 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
+  # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
+  joint: 0.005
+low_pass_filter_coeff: 1.  # Larger --> trust the filtered data more, trust the measurements less.
+
+## Properties of outgoing commands
+publish_period: 0.5  # 1/Nominal publish rate [seconds]
+low_latency_mode: true  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
+
+# What type of topic does your robot driver expect?
+# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController)
+# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots)
+command_out_type: trajectory_msgs/JointTrajectory
+
+# What to publish? Can save some bandwidth as most robots only require positions or velocities
+publish_joint_positions: true
+publish_joint_velocities: true
+publish_joint_accelerations: false
+
+## MoveIt properties
+move_group_name: engineer_arm  # Often 'manipulator' or 'arm'
+planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'world'
+
+## Other frames
+ee_frame_name: link6  # The name of the end effector link, used to return the EE pose
+robot_link_command_frame: link6  # commands must be given in the frame of a robot link. Usually either the base or end effector
+
+## Stopping behaviour
+incoming_command_timeout: 0.1  # Stop servoing if X seconds elapse without a new command
+# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
+# Important because ROS may drop some messages and we need the robot to halt reliably.
+num_outgoing_halt_msgs_to_publish: 4
+
+## Configure handling of singularities and joint limits
+lower_singularity_threshold: 17  # Start decelerating when the condition number hits this (close to singularity)
+hard_stop_singularity_threshold: 30 # Stop when the condition number hits this
+joint_limit_margin: 0.001 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+
+## Topic names
+cartesian_command_in_topic: delta_twist_cmds  # Topic for incoming Cartesian twist commands
+joint_command_in_topic: delta_joint_cmds # Topic for incoming joint angle commands
+joint_topic: joint_states
+status_topic: status # Publish status to this topic
+command_out_topic: /controllers/arm_trajectory_controller/command # Publish outgoing commands here
+
+## Collision checking for the entire robot body
+check_collisions: false # Check collisions?
+collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+# Two collision check algorithms are available:
+# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
+# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
+collision_check_type: threshold_distance
+# Parameters for "threshold_distance"-type collision checking
+self_collision_proximity_threshold: 0.0002 # Start decelerating when a self-collision is this far [m]
+scene_collision_proximity_threshold: 0.0005 # Start decelerating when a scene collision is this far [m]
+# Parameters for "stop_distance"-type collision checking
+collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency
+min_allowable_collision_distance: 0.001 # Stop if a collision is closer than this [m]
diff --git a/engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml b/engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml
new file mode 100644
index 0000000..2f60968
--- /dev/null
+++ b/engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml
@@ -0,0 +1,23 @@
+<launch>
+    <arg name="start_state_max_bounds_error" default="0.1" />
+    <arg name="jiggle_fraction" default="0.05" />
+    <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
+    <arg name="planning_adapters"
+         default="default_planner_request_adapters/AddTimeParameterization
+                default_planner_request_adapters/ResolveConstraintFrames
+                default_planner_request_adapters/FixWorkspaceBounds
+                default_planner_request_adapters/FixStartStateBounds
+                default_planner_request_adapters/FixStartStateCollision
+                default_planner_request_adapters/FixStartStatePathConstraints"
+    />
+
+    <param name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
+    <param name="request_adapters" value="$(arg planning_adapters)" />
+    <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+    <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
+
+    <!-- Add MoveGroup capabilities specific to this pipeline -->
+    <!-- <param name="capabilities" value="" /> -->
+
+    <rosparam command="load" file="$(find engineer2_arm_config)/config/chomp_planning.yaml" />
+</launch>
diff --git a/engineer2_arm_config/launch/engineer_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/engineer_moveit_controller_manager.launch.xml
new file mode 100644
index 0000000..db614c0
--- /dev/null
+++ b/engineer2_arm_config/launch/engineer_moveit_controller_manager.launch.xml
@@ -0,0 +1,10 @@
+<launch>
+
+    <!-- loads moveit_controller_manager on the parameter server which is taken as argument
+      if no argument is passed, moveit_simple_controller_manager will be set -->
+    <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>
+    <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
+
+    <!-- loads ros_controllers to the param server -->
+    <rosparam file="$(find engineer2_arm_config)/config/ros_controllers.yaml"/>
+</launch>
diff --git a/engineer2_arm_config/launch/engineer_moveit_sensor_manager.launch.xml b/engineer2_arm_config/launch/engineer_moveit_sensor_manager.launch.xml
new file mode 100644
index 0000000..5d02698
--- /dev/null
+++ b/engineer2_arm_config/launch/engineer_moveit_sensor_manager.launch.xml
@@ -0,0 +1,3 @@
+<launch>
+
+</launch>
diff --git a/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml
new file mode 100644
index 0000000..d6865a4
--- /dev/null
+++ b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml
@@ -0,0 +1,9 @@
+<launch>
+
+  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
+  <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
+
+  <!-- The rest of the params are specific to this plugin -->
+  <rosparam file="$(find engineer2_arm_config)/config/fake_controllers.yaml"/>
+
+</launch>
diff --git a/engineer2_arm_config/launch/load_controllers.launch b/engineer2_arm_config/launch/load_controllers.launch
new file mode 100644
index 0000000..69275b2
--- /dev/null
+++ b/engineer2_arm_config/launch/load_controllers.launch
@@ -0,0 +1,49 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- By default, we are not load controllers -->
+    <arg name="spawn_controllers" default="true"/>
+    <arg name="use_simulation" default="false"/>
+
+    <group unless="$(arg use_simulation)">
+        <rosparam file="$(find rm_config)/config/rm_controllers/engineer.yaml" command="load"/>
+    </group>
+    <group if="$(arg use_simulation)">
+        <rosparam file="$(find engineer_middleware)/config/simulation_controllers.yaml" command="load"/>
+    </group>
+
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+
+    <!-- Load the controllers -->
+
+    <node name="controller_loader" pkg="controller_manager" type="controller_manager" output="screen"
+          args="load
+                controllers/robot_state_controller
+                controllers/joint_state_controller
+                controllers/joint1_calibration_controller
+                controllers/joint2_calibration_controller
+                controllers/joint3_calibration_controller
+                controllers/joint4_calibration_controller
+                controllers/joint5_calibration_controller
+                controllers/joint6_calibration_controller
+                controllers/joint7_calibration_controller
+                controllers/arm_trajectory_controller
+                controllers/chassis_controller
+                controllers/gpio_controller
+                controllers/gimbal_controller
+                controllers/ore_bin_rotate_controller
+                controllers/extend_arm_front_controller
+                controllers/extend_arm_back_controller
+                controllers/extend_arm_front_calibration_controller
+                controllers/extend_arm_back_calibration_controller
+                controllers/ore_bin_lifter_controller
+                controllers/ore_bin_lifter_calibration_controller
+                controllers/gimbal_lifter_controller
+                controllers/gimbal_lifter_calibration_controller
+                controllers/arm_trajectory_controller
+                "
+    />
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="odom_tf_publisher"
+          args="0 0 0 0 0 0 1 map odom"/>
+</launch>
diff --git a/engineer2_arm_config/launch/load_move_group.launch b/engineer2_arm_config/launch/load_move_group.launch
new file mode 100644
index 0000000..a66a2a9
--- /dev/null
+++ b/engineer2_arm_config/launch/load_move_group.launch
@@ -0,0 +1,18 @@
+<launch>
+    <arg name="load_robot_description" default="false"/>
+
+    <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
+    <include file="$(find engineer2_arm_config)/launch/planning_context.launch">
+        <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+    </include>
+
+    <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+    <include file="$(find engineer2_arm_config)/launch/move_group.launch">
+        <arg name="allow_trajectory_execution" value="true"/>
+        <arg name="fake_execution" value="false"/>
+        <arg name="info" value="true"/>
+        <arg name="debug" value="false"/>
+        <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+    </include>
+
+</launch>
diff --git a/engineer2_arm_config/launch/move_group.launch b/engineer2_arm_config/launch/move_group.launch
new file mode 100644
index 0000000..3bc6450
--- /dev/null
+++ b/engineer2_arm_config/launch/move_group.launch
@@ -0,0 +1,65 @@
+<launch>
+
+    <!-- GDB Debug Option -->
+    <arg name="debug" default="false"/>
+    <arg unless="$(arg debug)" name="launch_prefix" value=""/>
+    <arg if="$(arg debug)" name="launch_prefix"
+         value="gdb -x $(find engineer2_arm_config)/launch/gdb_settings.gdb --ex run --args"/>
+
+    <!-- Verbose Mode Option -->
+    <arg name="info" default="$(arg debug)"/>
+    <arg unless="$(arg info)" name="command_args" value=""/>
+    <arg if="$(arg info)" name="command_args" value="--debug"/>
+
+    <!-- move_group settings -->
+    <arg name="pipeline" default="ompl"/>
+    <arg name="allow_trajectory_execution" default="true"/>
+    <arg name="fake_execution" default="false"/>
+    <arg name="max_safe_path_cost" default="3"/>
+    <arg name="jiggle_fraction" default="0."/>
+    <arg name="publish_monitored_planning_scene" default="true"/>
+
+    <arg name="capabilities" default=""/>
+    <arg name="disable_capabilities" default=""/>
+
+    <arg name="load_robot_description" default="true"/>
+    <!-- load URDF, SRDF and joint_limits configuration -->
+    <include file="$(find engineer2_arm_config)/launch/planning_context.launch">
+        <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+    </include>
+
+    <!-- Planning Functionality -->
+    <include ns="move_group" file="$(find engineer2_arm_config)/launch/planning_pipeline.launch.xml">
+        <arg name="pipeline" value="$(arg pipeline)"/>
+    </include>
+
+    <!-- Trajectory Execution Functionality -->
+    <include ns="move_group" file="$(find engineer2_arm_config)/launch/trajectory_execution.launch.xml"
+             if="$(arg allow_trajectory_execution)">
+        <arg name="moveit_manage_controllers" value="true"/>
+        <arg name="moveit_controller_manager" value="engineer" unless="$(arg fake_execution)"/>
+        <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
+    </include>
+
+    <!-- Start the actual move_group node/action server -->
+    <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group"
+          respawn="false" output="screen" args="$(arg command_args)">
+        <!-- Set the display variable, in case OpenGL code is used internally -->
+        <env name="DISPLAY" value="$(optenv DISPLAY :0)"/>
+
+        <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
+        <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
+        <param name="jiggle_fraction" value="$(arg jiggle_fraction)"/>
+        <param name="capabilities" value="$(arg capabilities)"/>
+        <param name="disable_capabilities" value="$(arg disable_capabilities)"/>
+
+
+        <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
+        <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)"/>
+        <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)"/>
+        <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)"/>
+        <param name="planning_scene_monitor/publish_transforms_updates"
+               value="$(arg publish_monitored_planning_scene)"/>
+    </node>
+
+</launch>
diff --git a/engineer2_arm_config/launch/moveit.rviz b/engineer2_arm_config/launch/moveit.rviz
new file mode 100644
index 0000000..a0a4838
--- /dev/null
+++ b/engineer2_arm_config/launch/moveit.rviz
@@ -0,0 +1,540 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 84
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /MotionPlanning1
+        - /MotionPlanning1/Planning Request1
+        - /MotionPlanning1/Planning Metrics1
+        - /MotionPlanning1/Planned Path1
+        - /TF1
+        - /TF1/Frames1
+      Splitter Ratio: 0.5
+    Tree Height: 315
+  - Class: rviz/Help
+    Name: Help
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Acceleration_Scaling_Factor: 0.1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          camera_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          extend_arm_back:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          extend_arm_front:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          gimbal_lifter:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          mimic_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ore_bin_lifter:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ore_bin_rotator:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          pitch:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tools_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          tools_link_st:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          yaw:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Loop Animation: false
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 0.05 s
+        Trail Step Size: 1
+        Trajectory Topic: move_group/display_planned_path
+        Use Sim Time: false
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: engineer_arm
+        Query Goal State: true
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: move_group/monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 1
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          camera_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          camera_optical_frame:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          extend_arm_back:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          extend_arm_front:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          gimbal_lifter:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_back_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          left_front_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          mimic_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ore_bin_lifter:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          ore_bin_rotator:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          pitch:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_back_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          right_front_wheel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tools_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          tools_link_st:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          yaw:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 0.5
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+      Velocity_Scaling_Factor: 0.1
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base_link:
+          Value: true
+        camera_link:
+          Value: false
+        camera_optical_frame:
+          Value: false
+        exchanger:
+          Value: true
+        extend_arm_back:
+          Value: false
+        extend_arm_front:
+          Value: false
+        gimbal_lifter:
+          Value: false
+        left_back_wheel:
+          Value: false
+        left_front_wheel:
+          Value: false
+        link1:
+          Value: false
+        link2:
+          Value: false
+        link3:
+          Value: false
+        link4:
+          Value: false
+        link5:
+          Value: false
+        link6:
+          Value: false
+        link7:
+          Value: true
+        map:
+          Value: false
+        mimic_link1:
+          Value: false
+        odom:
+          Value: false
+        ore_bin_lifter:
+          Value: false
+        ore_bin_rotator:
+          Value: false
+        pitch:
+          Value: false
+        real_world:
+          Value: false
+        right_back_wheel:
+          Value: false
+        right_front_wheel:
+          Value: false
+        tools_link:
+          Value: false
+        tools_link_st:
+          Value: false
+        yaw:
+          Value: false
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        base_link:
+          {}
+        exchanger:
+          {}
+        extend_arm_back:
+          {}
+        extend_arm_front:
+          {}
+        gimbal_lifter:
+          camera_link:
+            {}
+          camera_optical_frame:
+            {}
+        left_back_wheel:
+          {}
+        left_front_wheel:
+          {}
+        link1:
+          {}
+        link2:
+          {}
+        link3:
+          {}
+        link4:
+          {}
+        link5:
+          {}
+        link6:
+          {}
+        link7:
+          tools_link:
+            {}
+          tools_link_st:
+            {}
+        map:
+          {}
+        mimic_link1:
+          {}
+        odom:
+          {}
+        ore_bin_lifter:
+          {}
+        ore_bin_rotator:
+          {}
+        pitch:
+          {}
+        real_world:
+          {}
+        right_back_wheel:
+          {}
+        right_front_wheel:
+          {}
+        yaw:
+          {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.4108070135116577
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.75
+      Focal Point:
+        X: 0.23874804377555847
+        Y: 0.04174364358186722
+        Z: 0.6224369406700134
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.48979732394218445
+      Target Frame: base_link
+      Yaw: 3.2827558517456055
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Help:
+    collapsed: false
+  Hide Left Dock: false
+  Hide Right Dock: false
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Trajectory Slider:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f30000039efc0200000007fb000000100044006900730070006c006100790073010000003d000001cc000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000020f000001cc0000017d00ffffff000004880000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Views:
+    collapsed: false
+  Width: 1665
+  X: 757
+  Y: 27
diff --git a/engineer2_arm_config/launch/ompl-chomp_planning_pipeline.launch.xml b/engineer2_arm_config/launch/ompl-chomp_planning_pipeline.launch.xml
new file mode 100644
index 0000000..a4531f3
--- /dev/null
+++ b/engineer2_arm_config/launch/ompl-chomp_planning_pipeline.launch.xml
@@ -0,0 +1,20 @@
+<launch>
+  <!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
+  <include file="$(find engineer2_arm_config)/launch/ompl_planning_pipeline.launch.xml">
+    <arg name="planning_adapters"
+         default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
+                  default_planner_request_adapters/AddTimeParameterization
+                  default_planner_request_adapters/FixWorkspaceBounds
+                  default_planner_request_adapters/FixStartStateBounds
+                  default_planner_request_adapters/FixStartStateCollision
+                  default_planner_request_adapters/FixStartStatePathConstraints
+                  chomp/OptimizerAdapter"
+                  />
+  </include>
+
+  <!-- load chomp config -->
+  <rosparam command="load" file="$(find engineer2_arm_config)/config/chomp_planning.yaml" />
+
+  <!-- override trajectory_initialization_method: Use OMPL-generated trajectory -->
+  <param name="trajectory_initialization_method" value="fillTrajectory"/>
+</launch>
diff --git a/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml b/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml
new file mode 100644
index 0000000..c219994
--- /dev/null
+++ b/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml
@@ -0,0 +1,25 @@
+<launch>
+
+    <!-- OMPL Plugin for MoveIt -->
+    <arg name="planning_plugin" value="ompl_interface/OMPLPlanner"/>
+
+    <!-- The request adapters (plugins) used when planning with OMPL.
+         ORDER MATTERS -->
+    <arg name="planning_adapters"
+         value="default_planner_request_adapters/AddTimeParameterization
+              default_planner_request_adapters/ResolveConstraintFrames
+              default_planner_request_adapters/FixWorkspaceBounds
+              default_planner_request_adapters/FixStartStateBounds
+              default_planner_request_adapters/FixStartStateCollision
+              default_planner_request_adapters/FixStartStatePathConstraints"
+    />
+
+    <arg name="start_state_max_bounds_error" value="0.1"/>
+
+    <param name="planning_plugin" value="$(arg planning_plugin)"/>
+    <param name="request_adapters" value="$(arg planning_adapters)"/>
+    <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)"/>
+
+    <rosparam command="load" file="$(find engineer2_arm_config)/config/ompl_planning.yaml"/>
+
+</launch>
diff --git a/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
new file mode 100644
index 0000000..c7c4cf5
--- /dev/null
+++ b/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
@@ -0,0 +1,15 @@
+<launch>
+
+  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
+  <arg name="planning_adapters" default="" />
+
+  <param name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+
+  <!-- Define default planner (for all groups) -->
+  <param name="default_planner_config" value="PTP" />
+
+  <!-- MoveGroup capabilities to load for this pipeline, append sequence capability -->
+  <param name="capabilities" value="pilz_industrial_motion_planner/MoveGroupSequenceAction
+                                    pilz_industrial_motion_planner/MoveGroupSequenceService" />
+</launch>
diff --git a/engineer2_arm_config/launch/planning_context.launch b/engineer2_arm_config/launch/planning_context.launch
new file mode 100644
index 0000000..ff24db3
--- /dev/null
+++ b/engineer2_arm_config/launch/planning_context.launch
@@ -0,0 +1,25 @@
+<launch>
+    <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
+    <arg name="load_robot_description" default="false"/>
+
+    <!-- The name of the parameter under which the URDF is loaded -->
+    <arg name="robot_description" default="robot_description"/>
+
+    <!-- Load universal robot description format (URDF) -->
+    <param if="$(arg load_robot_description)" name="$(arg robot_description)"
+           command="xacro  '$(find rm_description)/urdf/engineer/engineer.urdf.xacro'"/>
+
+    <!-- The semantic description that corresponds to the URDF -->
+    <param name="$(arg robot_description)_semantic" textfile="$(find engineer2_arm_config)/config/engineer2.srdf"/>
+
+    <!-- Load updated joint limits (override information from URDF) -->
+    <group ns="$(arg robot_description)_planning">
+        <rosparam command="load" file="$(find engineer2_arm_config)/config/joint_limits.yaml"/>
+    </group>
+
+    <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
+    <group ns="$(arg robot_description)_kinematics">
+        <rosparam command="load" file="$(find engineer2_arm_config)/config/kinematics.yaml"/>
+    </group>
+
+</launch>
diff --git a/engineer2_arm_config/launch/planning_pipeline.launch.xml b/engineer2_arm_config/launch/planning_pipeline.launch.xml
new file mode 100644
index 0000000..6882ffa
--- /dev/null
+++ b/engineer2_arm_config/launch/planning_pipeline.launch.xml
@@ -0,0 +1,10 @@
+<launch>
+
+    <!-- This file makes it easy to include different planning pipelines;
+         It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->
+
+    <arg name="pipeline" default="ompl"/>
+
+    <include file="$(find engineer2_arm_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml"/>
+
+</launch>
diff --git a/engineer2_arm_config/launch/rviz.launch b/engineer2_arm_config/launch/rviz.launch
new file mode 100644
index 0000000..2c217fe
--- /dev/null
+++ b/engineer2_arm_config/launch/rviz.launch
@@ -0,0 +1,14 @@
+<launch>
+
+    <arg name="debug" default="false"/>
+    <arg name="rviz_config" value="$(find engineer2_arm_config)/launch/moveit.rviz"/>
+    <arg unless="$(arg debug)" name="launch_prefix" value=""/>
+    <arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args"/>
+    <arg if="$(eval rviz_config=='')" name="command_args" value=""/>
+    <arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)"/>
+
+    <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
+          args="$(arg command_args)" output="screen">
+    </node>
+
+</launch>
diff --git a/engineer2_arm_config/launch/servo.launch b/engineer2_arm_config/launch/servo.launch
new file mode 100644
index 0000000..d874e7b
--- /dev/null
+++ b/engineer2_arm_config/launch/servo.launch
@@ -0,0 +1,6 @@
+<launch>
+    <node name="servo_server" pkg="moveit_servo" type="servo_server" output="screen">
+        <param name="parameter_ns" type="string" value="engineer_middleware"/>
+        <rosparam command="load" file="$(find engineer2_arm_config)/config/servo.yaml"/>
+    </node>
+</launch>
diff --git a/engineer2_arm_config/launch/trajectory_execution.launch.xml b/engineer2_arm_config/launch/trajectory_execution.launch.xml
new file mode 100644
index 0000000..1801408
--- /dev/null
+++ b/engineer2_arm_config/launch/trajectory_execution.launch.xml
@@ -0,0 +1,21 @@
+<launch>
+
+    <!-- This file makes it easy to include the settings for trajectory execution  -->
+
+    <!-- Flag indicating whether MoveIt is allowed to load/unload  or switch controllers -->
+    <arg name="moveit_manage_controllers" default="true"/>
+    <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
+
+    <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
+    <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
+    <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
+    <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
+    <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
+    <param name="trajectory_execution/allowed_start_tolerance" value="0.05"/> <!-- default 0.01 -->
+
+    <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
+    <arg name="moveit_controller_manager" default="engineer"/>
+    <include
+            file="$(find engineer2_arm_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml"/>
+
+</launch>
diff --git a/engineer2_arm_config/package.xml b/engineer2_arm_config/package.xml
new file mode 100644
index 0000000..3cc0444
--- /dev/null
+++ b/engineer2_arm_config/package.xml
@@ -0,0 +1,27 @@
+<package>
+    <name>engineer2_arm_config</name>
+    <version>1.0.0</version>
+    <description>
+        An automatically generated package with all the configuration and launch files for using the engineer with the
+        MoveIt Motion Planning Framework
+    </description>
+    <author email="liaoqiayuan@gmail.com">QiayuanLiao</author>
+    <maintainer email="liaoqiayuan@gmail.com">QiayuanLiao</maintainer>
+
+    <license>BSD</license>
+
+    <url type="website">http://moveit.ros.org/</url>
+    <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
+    <url type="repository">https://github.com/ros-planning/moveit</url>
+
+    <buildtool_depend>catkin</buildtool_depend>
+
+    <run_depend>moveit_ros_move_group</run_depend>
+    <run_depend>moveit_fake_controller_manager</run_depend>
+    <run_depend>moveit_kinematics</run_depend>
+    <run_depend>moveit_planners_ompl</run_depend>
+    <run_depend>moveit_ros_visualization</run_depend>
+    <run_depend>moveit_setup_assistant</run_depend>
+    <run_depend>tf2_ros</run_depend>
+    <run_depend>xacro</run_depend>
+</package>
diff --git a/engineer_middleware/config/engineer2.yaml b/engineer_middleware/config/engineer2.yaml
new file mode 100644
index 0000000..24d6f86
--- /dev/null
+++ b/engineer_middleware/config/engineer2.yaml
@@ -0,0 +1,9 @@
+chassis:
+  x:
+    pid: { p: 4.5, i: 0.0, d: 0.2, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+  y:
+    pid: { p: 4.5, i: 0.0, d: 0.2, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+  yaw:
+    pid: { p: 5., i: 0.0, d: 0.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+  yaw_start_threshold: 0.05
+  max_vel: 10
diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
new file mode 100644
index 0000000..e69de29
diff --git a/engineer_middleware/config/steps_list.yaml b/engineer_middleware/config/engineer_steps_list.yaml
similarity index 100%
rename from engineer_middleware/config/steps_list.yaml
rename to engineer_middleware/config/engineer_steps_list.yaml
diff --git a/engineer_middleware/launch/load.launch b/engineer_middleware/launch/load.launch
index 778c77c..816a8c9 100644
--- a/engineer_middleware/launch/load.launch
+++ b/engineer_middleware/launch/load.launch
@@ -1,7 +1,8 @@
 <launch>
-    <rosparam file="$(find engineer_middleware)/config/steps_list.yaml" command="load"
+    <arg name="robot_type" default="$(env ROBOT_TYPE)" doc="Robot type [engineer, engineer2]"/>
+    <rosparam file="$(find engineer_middleware)/config/$(arg robot_type)_steps_list.yaml" command="load"
               ns="engineer_middleware"/>
-    <rosparam file="$(find engineer_middleware)/config/engineer.yaml" command="load"
+    <rosparam file="$(find engineer_middleware)/config/$(arg robot_type).yaml" command="load"
               ns="engineer_middleware"/>
     <rosparam file="$(find engineer_middleware)/config/scenes_list.yaml" command="load"
               ns="engineer_middleware"/>
diff --git a/engineer_middleware/launch/sim.launch b/engineer_middleware/launch/sim.launch
index 394f0ff..7eebf7e 100644
--- a/engineer_middleware/launch/sim.launch
+++ b/engineer_middleware/launch/sim.launch
@@ -1,10 +1,11 @@
 <launch>
+    <arg name="robot_type" default="$(env ROBOT_TYPE)" doc="Robot type [engineer, engineer2]"/>
     <arg name="world" default="empty_world" doc="big_resource small_resource"/>
     <include file="$(find rm_gazebo)/launch/$(arg world).launch"/>
     <include file="$(find engineer_middleware)/launch/load.launch"/>
-    <include file="$(find engineer_arm_config)/launch/load_move_group.launch"/>
-    <include file="$(find rm_config)/launch/manual.launch"/>
-    <include file="$(find engineer_arm_config)/launch/load_controllers.launch">
+    <include file="$(find $(arg robot_type)_arm_config)/launch/load_move_group.launch"/>
+<!--    <include file="$(find rm_config)/launch/manual.launch"/>-->
+    <include file="$(find $(arg robot_type)_arm_config)/launch/load_controllers.launch">
         <arg name="use_simulation" value="true"/>
     </include>
 </launch>

From 85838176eee47efd34ed046b46faac8ff2b88a22 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 1 Jul 2024 17:41:54 +0800
Subject: [PATCH 104/147] Modify Lv5 exchange pos of engineer.

---
 engineer_middleware/config/engineer_steps_list.yaml      | 8 ++++----
 engineer_middleware/include/engineer_middleware/motion.h | 4 ++--
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/engineer_middleware/config/engineer_steps_list.yaml b/engineer_middleware/config/engineer_steps_list.yaml
index cd2f7a9..2a50cb9 100644
--- a/engineer_middleware/config/engineer_steps_list.yaml
+++ b/engineer_middleware/config/engineer_steps_list.yaml
@@ -1284,7 +1284,7 @@ steps_list:
         change: "-1"
     - step: "exchange pos"
       arm:
-        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
+        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1340,7 +1340,7 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "exchange pos"
       arm:
-        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
+        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1399,7 +1399,7 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "exchange pos"
       arm:
-        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
+        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1458,7 +1458,7 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "exchange pos"
       arm:
-        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
+        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
         common:
           <<: *NORMALLY
         tolerance:
diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 3ad3ff1..06f19fb 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -460,8 +460,8 @@ class JointMotion : public MoveitMotionBase
     {
       error = std::abs(final_target_[i] - current[i]);
       joint_reached = (error < tolerance_joints_[i]);
-      if (!joint_reached)
-        ROS_INFO_STREAM("Joint" << i + 1 << " didn't reach configured tolerance range,error: " << error);
+      //      if (!joint_reached)
+      //        ROS_INFO_STREAM("Joint" << i + 1 << " didn't reach configured tolerance range,error: " << error);
       flag &= joint_reached;
     }
     return flag;

From 69188ab50d4386968b1aa09d13bc5149c0351578 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sun, 7 Jul 2024 17:53:15 +0800
Subject: [PATCH 105/147] Record engineer2 arm config pack.Can be used to run
 gazebo and moveit.

---
 engineer2_arm_config/.setup_assistant         |    6 +-
 engineer2_arm_config/CMakeLists.txt           |    2 +-
 .../config/cartesian_limits.yaml              |    5 +
 .../config/chomp_planning.yaml                |    6 +-
 engineer2_arm_config/config/engineer2.srdf    |   51 +-
 .../config/fake_controllers.yaml              |    4 +-
 .../config/gazebo_controllers.yaml            |    4 +
 .../config/gazebo_engineer2.urdf              | 2628 +++++++++++++++++
 engineer2_arm_config/config/joint_limits.yaml |   36 +-
 engineer2_arm_config/config/kinematics.yaml   |    4 +
 .../config/ompl_planning.yaml                 |   52 +-
 .../config/ros_controllers.yaml               |   57 +-
 engineer2_arm_config/config/sensors_3d.yaml   |    1 -
 .../config/simple_moveit_controllers.yaml     |   12 +
 .../config/stomp_planning.yaml                |   39 +
 .../launch/chomp_planning_pipeline.launch.xml |   26 +-
 ...eer2_moveit_controller_manager.launch.xml} |    0
 ...ngineer2_moveit_sensor_manager.launch.xml} |    0
 .../fake_moveit_controller_manager.launch.xml |    5 +-
 engineer2_arm_config/launch/gazebo.launch     |   34 +
 .../launch/joystick_control.launch            |   17 +
 .../launch/load_controllers.launch            |   22 +-
 engineer2_arm_config/launch/move_group.launch |  145 +-
 engineer2_arm_config/launch/moveit.rviz       |  451 +--
 .../launch/moveit_rviz.launch                 |   15 +
 .../launch/ompl_planning_pipeline.launch.xml  |   35 +-
 .../launch/planning_context.launch            |   35 +-
 .../launch/planning_pipeline.launch.xml       |    8 +-
 ...ntrol_moveit_controller_manager.launch.xml |    4 +
 .../launch/ros_controllers.launch             |   11 +
 .../launch/sensor_manager.launch.xml          |   17 +
 .../launch/setup_assistant.launch             |   16 +
 ...imple_moveit_controller_manager.launch.xml |    8 +
 .../launch/stomp_planning_pipeline.launch.xml |   23 +
 .../launch/trajectory_execution.launch.xml    |   30 +-
 engineer2_arm_config/package.xml              |   56 +-
 .../launch/load_controllers.launch            |    2 +-
 .../engineer2_simulation_controllers.yaml     |   31 +
 .../config/engineer2_steps_list.yaml          |   24 +
 ...l => engineer_simulation_controllers.yaml} |    0
 40 files changed, 3234 insertions(+), 688 deletions(-)
 create mode 100644 engineer2_arm_config/config/cartesian_limits.yaml
 create mode 100644 engineer2_arm_config/config/gazebo_controllers.yaml
 create mode 100644 engineer2_arm_config/config/gazebo_engineer2.urdf
 create mode 100644 engineer2_arm_config/config/simple_moveit_controllers.yaml
 create mode 100644 engineer2_arm_config/config/stomp_planning.yaml
 rename engineer2_arm_config/launch/{engineer_moveit_controller_manager.launch.xml => engineer2_moveit_controller_manager.launch.xml} (100%)
 rename engineer2_arm_config/launch/{engineer_moveit_sensor_manager.launch.xml => engineer2_moveit_sensor_manager.launch.xml} (100%)
 create mode 100644 engineer2_arm_config/launch/gazebo.launch
 create mode 100644 engineer2_arm_config/launch/joystick_control.launch
 create mode 100644 engineer2_arm_config/launch/moveit_rviz.launch
 create mode 100644 engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml
 create mode 100644 engineer2_arm_config/launch/ros_controllers.launch
 create mode 100644 engineer2_arm_config/launch/sensor_manager.launch.xml
 create mode 100644 engineer2_arm_config/launch/setup_assistant.launch
 create mode 100644 engineer2_arm_config/launch/simple_moveit_controller_manager.launch.xml
 create mode 100644 engineer2_arm_config/launch/stomp_planning_pipeline.launch.xml
 create mode 100644 engineer_middleware/config/engineer2_simulation_controllers.yaml
 rename engineer_middleware/config/{simulation_controllers.yaml => engineer_simulation_controllers.yaml} (100%)

diff --git a/engineer2_arm_config/.setup_assistant b/engineer2_arm_config/.setup_assistant
index 197025b..7ab7a69 100644
--- a/engineer2_arm_config/.setup_assistant
+++ b/engineer2_arm_config/.setup_assistant
@@ -6,6 +6,6 @@ moveit_setup_assistant_config:
   SRDF:
     relative_path: config/engineer2.srdf
   CONFIG:
-    author_name: QiayuanLiao
-    author_email: liaoqiayuan@gmail.com
-    generated_timestamp: 1617294211
+    author_name: Chonghong Cai
+    author_email: 792166012@qq.com
+    generated_timestamp: 1720344342
diff --git a/engineer2_arm_config/CMakeLists.txt b/engineer2_arm_config/CMakeLists.txt
index ad88885..3121434 100644
--- a/engineer2_arm_config/CMakeLists.txt
+++ b/engineer2_arm_config/CMakeLists.txt
@@ -6,5 +6,5 @@ find_package(catkin REQUIRED)
 catkin_package()
 
 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-        PATTERN "setup_assistant.launch" EXCLUDE)
+  PATTERN "setup_assistant.launch" EXCLUDE)
 install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/engineer2_arm_config/config/cartesian_limits.yaml b/engineer2_arm_config/config/cartesian_limits.yaml
new file mode 100644
index 0000000..7df72f6
--- /dev/null
+++ b/engineer2_arm_config/config/cartesian_limits.yaml
@@ -0,0 +1,5 @@
+cartesian_limits:
+  max_trans_vel: 1
+  max_trans_acc: 2.25
+  max_trans_dec: -5
+  max_rot_vel: 1.57
diff --git a/engineer2_arm_config/config/chomp_planning.yaml b/engineer2_arm_config/config/chomp_planning.yaml
index 0831fda..eb9c912 100644
--- a/engineer2_arm_config/config/chomp_planning.yaml
+++ b/engineer2_arm_config/config/chomp_planning.yaml
@@ -7,12 +7,12 @@ learning_rate: 0.01
 smoothness_cost_velocity: 0.0
 smoothness_cost_acceleration: 1.0
 smoothness_cost_jerk: 0.0
-ridge_factor: 0.01
+ridge_factor: 0.0
 use_pseudo_inverse: false
 pseudo_inverse_ridge_factor: 1e-4
 joint_update_limit: 0.1
 collision_clearance: 0.2
 collision_threshold: 0.07
 use_stochastic_descent: true
-enable_failure_recovery: true
-max_recovery_attempts: 15
+enable_failure_recovery: false
+max_recovery_attempts: 5
diff --git a/engineer2_arm_config/config/engineer2.srdf b/engineer2_arm_config/config/engineer2.srdf
index a9012bb..1c6d7cc 100644
--- a/engineer2_arm_config/config/engineer2.srdf
+++ b/engineer2_arm_config/config/engineer2.srdf
@@ -3,53 +3,34 @@
     This is a format for representing semantic information about the robot structure.
     A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
 -->
-<robot name="engineer">
+<robot name="engineer2">
     <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
     <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
     <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
     <group name="engineer_arm">
-        <joint name="joint1"/>
-        <joint name="joint2"/>
-        <joint name="joint3"/>
-        <joint name="joint4"/>
-        <joint name="joint5"/>
-        <joint name="joint6"/>
+        <chain base_link="base_link" tip_link="link6"/>
     </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="home" group="engineer_arm">
-        <joint name="joint1" value="0.16"/>
+        <joint name="joint1" value="0.0101"/>
         <joint name="joint2" value="0"/>
         <joint name="joint3" value="0"/>
         <joint name="joint4" value="0"/>
-        <joint name="joint5" value="1.92225"/>
+        <joint name="joint5" value="0"/>
         <joint name="joint6" value="0"/>
-        <joint name="joint7" value="0"/>
     </group_state>
-        <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-        <disable_collisions link1="base_link" link2="gimbal_base" reason="Adjacent"/>
-        <disable_collisions link1="base_link" link2="left_back_wheel" reason="Adjacent"/>
-        <disable_collisions link1="base_link" link2="left_front_wheel" reason="Adjacent"/>
-        <disable_collisions link1="base_link" link2="link1" reason="Adjacent"/>
-        <disable_collisions link1="base_link" link2="link2" reason="Default"/>
-        <disable_collisions link1="base_link" link2="link3" reason="Never"/>
-        <disable_collisions link1="base_link" link2="link5" reason="Default"/>
-        <disable_collisions link1="base_link" link2="link6" reason="Default"/>
-        <disable_collisions link1="gimbal_base" link2="link1" reason="Never"/>
-        <disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
-        <disable_collisions link1="link1" link2="link3" reason="Never"/>
-        <disable_collisions link1="link1" link2="link4" reason="Never"/>
-        <disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
-        <disable_collisions link1="link2" link2="link4" reason="Never"/>
-        <disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
-        <disable_collisions link1="link3" link2="link5" reason="Default"/>
-        <disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
-        <disable_collisions link1="link4" link2="link6" reason="Never"/>
-        <disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
-        <disable_collisions link1="link6" link2="link7" reason="Never"/>
-        <disable_collisions link1="link5" link2="link7" reason="Never"/>
-        <disable_collisions link1="link1" link2="camera_link" reason="Never"/>
-        <disable_collisions link1="link7" link2="ore_bin_lifter" reason="Never"/>
-        <disable_collisions link1="link7" link2="ore_bin_rotator" reason="Never"/>
+    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="base_link" link2="link6" reason="User"/>
+    <disable_collisions link1="base_link" link2="link5" reason="User"/>
+    <disable_collisions link1="base_link" link2="link4" reason="User"/>
+    <disable_collisions link1="link1" link2="link4" reason="User"/>
+    <disable_collisions link1="link1" link2="link5" reason="User"/>
+    <disable_collisions link1="link1" link2="link6" reason="User"/>
+    <disable_collisions link1="link2" link2="link4" reason="User"/>
+    <disable_collisions link1="link2" link2="link5" reason="User"/>
+    <disable_collisions link1="base_link" link2="link2" reason="User"/>
+    <disable_collisions link1="link2" link2="link6" reason="User"/>
+    <disable_collisions link1="base_link" link2="link3" reason="User"/>
 </robot>
diff --git a/engineer2_arm_config/config/fake_controllers.yaml b/engineer2_arm_config/config/fake_controllers.yaml
index c51e46f..4167cc8 100644
--- a/engineer2_arm_config/config/fake_controllers.yaml
+++ b/engineer2_arm_config/config/fake_controllers.yaml
@@ -9,5 +9,5 @@ controller_list:
       - joint5
       - joint6
 initial:  # Define initial robot poses per group
-#  - group: engineer_arm
-#    pose: home
+  - group: engineer_arm
+    pose: home
diff --git a/engineer2_arm_config/config/gazebo_controllers.yaml b/engineer2_arm_config/config/gazebo_controllers.yaml
new file mode 100644
index 0000000..e4d2eb0
--- /dev/null
+++ b/engineer2_arm_config/config/gazebo_controllers.yaml
@@ -0,0 +1,4 @@
+# Publish joint_states
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
diff --git a/engineer2_arm_config/config/gazebo_engineer2.urdf b/engineer2_arm_config/config/gazebo_engineer2.urdf
new file mode 100644
index 0000000..e04abdc
--- /dev/null
+++ b/engineer2_arm_config/config/gazebo_engineer2.urdf
@@ -0,0 +1,2628 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from /home/cch/rm_ws/src/rm_description/urdf/engineer2/engineer2.urdf.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="engineer2">
+    <link name="base_link">
+        <inertial>
+            <mass value="15.736541" />
+            <origin xyz="0.05048 0.003745 0.123835" rpy="0 0 0" />
+            <inertia ixx="6.818e-1" ixy="-5.361e-3" ixz="-2.802e-2" iyy="6.637e-1" iyz="-1.676e-2" izz="9.575e-1" />
+        </inertial>
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/base_link.stl" />
+            </geometry>
+        </visual>
+        <collision>
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/base_link.stl" />
+            </geometry>
+        </collision>
+    </link>
+    <!--  link  -->
+    <link name="left_front_wheel">
+        <visual>
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/right_threaded_14roller_mecanum_wheel.stl" />
+            </geometry>
+        </visual>
+        <inertial>
+            <mass value="1.613177" />
+            <inertia ixx="1.416e-3" ixy="0.0" ixz="0.0" iyy="2.749e-3" iyz="0.0" izz="1.416e-3" />
+        </inertial>
+    </link>
+    <!--  joint  -->
+    <joint name="left_front_wheel_joint" type="revolute">
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="0.213 0.1945 0.07" />
+        <!--      limit not work while type="continuous"-->
+        <limit effort="5" lower="-1e9" upper="1e9" velocity="50" />
+        <dynamics damping="0.0" friction="0.05" />
+        <parent link="base_link" />
+        <child link="left_front_wheel" />
+    </joint>
+    <!--  link  -->
+    <link name="left_front_roller_0">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.0                      -0.0                      -0.7853981633974484" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_front_roller_0_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="3.7351727373994275e-18 0 -0.061000000000000006" />
+        <axis xyz="0.7071067811865476 -0.7071067811865476 0.0" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_front_wheel" />
+        <child link="left_front_roller_0" />
+    </joint>
+    <gazebo reference="left_front_roller_0">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+        <plugin name="gazebo_ros_control">
+            <robotNamespace>/</robotNamespace>
+        </plugin>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_front_roller_1">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.1397561040427883                      -0.3119658732170331                      -0.8374943569350152" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_front_roller_1_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.0264779570502484 0 -0.0549537786730376" />
+        <axis xyz="0.6370195008447469 -0.7071067811865476 0.30693021282289823" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_front_wheel" />
+        <child link="left_front_roller_1" />
+    </joint>
+    <gazebo reference="left_front_roller_1">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_front_roller_2">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.33184050390145986                      -0.5859793018876829                      -1.0135101754191935" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_front_roller_2_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.04770700955585395 0 -0.03801369804738366" />
+        <axis xyz="0.4406515355292263 -0.7071067811865476 0.55301557323075" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_front_wheel" />
+        <child link="left_front_roller_2" />
+    </joint>
+    <gazebo reference="left_front_roller_2">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_front_roller_3">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.6209334157717955                      -0.760761096270827                      -1.352403136081973" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_front_roller_3_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.05947877882794464 0 -0.013537904902031354" />
+        <axis xyz="0.15693023539811435 -0.7071067811865476 0.6894729155071231" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_front_wheel" />
+        <child link="left_front_roller_3" />
+    </joint>
+    <gazebo reference="left_front_roller_3">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_front_roller_4">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.9253265685651103                      -0.7604552807523317                      -1.7904967611333005" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_front_roller_4_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.05945966740983209 0 0.013621598713519345" />
+        <axis xyz="-0.15790040690051604 -0.7071067811865476 0.6892513775834267" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_front_wheel" />
+        <child link="left_front_roller_4" />
+    </joint>
+    <gazebo reference="left_front_roller_4">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_front_roller_5">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.9952510398634623                      -0.5852345045480243                      -2.1288744267289648" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_front_roller_5_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.047653463887214025 0 0.038080800668446955" />
+        <axis xyz="-0.44142938337167287 -0.7071067811865476 0.5523948764209392" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_front_wheel" />
+        <child link="left_front_roller_5" />
+    </joint>
+    <gazebo reference="left_front_roller_5">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_front_roller_6">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.6776460855750719                      -0.3110237228468675                      -2.3044349087240077" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_front_roller_6_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.02640059185120231 0 0.054990987897165756" />
+        <axis xyz="-0.6374508269874306 -0.7071067811865476 0.30603340205448304" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_front_wheel" />
+        <child link="left_front_roller_6" />
+    </joint>
+    <gazebo reference="left_front_roller_6">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_front_roller_7">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.002402487564129699                      0.0009951440258853248                      -2.356193995036202" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_front_roller_7_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-8.584810268382325e-05 0 0.06099993959098047" />
+        <axis xyz="-0.7071060809303613 -0.7071067811865476 -0.0009951438616348823" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_front_wheel" />
+        <child link="left_front_roller_7" />
+    </joint>
+    <gazebo reference="left_front_roller_7">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_front_roller_8">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.6809319806086911                      0.3129076709989273                      -2.303760494768245" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_front_roller_8_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.026555269806428094 0 0.054916460606158964" />
+        <axis xyz="-0.6365869130062115 -0.7071067811865476 -0.3078264156780931" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_front_wheel" />
+        <child link="left_front_roller_8" />
+    </joint>
+    <gazebo reference="left_front_roller_8">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_front_roller_9">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.9959421340702964                      0.5867231523205594                      -2.127288857247016" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_front_roller_9_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.04776046073486529 0 0.03794652013549321" />
+        <axis xyz="-0.43987281492195274 -0.7071067811865476 -0.5536351747248702" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_front_wheel" />
+        <child link="left_front_roller_9" />
+    </joint>
+    <gazebo reference="left_front_roller_9">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_front_roller_10">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.9238936146297452                      0.7610651149346043                      -1.7878810957525777" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_front_roller_10_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.05949777244098466 0 0.013454184277049528" />
+        <axis xyz="-0.15595975307598609 -0.7071067811865476 -0.6896930878444973" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_front_wheel" />
+        <child link="left_front_roller_10" />
+    </joint>
+    <gazebo reference="left_front_roller_10">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_front_roller_11">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.6188607822812026                      0.7601476705240283                      -1.3497898326199715" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_front_roller_11_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.059440438224499535 0 -0.013705265545747565" />
+        <axis xyz="0.15887026566164672 -0.7071067811865476 -0.6890284745121915" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_front_wheel" />
+        <child link="left_front_roller_11" />
+    </joint>
+    <gazebo reference="left_front_roller_11">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_front_roller_12">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.33035697597807884                      0.5844887627649618                      -1.0119279494343745" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_front_roller_12_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.04759982383499931 0 -0.03814782786577804" />
+        <axis xyz="0.4422063569086687 -0.7071067811865476 -0.5517730855248045" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_front_wheel" />
+        <child link="left_front_roller_12" />
+    </joint>
+    <gazebo reference="left_front_roller_12">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_front_roller_13">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.13877562966172519                      0.3100812212384385                      -0.8368223213782329" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_front_roller_13_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.0263231743625212 0 -0.05502808820484596" />
+        <axis xyz="0.637880890579968 -0.7071067811865476 -0.30513598514909207" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_front_wheel" />
+        <child link="left_front_roller_13" />
+    </joint>
+    <gazebo reference="left_front_roller_13">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_front_wheel">
+        <visual>
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/left_threaded_14roller_mecanum_wheel.stl" />
+            </geometry>
+        </visual>
+        <inertial>
+            <mass value="1.613177" />
+            <inertia ixx="1.416e-3" ixy="0.0" ixz="0.0" iyy="2.749e-3" iyz="0.0" izz="1.416e-3" />
+        </inertial>
+    </link>
+    <!--  joint  -->
+    <joint name="right_front_wheel_joint" type="revolute">
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="0.213 -0.1945 0.07" />
+        <!--      limit not work while type="continuous"-->
+        <limit effort="5" lower="-1e9" upper="1e9" velocity="50" />
+        <dynamics damping="0.0" friction="0.05" />
+        <parent link="base_link" />
+        <child link="right_front_wheel" />
+    </joint>
+    <!--  link  -->
+    <link name="right_front_roller_0">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.0                      0.0                      0.7853981633974484" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_front_roller_0_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="3.7351727373994275e-18 0 -0.061000000000000006" />
+        <axis xyz="0.7071067811865476 0.7071067811865476 0.0" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_front_wheel" />
+        <child link="right_front_roller_0" />
+    </joint>
+    <gazebo reference="right_front_roller_0">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_front_roller_1">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.1397561040427883                      -0.3119658732170331                      0.8374943569350152" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_front_roller_1_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.0264779570502484 0 -0.0549537786730376" />
+        <axis xyz="0.6370195008447469 0.7071067811865476 0.30693021282289823" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_front_wheel" />
+        <child link="right_front_roller_1" />
+    </joint>
+    <gazebo reference="right_front_roller_1">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_front_roller_2">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.33184050390145986                      -0.5859793018876829                      1.0135101754191935" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_front_roller_2_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.04770700955585395 0 -0.03801369804738366" />
+        <axis xyz="0.4406515355292263 0.7071067811865476 0.55301557323075" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_front_wheel" />
+        <child link="right_front_roller_2" />
+    </joint>
+    <gazebo reference="right_front_roller_2">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_front_roller_3">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.6209334157717955                      -0.760761096270827                      1.352403136081973" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_front_roller_3_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.05947877882794464 0 -0.013537904902031354" />
+        <axis xyz="0.15693023539811435 0.7071067811865476 0.6894729155071231" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_front_wheel" />
+        <child link="right_front_roller_3" />
+    </joint>
+    <gazebo reference="right_front_roller_3">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_front_roller_4">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.9253265685651103                      -0.7604552807523317                      1.7904967611333005" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_front_roller_4_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.05945966740983209 0 0.013621598713519345" />
+        <axis xyz="-0.15790040690051604 0.7071067811865476 0.6892513775834267" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_front_wheel" />
+        <child link="right_front_roller_4" />
+    </joint>
+    <gazebo reference="right_front_roller_4">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_front_roller_5">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.9952510398634623                      -0.5852345045480243                      2.1288744267289648" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_front_roller_5_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.047653463887214025 0 0.038080800668446955" />
+        <axis xyz="-0.44142938337167287 0.7071067811865476 0.5523948764209392" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_front_wheel" />
+        <child link="right_front_roller_5" />
+    </joint>
+    <gazebo reference="right_front_roller_5">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_front_roller_6">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.6776460855750719                      -0.3110237228468675                      2.3044349087240077" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_front_roller_6_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.02640059185120231 0 0.054990987897165756" />
+        <axis xyz="-0.6374508269874306 0.7071067811865476 0.30603340205448304" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_front_wheel" />
+        <child link="right_front_roller_6" />
+    </joint>
+    <gazebo reference="right_front_roller_6">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_front_roller_7">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.002402487564129699                      0.0009951440258853248                      2.356193995036202" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_front_roller_7_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-8.584810268382325e-05 0 0.06099993959098047" />
+        <axis xyz="-0.7071060809303613 0.7071067811865476 -0.0009951438616348823" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_front_wheel" />
+        <child link="right_front_roller_7" />
+    </joint>
+    <gazebo reference="right_front_roller_7">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_front_roller_8">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.6809319806086911                      0.3129076709989273                      2.303760494768245" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_front_roller_8_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.026555269806428094 0 0.054916460606158964" />
+        <axis xyz="-0.6365869130062115 0.7071067811865476 -0.3078264156780931" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_front_wheel" />
+        <child link="right_front_roller_8" />
+    </joint>
+    <gazebo reference="right_front_roller_8">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_front_roller_9">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.9959421340702964                      0.5867231523205594                      2.127288857247016" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_front_roller_9_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.04776046073486529 0 0.03794652013549321" />
+        <axis xyz="-0.43987281492195274 0.7071067811865476 -0.5536351747248702" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_front_wheel" />
+        <child link="right_front_roller_9" />
+    </joint>
+    <gazebo reference="right_front_roller_9">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_front_roller_10">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.9238936146297452                      0.7610651149346043                      1.7878810957525777" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_front_roller_10_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.05949777244098466 0 0.013454184277049528" />
+        <axis xyz="-0.15595975307598609 0.7071067811865476 -0.6896930878444973" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_front_wheel" />
+        <child link="right_front_roller_10" />
+    </joint>
+    <gazebo reference="right_front_roller_10">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_front_roller_11">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.6188607822812026                      0.7601476705240283                      1.3497898326199715" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_front_roller_11_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.059440438224499535 0 -0.013705265545747565" />
+        <axis xyz="0.15887026566164672 0.7071067811865476 -0.6890284745121915" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_front_wheel" />
+        <child link="right_front_roller_11" />
+    </joint>
+    <gazebo reference="right_front_roller_11">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_front_roller_12">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.33035697597807884                      0.5844887627649618                      1.0119279494343745" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_front_roller_12_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.04759982383499931 0 -0.03814782786577804" />
+        <axis xyz="0.4422063569086687 0.7071067811865476 -0.5517730855248045" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_front_wheel" />
+        <child link="right_front_roller_12" />
+    </joint>
+    <gazebo reference="right_front_roller_12">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_front_roller_13">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.13877562966172519                      0.3100812212384385                      0.8368223213782329" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_front_roller_13_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.0263231743625212 0 -0.05502808820484596" />
+        <axis xyz="0.637880890579968 0.7071067811865476 -0.30513598514909207" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_front_wheel" />
+        <child link="right_front_roller_13" />
+    </joint>
+    <gazebo reference="right_front_roller_13">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_back_wheel">
+        <visual>
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/left_threaded_14roller_mecanum_wheel.stl" />
+            </geometry>
+        </visual>
+        <inertial>
+            <mass value="1.613177" />
+            <inertia ixx="1.416e-3" ixy="0.0" ixz="0.0" iyy="2.749e-3" iyz="0.0" izz="1.416e-3" />
+        </inertial>
+    </link>
+    <!--  joint  -->
+    <joint name="left_back_wheel_joint" type="revolute">
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="-0.213 0.1945 0.07" />
+        <!--      limit not work while type="continuous"-->
+        <limit effort="5" lower="-1e9" upper="1e9" velocity="50" />
+        <dynamics damping="0.0" friction="0.05" />
+        <parent link="base_link" />
+        <child link="left_back_wheel" />
+    </joint>
+    <!--  link  -->
+    <link name="left_back_roller_0">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.0                      0.0                      0.7853981633974484" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_back_roller_0_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="3.7351727373994275e-18 0 -0.061000000000000006" />
+        <axis xyz="0.7071067811865476 0.7071067811865476 0.0" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_back_wheel" />
+        <child link="left_back_roller_0" />
+    </joint>
+    <gazebo reference="left_back_roller_0">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_back_roller_1">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.1397561040427883                      -0.3119658732170331                      0.8374943569350152" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_back_roller_1_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.0264779570502484 0 -0.0549537786730376" />
+        <axis xyz="0.6370195008447469 0.7071067811865476 0.30693021282289823" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_back_wheel" />
+        <child link="left_back_roller_1" />
+    </joint>
+    <gazebo reference="left_back_roller_1">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_back_roller_2">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.33184050390145986                      -0.5859793018876829                      1.0135101754191935" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_back_roller_2_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.04770700955585395 0 -0.03801369804738366" />
+        <axis xyz="0.4406515355292263 0.7071067811865476 0.55301557323075" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_back_wheel" />
+        <child link="left_back_roller_2" />
+    </joint>
+    <gazebo reference="left_back_roller_2">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_back_roller_3">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.6209334157717955                      -0.760761096270827                      1.352403136081973" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_back_roller_3_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.05947877882794464 0 -0.013537904902031354" />
+        <axis xyz="0.15693023539811435 0.7071067811865476 0.6894729155071231" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_back_wheel" />
+        <child link="left_back_roller_3" />
+    </joint>
+    <gazebo reference="left_back_roller_3">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_back_roller_4">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.9253265685651103                      -0.7604552807523317                      1.7904967611333005" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_back_roller_4_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.05945966740983209 0 0.013621598713519345" />
+        <axis xyz="-0.15790040690051604 0.7071067811865476 0.6892513775834267" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_back_wheel" />
+        <child link="left_back_roller_4" />
+    </joint>
+    <gazebo reference="left_back_roller_4">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_back_roller_5">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.9952510398634623                      -0.5852345045480243                      2.1288744267289648" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_back_roller_5_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.047653463887214025 0 0.038080800668446955" />
+        <axis xyz="-0.44142938337167287 0.7071067811865476 0.5523948764209392" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_back_wheel" />
+        <child link="left_back_roller_5" />
+    </joint>
+    <gazebo reference="left_back_roller_5">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_back_roller_6">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.6776460855750719                      -0.3110237228468675                      2.3044349087240077" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_back_roller_6_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.02640059185120231 0 0.054990987897165756" />
+        <axis xyz="-0.6374508269874306 0.7071067811865476 0.30603340205448304" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_back_wheel" />
+        <child link="left_back_roller_6" />
+    </joint>
+    <gazebo reference="left_back_roller_6">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_back_roller_7">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.002402487564129699                      0.0009951440258853248                      2.356193995036202" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_back_roller_7_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-8.584810268382325e-05 0 0.06099993959098047" />
+        <axis xyz="-0.7071060809303613 0.7071067811865476 -0.0009951438616348823" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_back_wheel" />
+        <child link="left_back_roller_7" />
+    </joint>
+    <gazebo reference="left_back_roller_7">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_back_roller_8">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.6809319806086911                      0.3129076709989273                      2.303760494768245" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_back_roller_8_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.026555269806428094 0 0.054916460606158964" />
+        <axis xyz="-0.6365869130062115 0.7071067811865476 -0.3078264156780931" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_back_wheel" />
+        <child link="left_back_roller_8" />
+    </joint>
+    <gazebo reference="left_back_roller_8">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_back_roller_9">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.9959421340702964                      0.5867231523205594                      2.127288857247016" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_back_roller_9_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.04776046073486529 0 0.03794652013549321" />
+        <axis xyz="-0.43987281492195274 0.7071067811865476 -0.5536351747248702" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_back_wheel" />
+        <child link="left_back_roller_9" />
+    </joint>
+    <gazebo reference="left_back_roller_9">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_back_roller_10">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.9238936146297452                      0.7610651149346043                      1.7878810957525777" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_back_roller_10_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.05949777244098466 0 0.013454184277049528" />
+        <axis xyz="-0.15595975307598609 0.7071067811865476 -0.6896930878444973" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_back_wheel" />
+        <child link="left_back_roller_10" />
+    </joint>
+    <gazebo reference="left_back_roller_10">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_back_roller_11">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.6188607822812026                      0.7601476705240283                      1.3497898326199715" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_back_roller_11_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.059440438224499535 0 -0.013705265545747565" />
+        <axis xyz="0.15887026566164672 0.7071067811865476 -0.6890284745121915" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_back_wheel" />
+        <child link="left_back_roller_11" />
+    </joint>
+    <gazebo reference="left_back_roller_11">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_back_roller_12">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.33035697597807884                      0.5844887627649618                      1.0119279494343745" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_back_roller_12_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.04759982383499931 0 -0.03814782786577804" />
+        <axis xyz="0.4422063569086687 0.7071067811865476 -0.5517730855248045" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_back_wheel" />
+        <child link="left_back_roller_12" />
+    </joint>
+    <gazebo reference="left_back_roller_12">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="left_back_roller_13">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.13877562966172519                      0.3100812212384385                      0.8368223213782329" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="left_back_roller_13_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.0263231743625212 0 -0.05502808820484596" />
+        <axis xyz="0.637880890579968 0.7071067811865476 -0.30513598514909207" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="left_back_wheel" />
+        <child link="left_back_roller_13" />
+    </joint>
+    <gazebo reference="left_back_roller_13">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_back_wheel">
+        <visual>
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/right_threaded_14roller_mecanum_wheel.stl" />
+            </geometry>
+        </visual>
+        <inertial>
+            <mass value="1.613177" />
+            <inertia ixx="1.416e-3" ixy="0.0" ixz="0.0" iyy="2.749e-3" iyz="0.0" izz="1.416e-3" />
+        </inertial>
+    </link>
+    <!--  joint  -->
+    <joint name="right_back_wheel_joint" type="revolute">
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="-0.213 -0.1945 0.07" />
+        <!--      limit not work while type="continuous"-->
+        <limit effort="5" lower="-1e9" upper="1e9" velocity="50" />
+        <dynamics damping="0.0" friction="0.05" />
+        <parent link="base_link" />
+        <child link="right_back_wheel" />
+    </joint>
+    <!--  link  -->
+    <link name="right_back_roller_0">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.0                      -0.0                      -0.7853981633974484" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_back_roller_0_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="3.7351727373994275e-18 0 -0.061000000000000006" />
+        <axis xyz="0.7071067811865476 -0.7071067811865476 0.0" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_back_wheel" />
+        <child link="right_back_roller_0" />
+    </joint>
+    <gazebo reference="right_back_roller_0">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_back_roller_1">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.1397561040427883                      -0.3119658732170331                      -0.8374943569350152" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_back_roller_1_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.0264779570502484 0 -0.0549537786730376" />
+        <axis xyz="0.6370195008447469 -0.7071067811865476 0.30693021282289823" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_back_wheel" />
+        <child link="right_back_roller_1" />
+    </joint>
+    <gazebo reference="right_back_roller_1">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_back_roller_2">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.33184050390145986                      -0.5859793018876829                      -1.0135101754191935" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_back_roller_2_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.04770700955585395 0 -0.03801369804738366" />
+        <axis xyz="0.4406515355292263 -0.7071067811865476 0.55301557323075" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_back_wheel" />
+        <child link="right_back_roller_2" />
+    </joint>
+    <gazebo reference="right_back_roller_2">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_back_roller_3">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.6209334157717955                      -0.760761096270827                      -1.352403136081973" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_back_roller_3_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.05947877882794464 0 -0.013537904902031354" />
+        <axis xyz="0.15693023539811435 -0.7071067811865476 0.6894729155071231" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_back_wheel" />
+        <child link="right_back_roller_3" />
+    </joint>
+    <gazebo reference="right_back_roller_3">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_back_roller_4">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.9253265685651103                      -0.7604552807523317                      -1.7904967611333005" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_back_roller_4_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.05945966740983209 0 0.013621598713519345" />
+        <axis xyz="-0.15790040690051604 -0.7071067811865476 0.6892513775834267" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_back_wheel" />
+        <child link="right_back_roller_4" />
+    </joint>
+    <gazebo reference="right_back_roller_4">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_back_roller_5">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.9952510398634623                      -0.5852345045480243                      -2.1288744267289648" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_back_roller_5_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.047653463887214025 0 0.038080800668446955" />
+        <axis xyz="-0.44142938337167287 -0.7071067811865476 0.5523948764209392" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_back_wheel" />
+        <child link="right_back_roller_5" />
+    </joint>
+    <gazebo reference="right_back_roller_5">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_back_roller_6">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="0.6776460855750719                      -0.3110237228468675                      -2.3044349087240077" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_back_roller_6_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="0.02640059185120231 0 0.054990987897165756" />
+        <axis xyz="-0.6374508269874306 -0.7071067811865476 0.30603340205448304" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_back_wheel" />
+        <child link="right_back_roller_6" />
+    </joint>
+    <gazebo reference="right_back_roller_6">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_back_roller_7">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.002402487564129699                      0.0009951440258853248                      -2.356193995036202" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_back_roller_7_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-8.584810268382325e-05 0 0.06099993959098047" />
+        <axis xyz="-0.7071060809303613 -0.7071067811865476 -0.0009951438616348823" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_back_wheel" />
+        <child link="right_back_roller_7" />
+    </joint>
+    <gazebo reference="right_back_roller_7">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_back_roller_8">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.6809319806086911                      0.3129076709989273                      -2.303760494768245" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_back_roller_8_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.026555269806428094 0 0.054916460606158964" />
+        <axis xyz="-0.6365869130062115 -0.7071067811865476 -0.3078264156780931" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_back_wheel" />
+        <child link="right_back_roller_8" />
+    </joint>
+    <gazebo reference="right_back_roller_8">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_back_roller_9">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.9959421340702964                      0.5867231523205594                      -2.127288857247016" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_back_roller_9_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.04776046073486529 0 0.03794652013549321" />
+        <axis xyz="-0.43987281492195274 -0.7071067811865476 -0.5536351747248702" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_back_wheel" />
+        <child link="right_back_roller_9" />
+    </joint>
+    <gazebo reference="right_back_roller_9">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_back_roller_10">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.9238936146297452                      0.7610651149346043                      -1.7878810957525777" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_back_roller_10_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.05949777244098466 0 0.013454184277049528" />
+        <axis xyz="-0.15595975307598609 -0.7071067811865476 -0.6896930878444973" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_back_wheel" />
+        <child link="right_back_roller_10" />
+    </joint>
+    <gazebo reference="right_back_roller_10">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_back_roller_11">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.6188607822812026                      0.7601476705240283                      -1.3497898326199715" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_back_roller_11_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.059440438224499535 0 -0.013705265545747565" />
+        <axis xyz="0.15887026566164672 -0.7071067811865476 -0.6890284745121915" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_back_wheel" />
+        <child link="right_back_roller_11" />
+    </joint>
+    <gazebo reference="right_back_roller_11">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_back_roller_12">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.33035697597807884                      0.5844887627649618                      -1.0119279494343745" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_back_roller_12_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.04759982383499931 0 -0.03814782786577804" />
+        <axis xyz="0.4422063569086687 -0.7071067811865476 -0.5517730855248045" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_back_wheel" />
+        <child link="right_back_roller_12" />
+    </joint>
+    <gazebo reference="right_back_roller_12">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <!--  link  -->
+    <link name="right_back_roller_13">
+        <collision>
+            <!--  quat_xyzw dosen't work ?! covert to rpy-->
+            <origin rpy="-0.13877562966172519                      0.3100812212384385                      -0.8368223213782329" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/common/roller.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.036" />
+            <inertia ixx="3128e-9" ixy="0.0" ixz="0.0" iyy="3128e-9" iyz="0.0" izz="3128e-9" />
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </inertial>
+    </link>
+    <joint name="right_back_roller_13_joint" type="continuous">
+        <origin rpy="0 0 0" xyz="-0.0263231743625212 0 -0.05502808820484596" />
+        <axis xyz="0.637880890579968 -0.7071067811865476 -0.30513598514909207" />
+        <dynamics damping="0.0" friction="0.001" />
+        <parent link="right_back_wheel" />
+        <child link="right_back_roller_13" />
+    </joint>
+    <gazebo reference="right_back_roller_13">
+        <mu1 value="2000.0" />
+        <mu2 value="2000.0" />
+        <kp value="1000000.0" />
+        <kd value="100.0" />
+        <material>Gazebo/Black</material>
+    </gazebo>
+    <transmission name="left_front_wheel_trans">
+        <type>transmission_interface/SimpleTransmission</type>
+        <actuator name="left_front_wheel_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>19.2032</mechanicalReduction>
+        </actuator>
+        <joint name="left_front_wheel_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+    </transmission>
+    <transmission name="right_front_wheel_trans">
+        <type>transmission_interface/SimpleTransmission</type>
+        <actuator name="right_front_wheel_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>-19.2032</mechanicalReduction>
+        </actuator>
+        <joint name="right_front_wheel_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+    </transmission>
+    <transmission name="left_back_wheel_trans">
+        <type>transmission_interface/SimpleTransmission</type>
+        <actuator name="left_back_wheel_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>19.2032</mechanicalReduction>
+        </actuator>
+        <joint name="left_back_wheel_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+    </transmission>
+    <transmission name="right_back_wheel_trans">
+        <type>transmission_interface/SimpleTransmission</type>
+        <actuator name="right_back_wheel_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>-19.2032</mechanicalReduction>
+        </actuator>
+        <joint name="right_back_wheel_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+    </transmission>
+    <!--  link  -->
+    <link name="link1">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/link1.stl" />
+            </geometry>
+        </visual>
+        <inertial>
+            <mass value="2.49351" />
+            <origin xyz="0.046738  -0.154238  0.018292 " rpy="0 0 0" />
+            <inertia ixx="2.140e-2" ixy="3.902e-3" ixz="7.061e-4" iyy="7.823e-3" iyz="3.525e-4" izz="2.238e-2" />
+        </inertial>
+        <collision>
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/link1.stl" />
+            </geometry>
+        </collision>
+    </link>
+    <link name="link2">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/link2.stl" />
+            </geometry>
+        </visual>
+        <inertial>
+            <mass value="0.601539" />
+            <origin xyz="0.225614  -0.000046  0.016881 " rpy="0 0 0" />
+            <inertia ixx="3.293e-4" ixy="1.357375e-6" ixz="1.603e-4" iyy="5.129e-3" iyz="-0.773379e-6" izz="5.089e-3" />
+        </inertial>
+        <collision>
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/link2.stl" />
+            </geometry>
+        </collision>
+    </link>
+    <link name="link3">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0.0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/link3.stl" />
+            </geometry>
+        </visual>
+        <inertial>
+            <mass value="1.031279" />
+            <origin xyz="0.097823  0.01004 0.063108 " rpy="0 0 0" />
+            <origin xyz="0. 0. 0." />
+            <inertia ixx="7.709e-4" ixy="-2.296e-4" ixz="-2.126e-4" iyy="1.409e-3" iyz="-5.6694522e-5" izz="1.598e-3" />
+        </inertial>
+        <collision>
+            <origin rpy="0. 0. 0." xyz="0. 0. 0." />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/link3.stl" />
+            </geometry>
+        </collision>
+    </link>
+    <link name="link4">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/link4.stl" />
+            </geometry>
+        </visual>
+        <inertial>
+            <mass value="0.874408" />
+            <origin xyz="0.047821  0.00001  0.000115" rpy="0 0 0" />
+            <inertia ixx="1.089e-3" ixy="0.335543e-6" ixz="-3.021042e-6" iyy="7.983e-4" iyz="5.1665e-8" izz="1.568e-3" />
+        </inertial>
+        <collision>
+            <origin rpy="0. 0. 0." xyz="0. 0. 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/link4.stl" />
+            </geometry>
+        </collision>
+    </link>
+    <link name="link5">
+        <visual>
+            <origin rpy="0 0 0" xyz="0. 0. 0." />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/link5.stl" />
+            </geometry>
+        </visual>
+        <inertial>
+            <mass value="0.093643" />
+            <origin xyz="0.015715  -0.001439  -0.000004 " rpy="0 0 0" />
+            <inertia ixx="1.235e-4" ixy="1.570201e-6" ixz="-3.038e-9" iyy="5.1654282e-5" iyz="0.723e-9" izz="1.220e-4" />
+        </inertial>
+        <collision>
+            <origin rpy="0. 0. 0." xyz="0. 0.0 0." />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/link5.stl" />
+            </geometry>
+        </collision>
+    </link>
+    <link name="link6">
+        <visual>
+            <origin rpy="0 0 0" xyz="0. 0. 0." />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/link6.stl" />
+            </geometry>
+        </visual>
+        <inertial>
+            <mass value="0.23135" />
+            <origin xyz="-0.00304 0. -0.000003" rpy="0 0 0" />
+            <inertia ixx="7.760144e-5" ixy="0.01e-9" ixz="0.088e-9" iyy="6.5932798e-5" iyz="-0.008e-9" izz="6.5988593e-5" />
+        </inertial>
+        <collision>
+            <origin rpy="0. 0. 0." xyz="0. 0. 0." />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/link6.stl" />
+            </geometry>
+        </collision>
+    </link>
+    <link name="fake_link4">
+        <inertial>
+            <mass value="0.0001" />
+            <origin xyz="0 0 0" />
+            <inertia ixx="1.0e-8" ixy="0" ixz="0" iyy="1.0e-8" iyz="0" izz="1.0e-8" />
+        </inertial>
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0" />
+            <geometry>
+                <mesh filename="package://rm_description/meshes/engineer2/fake_link4.stl" />
+            </geometry>
+        </visual>
+    </link>
+    <joint name="joint1" type="prismatic">
+        <parent link="base_link" />
+        <child link="link1" />
+        <origin rpy="0 0 0" xyz="0. 0. 0.260" />
+        <axis xyz="0 0 1" />
+        <limit effort="1000" lower="0" upper="0.3" velocity="2.25" />
+        <dynamics damping="0.0" friction="0.1" />
+    </joint>
+    <joint name="joint2" type="revolute">
+        <parent link="link1" />
+        <child link="link2" />
+        <origin rpy="0 0 0" xyz="0.082 -0.2 0." />
+        <axis xyz="0 0 1" />
+        <limit effort="40" lower="-1.2" upper="1.2" velocity="10.46" />
+        <dynamics damping="0.0" friction="0.1" />
+    </joint>
+    <joint name="joint3" type="revolute">
+        <parent link="link2" />
+        <child link="link3" />
+        <origin rpy="0 0 0" xyz="0.275 0. 0." />
+        <axis xyz="0 0 1" />
+        <limit effort="25" lower="-1.2" upper="1.2" velocity="3.76" />
+        <dynamics damping="0.0" friction="0.1" />
+    </joint>
+    <joint name="joint4" type="revolute">
+        <parent link="link3" />
+        <child link="link4" />
+        <origin rpy="0 0 0" xyz="0.153 0. 0.086605" />
+        <axis xyz="1 0 0" />
+        <limit effort="7" lower="-3.14" upper="3.14" velocity="20.94" />
+        <dynamics damping="0.0" friction="0.1" />
+    </joint>
+    <joint name="joint5" type="revolute">
+        <parent link="link4" />
+        <child link="link5" />
+        <origin rpy="0 0 0" xyz="0.1055 0.0 0." />
+        <axis xyz="0 1 0" />
+        <limit effort="14" lower="-1.57" upper="1.57" velocity="10" />
+        <dynamics damping="0.0" friction="0.1" />
+    </joint>
+    <joint name="joint6" type="revolute">
+        <parent link="link5" />
+        <child link="link6" />
+        <origin rpy="0 0 0" xyz="0.026183 0 0" />
+        <axis xyz="1 0 0" />
+        <limit effort="14" lower="-6" upper="6" velocity="10" />
+        <dynamics damping="0.0" friction="0.1" />
+    </joint>
+    <joint name="middle_pitch" type="revolute">
+        <parent link="link3" />
+        <child link="fake_link4" />
+        <origin rpy="0 0 0" xyz="0.0 0 0" />
+        <axis xyz="1 0 0" />
+        <limit effort="25" lower="0" upper="1.57" velocity="3.76" />
+        <dynamics damping="0.0" friction="0.1" />
+    </joint>
+    <!-- transmission -->
+    <transmission name="trans_joint1">
+        <type>transmission_interface/SimpleTransmission</type>
+        <actuator name="joint1_motor">
+            <mechanicalReduction>421.49</mechanicalReduction>
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </actuator>
+        <joint name="joint1">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <offset>0.</offset>
+        </joint>
+    </transmission>
+    <transmission name="trans_joint2">
+        <type>transmission_interface/SimpleTransmission</type>
+        <actuator name="joint2_motor">
+            <mechanicalReduction>1</mechanicalReduction>
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </actuator>
+        <joint name="joint2">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <offset>0.</offset>
+        </joint>
+    </transmission>
+    <transmission name="trans_joint3">
+        <type>transmission_interface/SimpleTransmission</type>
+        <actuator name="joint3_motor">
+            <mechanicalReduction>1</mechanicalReduction>
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </actuator>
+        <joint name="joint3">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <offset>0</offset>
+        </joint>
+    </transmission>
+    <transmission name="trans_joint4">
+        <type>transmission_interface/SimpleTransmission</type>
+        <actuator name="joint4_motor">
+            <mechanicalReduction>1</mechanicalReduction>
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </actuator>
+        <joint name="joint4">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <offset>0</offset>
+        </joint>
+    </transmission>
+    <transmission name="trans_diff_ee">
+        <type>transmission_interface/DifferentialTransmission</type>
+        <actuator name="left_diff_motor">
+            <role>actuator1</role>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+        <actuator name="right_diff_motor">
+            <role>actuator2</role>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+        <joint name="joint5">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <offset>0</offset>
+            <role>joint1</role>
+        </joint>
+        <joint name="joint6">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <offset>0</offset>
+            <role>joint2</role>
+        </joint>
+    </transmission>
+    <!--        <xacro:unless value="$(arg use_simulation)">-->
+    <!--            <xacro:include filename="$(find rm_description)/urdf/common/camera.urdf.xacro"/>-->
+    <!--            <xacro:camera_sensor xyz="-0.0232 0 0.3765" rpy="0. 0. 0." frame_id="$(arg camera_link)" parent="gimbal_lifter"-->
+    <!--                                 camera_sim="$(arg use_simulation)" camera_type="hk_camera"/>-->
+    <!--            <xacro:camera_optical_frame xyz="0.0035 0. 0.3765" rpy="-1.570796 0. -1.570796"-->
+    <!--                                        frame_id="$(arg camera_optical_frame)" parent="gimbal_lifter"-->
+    <!--                                        camera_sim="$(arg use_simulation)"/>-->
+    <!--        </xacro:unless>-->
+    <transmission name="trans_left_front_roller_0_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_front_roller_0_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_front_roller_0_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_front_roller_1_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_front_roller_1_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_front_roller_1_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_front_roller_2_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_front_roller_2_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_front_roller_2_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_front_roller_3_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_front_roller_3_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_front_roller_3_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_front_roller_4_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_front_roller_4_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_front_roller_4_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_front_roller_5_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_front_roller_5_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_front_roller_5_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_front_roller_6_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_front_roller_6_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_front_roller_6_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_front_roller_7_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_front_roller_7_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_front_roller_7_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_front_roller_8_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_front_roller_8_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_front_roller_8_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_front_roller_9_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_front_roller_9_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_front_roller_9_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_front_roller_10_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_front_roller_10_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_front_roller_10_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_front_roller_11_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_front_roller_11_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_front_roller_11_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_front_roller_12_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_front_roller_12_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_front_roller_12_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_front_roller_13_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_front_roller_13_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_front_roller_13_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_front_roller_0_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_front_roller_0_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_front_roller_0_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_front_roller_1_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_front_roller_1_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_front_roller_1_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_front_roller_2_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_front_roller_2_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_front_roller_2_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_front_roller_3_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_front_roller_3_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_front_roller_3_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_front_roller_4_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_front_roller_4_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_front_roller_4_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_front_roller_5_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_front_roller_5_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_front_roller_5_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_front_roller_6_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_front_roller_6_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_front_roller_6_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_front_roller_7_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_front_roller_7_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_front_roller_7_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_front_roller_8_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_front_roller_8_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_front_roller_8_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_front_roller_9_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_front_roller_9_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_front_roller_9_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_front_roller_10_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_front_roller_10_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_front_roller_10_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_front_roller_11_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_front_roller_11_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_front_roller_11_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_front_roller_12_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_front_roller_12_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_front_roller_12_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_front_roller_13_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_front_roller_13_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_front_roller_13_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_back_roller_0_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_back_roller_0_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_back_roller_0_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_back_roller_1_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_back_roller_1_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_back_roller_1_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_back_roller_2_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_back_roller_2_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_back_roller_2_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_back_roller_3_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_back_roller_3_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_back_roller_3_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_back_roller_4_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_back_roller_4_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_back_roller_4_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_back_roller_5_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_back_roller_5_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_back_roller_5_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_back_roller_6_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_back_roller_6_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_back_roller_6_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_back_roller_7_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_back_roller_7_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_back_roller_7_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_back_roller_8_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_back_roller_8_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_back_roller_8_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_back_roller_9_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_back_roller_9_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_back_roller_9_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_back_roller_10_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_back_roller_10_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_back_roller_10_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_back_roller_11_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_back_roller_11_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_back_roller_11_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_back_roller_12_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_back_roller_12_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_back_roller_12_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_left_back_roller_13_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="left_back_roller_13_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="left_back_roller_13_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_back_roller_0_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_back_roller_0_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_back_roller_0_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_back_roller_1_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_back_roller_1_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_back_roller_1_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_back_roller_2_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_back_roller_2_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_back_roller_2_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_back_roller_3_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_back_roller_3_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_back_roller_3_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_back_roller_4_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_back_roller_4_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_back_roller_4_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_back_roller_5_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_back_roller_5_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_back_roller_5_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_back_roller_6_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_back_roller_6_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_back_roller_6_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_back_roller_7_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_back_roller_7_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_back_roller_7_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_back_roller_8_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_back_roller_8_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_back_roller_8_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_back_roller_9_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_back_roller_9_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_back_roller_9_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_back_roller_10_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_back_roller_10_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_back_roller_10_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_back_roller_11_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_back_roller_11_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_back_roller_11_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_back_roller_12_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_back_roller_12_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_back_roller_12_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_right_back_roller_13_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="right_back_roller_13_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="right_back_roller_13_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint5">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint5">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint5_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint6">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint6">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint6_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_middle_pitch">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="middle_pitch">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="middle_pitch_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+</robot>
diff --git a/engineer2_arm_config/config/joint_limits.yaml b/engineer2_arm_config/config/joint_limits.yaml
index f0cf0d3..6699247 100644
--- a/engineer2_arm_config/config/joint_limits.yaml
+++ b/engineer2_arm_config/config/joint_limits.yaml
@@ -10,31 +10,31 @@ default_acceleration_scaling_factor: 0.1
 joint_limits:
   joint1:
     has_velocity_limits: true
-    max_velocity: 1.80
-    has_acceleration_limits: true
-    max_acceleration: 3.5
+    max_velocity: 2.25
+    has_acceleration_limits: false
+    max_acceleration: 0
   joint2:
     has_velocity_limits: true
-    max_velocity: 2.75
-    has_acceleration_limits: true
-    max_acceleration: 6
+    max_velocity: 10.46
+    has_acceleration_limits: false
+    max_acceleration: 0
   joint3:
     has_velocity_limits: true
-    max_velocity: 1.80
-    has_acceleration_limits: true
-    max_acceleration: 5
+    max_velocity: 3.76
+    has_acceleration_limits: false
+    max_acceleration: 0
   joint4:
     has_velocity_limits: true
-    max_velocity: 19
-    has_acceleration_limits: true
-    max_acceleration: 50
+    max_velocity: 20.94
+    has_acceleration_limits: false
+    max_acceleration: 0
   joint5:
     has_velocity_limits: true
-    max_velocity: 15
-    has_acceleration_limits: true
-    max_acceleration: 20
+    max_velocity: 10
+    has_acceleration_limits: false
+    max_acceleration: 0
   joint6:
     has_velocity_limits: true
-    max_velocity: 50
-    has_acceleration_limits: true
-    max_acceleration: 90
+    max_velocity: 10
+    has_acceleration_limits: false
+    max_acceleration: 0
diff --git a/engineer2_arm_config/config/kinematics.yaml b/engineer2_arm_config/config/kinematics.yaml
index b298b37..3a127a7 100644
--- a/engineer2_arm_config/config/kinematics.yaml
+++ b/engineer2_arm_config/config/kinematics.yaml
@@ -1,3 +1,7 @@
 engineer_arm:
   kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
   kinematics_solver_timeout: 0.005
+  goal_joint_tolerance: 0.0001
+  goal_position_tolerance: 0.0001
+  goal_orientation_tolerance: 0.001
diff --git a/engineer2_arm_config/config/ompl_planning.yaml b/engineer2_arm_config/config/ompl_planning.yaml
index 8e69e09..205b9ad 100644
--- a/engineer2_arm_config/config/ompl_planning.yaml
+++ b/engineer2_arm_config/config/ompl_planning.yaml
@@ -3,8 +3,8 @@ planner_configs:
     type: geometric::AnytimePathShortening
     shortcut: true  # Attempt to shortcut all new solution paths
     hybridize: true  # Compute hybrid solution trajectories
-    max_hybrid_paths: 40  # Number of hybrid paths generated per iteration
-    num_planners: 20  # The number of default planners to use for planning
+    max_hybrid_paths: 24  # Number of hybrid paths generated per iteration
+    num_planners: 4  # The number of default planners to use for planning
     planners: ""  # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]"
   SBL:
     type: geometric::SBL
@@ -51,8 +51,8 @@ planner_configs:
     temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
     min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
     init_temperature: 10e-6  # initial temperature. default: 10e-6
-    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
-    frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    frontier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+    frontier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
     k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
   PRM:
     type: geometric::PRM
@@ -95,8 +95,8 @@ planner_configs:
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
     temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
     init_temperature: 100  # initial temperature. default: 100
-    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
-    frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    frontier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+    frontier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
     cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
   LBTRRT:
     type: geometric::LBTRRT
@@ -127,6 +127,43 @@ planner_configs:
     sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
     dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
     max_failures: 5000  # maximum consecutive failure limit. default: 5000
+  AITstar:
+    type: geometric::AITstar
+    use_k_nearest: 1  # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1
+    rewire_factor: 1.001  # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001
+    samples_per_batch: 100  # batch size. Valid values: [1:1:1000]. Default: 100
+    use_graph_pruning: 1  # enable graph pruning (1) or not (0). Default: 1
+    find_approximate_solutions: 0  # track approximate solutions (1) or not (0). Default: 0
+    set_max_num_goals: 1  # maximum number of goals sampled from sampleable goal regions. Valid values: [1:1:1000]. Default: 1
+  ABITstar:
+    type: geometric::ABITstar
+    use_k_nearest: 1  # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1
+    rewire_factor: 1.001  # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001
+    samples_per_batch: 100  # batch size. Valid values: [1:1:1000]. Default: 100
+    use_graph_pruning: 1  # enable graph pruning (1) or not (0). Default: 1
+    prune_threshold_as_fractional_cost_change: 0.1  # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1
+    delay_rewiring_to_first_solution: 0  # delay (1) or not (0) rewiring until a solution is found. Default: 0
+    use_just_in_time_sampling: 0  # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0
+    drop_unconnected_samples_on_prune: 0  # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0
+    stop_on_each_solution_improvement: 0  # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0
+    use_strict_queue_ordering: 0  # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0
+    find_approximate_solutions: 0  # track approximate solutions (1) or not (0). Default: 0
+    initial_inflation_factor: 1000000  # inflation factor for the initial search. Valid values: [1.0:0.01:1000000.0]. Default: 1000000
+    inflation_scaling_parameter: 10  # scaling parameter for the inflation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0
+    truncation_scaling_parameter: 5.0  # scaling parameter for the truncation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0
+  BITstar:
+    type: geometric::BITstar
+    use_k_nearest: 1  # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1
+    rewire_factor: 1.001  # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001
+    samples_per_batch: 100  # batch size. Valid values: [1:1:1000]. Default: 100
+    use_graph_pruning: 1  # enable graph pruning (1) or not (0). Default: 1
+    prune_threshold_as_fractional_cost_change: 0.1  # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1
+    delay_rewiring_to_first_solution: 0  # delay (1) or not (0) rewiring until a solution is found. Default: 0
+    use_just_in_time_sampling: 0  # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0
+    drop_unconnected_samples_on_prune: 0  # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0
+    stop_on_each_solution_improvement: 0  # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0
+    use_strict_queue_ordering: 0  # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0
+    find_approximate_solutions: 0  # track approximate solutions (1) or not (0). Default: 0
 engineer_arm:
   default_planner_config: RRTConnect
   planner_configs:
@@ -154,5 +191,8 @@ engineer_arm:
     - LazyPRMstar
     - SPARS
     - SPARStwo
+    - AITstar
+    - ABITstar
+    - BITstar
   projection_evaluator: joints(joint1,joint2)
   longest_valid_segment_fraction: 0.005
diff --git a/engineer2_arm_config/config/ros_controllers.yaml b/engineer2_arm_config/config/ros_controllers.yaml
index a4837dc..1ca5150 100644
--- a/engineer2_arm_config/config/ros_controllers.yaml
+++ b/engineer2_arm_config/config/ros_controllers.yaml
@@ -1,13 +1,5 @@
-# Simulation settings for using moveit_sim_controllers
-moveit_sim_hw_interface:
-  joint_model_group: engineer_arm
-  joint_model_group_pose: home
-# Settings for ros_control_boilerplate control loop
-generic_hw_control_loop:
-  loop_hz: 300
-  cycle_time_error_threshold: 0.01
-# Settings for ros_control hardware interface
-hardware_interface:
+arm_trajectory_controller:
+  type: effort_controllers/JointTrajectoryController
   joints:
     - joint1
     - joint2
@@ -15,17 +7,34 @@ hardware_interface:
     - joint4
     - joint5
     - joint6
-  sim_control_mode: 1  # 0: position, 1: velocity
-
-controller_list:
-  - name: controllers/arm_trajectory_controller
-    action_ns: follow_joint_trajectory
-    default: True
-    type: FollowJointTrajectory
-    joints:
-      - joint1
-      - joint2
-      - joint3
-      - joint4
-      - joint5
-      - joint6
+  gains:
+    joint1:
+      p: 100
+      d: 1
+      i: 1
+      i_clamp: 1
+    joint2:
+      p: 100
+      d: 1
+      i: 1
+      i_clamp: 1
+    joint3:
+      p: 100
+      d: 1
+      i: 1
+      i_clamp: 1
+    joint4:
+      p: 100
+      d: 1
+      i: 1
+      i_clamp: 1
+    joint5:
+      p: 100
+      d: 1
+      i: 1
+      i_clamp: 1
+    joint6:
+      p: 100
+      d: 1
+      i: 1
+      i_clamp: 1
diff --git a/engineer2_arm_config/config/sensors_3d.yaml b/engineer2_arm_config/config/sensors_3d.yaml
index add9f47..b29057e 100644
--- a/engineer2_arm_config/config/sensors_3d.yaml
+++ b/engineer2_arm_config/config/sensors_3d.yaml
@@ -1,2 +1 @@
 sensors:
-  - { }
diff --git a/engineer2_arm_config/config/simple_moveit_controllers.yaml b/engineer2_arm_config/config/simple_moveit_controllers.yaml
new file mode 100644
index 0000000..1da75d2
--- /dev/null
+++ b/engineer2_arm_config/config/simple_moveit_controllers.yaml
@@ -0,0 +1,12 @@
+controller_list:
+  - name: arm_trajectory_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: True
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
diff --git a/engineer2_arm_config/config/stomp_planning.yaml b/engineer2_arm_config/config/stomp_planning.yaml
new file mode 100644
index 0000000..d0e6815
--- /dev/null
+++ b/engineer2_arm_config/config/stomp_planning.yaml
@@ -0,0 +1,39 @@
+stomp/engineer_arm:
+  group_name: engineer_arm
+  optimization:
+    num_timesteps: 60
+    num_iterations: 40
+    num_iterations_after_valid: 0
+    num_rollouts: 30
+    max_rollouts: 30
+    initialization_method: 1  # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]
+    control_cost_weight: 0.0
+  task:
+    noise_generator:
+      - class: stomp_moveit/NormalDistributionSampling
+        stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
+    cost_functions:
+      - class: stomp_moveit/CollisionCheck
+        collision_penalty: 1.0
+        cost_weight: 1.0
+        kernel_window_percentage: 0.2
+        longest_valid_joint_move: 0.05
+    noisy_filters:
+      - class: stomp_moveit/JointLimits
+        lock_start: True
+        lock_goal: True
+      - class: stomp_moveit/MultiTrajectoryVisualization
+        line_width: 0.02
+        rgb: [255, 255, 0]
+        marker_array_topic: stomp_trajectories
+        marker_namespace: noisy
+    update_filters:
+      - class: stomp_moveit/PolynomialSmoother
+        poly_order: 6
+      - class: stomp_moveit/TrajectoryVisualization
+        line_width: 0.05
+        rgb: [0, 191, 255]
+        error_rgb: [255, 0, 0]
+        publish_intermediate: True
+        marker_topic: stomp_trajectory
+        marker_namespace: optimized
diff --git a/engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml b/engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml
index 2f60968..cf0de11 100644
--- a/engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml
+++ b/engineer2_arm_config/launch/chomp_planning_pipeline.launch.xml
@@ -1,23 +1,21 @@
 <launch>
-    <arg name="start_state_max_bounds_error" default="0.1" />
-    <arg name="jiggle_fraction" default="0.05" />
-    <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
-    <arg name="planning_adapters"
-         default="default_planner_request_adapters/AddTimeParameterization
+  <arg name="start_state_max_bounds_error" default="0.1" />
+  <arg name="jiggle_fraction" default="0.05" />
+  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
+  <arg name="planning_adapters"
+       default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
+                default_planner_request_adapters/AddTimeParameterization
                 default_planner_request_adapters/ResolveConstraintFrames
                 default_planner_request_adapters/FixWorkspaceBounds
                 default_planner_request_adapters/FixStartStateBounds
                 default_planner_request_adapters/FixStartStateCollision
                 default_planner_request_adapters/FixStartStatePathConstraints"
-    />
+                />
 
-    <param name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
-    <param name="request_adapters" value="$(arg planning_adapters)" />
-    <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
-    <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
+  <param name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
 
-    <!-- Add MoveGroup capabilities specific to this pipeline -->
-    <!-- <param name="capabilities" value="" /> -->
-
-    <rosparam command="load" file="$(find engineer2_arm_config)/config/chomp_planning.yaml" />
+  <rosparam command="load" file="$(find engineer2_arm_config)/config/chomp_planning.yaml" />
 </launch>
diff --git a/engineer2_arm_config/launch/engineer_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/engineer2_moveit_controller_manager.launch.xml
similarity index 100%
rename from engineer2_arm_config/launch/engineer_moveit_controller_manager.launch.xml
rename to engineer2_arm_config/launch/engineer2_moveit_controller_manager.launch.xml
diff --git a/engineer2_arm_config/launch/engineer_moveit_sensor_manager.launch.xml b/engineer2_arm_config/launch/engineer2_moveit_sensor_manager.launch.xml
similarity index 100%
rename from engineer2_arm_config/launch/engineer_moveit_sensor_manager.launch.xml
rename to engineer2_arm_config/launch/engineer2_moveit_sensor_manager.launch.xml
diff --git a/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml
index d6865a4..64a5b70 100644
--- a/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml
+++ b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml
@@ -1,9 +1,12 @@
 <launch>
 
+  <!-- execute the trajectory in 'interpolate' mode or jump to goal position in 'last point' mode -->
+  <arg name="fake_execution_type" default="interpolate" />
+
   <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
   <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
 
   <!-- The rest of the params are specific to this plugin -->
-  <rosparam file="$(find engineer2_arm_config)/config/fake_controllers.yaml"/>
+  <rosparam subst_value="true" file="$(find engineer2_arm_config)/config/fake_controllers.yaml"/>
 
 </launch>
diff --git a/engineer2_arm_config/launch/gazebo.launch b/engineer2_arm_config/launch/gazebo.launch
new file mode 100644
index 0000000..0cd31a0
--- /dev/null
+++ b/engineer2_arm_config/launch/gazebo.launch
@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Gazebo options -->
+  <arg name="gazebo_gui" default="true" doc="Start Gazebo GUI"/>
+  <arg name="paused" default="false" doc="Start Gazebo paused"/>
+  <arg name="world_name" default="worlds/empty.world" doc="Gazebo world file"/>
+  <arg name="world_pose" default="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" doc="Pose to spawn the robot at"/>
+  <arg name="initial_joint_positions" default=" -J joint1 0.0101 -J joint2 0 -J joint3 0 -J joint4 0 -J joint5 0 -J joint6 0" doc="Initial joint configuration of the robot"/>
+
+  <!-- Start Gazebo paused to allow the controllers to pickup the initial pose -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch" pass_all_args="true">
+    <arg name="paused" value="true"/>
+  </include>
+
+  <!-- Set the robot urdf on the parameter server -->
+  <param name="robot_description" textfile="$(find engineer2_arm_config)/config/gazebo_engineer2.urdf" />
+
+  <!-- Unpause the simulation after loading the robot model -->
+  <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
+
+  <!-- Spawn the robot in Gazebo -->
+  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg unpause) $(arg world_pose) $(arg initial_joint_positions)"
+    respawn="false" output="screen" />
+
+  <!-- Load the controller parameters onto the parameter server -->
+  <rosparam file="$(find engineer2_arm_config)/config/gazebo_controllers.yaml" />
+  <include file="$(dirname)/ros_controllers.launch"/>
+
+  <!-- Spawn the Gazebo ROS controllers -->
+  <node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />
+
+  <!-- Given the published joint states, publish tf for the robot links -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+</launch>
diff --git a/engineer2_arm_config/launch/joystick_control.launch b/engineer2_arm_config/launch/joystick_control.launch
new file mode 100644
index 0000000..9411f6e
--- /dev/null
+++ b/engineer2_arm_config/launch/joystick_control.launch
@@ -0,0 +1,17 @@
+<launch>
+  <!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->
+
+  <arg name="dev" default="/dev/input/js0" />
+
+  <!-- Launch joy node -->
+  <node pkg="joy" type="joy_node" name="joy">
+    <param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->
+    <param name="deadzone" value="0.2" />
+    <param name="autorepeat_rate" value="40" />
+    <param name="coalesce_interval" value="0.025" />
+  </node>
+
+  <!-- Launch python interface -->
+  <node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>
+
+</launch>
diff --git a/engineer2_arm_config/launch/load_controllers.launch b/engineer2_arm_config/launch/load_controllers.launch
index 69275b2..d1aab78 100644
--- a/engineer2_arm_config/launch/load_controllers.launch
+++ b/engineer2_arm_config/launch/load_controllers.launch
@@ -5,10 +5,10 @@
     <arg name="use_simulation" default="false"/>
 
     <group unless="$(arg use_simulation)">
-        <rosparam file="$(find rm_config)/config/rm_controllers/engineer.yaml" command="load"/>
+        <rosparam file="$(find rm_config)/config/rm_controllers/engineer2.yaml" command="load"/>
     </group>
     <group if="$(arg use_simulation)">
-        <rosparam file="$(find engineer_middleware)/config/simulation_controllers.yaml" command="load"/>
+        <rosparam file="$(find engineer_middleware)/config/engineer2_simulation_controllers.yaml" command="load"/>
     </group>
 
 
@@ -20,26 +20,8 @@
           args="load
                 controllers/robot_state_controller
                 controllers/joint_state_controller
-                controllers/joint1_calibration_controller
-                controllers/joint2_calibration_controller
-                controllers/joint3_calibration_controller
-                controllers/joint4_calibration_controller
-                controllers/joint5_calibration_controller
-                controllers/joint6_calibration_controller
-                controllers/joint7_calibration_controller
                 controllers/arm_trajectory_controller
                 controllers/chassis_controller
-                controllers/gpio_controller
-                controllers/gimbal_controller
-                controllers/ore_bin_rotate_controller
-                controllers/extend_arm_front_controller
-                controllers/extend_arm_back_controller
-                controllers/extend_arm_front_calibration_controller
-                controllers/extend_arm_back_calibration_controller
-                controllers/ore_bin_lifter_controller
-                controllers/ore_bin_lifter_calibration_controller
-                controllers/gimbal_lifter_controller
-                controllers/gimbal_lifter_calibration_controller
                 controllers/arm_trajectory_controller
                 "
     />
diff --git a/engineer2_arm_config/launch/move_group.launch b/engineer2_arm_config/launch/move_group.launch
index 3bc6450..ce737e2 100644
--- a/engineer2_arm_config/launch/move_group.launch
+++ b/engineer2_arm_config/launch/move_group.launch
@@ -1,65 +1,100 @@
 <launch>
 
-    <!-- GDB Debug Option -->
-    <arg name="debug" default="false"/>
-    <arg unless="$(arg debug)" name="launch_prefix" value=""/>
-    <arg if="$(arg debug)" name="launch_prefix"
-         value="gdb -x $(find engineer2_arm_config)/launch/gdb_settings.gdb --ex run --args"/>
-
-    <!-- Verbose Mode Option -->
-    <arg name="info" default="$(arg debug)"/>
-    <arg unless="$(arg info)" name="command_args" value=""/>
-    <arg if="$(arg info)" name="command_args" value="--debug"/>
-
-    <!-- move_group settings -->
-    <arg name="pipeline" default="ompl"/>
-    <arg name="allow_trajectory_execution" default="true"/>
-    <arg name="fake_execution" default="false"/>
-    <arg name="max_safe_path_cost" default="3"/>
-    <arg name="jiggle_fraction" default="0."/>
-    <arg name="publish_monitored_planning_scene" default="true"/>
-
-    <arg name="capabilities" default=""/>
-    <arg name="disable_capabilities" default=""/>
-
-    <arg name="load_robot_description" default="true"/>
-    <!-- load URDF, SRDF and joint_limits configuration -->
-    <include file="$(find engineer2_arm_config)/launch/planning_context.launch">
-        <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+  <!-- GDB Debug Option -->
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix"
+           value="gdb -x $(dirname)/gdb_settings.gdb --ex run --args" />
+
+  <!-- Verbose Mode Option -->
+  <arg name="info" default="$(arg debug)" />
+  <arg unless="$(arg info)" name="command_args" value="" />
+  <arg     if="$(arg info)" name="command_args" value="--debug" />
+
+  <!-- move_group settings -->
+  <arg name="pipeline" default="ompl" />
+  <arg name="allow_trajectory_execution" default="true"/>
+  <arg name="moveit_controller_manager" default="simple" />
+  <arg name="fake_execution_type" default="interpolate"/>
+  <arg name="max_safe_path_cost" default="1"/>
+  <arg name="publish_monitored_planning_scene" default="true"/>
+
+  <arg name="capabilities" default=""/>
+  <arg name="disable_capabilities" default=""/>
+  <!-- load these non-default MoveGroup capabilities (space seperated) -->
+  <!--
+  <arg name="capabilities" value="
+                a_package/AwsomeMotionPlanningCapability
+                another_package/GraspPlanningPipeline
+                " />
+  -->
+
+  <!-- inhibit these default MoveGroup capabilities (space seperated) -->
+  <!--
+  <arg name="disable_capabilities" value="
+                move_group/MoveGroupKinematicsService
+                move_group/ClearOctomapService
+                " />
+  -->
+
+  <arg name="load_robot_description" default="false" />
+  <!-- load URDF, SRDF and joint_limits configuration -->
+  <include file="$(dirname)/planning_context.launch">
+    <arg name="load_robot_description" value="$(arg load_robot_description)" />
+  </include>
+
+  <!-- Planning Pipelines -->
+  <group ns="move_group/planning_pipelines">
+
+    <!-- OMPL -->
+    <include file="$(dirname)/planning_pipeline.launch.xml">
+      <arg name="pipeline" value="ompl" />
     </include>
 
-    <!-- Planning Functionality -->
-    <include ns="move_group" file="$(find engineer2_arm_config)/launch/planning_pipeline.launch.xml">
-        <arg name="pipeline" value="$(arg pipeline)"/>
+    <!-- CHOMP -->
+    <include file="$(dirname)/planning_pipeline.launch.xml">
+      <arg name="pipeline" value="chomp" />
     </include>
 
-    <!-- Trajectory Execution Functionality -->
-    <include ns="move_group" file="$(find engineer2_arm_config)/launch/trajectory_execution.launch.xml"
-             if="$(arg allow_trajectory_execution)">
-        <arg name="moveit_manage_controllers" value="true"/>
-        <arg name="moveit_controller_manager" value="engineer" unless="$(arg fake_execution)"/>
-        <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
+    <!-- Pilz Industrial Motion -->
+    <include file="$(dirname)/planning_pipeline.launch.xml">
+      <arg name="pipeline" value="pilz_industrial_motion_planner" />
     </include>
 
-    <!-- Start the actual move_group node/action server -->
-    <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group"
-          respawn="false" output="screen" args="$(arg command_args)">
-        <!-- Set the display variable, in case OpenGL code is used internally -->
-        <env name="DISPLAY" value="$(optenv DISPLAY :0)"/>
-
-        <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
-        <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
-        <param name="jiggle_fraction" value="$(arg jiggle_fraction)"/>
-        <param name="capabilities" value="$(arg capabilities)"/>
-        <param name="disable_capabilities" value="$(arg disable_capabilities)"/>
-
-
-        <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
-        <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)"/>
-        <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)"/>
-        <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)"/>
-        <param name="planning_scene_monitor/publish_transforms_updates"
-               value="$(arg publish_monitored_planning_scene)"/>
-    </node>
+    <!-- Support custom planning pipeline -->
+    <include if="$(eval arg('pipeline') not in ['ompl', 'chomp', 'pilz_industrial_motion_planner'])"
+             file="$(dirname)/planning_pipeline.launch.xml">
+      <arg name="pipeline" value="$(arg pipeline)" />
+    </include>
+  </group>
+
+  <!-- Trajectory Execution Functionality -->
+  <include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
+    <arg name="moveit_manage_controllers" value="true" />
+    <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
+    <arg name="fake_execution_type" value="$(arg fake_execution_type)" />
+  </include>
+
+  <!-- Start the actual move_group node/action server -->
+  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
+    <!-- Set the display variable, in case OpenGL code is used internally -->
+    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />
+
+    <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
+    <param name="sense_for_plan/max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
+    <param name="default_planning_pipeline" value="$(arg pipeline)" />
+    <param name="capabilities" value="$(arg capabilities)" />
+    <param name="disable_capabilities" value="$(arg disable_capabilities)" />
+
+    <!-- do not copy dynamics information from /joint_states to internal robot monitoring
+         default to false, because almost nothing in move_group relies on this information -->
+    <param name="monitor_dynamics" value="false" />
+
+    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
+    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
+    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
+    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
+    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
+  </node>
 
 </launch>
diff --git a/engineer2_arm_config/launch/moveit.rviz b/engineer2_arm_config/launch/moveit.rviz
index a0a4838..6a876dd 100644
--- a/engineer2_arm_config/launch/moveit.rviz
+++ b/engineer2_arm_config/launch/moveit.rviz
@@ -5,13 +5,8 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /MotionPlanning1
-        - /MotionPlanning1/Planning Request1
-        - /MotionPlanning1/Planning Metrics1
-        - /MotionPlanning1/Planned Path1
-        - /TF1
-        - /TF1/Frames1
       Splitter Ratio: 0.5
-    Tree Height: 315
+    Tree Height: 226
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -19,10 +14,6 @@ Panels:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
-Preferences:
-  PromptSaveOnExit: true
-Toolbars:
-  toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
@@ -32,7 +23,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.029999999329447746
+        Line Width: 0.03
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -44,177 +35,30 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Acceleration_Scaling_Factor: 0.1
-      Class: moveit_rviz_plugin/MotionPlanning
+    - Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
-      Move Group Namespace: ""
-      MoveIt_Allow_Approximate_IK: false
-      MoveIt_Allow_External_Program: false
-      MoveIt_Allow_Replanning: false
-      MoveIt_Allow_Sensor_Positioning: false
-      MoveIt_Planning_Attempts: 10
-      MoveIt_Planning_Time: 5
-      MoveIt_Use_Cartesian_Path: false
-      MoveIt_Use_Constraint_Aware_IK: false
-      MoveIt_Workspace:
-        Center:
-          X: 0
-          Y: 0
-          Z: 0
-        Size:
-          X: 2
-          Y: 2
-          Z: 2
       Name: MotionPlanning
       Planned Path:
-        Color Enabled: false
-        Interrupt Display: false
-        Links:
-          All Links Enabled: true
-          Expand Joint Details: false
-          Expand Link Details: false
-          Expand Tree: false
-          Link Tree Style: Links in Alphabetic Order
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          camera_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          camera_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          extend_arm_back:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          extend_arm_front:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          gimbal_lifter:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link3:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link4:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link5:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link6:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link7:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          mimic_link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ore_bin_lifter:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ore_bin_rotator:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          pitch:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          tools_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          tools_link_st:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          yaw:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-        Loop Animation: false
+        Links: ~
+        Loop Animation: true
         Robot Alpha: 0.5
-        Robot Color: 150; 50; 150
         Show Robot Collision: false
         Show Robot Visual: true
         Show Trail: false
         State Display Time: 0.05 s
-        Trail Step Size: 1
         Trajectory Topic: move_group/display_planned_path
-        Use Sim Time: false
       Planning Metrics:
         Payload: 1
         Show Joint Torques: false
         Show Manipulability: false
         Show Manipulability Index: false
         Show Weight Limit: false
-        TextHeight: 0.07999999821186066
       Planning Request:
         Colliding Link Color: 255; 0; 0
         Goal State Alpha: 1
         Goal State Color: 250; 128; 0
         Interactive Marker Size: 0
         Joint Violation Color: 255; 0; 255
-        Planning Group: engineer_arm
         Query Goal State: true
         Query Start State: false
         Show Workspace: false
@@ -224,272 +68,19 @@ Visualization Manager:
       Robot Description: robot_description
       Scene Geometry:
         Scene Alpha: 1
-        Scene Color: 50; 230; 50
-        Scene Display Time: 0.009999999776482582
         Show Scene Geometry: true
         Voxel Coloring: Z-Axis
         Voxel Rendering: Occupied Voxels
       Scene Robot:
         Attached Body Color: 150; 50; 150
-        Links:
-          All Links Enabled: true
-          Expand Joint Details: false
-          Expand Link Details: false
-          Expand Tree: false
-          Link Tree Style: Links in Alphabetic Order
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          camera_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          camera_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          extend_arm_back:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          extend_arm_front:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          gimbal_lifter:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_back_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          left_front_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link3:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link4:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link5:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link6:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          link7:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          mimic_link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ore_bin_lifter:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          ore_bin_rotator:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          pitch:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_back_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          right_front_wheel:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          tools_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          tools_link_st:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          yaw:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
+        Links: ~
         Robot Alpha: 0.5
-        Show Robot Collision: false
-        Show Robot Visual: true
-      Value: true
-      Velocity_Scaling_Factor: 0.1
-    - Class: rviz/TF
-      Enabled: true
-      Filter (blacklist): ""
-      Filter (whitelist): ""
-      Frame Timeout: 15
-      Frames:
-        All Enabled: false
-        base_link:
-          Value: true
-        camera_link:
-          Value: false
-        camera_optical_frame:
-          Value: false
-        exchanger:
-          Value: true
-        extend_arm_back:
-          Value: false
-        extend_arm_front:
-          Value: false
-        gimbal_lifter:
-          Value: false
-        left_back_wheel:
-          Value: false
-        left_front_wheel:
-          Value: false
-        link1:
-          Value: false
-        link2:
-          Value: false
-        link3:
-          Value: false
-        link4:
-          Value: false
-        link5:
-          Value: false
-        link6:
-          Value: false
-        link7:
-          Value: true
-        map:
-          Value: false
-        mimic_link1:
-          Value: false
-        odom:
-          Value: false
-        ore_bin_lifter:
-          Value: false
-        ore_bin_rotator:
-          Value: false
-        pitch:
-          Value: false
-        real_world:
-          Value: false
-        right_back_wheel:
-          Value: false
-        right_front_wheel:
-          Value: false
-        tools_link:
-          Value: false
-        tools_link_st:
-          Value: false
-        yaw:
-          Value: false
-      Marker Alpha: 1
-      Marker Scale: 1
-      Name: TF
-      Show Arrows: true
-      Show Axes: true
-      Show Names: true
-      Tree:
-        base_link:
-          {}
-        exchanger:
-          {}
-        extend_arm_back:
-          {}
-        extend_arm_front:
-          {}
-        gimbal_lifter:
-          camera_link:
-            {}
-          camera_optical_frame:
-            {}
-        left_back_wheel:
-          {}
-        left_front_wheel:
-          {}
-        link1:
-          {}
-        link2:
-          {}
-        link3:
-          {}
-        link4:
-          {}
-        link5:
-          {}
-        link6:
-          {}
-        link7:
-          tools_link:
-            {}
-          tools_link_st:
-            {}
-        map:
-          {}
-        mimic_link1:
-          {}
-        odom:
-          {}
-        ore_bin_lifter:
-          {}
-        ore_bin_rotator:
-          {}
-        pitch:
-          {}
-        real_world:
-          {}
-        right_back_wheel:
-          {}
-        right_front_wheel:
-          {}
-        yaw:
-          {}
-      Update Interval: 0
+        Show Scene Robot: true
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Default Light: true
     Fixed Frame: base_link
-    Frame Rate: 30
   Name: root
   Tools:
     - Class: rviz/Interact
@@ -500,30 +91,30 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.4108070135116577
+      Distance: 2.0
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
+        Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: 0.23874804377555847
-        Y: 0.04174364358186722
-        Z: 0.6224369406700134
+        X: -0.1
+        Y: 0.25
+        Z: 0.30
       Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
+      Focal Shape Size: 0.05
       Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.48979732394218445
+      Near Clip Distance: 0.01
+      Pitch: 0.5
       Target Frame: base_link
-      Yaw: 3.2827558517456055
+      Yaw: -0.6232355833053589
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1016
+  Height: 848
   Help:
     collapsed: false
   Hide Left Dock: false
@@ -532,9 +123,9 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f30000039efc0200000007fb000000100044006900730070006c006100790073010000003d000001cc000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000020f000001cc0000017d00ffffff000004880000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
-  Width: 1665
-  X: 757
-  Y: 27
+  Width: 1291
+  X: 454
+  Y: 25
diff --git a/engineer2_arm_config/launch/moveit_rviz.launch b/engineer2_arm_config/launch/moveit_rviz.launch
new file mode 100644
index 0000000..a4605c0
--- /dev/null
+++ b/engineer2_arm_config/launch/moveit_rviz.launch
@@ -0,0 +1,15 @@
+<launch>
+
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+
+  <arg name="rviz_config" default="" />
+  <arg     if="$(eval rviz_config=='')" name="command_args" value="" />
+  <arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />
+
+  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
+        args="$(arg command_args)" output="screen">
+  </node>
+
+</launch>
diff --git a/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml b/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml
index c219994..ed05662 100644
--- a/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml
+++ b/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml
@@ -1,25 +1,24 @@
 <launch>
 
-    <!-- OMPL Plugin for MoveIt -->
-    <arg name="planning_plugin" value="ompl_interface/OMPLPlanner"/>
+  <!-- The request adapters (plugins) used when planning with OMPL. ORDER MATTERS! -->
+  <arg name="planning_adapters"
+       default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
+                default_planner_request_adapters/AddTimeParameterization
+                default_planner_request_adapters/ResolveConstraintFrames
+                default_planner_request_adapters/FixWorkspaceBounds
+                default_planner_request_adapters/FixStartStateBounds
+                default_planner_request_adapters/FixStartStateCollision
+                default_planner_request_adapters/FixStartStatePathConstraints"
+                />
 
-    <!-- The request adapters (plugins) used when planning with OMPL.
-         ORDER MATTERS -->
-    <arg name="planning_adapters"
-         value="default_planner_request_adapters/AddTimeParameterization
-              default_planner_request_adapters/ResolveConstraintFrames
-              default_planner_request_adapters/FixWorkspaceBounds
-              default_planner_request_adapters/FixStartStateBounds
-              default_planner_request_adapters/FixStartStateCollision
-              default_planner_request_adapters/FixStartStatePathConstraints"
-    />
+  <arg name="start_state_max_bounds_error" default="0.1" />
+  <arg name="jiggle_fraction" default="0.05" />
 
-    <arg name="start_state_max_bounds_error" value="0.1"/>
+  <param name="planning_plugin" value="ompl_interface/OMPLPlanner" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
 
-    <param name="planning_plugin" value="$(arg planning_plugin)"/>
-    <param name="request_adapters" value="$(arg planning_adapters)"/>
-    <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)"/>
-
-    <rosparam command="load" file="$(find engineer2_arm_config)/config/ompl_planning.yaml"/>
+  <rosparam command="load" file="$(find engineer2_arm_config)/config/ompl_planning.yaml"/>
 
 </launch>
diff --git a/engineer2_arm_config/launch/planning_context.launch b/engineer2_arm_config/launch/planning_context.launch
index ff24db3..e527542 100644
--- a/engineer2_arm_config/launch/planning_context.launch
+++ b/engineer2_arm_config/launch/planning_context.launch
@@ -1,25 +1,26 @@
 <launch>
-    <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
-    <arg name="load_robot_description" default="false"/>
+  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
+  <arg name="load_robot_description" default="false"/>
 
-    <!-- The name of the parameter under which the URDF is loaded -->
-    <arg name="robot_description" default="robot_description"/>
+  <!-- The name of the parameter under which the URDF is loaded -->
+  <arg name="robot_description" default="robot_description"/>
 
-    <!-- Load universal robot description format (URDF) -->
-    <param if="$(arg load_robot_description)" name="$(arg robot_description)"
-           command="xacro  '$(find rm_description)/urdf/engineer/engineer.urdf.xacro'"/>
+  <!-- Load universal robot description format (URDF) -->
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro  '$(find rm_description)/urdf/engineer2/engineer2.urdf.xacro'"/>
 
-    <!-- The semantic description that corresponds to the URDF -->
-    <param name="$(arg robot_description)_semantic" textfile="$(find engineer2_arm_config)/config/engineer2.srdf"/>
+  <!-- The semantic description that corresponds to the URDF -->
+  <param name="$(arg robot_description)_semantic" textfile="$(find engineer2_arm_config)/config/engineer2.srdf" />
 
-    <!-- Load updated joint limits (override information from URDF) -->
-    <group ns="$(arg robot_description)_planning">
-        <rosparam command="load" file="$(find engineer2_arm_config)/config/joint_limits.yaml"/>
-    </group>
+  <!-- Load updated joint limits (override information from URDF) -->
+  <group ns="$(arg robot_description)_planning">
+    <rosparam command="load" file="$(find engineer2_arm_config)/config/joint_limits.yaml"/>
+    <rosparam command="load" file="$(find engineer2_arm_config)/config/cartesian_limits.yaml"/>
+  </group>
 
-    <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
-    <group ns="$(arg robot_description)_kinematics">
-        <rosparam command="load" file="$(find engineer2_arm_config)/config/kinematics.yaml"/>
-    </group>
+  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
+  <group ns="$(arg robot_description)_kinematics">
+    <rosparam command="load" file="$(find engineer2_arm_config)/config/kinematics.yaml"/>
+
+  </group>
 
 </launch>
diff --git a/engineer2_arm_config/launch/planning_pipeline.launch.xml b/engineer2_arm_config/launch/planning_pipeline.launch.xml
index 6882ffa..4b4d0d6 100644
--- a/engineer2_arm_config/launch/planning_pipeline.launch.xml
+++ b/engineer2_arm_config/launch/planning_pipeline.launch.xml
@@ -1,10 +1,10 @@
 <launch>
 
-    <!-- This file makes it easy to include different planning pipelines;
-         It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->
+  <!-- This file makes it easy to include different planning pipelines;
+       It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->
 
-    <arg name="pipeline" default="ompl"/>
+  <arg name="pipeline" default="ompl" />
 
-    <include file="$(find engineer2_arm_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml"/>
+  <include ns="$(arg pipeline)" file="$(dirname)/$(arg pipeline)_planning_pipeline.launch.xml" />
 
 </launch>
diff --git a/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml
new file mode 100644
index 0000000..9ebc91c
--- /dev/null
+++ b/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml
@@ -0,0 +1,4 @@
+<launch>
+	<!-- Define MoveIt controller manager plugin -->
+	<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager" />
+</launch>
diff --git a/engineer2_arm_config/launch/ros_controllers.launch b/engineer2_arm_config/launch/ros_controllers.launch
new file mode 100644
index 0000000..f149647
--- /dev/null
+++ b/engineer2_arm_config/launch/ros_controllers.launch
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find engineer2_arm_config)/config/ros_controllers.yaml" command="load"/>
+
+  <!-- Load the controllers -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" args="arm_trajectory_controller "/>
+
+</launch>
diff --git a/engineer2_arm_config/launch/sensor_manager.launch.xml b/engineer2_arm_config/launch/sensor_manager.launch.xml
new file mode 100644
index 0000000..fb429e6
--- /dev/null
+++ b/engineer2_arm_config/launch/sensor_manager.launch.xml
@@ -0,0 +1,17 @@
+<launch>
+
+  <!-- This file makes it easy to include the settings for sensor managers -->
+
+  <!-- Params for 3D sensors config -->
+  <rosparam command="load" file="$(find engineer2_arm_config)/config/sensors_3d.yaml" />
+
+  <!-- Params for the octomap monitor -->
+  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
+  <param name="octomap_resolution" type="double" value="0.025" />
+  <param name="max_range" type="double" value="5.0" />
+
+  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
+  <arg name="moveit_sensor_manager" default="engineer2" />
+  <include file="$(dirname)/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
+
+</launch>
diff --git a/engineer2_arm_config/launch/setup_assistant.launch b/engineer2_arm_config/launch/setup_assistant.launch
new file mode 100644
index 0000000..6e40bfd
--- /dev/null
+++ b/engineer2_arm_config/launch/setup_assistant.launch
@@ -0,0 +1,16 @@
+<!-- Re-launch the MoveIt Setup Assistant with this configuration package already loaded -->
+<launch>
+
+  <!-- Debug Info -->
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+
+  <!-- Run -->
+  <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
+        args="--config_pkg=engineer2_arm_config"
+        launch-prefix="$(arg launch_prefix)"
+        required="true"
+        output="screen" />
+
+</launch>
diff --git a/engineer2_arm_config/launch/simple_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/simple_moveit_controller_manager.launch.xml
new file mode 100644
index 0000000..1df16f6
--- /dev/null
+++ b/engineer2_arm_config/launch/simple_moveit_controller_manager.launch.xml
@@ -0,0 +1,8 @@
+<launch>
+  <!-- Define the MoveIt controller manager plugin to use for trajectory execution -->
+  <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
+
+  <!-- Load controller list to the parameter server -->
+  <rosparam file="$(find engineer2_arm_config)/config/simple_moveit_controllers.yaml" />
+  <rosparam file="$(find engineer2_arm_config)/config/ros_controllers.yaml" />
+</launch>
diff --git a/engineer2_arm_config/launch/stomp_planning_pipeline.launch.xml b/engineer2_arm_config/launch/stomp_planning_pipeline.launch.xml
new file mode 100644
index 0000000..0876e8e
--- /dev/null
+++ b/engineer2_arm_config/launch/stomp_planning_pipeline.launch.xml
@@ -0,0 +1,23 @@
+<launch>
+  <!-- Stomp Plugin for MoveIt -->
+  <arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" />
+
+  <arg name="start_state_max_bounds_error" value="0.1" />
+  <arg name="jiggle_fraction" value="0.05" />
+  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
+  <arg name="planning_adapters"
+       default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
+                default_planner_request_adapters/AddTimeParameterization
+                default_planner_request_adapters/FixWorkspaceBounds
+                default_planner_request_adapters/FixStartStateBounds
+                default_planner_request_adapters/FixStartStateCollision
+                default_planner_request_adapters/FixStartStatePathConstraints" />
+
+
+  <param name="planning_plugin" value="$(arg planning_plugin)" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
+
+  <rosparam command="load" file="$(find engineer2_arm_config)/config/stomp_planning.yaml"/>
+</launch>
diff --git a/engineer2_arm_config/launch/trajectory_execution.launch.xml b/engineer2_arm_config/launch/trajectory_execution.launch.xml
index 1801408..20c3dfc 100644
--- a/engineer2_arm_config/launch/trajectory_execution.launch.xml
+++ b/engineer2_arm_config/launch/trajectory_execution.launch.xml
@@ -1,21 +1,23 @@
 <launch>
+  <!-- This file summarizes all settings required for trajectory execution  -->
 
-    <!-- This file makes it easy to include the settings for trajectory execution  -->
+  <!-- Define moveit controller manager plugin: fake, simple, or ros_control -->
+  <arg name="moveit_controller_manager" />
+  <arg name="fake_execution_type" default="interpolate" />
 
-    <!-- Flag indicating whether MoveIt is allowed to load/unload  or switch controllers -->
-    <arg name="moveit_manage_controllers" default="true"/>
-    <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
+  <!-- Flag indicating whether MoveIt is allowed to load/unload  or switch controllers -->
+  <arg name="moveit_manage_controllers" default="true"/>
+  <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
 
-    <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
-    <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
-    <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
-    <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
-    <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
-    <param name="trajectory_execution/allowed_start_tolerance" value="0.05"/> <!-- default 0.01 -->
+  <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
+  <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
+  <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
+  <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
+  <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
+  <param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->
 
-    <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
-    <arg name="moveit_controller_manager" default="engineer"/>
-    <include
-            file="$(find engineer2_arm_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml"/>
+  <!-- We use pass_all_args=true here to pass fake_execution_type, which is required by fake controllers, but not by real-robot controllers.
+       As real-robot controller_manager.launch files shouldn't be required to define this argument, we use the trick of passing all args. -->
+  <include file="$(dirname)/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" pass_all_args="true" />
 
 </launch>
diff --git a/engineer2_arm_config/package.xml b/engineer2_arm_config/package.xml
index 3cc0444..8c33136 100644
--- a/engineer2_arm_config/package.xml
+++ b/engineer2_arm_config/package.xml
@@ -1,27 +1,41 @@
 <package>
-    <name>engineer2_arm_config</name>
-    <version>1.0.0</version>
-    <description>
-        An automatically generated package with all the configuration and launch files for using the engineer with the
-        MoveIt Motion Planning Framework
-    </description>
-    <author email="liaoqiayuan@gmail.com">QiayuanLiao</author>
-    <maintainer email="liaoqiayuan@gmail.com">QiayuanLiao</maintainer>
 
-    <license>BSD</license>
+  <name>engineer2_arm_config</name>
+  <version>0.3.0</version>
+  <description>
+     An automatically generated package with all the configuration and launch files for using the engineer2 with the MoveIt Motion Planning Framework
+  </description>
+  <author email="792166012@qq.com">Chonghong Cai</author>
+  <maintainer email="792166012@qq.com">Chonghong Cai</maintainer>
 
-    <url type="website">http://moveit.ros.org/</url>
-    <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
-    <url type="repository">https://github.com/ros-planning/moveit</url>
+  <license>BSD</license>
+
+  <url type="website">http://moveit.ros.org/</url>
+  <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
+  <url type="repository">https://github.com/ros-planning/moveit</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>moveit_ros_move_group</run_depend>
+  <run_depend>moveit_fake_controller_manager</run_depend>
+  <run_depend>moveit_kinematics</run_depend>
+  <run_depend>moveit_planners</run_depend>
+  <run_depend>moveit_ros_visualization</run_depend>
+  <run_depend>moveit_setup_assistant</run_depend>
+  <run_depend>moveit_simple_controller_manager</run_depend>
+  <run_depend>joint_state_publisher</run_depend>
+  <run_depend>joint_state_publisher_gui</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+  <run_depend>rviz</run_depend>
+  <run_depend>tf2_ros</run_depend>
+  <run_depend>xacro</run_depend>
+  <!-- The next 2 packages are required for the gazebo simulation.
+       We don't include them by default to prevent installing gazebo and all its dependencies. -->
+  <!-- <run_depend>joint_trajectory_controller</run_depend> -->
+  <!-- <run_depend>gazebo_ros_control</run_depend> -->
+  <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
+  <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
+  <run_depend>rm_description</run_depend>
 
-    <buildtool_depend>catkin</buildtool_depend>
 
-    <run_depend>moveit_ros_move_group</run_depend>
-    <run_depend>moveit_fake_controller_manager</run_depend>
-    <run_depend>moveit_kinematics</run_depend>
-    <run_depend>moveit_planners_ompl</run_depend>
-    <run_depend>moveit_ros_visualization</run_depend>
-    <run_depend>moveit_setup_assistant</run_depend>
-    <run_depend>tf2_ros</run_depend>
-    <run_depend>xacro</run_depend>
 </package>
diff --git a/engineer_arm_config/launch/load_controllers.launch b/engineer_arm_config/launch/load_controllers.launch
index 69275b2..39c6842 100644
--- a/engineer_arm_config/launch/load_controllers.launch
+++ b/engineer_arm_config/launch/load_controllers.launch
@@ -8,7 +8,7 @@
         <rosparam file="$(find rm_config)/config/rm_controllers/engineer.yaml" command="load"/>
     </group>
     <group if="$(arg use_simulation)">
-        <rosparam file="$(find engineer_middleware)/config/simulation_controllers.yaml" command="load"/>
+        <rosparam file="$(find engineer_middleware)/config/engineer_simulation_controllers.yaml" command="load"/>
     </group>
 
 
diff --git a/engineer_middleware/config/engineer2_simulation_controllers.yaml b/engineer_middleware/config/engineer2_simulation_controllers.yaml
new file mode 100644
index 0000000..964f39d
--- /dev/null
+++ b/engineer_middleware/config/engineer2_simulation_controllers.yaml
@@ -0,0 +1,31 @@
+controllers:
+  joint_state_controller:
+    type: joint_state_controller/JointStateController
+    publish_rate: 100
+  robot_state_controller:
+    type: robot_state_controller/RobotStateController
+    publish_rate: 100
+
+  arm_trajectory_controller:
+    type: effort_controllers/JointTrajectoryController
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+    gains:
+      joint1: { p: 20000.0, i: 0, d: 400.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint2: { p: 100000, i: 0, d: 1000.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint3: { p: 5000.0, i: 0, d: 100.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint4: { p: 10000.0, i: 0.0, d: 60, i_clamp_max: 0, i_clamp_min: 0.0, antiwindup: true, publish_state: true }
+      joint5: { p: 46000.0, i: 0, d: 200.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint6: { p: 20000.0, i: 0, d: 120., i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+    constraints:
+      joint1: { trajectory: 0.05, goal: 0.05 }
+      joint2: { trajectory: 0.05, goal: 0.05 }
+      joint3: { trajectory: 0.05, goal: 0.05 }
+      joint4: { trajectory: 0.05, goal: 0.05 }
+      joint5: { trajectory: 0.05, goal: 0.05 }
+      joint6: { trajectory: 0.05, goal: 0.05 }
diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index e69de29..debfca0 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -0,0 +1,24 @@
+common:
+  speed:
+    slowly: &SLOWLY
+      speed: 0.15
+      accel: 0.1
+      timeout: 15.
+    normally: &NORMALLY
+      speed: 0.4
+      accel: 0.3
+      timeout: 3
+    quickly: &QUICKLY
+      speed: 0.6
+      accel: 0.6
+      timeout: 7.
+
+  tolerance:
+    small_tolerance: &SMALL_TOLERANCE
+      tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.03 ]
+    normal_tolerance: &NORMAL_TOLERANCE
+      tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.07 ]
+    bigger_tolerance: &BIGGER_TOLERANCE
+      tolerance_joints: [ 0.015, 0.015, 0.02, 0.35, 0.2, 0.2, 0.1 ]
+    max_tolerance: &MAX_TOLERANCE
+      tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3]
diff --git a/engineer_middleware/config/simulation_controllers.yaml b/engineer_middleware/config/engineer_simulation_controllers.yaml
similarity index 100%
rename from engineer_middleware/config/simulation_controllers.yaml
rename to engineer_middleware/config/engineer_simulation_controllers.yaml

From 7867306b281337baa1e12ae4a2e286830821cf66 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 8 Jul 2024 10:31:15 +0800
Subject: [PATCH 106/147] Fix wrong collision check matrix.

---
 engineer2_arm_config/config/engineer2.srdf | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/engineer2_arm_config/config/engineer2.srdf b/engineer2_arm_config/config/engineer2.srdf
index 1c6d7cc..12140bf 100644
--- a/engineer2_arm_config/config/engineer2.srdf
+++ b/engineer2_arm_config/config/engineer2.srdf
@@ -22,15 +22,15 @@
         <joint name="joint6" value="0"/>
     </group_state>
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-    <disable_collisions link1="base_link" link2="link6" reason="User"/>
-    <disable_collisions link1="base_link" link2="link5" reason="User"/>
-    <disable_collisions link1="base_link" link2="link4" reason="User"/>
     <disable_collisions link1="link1" link2="link4" reason="User"/>
-    <disable_collisions link1="link1" link2="link5" reason="User"/>
-    <disable_collisions link1="link1" link2="link6" reason="User"/>
-    <disable_collisions link1="link2" link2="link4" reason="User"/>
-    <disable_collisions link1="link2" link2="link5" reason="User"/>
-    <disable_collisions link1="base_link" link2="link2" reason="User"/>
-    <disable_collisions link1="link2" link2="link6" reason="User"/>
-    <disable_collisions link1="base_link" link2="link3" reason="User"/>
+    <disable_collisions link1="link5" link2="link6" reason="User"/>
+    <disable_collisions link1="base_link" link2="link1" reason="User"/>
+    <disable_collisions link1="link1" link2="link2" reason="User"/>
+    <disable_collisions link1="link1" link2="link3" reason="User"/>
+    <disable_collisions link1="link2" link2="link3" reason="User"/>
+    <disable_collisions link1="link3" link2="link4" reason="User"/>
+    <disable_collisions link1="link3" link2="link5" reason="User"/>
+    <disable_collisions link1="link4" link2="link5" reason="User"/>
+    <disable_collisions link1="link3" link2="link6" reason="User"/>
+    <disable_collisions link1="link4" link2="link6" reason="User"/>
 </robot>

From 41f9170570b3d2bc848b5412afb5c37e5b285625 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 8 Jul 2024 10:34:40 +0800
Subject: [PATCH 107/147] Fix bug: moveit can't find valid controller when
 using gazebo.

---
 .../config/fake_controllers.yaml              |  2 +-
 .../config/ros_controllers.yaml               | 57 ++++++--------
 .../fake_moveit_controller_manager.launch.xml |  2 +-
 engineer2_arm_config/launch/gazebo.launch     |  2 +-
 .../launch/joystick_control.launch            | 17 ----
 .../launch/load_controllers.launch            |  1 -
 engineer2_arm_config/launch/move_group.launch | 78 ++++++-------------
 .../launch/planning_pipeline.launch.xml       |  2 +-
 .../launch/trajectory_execution.launch.xml    |  9 ++-
 .../engineer2_simulation_controllers.yaml     | 50 ++++++++++--
 10 files changed, 101 insertions(+), 119 deletions(-)
 delete mode 100644 engineer2_arm_config/launch/joystick_control.launch

diff --git a/engineer2_arm_config/config/fake_controllers.yaml b/engineer2_arm_config/config/fake_controllers.yaml
index 4167cc8..42c4827 100644
--- a/engineer2_arm_config/config/fake_controllers.yaml
+++ b/engineer2_arm_config/config/fake_controllers.yaml
@@ -1,5 +1,5 @@
 controller_list:
-  - name: fake_engineer_arm_controller
+  - name: arm_trajectory_controller
     type: $(arg fake_execution_type)
     joints:
       - joint1
diff --git a/engineer2_arm_config/config/ros_controllers.yaml b/engineer2_arm_config/config/ros_controllers.yaml
index 1ca5150..a4837dc 100644
--- a/engineer2_arm_config/config/ros_controllers.yaml
+++ b/engineer2_arm_config/config/ros_controllers.yaml
@@ -1,5 +1,13 @@
-arm_trajectory_controller:
-  type: effort_controllers/JointTrajectoryController
+# Simulation settings for using moveit_sim_controllers
+moveit_sim_hw_interface:
+  joint_model_group: engineer_arm
+  joint_model_group_pose: home
+# Settings for ros_control_boilerplate control loop
+generic_hw_control_loop:
+  loop_hz: 300
+  cycle_time_error_threshold: 0.01
+# Settings for ros_control hardware interface
+hardware_interface:
   joints:
     - joint1
     - joint2
@@ -7,34 +15,17 @@ arm_trajectory_controller:
     - joint4
     - joint5
     - joint6
-  gains:
-    joint1:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    joint2:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    joint3:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    joint4:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    joint5:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    joint6:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
+  sim_control_mode: 1  # 0: position, 1: velocity
+
+controller_list:
+  - name: controllers/arm_trajectory_controller
+    action_ns: follow_joint_trajectory
+    default: True
+    type: FollowJointTrajectory
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
diff --git a/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml
index 64a5b70..4d5bbea 100644
--- a/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml
+++ b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml
@@ -7,6 +7,6 @@
   <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
 
   <!-- The rest of the params are specific to this plugin -->
-  <rosparam subst_value="true" file="$(find engineer2_arm_config)/config/fake_controllers.yaml"/>
+  <rosparam  file="$(find engineer2_arm_config)/config/fake_controllers.yaml"/>
 
 </launch>
diff --git a/engineer2_arm_config/launch/gazebo.launch b/engineer2_arm_config/launch/gazebo.launch
index 0cd31a0..9f7e235 100644
--- a/engineer2_arm_config/launch/gazebo.launch
+++ b/engineer2_arm_config/launch/gazebo.launch
@@ -13,7 +13,7 @@
   </include>
 
   <!-- Set the robot urdf on the parameter server -->
-  <param name="robot_description" textfile="$(find engineer2_arm_config)/config/gazebo_engineer2.urdf" />
+  <param name="robot_description" command="xacro  '$(find rm_description)/urdf/engineer2/engineer2.urdf.xacro'" />
 
   <!-- Unpause the simulation after loading the robot model -->
   <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
diff --git a/engineer2_arm_config/launch/joystick_control.launch b/engineer2_arm_config/launch/joystick_control.launch
deleted file mode 100644
index 9411f6e..0000000
--- a/engineer2_arm_config/launch/joystick_control.launch
+++ /dev/null
@@ -1,17 +0,0 @@
-<launch>
-  <!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->
-
-  <arg name="dev" default="/dev/input/js0" />
-
-  <!-- Launch joy node -->
-  <node pkg="joy" type="joy_node" name="joy">
-    <param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->
-    <param name="deadzone" value="0.2" />
-    <param name="autorepeat_rate" value="40" />
-    <param name="coalesce_interval" value="0.025" />
-  </node>
-
-  <!-- Launch python interface -->
-  <node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>
-
-</launch>
diff --git a/engineer2_arm_config/launch/load_controllers.launch b/engineer2_arm_config/launch/load_controllers.launch
index d1aab78..78d5395 100644
--- a/engineer2_arm_config/launch/load_controllers.launch
+++ b/engineer2_arm_config/launch/load_controllers.launch
@@ -22,7 +22,6 @@
                 controllers/joint_state_controller
                 controllers/arm_trajectory_controller
                 controllers/chassis_controller
-                controllers/arm_trajectory_controller
                 "
     />
 
diff --git a/engineer2_arm_config/launch/move_group.launch b/engineer2_arm_config/launch/move_group.launch
index ce737e2..c8771cc 100644
--- a/engineer2_arm_config/launch/move_group.launch
+++ b/engineer2_arm_config/launch/move_group.launch
@@ -1,84 +1,52 @@
 <launch>
 
-  <!-- GDB Debug Option -->
-  <arg name="debug" default="false" />
-  <arg unless="$(arg debug)" name="launch_prefix" value="" />
-  <arg     if="$(arg debug)" name="launch_prefix"
-           value="gdb -x $(dirname)/gdb_settings.gdb --ex run --args" />
+    <!-- GDB Debug Option -->
+    <arg name="debug" default="false"/>
+    <arg unless="$(arg debug)" name="launch_prefix" value=""/>
+    <arg if="$(arg debug)" name="launch_prefix"
+         value="gdb -x $(find engineer_arm_config)/launch/gdb_settings.gdb --ex run --args"/>
 
-  <!-- Verbose Mode Option -->
-  <arg name="info" default="$(arg debug)" />
-  <arg unless="$(arg info)" name="command_args" value="" />
-  <arg     if="$(arg info)" name="command_args" value="--debug" />
+    <!-- Verbose Mode Option -->
+    <arg name="info" default="$(arg debug)"/>
+    <arg unless="$(arg info)" name="command_args" value=""/>
+    <arg if="$(arg info)" name="command_args" value="--debug"/>
 
   <!-- move_group settings -->
   <arg name="pipeline" default="ompl" />
   <arg name="allow_trajectory_execution" default="true"/>
   <arg name="moveit_controller_manager" default="simple" />
   <arg name="fake_execution_type" default="interpolate"/>
+  <arg name="fake_execution" default="false"/>
   <arg name="max_safe_path_cost" default="1"/>
   <arg name="publish_monitored_planning_scene" default="true"/>
 
   <arg name="capabilities" default=""/>
   <arg name="disable_capabilities" default=""/>
-  <!-- load these non-default MoveGroup capabilities (space seperated) -->
-  <!--
-  <arg name="capabilities" value="
-                a_package/AwsomeMotionPlanningCapability
-                another_package/GraspPlanningPipeline
-                " />
-  -->
-
-  <!-- inhibit these default MoveGroup capabilities (space seperated) -->
-  <!--
-  <arg name="disable_capabilities" value="
-                move_group/MoveGroupKinematicsService
-                move_group/ClearOctomapService
-                " />
-  -->
 
   <arg name="load_robot_description" default="false" />
   <!-- load URDF, SRDF and joint_limits configuration -->
-  <include file="$(dirname)/planning_context.launch">
+  <include file="$(find engineer2_arm_config)/launch/planning_context.launch">
     <arg name="load_robot_description" value="$(arg load_robot_description)" />
   </include>
 
-  <!-- Planning Pipelines -->
-  <group ns="move_group/planning_pipelines">
-
-    <!-- OMPL -->
-    <include file="$(dirname)/planning_pipeline.launch.xml">
-      <arg name="pipeline" value="ompl" />
-    </include>
-
-    <!-- CHOMP -->
-    <include file="$(dirname)/planning_pipeline.launch.xml">
-      <arg name="pipeline" value="chomp" />
+    <!-- Planning Functionality -->
+    <include ns="move_group" file="$(find engineer2_arm_config)/launch/planning_pipeline.launch.xml">
+        <arg name="pipeline" value="$(arg pipeline)"/>
     </include>
 
-    <!-- Pilz Industrial Motion -->
-    <include file="$(dirname)/planning_pipeline.launch.xml">
-      <arg name="pipeline" value="pilz_industrial_motion_planner" />
+    <!-- Trajectory Execution Functionality -->
+    <include ns="move_group" file="$(find engineer2_arm_config)/launch/trajectory_execution.launch.xml"
+             if="$(arg allow_trajectory_execution)">
+        <arg name="moveit_manage_controllers" value="true"/>
+        <arg name="moveit_controller_manager" value="engineer" unless="$(arg fake_execution)"/>
+        <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
     </include>
 
-    <!-- Support custom planning pipeline -->
-    <include if="$(eval arg('pipeline') not in ['ompl', 'chomp', 'pilz_industrial_motion_planner'])"
-             file="$(dirname)/planning_pipeline.launch.xml">
-      <arg name="pipeline" value="$(arg pipeline)" />
-    </include>
-  </group>
-
-  <!-- Trajectory Execution Functionality -->
-  <include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
-    <arg name="moveit_manage_controllers" value="true" />
-    <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
-    <arg name="fake_execution_type" value="$(arg fake_execution_type)" />
-  </include>
-
   <!-- Start the actual move_group node/action server -->
-  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
+  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group"
+        respawn="false" output="screen" args="$(arg command_args)">
     <!-- Set the display variable, in case OpenGL code is used internally -->
-    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />
+    <env name="DISPLAY" value="$(optenv DISPLAY :0)"/>
 
     <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
     <param name="sense_for_plan/max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
diff --git a/engineer2_arm_config/launch/planning_pipeline.launch.xml b/engineer2_arm_config/launch/planning_pipeline.launch.xml
index 4b4d0d6..0929525 100644
--- a/engineer2_arm_config/launch/planning_pipeline.launch.xml
+++ b/engineer2_arm_config/launch/planning_pipeline.launch.xml
@@ -5,6 +5,6 @@
 
   <arg name="pipeline" default="ompl" />
 
-  <include ns="$(arg pipeline)" file="$(dirname)/$(arg pipeline)_planning_pipeline.launch.xml" />
+    <include file="$(find engineer2_arm_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml"/>
 
 </launch>
diff --git a/engineer2_arm_config/launch/trajectory_execution.launch.xml b/engineer2_arm_config/launch/trajectory_execution.launch.xml
index 20c3dfc..ae4b5af 100644
--- a/engineer2_arm_config/launch/trajectory_execution.launch.xml
+++ b/engineer2_arm_config/launch/trajectory_execution.launch.xml
@@ -2,7 +2,7 @@
   <!-- This file summarizes all settings required for trajectory execution  -->
 
   <!-- Define moveit controller manager plugin: fake, simple, or ros_control -->
-  <arg name="moveit_controller_manager" />
+  <arg name="moveit_controller_manager" default="engineer2" />
   <arg name="fake_execution_type" default="interpolate" />
 
   <!-- Flag indicating whether MoveIt is allowed to load/unload  or switch controllers -->
@@ -16,8 +16,9 @@
   <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
   <param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->
 
-  <!-- We use pass_all_args=true here to pass fake_execution_type, which is required by fake controllers, but not by real-robot controllers.
-       As real-robot controller_manager.launch files shouldn't be required to define this argument, we use the trick of passing all args. -->
-  <include file="$(dirname)/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" pass_all_args="true" />
+    <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
+    <arg name="moveit_controller_manager" default="engineer2"/>
+    <include
+            file="$(find engineer_arm_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml"/>
 
 </launch>
diff --git a/engineer_middleware/config/engineer2_simulation_controllers.yaml b/engineer_middleware/config/engineer2_simulation_controllers.yaml
index 964f39d..e04d364 100644
--- a/engineer_middleware/config/engineer2_simulation_controllers.yaml
+++ b/engineer_middleware/config/engineer2_simulation_controllers.yaml
@@ -17,11 +17,11 @@ controllers:
       - joint6
     gains:
       joint1: { p: 20000.0, i: 0, d: 400.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
-      joint2: { p: 100000, i: 0, d: 1000.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
-      joint3: { p: 5000.0, i: 0, d: 100.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
-      joint4: { p: 10000.0, i: 0.0, d: 60, i_clamp_max: 0, i_clamp_min: 0.0, antiwindup: true, publish_state: true }
-      joint5: { p: 46000.0, i: 0, d: 200.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
-      joint6: { p: 20000.0, i: 0, d: 120., i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint2: { p: 100000, i: 0, d: 0.5, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint3: { p: 20.0, i: 0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint4: { p: 20.0, i: 0.0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0.0, antiwindup: true, publish_state: true }
+      joint5: { p: 20.0, i: 0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint6: { p: 5.0, i: 0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
     constraints:
       joint1: { trajectory: 0.05, goal: 0.05 }
       joint2: { trajectory: 0.05, goal: 0.05 }
@@ -29,3 +29,43 @@ controllers:
       joint4: { trajectory: 0.05, goal: 0.05 }
       joint5: { trajectory: 0.05, goal: 0.05 }
       joint6: { trajectory: 0.05, goal: 0.05 }
+
+  chassis_controller:
+    type: rm_chassis_controllers/OmniController
+    # ChassisBase
+    publish_rate: 100
+    enable_odom_tf: true
+    publish_odom_tf: true
+    power:
+      effort_coeff: 2.0
+      vel_coeff: 0.008
+      power_offset: -9.8
+    twist_angular: 0.5233
+    timeout: 0.1
+    pid_follow: { p: 4.0, i: 0, d: 0.02, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true }
+    twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ]
+    max_odom_vel: 10.0
+    # OmniController
+    wheels:
+      left_front:
+        pose: [ 0.213, 0.1945, 0. ]
+        roller_angle: -0.785
+        joint: left_front_wheel_joint
+        <<: &wheel_setting
+          radius: 0.07
+          pid: { p: 0.4, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true }
+      right_front:
+        pose: [ 0.213, -0.1945, 0. ]
+        roller_angle: 0.785
+        joint: right_front_wheel_joint
+        <<: *wheel_setting
+      left_back:
+        pose: [ -0.213, 0.1945, 0. ]
+        roller_angle: 0.785
+        joint: left_back_wheel_joint
+        <<: *wheel_setting
+      right_back:
+        pose: [ -0.213, -0.1945, 0. ]
+        roller_angle: -0.785
+        joint: right_back_wheel_joint
+        <<: *wheel_setting

From 5b7a75f807924f4e748c76e85fa7349707377de7 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 10 Jul 2024 17:50:48 +0800
Subject: [PATCH 108/147] Add controllers for gold and silver structures on
 engineer2.

---
 .../launch/load_controllers.launch            |  5 +++
 .../engineer2_simulation_controllers.yaml     | 32 +++++++++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/engineer2_arm_config/launch/load_controllers.launch b/engineer2_arm_config/launch/load_controllers.launch
index 78d5395..f7015c2 100644
--- a/engineer2_arm_config/launch/load_controllers.launch
+++ b/engineer2_arm_config/launch/load_controllers.launch
@@ -22,6 +22,11 @@
                 controllers/joint_state_controller
                 controllers/arm_trajectory_controller
                 controllers/chassis_controller
+                controllers/silver_lifter_controller
+                controllers/silver_pusher_controller
+                controllers/silver_rotator_controller
+                controllers/gold_lifter_controller
+                controllers/gold_pusher_controller
                 "
     />
 
diff --git a/engineer_middleware/config/engineer2_simulation_controllers.yaml b/engineer_middleware/config/engineer2_simulation_controllers.yaml
index e04d364..e41c049 100644
--- a/engineer_middleware/config/engineer2_simulation_controllers.yaml
+++ b/engineer_middleware/config/engineer2_simulation_controllers.yaml
@@ -69,3 +69,35 @@ controllers:
         roller_angle: -0.785
         joint: right_back_wheel_joint
         <<: *wheel_setting
+
+  middle_pitch_controller:
+    type: effort_controllers/JointPositionController
+    joint: middle_pitch_joint
+    pid: { p: 5., i: 0., d: 1, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
+
+  silver_lifter_controller:
+    type: effort_controllers/JointPositionController
+    joint: silver_lifter_joint
+    pid: { p: 1000., i: 0., d: 100, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
+
+  silver_pusher_controller:
+    type: effort_controllers/JointPositionController
+    joint: silver_pusher_joint
+    pid: { p: 1000., i: 0., d: 100, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
+
+  silver_rotator_controller:
+    type: effort_controllers/JointPositionController
+    joint: silver_rotator_joint
+    pid: { p: 50., i: 0., d: 1, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
+
+  ######################### Gold  Ore Controllers ########################
+
+  gold_lifter_controller:
+    type: effort_controllers/JointPositionController
+    joint: gold_lifter_joint
+    pid: { p: 100., i: 0., d: 10, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
+
+  gold_pusher_controller:
+    type: effort_controllers/JointPositionController
+    joint: gold_pusher_joint
+    pid: { p: 1000., i: 0., d: 100, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }

From 5c77a3af9f88f0de540e19410a1b96cc10959444 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 10 Jul 2024 17:52:38 +0800
Subject: [PATCH 109/147] Add publishers for get ore structure controllers on
 engineer2.

---
 engineer2_arm_config/config/kinematics.yaml   |  3 --
 .../include/engineer_middleware/middleware.h  |  3 +-
 .../include/engineer_middleware/motion.h      |  9 +++-
 .../include/engineer_middleware/step.h        | 42 ++++++++++++++++++-
 .../include/engineer_middleware/step_queue.h  |  7 +++-
 engineer_middleware/src/middleware.cpp        | 17 +++++---
 6 files changed, 68 insertions(+), 13 deletions(-)

diff --git a/engineer2_arm_config/config/kinematics.yaml b/engineer2_arm_config/config/kinematics.yaml
index 3a127a7..c50afb8 100644
--- a/engineer2_arm_config/config/kinematics.yaml
+++ b/engineer2_arm_config/config/kinematics.yaml
@@ -2,6 +2,3 @@ engineer_arm:
   kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
   kinematics_solver_timeout: 0.005
-  goal_joint_tolerance: 0.0001
-  goal_position_tolerance: 0.0001
-  goal_orientation_tolerance: 0.001
diff --git a/engineer_middleware/include/engineer_middleware/middleware.h b/engineer_middleware/include/engineer_middleware/middleware.h
index cc9b809..4d89de4 100644
--- a/engineer_middleware/include/engineer_middleware/middleware.h
+++ b/engineer_middleware/include/engineer_middleware/middleware.h
@@ -83,7 +83,8 @@ class Middleware
   ChassisInterface chassis_interface_;
   ros::Publisher hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, planning_result_pub_,
       stone_num_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, extend_arm_f_pub_,
-      extend_arm_b_pub_;
+      extend_arm_b_pub_, silver_lifter_pub_, silver_pusher_pub_, silver_rotator_pub_, gold_pusher_pub_,
+      gold_lifter_pub_, middle_pitch_pub_;
   std::unordered_map<std::string, StepQueue> step_queues_;
   tf2_ros::Buffer tf_;
   tf2_ros::TransformListener tf_listener_;
diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 06f19fb..46024e0 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -675,15 +675,22 @@ class JointPointMotion : public PublishMotion<std_msgs::Float64>
   {
     ROS_ASSERT(motion.hasMember("target"));
     target_ = xmlRpcGetDouble(motion, "target", 0.0);
+    delay_ = xmlRpcGetDouble(motion, "delay", 0.5);
   }
   bool move() override
   {
+    start_time_ = ros::Time::now();
     msg_.data = target_;
     return PublishMotion::move();
   }
+  bool isFinish() override
+  {
+    return ((ros::Time::now() - start_time_).toSec() > delay_);
+  }
 
 private:
-  double target_;
+  double target_, delay_;
+  ros::Time start_time_;
 };
 
 class ExtendMotion : public PublishMotion<std_msgs::Float64>
diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h
index 3f5024d..7239b13 100644
--- a/engineer_middleware/include/engineer_middleware/step.h
+++ b/engineer_middleware/include/engineer_middleware/step.h
@@ -51,7 +51,9 @@ class Step
        ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub,
        ros::Publisher& reversal_pub, ros::Publisher& stone_num_pub, ros::Publisher& planning_result_pub,
        ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub,
-       ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub)
+       ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub,
+       ros::Publisher& silver_lifter_pub, ros::Publisher& silver_pusher_pub, ros::Publisher& silver_rotator_pub,
+       ros::Publisher& gold_pusher_pub, ros::Publisher& gold_lifter_pub, ros::Publisher& middle_pitch_pub)
     : planning_result_pub_(planning_result_pub), point_cloud_pub_(point_cloud_pub), arm_group_(arm_group)
   {
     ROS_ASSERT(step.hasMember("step"));
@@ -100,6 +102,18 @@ class Step
     }
     if (step.hasMember("chassis_target"))
       chassis_target_motion_ = new ChassisTargetMotion(step["chassis_target"], chassis_interface, tf);
+    if (step.hasMember("silver_lifter"))
+      silver_lifter_motion_ = new JointPointMotion(step["silver_lifter"], silver_lifter_pub);
+    if (step.hasMember("silver_pusher"))
+      silver_pusher_motion_ = new JointPointMotion(step["silver_pusher"], silver_pusher_pub);
+    if (step.hasMember("silver_rotator"))
+      silver_rotator_motion_ = new JointPointMotion(step["silver_rotator"], silver_rotator_pub);
+    if (step.hasMember("gold_pusher"))
+      gold_pusher_motion_ = new JointPointMotion(step["gold_pusher"], gold_pusher_pub);
+    if (step.hasMember("gold_lifter"))
+      gold_lifter_motion_ = new JointPointMotion(step["gold_lifter"], gold_lifter_pub);
+    if (step.hasMember("middle_pitch"))
+      middle_pitch_motion_ = new JointPointMotion(step["middle_pitch"], middle_pitch_pub);
   }
   bool move()
   {
@@ -143,6 +157,18 @@ class Step
       success &= extend_arm_front_motion_->move();
     if (chassis_target_motion_)
       success &= chassis_target_motion_->move();
+    if (silver_lifter_motion_)
+      success &= silver_lifter_motion_->move();
+    if (silver_pusher_motion_)
+      success &= silver_pusher_motion_->move();
+    if (silver_rotator_motion_)
+      success &= silver_rotator_motion_->move();
+    if (gold_pusher_motion_)
+      success &= gold_pusher_motion_->move();
+    if (gold_lifter_motion_)
+      success &= gold_lifter_motion_->move();
+    if (middle_pitch_motion_)
+      success &= middle_pitch_motion_->move();
     return success;
   }
   void stop()
@@ -184,6 +210,18 @@ class Step
       success &= reversal_motion_->isFinish();
     if (chassis_target_motion_)
       success &= chassis_target_motion_->isFinish();
+    if (silver_lifter_motion_)
+      success &= silver_lifter_motion_->isFinish();
+    if (silver_pusher_motion_)
+      success &= silver_pusher_motion_->isFinish();
+    if (silver_rotator_motion_)
+      success &= silver_rotator_motion_->isFinish();
+    if (gold_pusher_motion_)
+      success &= gold_pusher_motion_->isFinish();
+    if (gold_lifter_motion_)
+      success &= gold_lifter_motion_->isFinish();
+    if (middle_pitch_motion_)
+      success &= middle_pitch_motion_->isFinish();
     return success;
   }
   bool checkTimeout(ros::Duration period)
@@ -217,6 +255,8 @@ class Step
   HandMotion* hand_motion_{};
   JointPositionMotion* end_effector_motion_{};
   JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{};
+  JointPointMotion *silver_lifter_motion_{}, *silver_pusher_motion_{}, *silver_rotator_motion_{},
+      *gold_pusher_motion_{}, *gold_lifter_motion_{}, *middle_pitch_motion_{};
   ExtendMotion *extend_arm_front_motion_{}, *extend_arm_back_motion_{};
   StoneNumMotion* stone_num_motion_{};
   ChassisMotion* chassis_motion_{};
diff --git a/engineer_middleware/include/engineer_middleware/step_queue.h b/engineer_middleware/include/engineer_middleware/step_queue.h
index d2fa912..32424d7 100644
--- a/engineer_middleware/include/engineer_middleware/step_queue.h
+++ b/engineer_middleware/include/engineer_middleware/step_queue.h
@@ -57,14 +57,17 @@ class StepQueue
             ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub, ros::Publisher& reversal_pub,
             ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub,
             ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub,
-            ros::Publisher& extend_arm_b_pub)
+            ros::Publisher& extend_arm_b_pub, ros::Publisher& silver_lifter_pub, ros::Publisher& silver_pusher_pub,
+            ros::Publisher& silver_rotator_pub, ros::Publisher& gold_pusher_pub, ros::Publisher& gold_lifter_pub,
+            ros::Publisher& middle_pitch_pub)
     : chassis_interface_(chassis_interface)
   {
     ROS_ASSERT(steps.getType() == XmlRpc::XmlRpcValue::TypeArray);
     for (int i = 0; i < steps.size(); ++i)
       queue_.emplace_back(steps[i], scenes, tf, arm_group, chassis_interface, hand_pub, end_effector_pub, stone_num_pub,
                           gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, point_cloud_pub, ore_rotate_pub,
-                          ore_lift_pub, gimbal_lift_pub, extend_arm_f_pub, extend_arm_b_pub);
+                          ore_lift_pub, gimbal_lift_pub, extend_arm_f_pub, extend_arm_b_pub, silver_lifter_pub,
+                          silver_pusher_pub, silver_rotator_pub, gold_pusher_pub, gold_lifter_pub, middle_pitch_pub);
   }
   bool run(actionlib::SimpleActionServer<rm_msgs::EngineerAction>& as)
   {
diff --git a/engineer_middleware/src/middleware.cpp b/engineer_middleware/src/middleware.cpp
index 686e9f2..34b0105 100644
--- a/engineer_middleware/src/middleware.cpp
+++ b/engineer_middleware/src/middleware.cpp
@@ -58,6 +58,12 @@ Middleware::Middleware(ros::NodeHandle& nh)
   , gimbal_lift_pub_(nh.advertise<std_msgs::Float64>("/controllers/gimbal_lifter_controller/command", 10))
   , extend_arm_f_pub_(nh.advertise<std_msgs::Float64>("/controllers/extend_arm_front_controller/command", 10))
   , extend_arm_b_pub_(nh.advertise<std_msgs::Float64>("/controllers/extend_arm_back_controller/command", 10))
+  , silver_lifter_pub_(nh.advertise<std_msgs::Float64>("/controllers/silver_lifter_controller/command", 10))
+  , silver_pusher_pub_(nh.advertise<std_msgs::Float64>("/controllers/silver_pusher_controller/command", 10))
+  , silver_rotator_pub_(nh.advertise<std_msgs::Float64>("/controllers/silver_rotator_controller/command", 10))
+  , gold_pusher_pub_(nh.advertise<std_msgs::Float64>("/controllers/gold_pusher_controller/command", 10))
+  , gold_lifter_pub_(nh.advertise<std_msgs::Float64>("/controllers/gold_lifter_controller/command", 10))
+  , middle_pitch_pub_(nh.advertise<std_msgs::Float64>("/controllers/middle_pitch_controller/command", 10))
   , tf_listener_(tf_)
   , is_middleware_control_(false)
 {
@@ -71,11 +77,12 @@ Middleware::Middleware(ros::NodeHandle& nh)
     ROS_ASSERT(scenes_list.getType() == XmlRpc::XmlRpcValue::Type::TypeStruct);
     for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = steps_list.begin(); it != steps_list.end(); ++it)
     {
-      step_queues_.insert(
-          std::make_pair(it->first, StepQueue(it->second, scenes_list, tf_, arm_group_, chassis_interface_, hand_pub_,
-                                              end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, stone_num_pub_,
-                                              planning_result_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_,
-                                              gimbal_lift_pub_, extend_arm_f_pub_, extend_arm_b_pub_)));
+      step_queues_.insert(std::make_pair(
+          it->first, StepQueue(it->second, scenes_list, tf_, arm_group_, chassis_interface_, hand_pub_,
+                               end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, stone_num_pub_,
+                               planning_result_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_,
+                               extend_arm_f_pub_, extend_arm_b_pub_, silver_lifter_pub_, silver_pusher_pub_,
+                               silver_rotator_pub_, gold_pusher_pub_, gold_lifter_pub_, middle_pitch_pub_)));
     }
   }
   else

From 01da3baf2c40320da8d7239725833cb9bb9eb1c9 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 10 Jul 2024 17:53:00 +0800
Subject: [PATCH 110/147] Add basic step lists for engineer2.

---
 .../config/engineer2_steps_list.yaml          | 582 +++++++++++++++++-
 1 file changed, 580 insertions(+), 2 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index debfca0..7bb7e4a 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -7,11 +7,11 @@ common:
     normally: &NORMALLY
       speed: 0.4
       accel: 0.3
-      timeout: 3
+      timeout: 5
     quickly: &QUICKLY
       speed: 0.6
       accel: 0.6
-      timeout: 7.
+      timeout: 3.
 
   tolerance:
     small_tolerance: &SMALL_TOLERANCE
@@ -22,3 +22,581 @@ common:
       tolerance_joints: [ 0.015, 0.015, 0.02, 0.35, 0.2, 0.2, 0.1 ]
     max_tolerance: &MAX_TOLERANCE
       tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3]
+
+  joint1:
+    home: &JOINT1_HOME_POS
+      0.05
+    down: &JOINT1_DOWN_POS
+      0.
+    up: &JOINT1_UP_POS
+      0.579
+    exchange: &JOINT1_EXCHANGE_POS
+      0.35
+    ready_get_small: &JOINT1_READY_GET_SMALL_POS
+      0.4
+    get_small: &JOINT1_GET_SMALL_POS
+      0.3
+    ready_get_stored: &JOINT1_READY_GET_STORED_POS
+      0.57
+    get_stored: &JOINT1_GET_STORED_POS
+      0.5
+    get_stored_gold: &JOINT1_GET_STORED_GOLD_POS
+      0.029
+
+
+  joint2:
+    home: &JOINT2_HOME_POS
+      0.00001
+    exchange: &JOINT2_EXCHANGE_POS
+      -1.57
+    get_small_island: &JOINT2_GET_SMALL_ISLAND_POS
+      0.068
+    get_stored_silver3: &JOINT2_GET_STORED_SILVER3_POS
+      -2.12
+    get_stored_silver2: &JOINT2_GET_STORED_SILVER2_POS
+      -2.967
+    get_stored_silver1: &JOINT2_GET_STORED_SILVER1_POS
+      -4.1
+    ready_get_stored_gold: &JOINT2_READY_GET_STORED_GOLD_POS
+      -1.2
+    get_stored_gold: &JOINT2_GET_STORED_GOLD_POS
+      -1.32
+
+  joint3:
+    home: &JOINT3_HOME_POS
+      0.00001
+    get_small_island: &JOINT3_GET_SMALL_ISLAND_POS
+      -2.18
+    get_stored_silver3: &JOINT3_GET_STORED_SILVER3_POS
+      -1.788
+    get_stored_silver2: &JOINT3_GET_STORED_SILVER2_POS
+      -1.483
+    get_stored_silver1: &JOINT3_GET_STORED_SILVER1_POS
+      0.00001
+    ready_get_stored_gold: &JOINT3_READY_GET_STORED_GOLD_POS
+      -1.962
+    get_stored_gold: &JOINT3_GET_STORED_GOLD_POS
+      -2.181
+
+  joint4:
+    home: &JOINT4_HOME_POS
+      0.00001
+    r90: &JOINT4_R90_POS
+      -1.57
+    l90: &JOINT4_L90_POS
+      1.57
+
+
+  joint5:
+    home: &JOINT5_HOME_POS
+      0.00001
+    down_90: &JOINT5_DOWN_90
+      1.57
+    get_stored_gold: &JOINT5_GET_STORED_GOLD_POS
+      1.193
+
+  joint6:
+    home: &JOINT6_HOME_POS
+      0.0
+
+  silver_lift:
+    home: &SILVER_LIFTER_HOME_POS
+      0.0
+    lift: &SILVER_LIFTER_LIFT_POS
+      0.0
+
+  silver_push:
+    home: &SILVER_PUSHER_HOME_POS
+      0.0
+    get_silver: &SILVER_PUSHER_GET_SILVER_POS
+      0.0
+    pull_back_little: &SILVER_PUSHER_PULL_BACK_LITTLE_POS
+      0.0
+
+  silver_rotate:
+    home: &SILVER_ROTATOR_HOME_POS
+      0.0
+    90: &SILVER_ROTATOR_90_POS
+      1.57
+
+  gold_push:
+    home: &GOLD_PUSHER_HOME_POS
+      0.0
+    get_gold: &GOLD_PUSHER_GET_GOLD_POS
+      0.0
+  gold_lift:
+    home: &GOLD_LIFTER_HOME_POS
+      0.0
+    lift_gold: &GOLD_LIFTER_LIFT_GOLD_POS
+      0.0
+
+  arm:
+    home: &HOME
+      joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
+      common:
+        <<: *NORMALLY
+      tolerance:
+        <<: *NORMAL_TOLERANCE
+
+    lv4_exchange: &LV4_EXCHANGE
+      joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
+      common:
+        <<: *NORMALLY
+      tolerance:
+        <<: *NORMAL_TOLERANCE
+
+    lv5_l_exchange: &LV5_L_EXCHANGE
+      joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+      common:
+        <<: *NORMALLY
+      tolerance:
+        <<: *NORMAL_TOLERANCE
+
+    lv5_r_exchange: &LV5_R_EXCHANGE
+      joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+      common:
+        <<: *NORMALLY
+      tolerance:
+        <<: *NORMAL_TOLERANCE
+
+
+
+steps_list:
+  HOME:
+    - step: "arm home pos"
+      arm:
+        joints: [*JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  LV4_EXCHANGE:
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV4_EXCHANGE
+#    - step: "gimbal ready"
+
+  LV5_L_EXCHANGE:
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV5_L_EXCHANGE
+
+  LV5_R_EXCHANGE:
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV5_R_EXCHANGE
+
+
+
+
+  GET_SMALL_ISLAND: &GET_SMALL_ISLAND
+    - step: "ready get small island silver"
+      arm:
+        joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+#    - step: "open main gripper"
+
+    - step: "get small island silver"
+      arm:
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "pull up silver"
+      arm:
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  STORE_SILVER_TO_GRIPPER1: &STORE_SILVER_TO_GRIPPER1
+    - step: "ready to store"
+      arm:
+        joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+#    - step: "open gripper1"
+    - step: "store"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    #    - step: "close main gripper"
+
+    - step: "left arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  STORE_SILVER_TO_GRIPPER2: &STORE_SILVER_TO_GRIPPER2
+    - step: "ready to store"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    #    - step: "open gripper2"
+    - step: "store"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    #    - step: "close main gripper"
+
+    - step: "left arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  STORE_SILVER_TO_GRIPPER3: &STORE_SILVER_TO_GRIPPER3
+    - step: "ready to store"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    #    - step: "open gripper3"
+    - step: "store"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    #    - step: "close main gripper"
+    - step: "left arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  GET_SILVER_FROM_GRIPPER1:
+    - step: "ready to get silver from gripper1"
+      arm:
+        joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    #    - step: "open main gripper"
+    - step: "get silver"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    #    - step: "close gripper1"
+
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  GET_SILVER_FROM_GRIPPER2:
+    - step: "ready to get silver from gripper2"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    #    - step: "open main gripper"
+    - step: "store"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    #    - step: "close gripper2"
+
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  GET_SILVER_FROM_GRIPPER3:
+    - step: "ready to get silver from gripper3"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    #    - step: "open main gripper"
+    - step: "store"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    #    - step: "close gripper3"
+    - step: "left arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  GET_STORED_GOLD:
+    - step: "ready to get stored gold"
+      arm:
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+#    - step: "open main gripper"
+    - step: "get stored gold"
+      arm:
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+#    - step: "close gold gripper
+    - step: "move out gold"
+      arm:
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    - step: "lift up gold"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  THREE_SILVER_ARM:
+    - step: "ready get small island silver"
+      arm:
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+          #    - step: "open main gripper"
+
+    - step: "get small island silver"
+      arm:
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "pull up silver"
+      arm:
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    - step: "ready to store"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+          #    - step: "open gripper2"
+    - step: "store"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+          #    - step: "close main gripper"
+
+    - step: "left arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+  ###################### 2nd silver
+    - step: "ready get small island silver"
+      arm:
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+          #    - step: "open main gripper"
+
+    - step: "get small island silver"
+      arm:
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "pull up silver"
+      arm:
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    - step: "ready to store"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+          #    - step: "open gripper3"
+    - step: "store"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+          #    - step: "close main gripper"
+
+    - step: "left arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+  ############ 3rd silver
+    - step: "ready get small island silver"
+      arm:
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+          #    - step: "open main gripper"
+
+    - step: "get small island silver"
+      arm:
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "pull up silver"
+      arm:
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  THREE_SILVER_GRIPPER:
+    - step: "rotate down"
+      silver_rotator:
+        target: 0.0001
+        delay: 0.
+    - step: "lifter down"
+      silver_lifter:
+        target: 0.
+        delay: 0.
+    - step: "pusher back"
+      silver_pusher:
+        target: 0.
+        delay: 0.1
+#    - step: "open gripper"
+    - step: "get silver"
+      silver_pusher:
+        target: -0.046
+    - step: "pusher back little"
+      silver_pusher:
+        target: -0.04
+    - step: "lifter up"
+      silver_lifter:
+        target: 0.18
+        delay: 0.
+    - step: "rotate up"
+      silver_rotator:
+        target: 1.57
+    - step: "pusher back"
+      silver_pusher:
+        target: 0.
+        delay: 0.
+    - step: "lifter down"
+      silver_lifter:
+        target: 0.
+        delay: 0.
+
+  GET_MIDDLE_GOLD:
+    - step: "pusher back"
+      gold_pusher:
+        target: 0.
+        delay: 0.
+    - step: "lifter down"
+      gold_lifter:
+        target: 0
+        delay: 0.
+#    - step: "open gold gripper"
+    - step: "get gold"
+      gold_pusher:
+        target: -0.51
+    - step: "lift gold"
+      gold_lifter:
+        target: 0.05
+    - step: "pull back gold"
+      gold_pusher:
+        target: 0
+    - step: "lifter down"
+      gold_lifter:
+        target: 0
+        delay: 0.

From f54ac284cc590551694f11cfba674cbbf2afe30a Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 11 Jul 2024 20:29:32 +0800
Subject: [PATCH 111/147] Record.

---
 .../config/engineer2_steps_list.yaml          | 67 +++++++++++++++----
 .../include/engineer_middleware/motion.h      |  2 +-
 2 files changed, 56 insertions(+), 13 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index 7bb7e4a..d8c678c 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -30,6 +30,10 @@ common:
       0.
     up: &JOINT1_UP_POS
       0.579
+    ready_get_ground: &JOINT1_READY_GET_GROUND_POS
+      0.5
+    get_ground: &JOINT1_GET_GROUND_POS
+      0.4
     exchange: &JOINT1_EXCHANGE_POS
       0.35
     ready_get_small: &JOINT1_READY_GET_SMALL_POS
@@ -130,6 +134,12 @@ common:
     lift_gold: &GOLD_LIFTER_LIFT_GOLD_POS
       0.0
 
+  middle_pitch:
+    ground_stone: &MIDDLE_PITCH_GROUND_STONE
+      1.57
+    normal: &MIDDLE_PITCH_NORMAL
+      0.0
+
   arm:
     home: &HOME
       joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
@@ -190,7 +200,7 @@ steps_list:
 
 
 
-  GET_SMALL_ISLAND: &GET_SMALL_ISLAND
+  GET_SMALL_ISLAND:
     - step: "ready get small island silver"
       arm:
         joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
@@ -298,6 +308,38 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
 
+  GROUND_STONE:
+    - step: "arm ready get ground stone"
+      arm:
+        joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ready to get ground stone"
+      middle_pitch:
+        target: *MIDDLE_PITCH_GROUND_STONE
+#    - step: "open main gripper"
+    - step: "arm down get ground stone"
+      arm:
+        joints: [*JOINT1_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "arm pull up stone"
+      arm:
+        joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "middle pitch home"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
+
+
+
   GET_SILVER_FROM_GRIPPER1:
     - step: "ready to get silver from gripper1"
       arm:
@@ -319,7 +361,7 @@ steps_list:
 
     - step: "lift arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -347,7 +389,7 @@ steps_list:
 
     - step: "lift arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -374,7 +416,7 @@ steps_list:
     #    - step: "close gripper3"
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -414,6 +456,9 @@ steps_list:
           <<: *NORMAL_TOLERANCE
 
   THREE_SILVER_ARM:
+    - step: "silver rotator up"
+      silver_rotator:
+        target: 1.57
     - step: "ready get small island silver"
       arm:
         joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -545,11 +590,9 @@ steps_list:
     - step: "rotate down"
       silver_rotator:
         target: 0.0001
-        delay: 0.
     - step: "lifter down"
       silver_lifter:
         target: 0.
-        delay: 0.
     - step: "pusher back"
       silver_pusher:
         target: 0.
@@ -558,45 +601,45 @@ steps_list:
     - step: "get silver"
       silver_pusher:
         target: -0.046
+        delay: 0.5
     - step: "pusher back little"
       silver_pusher:
         target: -0.04
+        delay: 0.5
     - step: "lifter up"
       silver_lifter:
         target: 0.18
-        delay: 0.
     - step: "rotate up"
       silver_rotator:
         target: 1.57
+        delay: 0.5
     - step: "pusher back"
       silver_pusher:
         target: 0.
-        delay: 0.
     - step: "lifter down"
       silver_lifter:
         target: 0.
-        delay: 0.
 
   GET_MIDDLE_GOLD:
     - step: "pusher back"
       gold_pusher:
         target: 0.
-        delay: 0.
     - step: "lifter down"
       gold_lifter:
         target: 0
-        delay: 0.
 #    - step: "open gold gripper"
     - step: "get gold"
       gold_pusher:
         target: -0.51
+        delay: 0.5
     - step: "lift gold"
       gold_lifter:
         target: 0.05
+        delay: 0.5
     - step: "pull back gold"
       gold_pusher:
         target: 0
+        delay: 0.5
     - step: "lifter down"
       gold_lifter:
         target: 0
-        delay: 0.
diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 46024e0..e29665d 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -675,7 +675,7 @@ class JointPointMotion : public PublishMotion<std_msgs::Float64>
   {
     ROS_ASSERT(motion.hasMember("target"));
     target_ = xmlRpcGetDouble(motion, "target", 0.0);
-    delay_ = xmlRpcGetDouble(motion, "delay", 0.5);
+    delay_ = xmlRpcGetDouble(motion, "delay", 0.0);
   }
   bool move() override
   {

From 60c576c621437b5a6081ac804fdbbc4a49249fe1 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 16 Jul 2024 10:55:22 +0800
Subject: [PATCH 112/147] Add gimbal controller in engineer2 sim.

---
 .../launch/load_controllers.launch            |  1 +
 .../engineer2_simulation_controllers.yaml     | 29 +++++++++++++++++++
 2 files changed, 30 insertions(+)

diff --git a/engineer2_arm_config/launch/load_controllers.launch b/engineer2_arm_config/launch/load_controllers.launch
index f7015c2..2b45884 100644
--- a/engineer2_arm_config/launch/load_controllers.launch
+++ b/engineer2_arm_config/launch/load_controllers.launch
@@ -21,6 +21,7 @@
                 controllers/robot_state_controller
                 controllers/joint_state_controller
                 controllers/arm_trajectory_controller
+                controllers/gimbal_controller
                 controllers/chassis_controller
                 controllers/silver_lifter_controller
                 controllers/silver_pusher_controller
diff --git a/engineer_middleware/config/engineer2_simulation_controllers.yaml b/engineer_middleware/config/engineer2_simulation_controllers.yaml
index e41c049..a43ad6f 100644
--- a/engineer_middleware/config/engineer2_simulation_controllers.yaml
+++ b/engineer_middleware/config/engineer2_simulation_controllers.yaml
@@ -70,6 +70,35 @@ controllers:
         joint: right_back_wheel_joint
         <<: *wheel_setting
 
+  gimbal_controller:
+    type: rm_gimbal_controllers/Controller
+    yaw:
+      joint: "yaw_joint"
+      pid: { p: 0.5, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: false }
+      pid_pos: { p: 15.0, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true }
+      k_chassis_vel: 0.035
+      k_v: 1.0
+    pitch:
+      joint: "pitch_joint"
+      pid: { p: 0., i: 0.0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: false }
+      pid_pos: { p: 0.0, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true }
+      k_v: 1.0
+    feedforward:
+      gravity: 0.0
+      enable_gravity_compensation: false
+      mass_origin: [ 0.0,0.0,0.0 ]
+    bullet_solver:
+      resistance_coff_qd_10: 0.45
+      resistance_coff_qd_15: 1.0
+      resistance_coff_qd_16: 0.7
+      resistance_coff_qd_18: 0.55
+      resistance_coff_qd_30: 3.0
+      g: 9.81
+      delay: 0.1
+      dt: 0.001
+      timeout: 0.001
+      publish_rate: 50
+
   middle_pitch_controller:
     type: effort_controllers/JointPositionController
     joint: middle_pitch_joint

From 230cf98029fa83a2a54befe54c796173cf17c70e Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 16 Jul 2024 22:45:12 +0800
Subject: [PATCH 113/147] Modify gpio motion to control multi gpio ports.

---
 .../config/engineer2_steps_list.yaml          | 107 ++++++++++++++++--
 .../include/engineer_middleware/motion.h      |  29 ++++-
 2 files changed, 124 insertions(+), 12 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index d8c678c..eeb3f90 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -169,6 +169,43 @@ common:
       tolerance:
         <<: *NORMAL_TOLERANCE
 
+  gripper:
+    open_main_gripper: &OPEN_MAIN_GRIPPER
+      pin: 0
+      state: true
+    close_main_gripper: &CLOSE_MAIN_GRIPPER
+      pin: 0
+      state: false
+    open_s1: &OPEN_S1
+      pin: 1
+      state: true
+    close_s1: &CLOSE_S1
+      pin: 1
+      state: false
+    open_s2: &OPEN_S2
+      pin: 2
+      state: true
+    close_s2: &CLOSE_S2
+      pin: 2
+      state: false
+    open_s3: &OPEN_S3
+      pin: 3
+      state: true
+    close_s3: &CLOSE_S3
+      pin: 3
+      state: false
+    open_gold: &OPEN_GOLD
+      pin: 4
+      state: true
+    close_gold: &CLOSE_GOLD
+      pin: 4
+      state: false
+    open_s_gripper: &OPEN_S_GRIPPER
+      pin: 5
+      state: true
+    close_s_gripper: &CLOSE_S_GRIPPER
+      pin: 5
+      state: false
 
 
 steps_list:
@@ -226,7 +263,7 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-  STORE_SILVER_TO_GRIPPER1: &STORE_SILVER_TO_GRIPPER1
+  STORE_SILVER_TO_GRIPPER1:
     - step: "ready to store"
       arm:
         joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
@@ -253,7 +290,7 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-  STORE_SILVER_TO_GRIPPER2: &STORE_SILVER_TO_GRIPPER2
+  STORE_SILVER_TO_GRIPPER2:
     - step: "ready to store"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -281,7 +318,7 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-  STORE_SILVER_TO_GRIPPER3: &STORE_SILVER_TO_GRIPPER3
+  STORE_SILVER_TO_GRIPPER3:
     - step: "ready to store"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -340,7 +377,7 @@ steps_list:
 
 
 
-  GET_SILVER_FROM_GRIPPER1:
+  GET_STORED_SILVER1:
     - step: "ready to get silver from gripper1"
       arm:
         joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
@@ -367,7 +404,7 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-  GET_SILVER_FROM_GRIPPER2:
+  GET_STORED_SILVER2:
     - step: "ready to get silver from gripper2"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -395,7 +432,7 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-  GET_SILVER_FROM_GRIPPER3:
+  GET_STORED_SILVER3:
     - step: "ready to get silver from gripper3"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -455,7 +492,7 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-  THREE_SILVER_ARM:
+  ARM_THREE_SILVER:
     - step: "silver rotator up"
       silver_rotator:
         target: 1.57
@@ -586,7 +623,7 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-  THREE_SILVER_GRIPPER:
+  GRIPPER_THREE_SILVER:
     - step: "rotate down"
       silver_rotator:
         target: 0.0001
@@ -620,7 +657,7 @@ steps_list:
       silver_lifter:
         target: 0.
 
-  GET_MIDDLE_GOLD:
+  MID_BIG_ISLAND:
     - step: "pusher back"
       gold_pusher:
         target: 0.
@@ -643,3 +680,55 @@ steps_list:
     - step: "lifter down"
       gold_lifter:
         target: 0
+
+  TEST1:
+    - step: "main open"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+
+#
+  TEST2:
+    - step: "main close"
+      gripper:
+        <<: *CLOSE_MAIN_GRIPPER
+  TEST3:
+    - step: "s1 open"
+      gripper:
+        <<: *OPEN_S1
+  TEST4:
+    - step: "s1 c"
+      gripper:
+        <<: *CLOSE_S1
+  TEST5:
+    - step: "s2 open"
+      gripper:
+        <<: *OPEN_S2
+
+  TEST6:
+    - step: "s2 close"
+      gripper:
+        <<: *CLOSE_S2
+  TEST7:
+    - step: "s3 open"
+      gripper:
+        <<: *OPEN_S3
+  TEST8:
+    - step: "s3 close"
+      gripper:
+        <<: *CLOSE_S3
+  TEST9:
+    - step: "g1 open"
+      gripper:
+        <<: *OPEN_GOLD
+  TEST10:
+    - step: "g1 close"
+      gripper:
+        <<: *CLOSE_GOLD
+  TEST11:
+    - step: "g1 close"
+      gripper:
+        <<: *OPEN_S_GRIPPER
+  TEST12:
+    - step: "g1 close"
+      gripper:
+        <<: *CLOSE_S_GRIPPER
diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index e29665d..ffbd525 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -528,18 +528,41 @@ class GpioMotion : public PublishMotion<rm_msgs::GpioData>
   GpioMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface)
     : PublishMotion<rm_msgs::GpioData>(motion, interface)
   {
+    msg_.gpio_state.assign(6, false);
+    msg_.gpio_name.assign(6, "no_registered");
+    pin_ = motion["pin"];
     state_ = motion["state"];
-    msg_.gpio_state.push_back(0);
-    msg_.gpio_name.push_back("gripper");
+    switch (pin_)
+    {
+      case 0:
+        msg_.gpio_name[0] = "main_gripper";
+        break;
+      case 1:
+        msg_.gpio_name[1] = "silver_gripper1";
+        break;
+      case 2:
+        msg_.gpio_name[2] = "silver_gripper2";
+        break;
+      case 3:
+        msg_.gpio_name[3] = "silver_gripper3";
+        break;
+      case 4:
+        msg_.gpio_name[4] = "gold_gripper";
+        break;
+      case 5:
+        msg_.gpio_name[5] = "silver_pump";
+        break;
+    }
   }
   bool move() override
   {
-    msg_.gpio_state[0] = state_;
+    msg_.gpio_state[pin_] = state_;
     return PublishMotion::move();
   }
 
 private:
   bool state_;
+  int pin_;
 };
 
 class StoneNumMotion : public PublishMotion<std_msgs::String>

From 464d2a442ed20ea99763d6785c5653d64fc98632 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 17 Jul 2024 10:29:46 +0800
Subject: [PATCH 114/147] Modify gpio motion to avoid invalid gpio port.

---
 .../include/engineer_middleware/motion.h           | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index ffbd525..0827a84 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -528,8 +528,8 @@ class GpioMotion : public PublishMotion<rm_msgs::GpioData>
   GpioMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface)
     : PublishMotion<rm_msgs::GpioData>(motion, interface)
   {
-    msg_.gpio_state.assign(6, false);
-    msg_.gpio_name.assign(6, "no_registered");
+    msg_.gpio_state.assign(8, false);
+    msg_.gpio_name.assign(8, "no_registered");
     pin_ = motion["pin"];
     state_ = motion["state"];
     switch (pin_)
@@ -550,7 +550,15 @@ class GpioMotion : public PublishMotion<rm_msgs::GpioData>
         msg_.gpio_name[4] = "gold_gripper";
         break;
       case 5:
-        msg_.gpio_name[5] = "silver_pump";
+        msg_.gpio_name[5] = "unused";
+        ROS_WARN_STREAM("GPIO port 6 is unused now!");
+        break;
+      case 6:
+        msg_.gpio_name[6] = "unused";
+        ROS_WARN_STREAM("GPIO port 7 is unused now!");
+        break;
+      case 7:
+        msg_.gpio_name[7] = "silver_pump";
         break;
     }
   }

From 4b35ef9a475c1786325547bd3f35f17c0e40a5e0 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 18 Jul 2024 19:06:13 +0800
Subject: [PATCH 115/147] Add stone num motion.

---
 .../config/engineer2_steps_list.yaml          | 115 ++++++++----------
 1 file changed, 54 insertions(+), 61 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index eeb3f90..ea40b6d 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -41,7 +41,7 @@ common:
     get_small: &JOINT1_GET_SMALL_POS
       0.3
     ready_get_stored: &JOINT1_READY_GET_STORED_POS
-      0.57
+      0.545
     get_stored: &JOINT1_GET_STORED_POS
       0.5
     get_stored_gold: &JOINT1_GET_STORED_GOLD_POS
@@ -200,15 +200,19 @@ common:
     close_gold: &CLOSE_GOLD
       pin: 4
       state: false
-    open_s_gripper: &OPEN_S_GRIPPER
-      pin: 5
+    open_silver_gripper: &OPEN_S_GRIPPER
+      pin: 7
       state: true
-    close_s_gripper: &CLOSE_S_GRIPPER
-      pin: 5
+    close_silver_gripper: &CLOSE_S_GRIPPER
+      pin: 7
       state: false
 
 
 steps_list:
+  MIDDLE_PITCH_UP:
+    - step: "middld pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
   HOME:
     - step: "arm home pos"
       arm:
@@ -218,6 +222,18 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
 
+  HOME_WITH_PITCH:
+    - step: "arm home pos"
+      arm:
+        joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
+
   LV4_EXCHANGE:
     - step: "arm enter exchange pos"
       arm:
@@ -281,7 +297,9 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
     #    - step: "close main gripper"
-
+    - step: "add stone"
+      stone_num:
+        change: "+s1"
     - step: "left arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -309,7 +327,9 @@ steps_list:
           <<: *NORMAL_TOLERANCE
 
     #    - step: "close main gripper"
-
+    - step: "add stone"
+      stone_num:
+        change: "+s2"
     - step: "left arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -337,6 +357,9 @@ steps_list:
           <<: *NORMAL_TOLERANCE
 
     #    - step: "close main gripper"
+    - step: "add stone"
+      stone_num:
+        change: "+s3"
     - step: "left arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -395,7 +418,9 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
     #    - step: "close gripper1"
-
+    - step: "minus stone"
+      stone_num:
+        change: "-s1"
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -423,7 +448,9 @@ steps_list:
           <<: *NORMAL_TOLERANCE
 
     #    - step: "close gripper2"
-
+    - step: "minus stone"
+      stone_num:
+        change: "-s2"
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -451,6 +478,9 @@ steps_list:
           <<: *NORMAL_TOLERANCE
 
     #    - step: "close gripper3"
+    - step: "minus stone"
+      stone_num:
+        change: "-s3"
     - step: "left arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -476,6 +506,9 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
 #    - step: "close gold gripper
+    - step: "add stone"
+      stone_num:
+        change: "+g"
     - step: "move out gold"
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -656,6 +689,15 @@ steps_list:
     - step: "lifter down"
       silver_lifter:
         target: 0.
+    - step: "add stone1"
+      stone_num:
+        change: "+s1"
+    - step: "add stone1"
+      stone_num:
+        change: "+s2"
+    - step: "add stone1"
+      stone_num:
+        change: "+s3"
 
   MID_BIG_ISLAND:
     - step: "pusher back"
@@ -680,55 +722,6 @@ steps_list:
     - step: "lifter down"
       gold_lifter:
         target: 0
-
-  TEST1:
-    - step: "main open"
-      gripper:
-        <<: *OPEN_MAIN_GRIPPER
-
-#
-  TEST2:
-    - step: "main close"
-      gripper:
-        <<: *CLOSE_MAIN_GRIPPER
-  TEST3:
-    - step: "s1 open"
-      gripper:
-        <<: *OPEN_S1
-  TEST4:
-    - step: "s1 c"
-      gripper:
-        <<: *CLOSE_S1
-  TEST5:
-    - step: "s2 open"
-      gripper:
-        <<: *OPEN_S2
-
-  TEST6:
-    - step: "s2 close"
-      gripper:
-        <<: *CLOSE_S2
-  TEST7:
-    - step: "s3 open"
-      gripper:
-        <<: *OPEN_S3
-  TEST8:
-    - step: "s3 close"
-      gripper:
-        <<: *CLOSE_S3
-  TEST9:
-    - step: "g1 open"
-      gripper:
-        <<: *OPEN_GOLD
-  TEST10:
-    - step: "g1 close"
-      gripper:
-        <<: *CLOSE_GOLD
-  TEST11:
-    - step: "g1 close"
-      gripper:
-        <<: *OPEN_S_GRIPPER
-  TEST12:
-    - step: "g1 close"
-      gripper:
-        <<: *CLOSE_S_GRIPPER
+    - step: "add stone"
+      stone_num:
+        change: "+g"

From f55c27797e4c425c9a44607035e68e49024f4570 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Fri, 19 Jul 2024 01:35:20 +0800
Subject: [PATCH 116/147] Improve step lists of engineer2.

---
 .../config/engineer2_steps_list.yaml          | 314 ++++++++++++------
 1 file changed, 216 insertions(+), 98 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index ea40b6d..de2a86e 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -25,9 +25,9 @@ common:
 
   joint1:
     home: &JOINT1_HOME_POS
-      0.05
+      0.1
     down: &JOINT1_DOWN_POS
-      0.
+      0.0000001
     up: &JOINT1_UP_POS
       0.579
     ready_get_ground: &JOINT1_READY_GET_GROUND_POS
@@ -50,9 +50,15 @@ common:
 
   joint2:
     home: &JOINT2_HOME_POS
-      0.00001
+      1.0
+    zero: &JOINT2_ZERO_POS
+      0.0000001
     exchange: &JOINT2_EXCHANGE_POS
       -1.57
+    l_exchange: &JOINT2_L_EXCHANGE_POS
+      -0.984
+    r_exchange: &JOINT2_R_EXCHANGE_POS
+      -2.156
     get_small_island: &JOINT2_GET_SMALL_ISLAND_POS
       0.068
     get_stored_silver3: &JOINT2_GET_STORED_SILVER3_POS
@@ -60,7 +66,7 @@ common:
     get_stored_silver2: &JOINT2_GET_STORED_SILVER2_POS
       -2.967
     get_stored_silver1: &JOINT2_GET_STORED_SILVER1_POS
-      -4.1
+      -3.75
     ready_get_stored_gold: &JOINT2_READY_GET_STORED_GOLD_POS
       -1.2
     get_stored_gold: &JOINT2_GET_STORED_GOLD_POS
@@ -68,7 +74,13 @@ common:
 
   joint3:
     home: &JOINT3_HOME_POS
-      0.00001
+      0.9
+    zero: &JOINT3_ZERO_POS
+      0.00000001
+    l_exchange: &JOINT3_L_EXCHANGE_POS
+      -1.165
+    r_exchange: &JOINT3_R_EXCHANGE_POS
+      1.165
     get_small_island: &JOINT3_GET_SMALL_ISLAND_POS
       -2.18
     get_stored_silver3: &JOINT3_GET_STORED_SILVER3_POS
@@ -84,7 +96,9 @@ common:
 
   joint4:
     home: &JOINT4_HOME_POS
-      0.00001
+      1.022
+    zero: &JOINT4_ZERO_POS
+      0.000001
     r90: &JOINT4_R90_POS
       -1.57
     l90: &JOINT4_L90_POS
@@ -102,6 +116,10 @@ common:
   joint6:
     home: &JOINT6_HOME_POS
       0.0
+    l_90: &JOINT6_L_90
+      1.57
+    r_90: &JOINT6_R_90
+      -1.57
 
   silver_lift:
     home: &SILVER_LIFTER_HOME_POS
@@ -149,21 +167,21 @@ common:
         <<: *NORMAL_TOLERANCE
 
     lv4_exchange: &LV4_EXCHANGE
-      joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
+      joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
 
     lv5_l_exchange: &LV5_L_EXCHANGE
-      joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+      joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_L_EXCHANGE_POS, *JOINT3_L_EXCHANGE_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
 
     lv5_r_exchange: &LV5_R_EXCHANGE
-      joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+      joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90]
       common:
         <<: *NORMALLY
       tolerance:
@@ -252,117 +270,165 @@ steps_list:
 
 
 
+  OPEN_SILVER_GRIPPER:
+    - step: "open silver gripper"
+      gripper:
+        <<: *OPEN_S_GRIPPER
+
+  CLOSE_SILVER_GRIPPER:
+    - step: "close silver gripper"
+      gripper:
+        <<: *CLOSE_S_GRIPPER
 
   GET_SMALL_ISLAND:
     - step: "ready get small island silver"
       arm:
-        joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+        joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-#    - step: "open main gripper"
-
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
     - step: "get small island silver"
       arm:
-        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "pull up silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
   STORE_SILVER_TO_GRIPPER1:
+    - step: "silver rotator up"
+      silver_rotator:
+        target: 1.57
     - step: "ready to store"
       arm:
-        joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+        joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-#    - step: "open gripper1"
+    - step: "open gripper1"
+      gripper:
+        <<: *OPEN_S1
     - step: "store"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    #    - step: "close main gripper"
+    - step: "close main gripper"
+      gripper:
+        <<: *CLOSE_MAIN_GRIPPER
     - step: "add stone"
       stone_num:
         change: "+s1"
+    - step: "lift up arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
   STORE_SILVER_TO_GRIPPER2:
+    - step: "silver rotator up"
+      silver_rotator:
+        target: 1.57
     - step: "ready to store"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-    #    - step: "open gripper2"
+    - step: "open gripper2"
+      gripper:
+        <<: *OPEN_S2
     - step: "store"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-    #    - step: "close main gripper"
+    - step: "close main gripper"
+      gripper:
+        <<: *CLOSE_MAIN_GRIPPER
     - step: "add stone"
       stone_num:
         change: "+s2"
+    - step: "lift up arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
   STORE_SILVER_TO_GRIPPER3:
+    - step: "silver rotator up"
+      silver_rotator:
+        target: 1.57
     - step: "ready to store"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-    #    - step: "open gripper3"
+    - step: "open gripper3"
+      gripper:
+        <<: *OPEN_S3
     - step: "store"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-    #    - step: "close main gripper"
+    - step: "close main gripper"
+      gripper:
+        <<: *CLOSE_MAIN_GRIPPER
     - step: "add stone"
       stone_num:
         change: "+s3"
+    - step: "lift up arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -371,7 +437,7 @@ steps_list:
   GROUND_STONE:
     - step: "arm ready get ground stone"
       arm:
-        joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
+        joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
         common:
           <<: *NORMALLY
         tolerance:
@@ -379,17 +445,19 @@ steps_list:
     - step: "ready to get ground stone"
       middle_pitch:
         target: *MIDDLE_PITCH_GROUND_STONE
-#    - step: "open main gripper"
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
     - step: "arm down get ground stone"
       arm:
-        joints: [*JOINT1_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
+        joints: [*JOINT1_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "arm pull up stone"
       arm:
-        joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
+        joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
         common:
           <<: *NORMALLY
         tolerance:
@@ -403,27 +471,38 @@ steps_list:
   GET_STORED_SILVER1:
     - step: "ready to get silver from gripper1"
       arm:
-        joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+        joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-    #    - step: "open main gripper"
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
     - step: "get silver"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    #    - step: "close gripper1"
+    - step: "close gripper1"
+      gripper:
+        <<: *CLOSE_S1
     - step: "minus stone"
       stone_num:
         change: "-s1"
     - step: "lift arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "left arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -432,28 +511,37 @@ steps_list:
   GET_STORED_SILVER2:
     - step: "ready to get silver from gripper2"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-    #    - step: "open main gripper"
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
     - step: "store"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-    #    - step: "close gripper2"
+    - step: "close gripper2"
+      gripper:
+        <<: *CLOSE_S2
     - step: "minus stone"
       stone_num:
         change: "-s2"
     - step: "lift arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "left arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -462,28 +550,37 @@ steps_list:
   GET_STORED_SILVER3:
     - step: "ready to get silver from gripper3"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-    #    - step: "open main gripper"
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
     - step: "store"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-    #    - step: "close gripper3"
+    - step: "close gripper3"
+      gripper:
+        <<: *CLOSE_S3
     - step: "minus stone"
       stone_num:
         change: "-s3"
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "left arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -497,7 +594,9 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-#    - step: "open main gripper"
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
     - step: "get stored gold"
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS]
@@ -505,7 +604,9 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-#    - step: "close gold gripper
+    - step: "close gold gripper"
+      gripper:
+        <<: *CLOSE_GOLD
     - step: "add stone"
       stone_num:
         change: "+g"
@@ -516,7 +617,6 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
     - step: "lift up gold"
       arm:
         joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -531,126 +631,133 @@ steps_list:
         target: 1.57
     - step: "ready get small island silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-          #    - step: "open main gripper"
-
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
     - step: "get small island silver"
       arm:
-        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "pull up silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
     - step: "ready to store"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-          #    - step: "open gripper2"
+    - step: "open gripper2"
+      gripper:
+        <<: *OPEN_S2
     - step: "store"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-          #    - step: "close main gripper"
-
+    - step: "close main gripper"
+      gripper:
+        <<: *CLOSE_MAIN_GRIPPER
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-  ###################### 2nd silver
+
+  ############################################# 2nd silver
+
     - step: "ready get small island silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-          #    - step: "open main gripper"
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
 
     - step: "get small island silver"
       arm:
-        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "pull up silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
     - step: "ready to store"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-          #    - step: "open gripper3"
+    - step: "open gripper3"
+      gripper:
+        <<: *OPEN_S3
     - step: "store"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-          #    - step: "close main gripper"
-
+    - step: "close main gripper"
+      gripper:
+        <<: *CLOSE_MAIN_GRIPPER
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-  ############ 3rd silver
+
+  ####################################################### 3rd silver
+
     - step: "ready get small island silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-          #    - step: "open main gripper"
-
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
     - step: "get small island silver"
       arm:
-        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "pull up silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_HOME_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -666,8 +773,16 @@ steps_list:
     - step: "pusher back"
       silver_pusher:
         target: 0.
-        delay: 0.1
-#    - step: "open gripper"
+        delay: 1
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_S1
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_S2
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_S3
     - step: "get silver"
       silver_pusher:
         target: -0.046
@@ -706,10 +821,12 @@ steps_list:
     - step: "lifter down"
       gold_lifter:
         target: 0
-#    - step: "open gold gripper"
+    - step: "open gold gripper"
+      gripper:
+        <<: *OPEN_GOLD
     - step: "get gold"
       gold_pusher:
-        target: -0.51
+        target: -0.45
         delay: 0.5
     - step: "lift gold"
       gold_lifter:
@@ -719,6 +836,7 @@ steps_list:
       gold_pusher:
         target: 0
         delay: 0.5
+#    - step: "chassis left"
     - step: "lifter down"
       gold_lifter:
         target: 0

From 35c127d70148deabe8b1c00e8d9bcb9d2bb4ea01 Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Sat, 20 Jul 2024 03:26:12 +0800
Subject: [PATCH 117/147] Update parameters and step list.

---
 .../config/engineer_steps_list.yaml           | 69 ++++++++++++++++++-
 1 file changed, 67 insertions(+), 2 deletions(-)

diff --git a/engineer_middleware/config/engineer_steps_list.yaml b/engineer_middleware/config/engineer_steps_list.yaml
index 2a50cb9..92a1e58 100644
--- a/engineer_middleware/config/engineer_steps_list.yaml
+++ b/engineer_middleware/config/engineer_steps_list.yaml
@@ -42,7 +42,7 @@ common:
       bin_get_stone_left: &JOINT1_BIN_GET_STONE_LEFT
                      0.2
       bin_get_stone_right: &JOINT1_BIN_GET_STONE_RIGHT
-                            0.1131
+                            0.1198
       big_ready: &JOINT1_BIG_READY
                      0.005
       big_adjust_mid: &JOINT1_BIG_ADJUST_MID
@@ -82,7 +82,7 @@ common:
       bin_get_stone: &JOINT2_BIN_GET_STONE
                             0.13
       back_bin_position: &JOINT2_BACK_BIN_POSITION
-                             0.06651
+                             0.06451
       avoid_collision: &JOINT2_AVOID_COLLISION
                         0.1
       big_ready: &JOINT2_BIG_READY
@@ -258,6 +258,12 @@ common:
                   0.230
     small_island: &LIFTER_SMALL_ISLAND
                     0.05
+    gl_test1: &GL_TEST1
+      0.10
+    gl_test2: &GL_TEST2
+      0.15
+    gl_test3: &GL_TEST3
+      0.20
   gripper:
     open_gripper: &OPEN_GRIPPER
       pin: 0
@@ -399,6 +405,34 @@ steps_list:
     - step: "gimbal lifter tallest"
       gimbal_lifter:
         target: *TALLEST_POS
+  LEFT_EXCHANGE:
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "move arm to exchange pos"
+      arm:
+        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "gimbal lifter tallest"
+      gimbal_lifter:
+        target: *TALLEST_POS
+  RIGHT_EXCHANGE:
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "move arm to exchange pos"
+      arm:
+        joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "gimbal lifter tallest"
+      gimbal_lifter:
+        target: *TALLEST_POS
 
   ############ MID #############
 
@@ -1757,6 +1791,19 @@ steps_list:
       gimbal_lifter:
         target: *TALLEST_POS
 
+  GIMBAL_TEST1:
+    - step: "gimbal 1"
+      gimbal_lifter:
+        target: *GL_TEST1
+  GIMBAL_TEST2:
+    - step: "gimbal 2"
+      gimbal_lifter:
+        target: *GL_TEST2
+  GIMBAL_TEST3:
+    - step: "gimbal 3"
+      gimbal_lifter:
+        target: *GL_TEST3
+
   ############## ORE ROTATOR ###############
   ORE_ROTATOR_INIT:
     - step: "ore rotator init pos"
@@ -1906,3 +1953,21 @@ steps_list:
         target_frame: arm
         common:
           timeout: 2.
+
+  JOINT2_TEST_F:
+    - step: "joint2 front"
+      arm:
+        joints: ["KEEP",*JOINT2_FURTHEST_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  JOINT2_TEST_B:
+    - step: "joint2 front"
+      arm:
+        joints: [ "KEEP",*JOINT2_BACK_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE

From 60416368383617ee234806fc56e740dd8d7f6358 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 20 Jul 2024 21:22:38 +0800
Subject: [PATCH 118/147] Record.

---
 .../config/engineer2_steps_list.yaml          | 160 ++++++++++++++++--
 1 file changed, 144 insertions(+), 16 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index de2a86e..0acc4b7 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -29,7 +29,7 @@ common:
     down: &JOINT1_DOWN_POS
       0.0000001
     up: &JOINT1_UP_POS
-      0.579
+      0.545
     ready_get_ground: &JOINT1_READY_GET_GROUND_POS
       0.5
     get_ground: &JOINT1_GET_GROUND_POS
@@ -41,7 +41,7 @@ common:
     get_small: &JOINT1_GET_SMALL_POS
       0.3
     ready_get_stored: &JOINT1_READY_GET_STORED_POS
-      0.545
+      0.54
     get_stored: &JOINT1_GET_STORED_POS
       0.5
     get_stored_gold: &JOINT1_GET_STORED_GOLD_POS
@@ -108,6 +108,8 @@ common:
   joint5:
     home: &JOINT5_HOME_POS
       0.00001
+    down_45: &JOINT5_DOWN_45
+      0.77
     down_90: &JOINT5_DOWN_90
       1.57
     get_stored_gold: &JOINT5_GET_STORED_GOLD_POS
@@ -167,7 +169,7 @@ common:
         <<: *NORMAL_TOLERANCE
 
     lv4_exchange: &LV4_EXCHANGE
-      joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_HOME_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
+      joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_45, *JOINT6_HOME_POS ]
       common:
         <<: *NORMALLY
       tolerance:
@@ -187,6 +189,30 @@ common:
       tolerance:
         <<: *NORMAL_TOLERANCE
 
+  gimbal:
+    right_pos: &RIGHT_POS
+      frame: base_link
+      position: [ 0.001 , -3., 0. ]
+    left_pos: &LEFT_POS
+      frame: base_link
+      position: [ 0.001, 3., 0. ]
+    front_pos: &FRONT_POS
+      frame: base_link
+      position: [ 2., 0.001, 0. ]
+    follow_ee: &FOLLOW_EE
+      frame: link5
+      position: [ 0.001, 0.001, 0. ]
+
+  chassis_forward: &CHASSIS_FORWARD
+    frame: base_link
+    position: [ 0.1, 0. ]
+    yaw: 0.0
+    chassis_tolerance_position: 0.1
+    chassis_tolerance_angular: 0.2
+    common:
+      timeout: 2.
+
+
   gripper:
     open_main_gripper: &OPEN_MAIN_GRIPPER
       pin: 0
@@ -227,6 +253,21 @@ common:
 
 
 steps_list:
+  GIMBAL_FRONT:
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
+
+  GIMBAL_RIGHT:
+    - step: "gimbal look right"
+      gimbal:
+        <<: *RIGHT_POS
+
+  GIMBAL_FOLLOW_EE:
+    - step: "gimbal follow ee"
+      gimbal:
+        <<: *FOLLOW_EE
+
   MIDDLE_PITCH_UP:
     - step: "middld pitch up"
       middle_pitch:
@@ -310,6 +351,9 @@ steps_list:
     - step: "silver rotator up"
       silver_rotator:
         target: 1.57
+    - step: "open silver gripper"
+      gripper:
+        <<: *OPEN_S_GRIPPER
     - step: "ready to store"
       arm:
         joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
@@ -352,6 +396,9 @@ steps_list:
     - step: "silver rotator up"
       silver_rotator:
         target: 1.57
+    - step: "open silver gripper"
+      gripper:
+        <<: *OPEN_S_GRIPPER
     - step: "ready to store"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -394,6 +441,9 @@ steps_list:
     - step: "silver rotator up"
       silver_rotator:
         target: 1.57
+    - step: "open silver gripper"
+      gripper:
+        <<: *OPEN_S_GRIPPER
     - step: "ready to store"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -629,6 +679,9 @@ steps_list:
     - step: "silver rotator up"
       silver_rotator:
         target: 1.57
+    - step: "open silver gripper"
+      gripper:
+        <<: *OPEN_S_GRIPPER
     - step: "ready get small island silver"
       arm:
         joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -639,6 +692,7 @@ steps_list:
     - step: "open main gripper"
       gripper:
         <<: *OPEN_MAIN_GRIPPER
+  ARM_THREE_SILVER0:
     - step: "get small island silver"
       arm:
         joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -764,16 +818,25 @@ steps_list:
           <<: *NORMAL_TOLERANCE
 
   GRIPPER_THREE_SILVER:
+#    - step: "gimbal look back"
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_S1
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_S2
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_S3
     - step: "rotate down"
       silver_rotator:
         target: 0.0001
-    - step: "lifter down"
+    - step: "lifter ready"
       silver_lifter:
-        target: 0.
+        target: 0.07
     - step: "pusher back"
       silver_pusher:
-        target: 0.
-        delay: 1
+        target: -0.05
     - step: "open gripper"
       gripper:
         <<: *OPEN_S1
@@ -783,27 +846,41 @@ steps_list:
     - step: "open gripper"
       gripper:
         <<: *OPEN_S3
+    - step: "open silver gripper"
+      gripper:
+        <<: *OPEN_S_GRIPPER
+  GRIPPER_THREE_SILVER0:
     - step: "get silver"
       silver_pusher:
-        target: -0.046
-        delay: 0.5
+        target: -0.03
+        delay: 0.3
     - step: "pusher back little"
       silver_pusher:
-        target: -0.04
-        delay: 0.5
+        target: -0.03
+        delay: 0.1
+#    - step: "rotate up"
+#      silver_rotator:
+#        target: 0.02
+#        delay: 0.5
     - step: "lifter up"
       silver_lifter:
-        target: 0.18
-    - step: "rotate up"
-      silver_rotator:
-        target: 1.57
+        target: 0.2
         delay: 0.5
     - step: "pusher back"
       silver_pusher:
         target: 0.
+        delay: 0.1
+    - step: "rotate up"
+      silver_rotator:
+        target: 1.64
+        delay: 0.5
     - step: "lifter down"
       silver_lifter:
-        target: 0.
+        target: 0.05
+        delay: 0.15
+    - step: "lifter down"
+      silver_lifter:
+        target: 0.0
     - step: "add stone1"
       stone_num:
         change: "+s1"
@@ -813,6 +890,7 @@ steps_list:
     - step: "add stone1"
       stone_num:
         change: "+s3"
+#    - step: "gimbal look front"
 
   MID_BIG_ISLAND:
     - step: "pusher back"
@@ -824,6 +902,7 @@ steps_list:
     - step: "open gold gripper"
       gripper:
         <<: *OPEN_GOLD
+  MID_BIG_ISLAND0:
     - step: "get gold"
       gold_pusher:
         target: -0.45
@@ -843,3 +922,52 @@ steps_list:
     - step: "add stone"
       stone_num:
         change: "+g"
+  OPEN_MAIN:
+    - step: "main open"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+  CLOSE_MAIN:
+    - step: "main close"
+      gripper:
+        <<: *CLOSE_MAIN_GRIPPER
+  OPEN_S1:
+    - step: "s1 open"
+      gripper:
+        <<: *OPEN_S1
+  CLOSE_S1:
+    - step: "s1 c"
+      gripper:
+        <<: *CLOSE_S1
+  OPEN_S2:
+    - step: "s2 open"
+      gripper:
+        <<: *OPEN_S2
+
+  CLOSE_S2:
+    - step: "s2 close"
+      gripper:
+        <<: *CLOSE_S2
+  OPEN_S3:
+    - step: "s3 open"
+      gripper:
+        <<: *OPEN_S3
+  CLSOE_S3:
+    - step: "s3 close"
+      gripper:
+        <<: *CLOSE_S3
+  OPEN_G:
+    - step: "g1 open"
+      gripper:
+        <<: *OPEN_GOLD
+  CLOSE_G:
+    - step: "g1 close"
+      gripper:
+        <<: *CLOSE_GOLD
+  OPEN_SG:
+    - step: "g1 close"
+      gripper:
+        <<: *OPEN_S_GRIPPER
+  CLOSE_SG:
+    - step: "g1 close"
+      gripper:
+        <<: *CLOSE_S_GRIPPER

From ba32d30838d575c9ac6ca7bd89bd4eae99a52690 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 22 Jul 2024 21:05:01 +0800
Subject: [PATCH 119/147] Add get side gold motion.

---
 .../config/engineer2_steps_list.yaml          | 58 ++++++++++++++++---
 1 file changed, 51 insertions(+), 7 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index 0acc4b7..9242e42 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -159,6 +159,8 @@ common:
       1.57
     normal: &MIDDLE_PITCH_NORMAL
       0.0
+    highest: &MIDDLE_PITCH_HIGHEST
+      -0.1
 
   arm:
     home: &HOME
@@ -269,7 +271,7 @@ steps_list:
         <<: *FOLLOW_EE
 
   MIDDLE_PITCH_UP:
-    - step: "middld pitch up"
+    - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
   HOME:
@@ -896,25 +898,27 @@ steps_list:
     - step: "pusher back"
       gold_pusher:
         target: 0.
-    - step: "lifter down"
+    - step: "lifter ready"
       gold_lifter:
-        target: 0
+        target: 0.015
     - step: "open gold gripper"
       gripper:
         <<: *OPEN_GOLD
   MID_BIG_ISLAND0:
     - step: "get gold"
       gold_pusher:
-        target: -0.45
+        target: -0.443
         delay: 0.5
+  MID_BIG_ISLAND00:
     - step: "lift gold"
       gold_lifter:
-        target: 0.05
+        target: 0.08
         delay: 0.5
+  MID_BIG_ISLAND000:
     - step: "pull back gold"
       gold_pusher:
         target: 0
-        delay: 0.5
+        delay: 0.6
 #    - step: "chassis left"
     - step: "lifter down"
       gold_lifter:
@@ -922,6 +926,46 @@ steps_list:
     - step: "add stone"
       stone_num:
         change: "+g"
+
+  SIDE_BIG_ISLAND:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_HIGHEST
+    - step: "arm ready"
+      arm:
+        joints: [ 0.037,0.929,-1.963,-1.474,-1.434,-1.528 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "open main gripper"
+      gripper:
+          <<: *OPEN_MAIN_GRIPPER
+  SIDE_BIG_ISLAND0:
+    - step: "arm get gold"
+      arm:
+        joints: [ 0.037,0.093,-1.38,-1.474,-1.571,-1.528 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+  #SIDE_BIG_ISLAND00:
+    - step: "arm lift gold"
+      arm:
+        joints: [ 0.127,0.093,-1.38,-1.474,-1.571,-1.528 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+  #SIDE_BIG_ISLAND000:
+    - step: "arm pull back gold"
+      arm:
+        joints: [ 0.127,1.04,-1.99,-1.474,-1.321,-1.528 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
   OPEN_MAIN:
     - step: "main open"
       gripper:
@@ -951,7 +995,7 @@ steps_list:
     - step: "s3 open"
       gripper:
         <<: *OPEN_S3
-  CLSOE_S3:
+  CLOSE_S3:
     - step: "s3 close"
       gripper:
         <<: *CLOSE_S3

From 45e65eb3323dbf67350acaef5e1ba5197beb4e8d Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 22 Jul 2024 21:05:41 +0800
Subject: [PATCH 120/147] Enable rm manual in sim.launch.

---
 engineer_middleware/launch/sim.launch | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/engineer_middleware/launch/sim.launch b/engineer_middleware/launch/sim.launch
index 7eebf7e..c8057e2 100644
--- a/engineer_middleware/launch/sim.launch
+++ b/engineer_middleware/launch/sim.launch
@@ -4,7 +4,7 @@
     <include file="$(find rm_gazebo)/launch/$(arg world).launch"/>
     <include file="$(find engineer_middleware)/launch/load.launch"/>
     <include file="$(find $(arg robot_type)_arm_config)/launch/load_move_group.launch"/>
-<!--    <include file="$(find rm_config)/launch/manual.launch"/>-->
+    <include file="$(find rm_config)/launch/manual.launch"/>
     <include file="$(find $(arg robot_type)_arm_config)/launch/load_controllers.launch">
         <arg name="use_simulation" value="true"/>
     </include>

From f082714aaae50ac8879f538a7d24f58edc98b1a4 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 24 Jul 2024 05:13:26 +0800
Subject: [PATCH 121/147] Modify arm get small island and store to silver
 gripper motion.

---
 .../config/engineer2_steps_list.yaml          | 325 +++++++++---------
 1 file changed, 163 insertions(+), 162 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index 9242e42..fc7c752 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -19,17 +19,13 @@ common:
     normal_tolerance: &NORMAL_TOLERANCE
       tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.07 ]
     bigger_tolerance: &BIGGER_TOLERANCE
-      tolerance_joints: [ 0.015, 0.015, 0.02, 0.35, 0.2, 0.2, 0.1 ]
+      tolerance_joints: [ 0.015, 0.015, 0.03, 0.35, 0.2, 0.2, 0.1 ]
     max_tolerance: &MAX_TOLERANCE
       tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3]
 
   joint1:
     home: &JOINT1_HOME_POS
       0.1
-    down: &JOINT1_DOWN_POS
-      0.0000001
-    up: &JOINT1_UP_POS
-      0.545
     ready_get_ground: &JOINT1_READY_GET_GROUND_POS
       0.5
     get_ground: &JOINT1_GET_GROUND_POS
@@ -37,40 +33,50 @@ common:
     exchange: &JOINT1_EXCHANGE_POS
       0.35
     ready_get_small: &JOINT1_READY_GET_SMALL_POS
-      0.4
+      0.42
     get_small: &JOINT1_GET_SMALL_POS
       0.3
     ready_get_stored: &JOINT1_READY_GET_STORED_POS
       0.54
     get_stored: &JOINT1_GET_STORED_POS
-      0.5
+      0.478
     get_stored_gold: &JOINT1_GET_STORED_GOLD_POS
-      0.029
+      0.05
+    ready_store: &JOINT1_READY_STORE_POS
+      0.44
+    store: &JOINT1_STORE_POS
+      0.34
 
 
   joint2:
     home: &JOINT2_HOME_POS
       1.0
-    zero: &JOINT2_ZERO_POS
-      0.0000001
     exchange: &JOINT2_EXCHANGE_POS
       -1.57
     l_exchange: &JOINT2_L_EXCHANGE_POS
       -0.984
     r_exchange: &JOINT2_R_EXCHANGE_POS
       -2.156
-    get_small_island: &JOINT2_GET_SMALL_ISLAND_POS
-      0.068
-    get_stored_silver3: &JOINT2_GET_STORED_SILVER3_POS
-      -2.12
-    get_stored_silver2: &JOINT2_GET_STORED_SILVER2_POS
-      -2.967
-    get_stored_silver1: &JOINT2_GET_STORED_SILVER1_POS
-      -3.75
-    ready_get_stored_gold: &JOINT2_READY_GET_STORED_GOLD_POS
-      -1.2
+    get_small_island_l: &JOINT2_GET_SMALL_ISLAND_L_POS
+      -0.098
+    get_small_island_m: &JOINT2_GET_SMALL_ISLAND_M_POS
+                          -1.101
+    get_small_island_r: &JOINT2_GET_SMALL_ISLAND_R_POS
+                          -2.052
+    store_to_s1: &JOINT2_STORE_TO_S1_POS
+      -3.22
+    store_to_s2: &JOINT2_STORE_TO_S2_POS
+      -2.342
+    store_to_s3: &JOINT2_STORE_TO_S3_POS
+      -1.813
+    get_stored_silver3: &JOINT2_GET_STORED_S3_POS
+      -2.135
+    get_stored_silver2: &JOINT2_GET_STORED_S2_POS
+      -2.941
+    get_stored_silver1: &JOINT2_GET_STORED_S1_POS
+      -3.84
     get_stored_gold: &JOINT2_GET_STORED_GOLD_POS
-      -1.32
+      -1.062
 
   joint3:
     home: &JOINT3_HOME_POS
@@ -81,18 +87,28 @@ common:
       -1.165
     r_exchange: &JOINT3_R_EXCHANGE_POS
       1.165
-    get_small_island: &JOINT3_GET_SMALL_ISLAND_POS
-      -2.18
+    get_small_island_l: &JOINT3_GET_SMALL_ISLAND_L_POS
+                          -1.948
+    get_small_island_m: &JOINT3_GET_SMALL_ISLAND_M_POS
+                          -1.833
+    get_small_island_r: &JOINT3_GET_SMALL_ISLAND_R_POS
+                          -0.815
+    store_to_s1: &JOINT3_STORE_TO_S1_POS
+      -1.024
+    store_to_s2: &JOINT3_STORE_TO_S2_POS
+      -1.605
+    store_to_s3: &JOINT3_STORE_TO_S3_POS
+      -1.306
     get_stored_silver3: &JOINT3_GET_STORED_SILVER3_POS
-      -1.788
+      -1.512
     get_stored_silver2: &JOINT3_GET_STORED_SILVER2_POS
-      -1.483
+      -1.209
     get_stored_silver1: &JOINT3_GET_STORED_SILVER1_POS
-      0.00001
+      -0.187
     ready_get_stored_gold: &JOINT3_READY_GET_STORED_GOLD_POS
-      -1.962
+      -1.67
     get_stored_gold: &JOINT3_GET_STORED_GOLD_POS
-      -2.181
+      -1.815
 
   joint4:
     home: &JOINT4_HOME_POS
@@ -113,46 +129,27 @@ common:
     down_90: &JOINT5_DOWN_90
       1.57
     get_stored_gold: &JOINT5_GET_STORED_GOLD_POS
-      1.193
+      1.586
+    store_to_s1: &JOINT5_STORE_TO_S1_POS
+      -0.227
+    store_to_s2: &JOINT5_STORE_TO_S2_POS
+      -0.522
+    store_to_s3: &JOINT5_STORE_TO_S3_POS
+      -1.368
 
   joint6:
     home: &JOINT6_HOME_POS
       0.0
     l_90: &JOINT6_L_90
-      1.57
+      3.14
     r_90: &JOINT6_R_90
-      -1.57
-
-  silver_lift:
-    home: &SILVER_LIFTER_HOME_POS
-      0.0
-    lift: &SILVER_LIFTER_LIFT_POS
-      0.0
-
-  silver_push:
-    home: &SILVER_PUSHER_HOME_POS
-      0.0
-    get_silver: &SILVER_PUSHER_GET_SILVER_POS
-      0.0
-    pull_back_little: &SILVER_PUSHER_PULL_BACK_LITTLE_POS
-      0.0
-
-  silver_rotate:
-    home: &SILVER_ROTATOR_HOME_POS
-      0.0
-    90: &SILVER_ROTATOR_90_POS
-      1.57
-
-  gold_push:
-    home: &GOLD_PUSHER_HOME_POS
-      0.0
-    get_gold: &GOLD_PUSHER_GET_GOLD_POS
-      0.0
-  gold_lift:
-    home: &GOLD_LIFTER_HOME_POS
-      0.0
-    lift_gold: &GOLD_LIFTER_LIFT_GOLD_POS
-      0.0
+      -3.14
+    get_small3: &JOINT6_SMALL3
+      4.387
+    get_small2: &JOINT6_SMALL2
+      4.387
+    get_small1: &JOINT6_SMALL1
+#      2.7
 
   middle_pitch:
     ground_stone: &MIDDLE_PITCH_GROUND_STONE
@@ -201,6 +198,9 @@ common:
     front_pos: &FRONT_POS
       frame: base_link
       position: [ 2., 0.001, 0. ]
+    back_pos: &BACK_POS
+      frame: base_link
+      position: [ -2., 0.001, 0. ]
     follow_ee: &FOLLOW_EE
       frame: link5
       position: [ 0.001, 0.001, 0. ]
@@ -326,7 +326,7 @@ steps_list:
   GET_SMALL_ISLAND:
     - step: "ready get small island silver"
       arm:
-        joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+        joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1]
         common:
           <<: *NORMALLY
         tolerance:
@@ -336,14 +336,14 @@ steps_list:
         <<: *OPEN_MAIN_GRIPPER
     - step: "get small island silver"
       arm:
-        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "pull up silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -356,19 +356,20 @@ steps_list:
     - step: "open silver gripper"
       gripper:
         <<: *OPEN_S_GRIPPER
+
+    - step: "open gripper1"
+      gripper:
+        <<: *OPEN_S1
     - step: "ready to store"
       arm:
-        joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+        joints: [*JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S1_POS, *JOINT3_STORE_TO_S1_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S1_POS, *JOINT6_HOME_POS]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "open gripper1"
-      gripper:
-        <<: *OPEN_S1
     - step: "store"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S1_POS, *JOINT3_STORE_TO_S1_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S1_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -376,19 +377,19 @@ steps_list:
     - step: "close main gripper"
       gripper:
         <<: *CLOSE_MAIN_GRIPPER
-    - step: "add stone"
-      stone_num:
-        change: "+s1"
+#    - step: "add stone"
+#      stone_num:
+#        change: "+s1"
     - step: "lift up arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S1_POS, *JOINT3_STORE_TO_S1_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S1_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -401,19 +402,19 @@ steps_list:
     - step: "open silver gripper"
       gripper:
         <<: *OPEN_S_GRIPPER
+    - step: "open gripper2"
+      gripper:
+        <<: *OPEN_S2
     - step: "ready to store"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "open gripper2"
-      gripper:
-        <<: *OPEN_S2
     - step: "store"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -421,19 +422,19 @@ steps_list:
     - step: "close main gripper"
       gripper:
         <<: *CLOSE_MAIN_GRIPPER
-    - step: "add stone"
-      stone_num:
-        change: "+s2"
+#    - step: "add stone"
+#      stone_num:
+#        change: "+s2"
     - step: "lift up arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -446,41 +447,39 @@ steps_list:
     - step: "open silver gripper"
       gripper:
         <<: *OPEN_S_GRIPPER
+    - step: "open gripper3"
+      gripper:
+        <<: *OPEN_S3
     - step: "ready to store"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
-    - step: "open gripper3"
-      gripper:
-        <<: *OPEN_S3
     - step: "store"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-
     - step: "close main gripper"
       gripper:
         <<: *CLOSE_MAIN_GRIPPER
-    - step: "add stone"
-      stone_num:
-        change: "+s3"
+#    - step: "add stone"
+#      stone_num:
+#        change: "+s3"
     - step: "lift up arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -523,7 +522,7 @@ steps_list:
   GET_STORED_SILVER1:
     - step: "ready to get silver from gripper1"
       arm:
-        joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+        joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
         common:
           <<: *NORMALLY
         tolerance:
@@ -534,7 +533,7 @@ steps_list:
         <<: *OPEN_MAIN_GRIPPER
     - step: "get silver"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -542,19 +541,19 @@ steps_list:
     - step: "close gripper1"
       gripper:
         <<: *CLOSE_S1
-    - step: "minus stone"
-      stone_num:
-        change: "-s1"
+#    - step: "minus stone"
+#      stone_num:
+#        change: "-s1"
     - step: "lift arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -563,7 +562,7 @@ steps_list:
   GET_STORED_SILVER2:
     - step: "ready to get silver from gripper2"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -573,7 +572,7 @@ steps_list:
         <<: *OPEN_MAIN_GRIPPER
     - step: "store"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -581,28 +580,35 @@ steps_list:
     - step: "close gripper2"
       gripper:
         <<: *CLOSE_S2
-    - step: "minus stone"
-      stone_num:
-        change: "-s2"
+#    - step: "minus stone"
+#      stone_num:
+#        change: "-s2"
     - step: "lift arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
   GET_STORED_SILVER3:
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "ready to get silver from gripper3"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -610,9 +616,9 @@ steps_list:
     - step: "open main gripper"
       gripper:
         <<: *OPEN_MAIN_GRIPPER
-    - step: "store"
+    - step: "get ore"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -620,19 +626,19 @@ steps_list:
     - step: "close gripper3"
       gripper:
         <<: *CLOSE_S3
-    - step: "minus stone"
-      stone_num:
-        change: "-s3"
+#    - step: "minus stone"
+#      stone_num:
+#        change: "-s3"
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.561, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -655,20 +661,20 @@ steps_list:
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "close gold gripper"
       gripper:
         <<: *CLOSE_GOLD
-    - step: "add stone"
-      stone_num:
-        change: "+g"
+#    - step: "add stone"
+#      stone_num:
+#        change: "+g"
     - step: "move out gold"
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "lift up gold"
       arm:
         joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -684,9 +690,9 @@ steps_list:
     - step: "open silver gripper"
       gripper:
         <<: *OPEN_S_GRIPPER
-    - step: "ready get small island silver"
+    - step: "ready get small island L silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -697,31 +703,28 @@ steps_list:
   ARM_THREE_SILVER0:
     - step: "get small island silver"
       arm:
-        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "pull up silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ready to store"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "open gripper2"
-      gripper:
-        <<: *OPEN_S2
     - step: "store"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -729,9 +732,12 @@ steps_list:
     - step: "close main gripper"
       gripper:
         <<: *CLOSE_MAIN_GRIPPER
-    - step: "left arm"
+    #    - step: "add stone"
+    #      stone_num:
+    #        change: "+s2"
+    - step: "lift up arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -739,9 +745,9 @@ steps_list:
 
   ############################################# 2nd silver
 
-    - step: "ready get small island silver"
+    - step: "ready get small island M silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_M_POS, *JOINT3_GET_SMALL_ISLAND_M_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL2 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -752,31 +758,28 @@ steps_list:
 
     - step: "get small island silver"
       arm:
-        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_M_POS, *JOINT3_GET_SMALL_ISLAND_M_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL2  ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "pull up silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_M_POS, *JOINT3_GET_SMALL_ISLAND_M_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL2  ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ready to store"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "open gripper3"
-      gripper:
-        <<: *OPEN_S3
     - step: "store"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -784,9 +787,12 @@ steps_list:
     - step: "close main gripper"
       gripper:
         <<: *CLOSE_MAIN_GRIPPER
-    - step: "left arm"
+    #    - step: "add stone"
+    #      stone_num:
+    #        change: "+s3"
+    - step: "lift up arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_SILVER3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -796,7 +802,7 @@ steps_list:
 
     - step: "ready get small island silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -806,14 +812,14 @@ steps_list:
         <<: *OPEN_MAIN_GRIPPER
     - step: "get small island silver"
       arm:
-        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "pull up silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_POS, *JOINT3_GET_SMALL_ISLAND_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -838,7 +844,7 @@ steps_list:
         target: 0.07
     - step: "pusher back"
       silver_pusher:
-        target: -0.05
+        target: -0.00001
     - step: "open gripper"
       gripper:
         <<: *OPEN_S1
@@ -855,11 +861,11 @@ steps_list:
     - step: "get silver"
       silver_pusher:
         target: -0.03
-        delay: 0.3
+        delay: 0.5
     - step: "pusher back little"
       silver_pusher:
-        target: -0.03
-        delay: 0.1
+        target: -0.015
+        delay: 0.2
 #    - step: "rotate up"
 #      silver_rotator:
 #        target: 0.02
@@ -900,7 +906,7 @@ steps_list:
         target: 0.
     - step: "lifter ready"
       gold_lifter:
-        target: 0.015
+        target: 0.00001
     - step: "open gold gripper"
       gripper:
         <<: *OPEN_GOLD
@@ -909,20 +915,15 @@ steps_list:
       gold_pusher:
         target: -0.443
         delay: 0.5
-  MID_BIG_ISLAND00:
     - step: "lift gold"
       gold_lifter:
-        target: 0.08
+        target: 0.06
         delay: 0.5
-  MID_BIG_ISLAND000:
     - step: "pull back gold"
       gold_pusher:
-        target: 0
+        target: -0.054
         delay: 0.6
 #    - step: "chassis left"
-    - step: "lifter down"
-      gold_lifter:
-        target: 0
     - step: "add stone"
       stone_num:
         change: "+g"

From 7c3ab8c041f516d6fe39a5bba843d94b3d158dd7 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 24 Jul 2024 05:22:13 +0800
Subject: [PATCH 122/147] Uncomment necessary data.

---
 engineer_middleware/config/engineer2_steps_list.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index fc7c752..b0acfa2 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -149,7 +149,7 @@ common:
     get_small2: &JOINT6_SMALL2
       4.387
     get_small1: &JOINT6_SMALL1
-#      2.7
+      2.7
 
   middle_pitch:
     ground_stone: &MIDDLE_PITCH_GROUND_STONE

From aaff087b0a85522e6ff1e57438110095e44f09ee Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 25 Jul 2024 06:36:35 +0800
Subject: [PATCH 123/147] Rewrite major motion.

---
 .../config/engineer2_steps_list.yaml          | 872 ++++++++++++++++--
 1 file changed, 776 insertions(+), 96 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index b0acfa2..4266efc 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -66,15 +66,15 @@ common:
     store_to_s1: &JOINT2_STORE_TO_S1_POS
       -3.22
     store_to_s2: &JOINT2_STORE_TO_S2_POS
-      -2.342
+      -2.364
     store_to_s3: &JOINT2_STORE_TO_S3_POS
-      -1.813
+      -1.763
     get_stored_silver3: &JOINT2_GET_STORED_S3_POS
-      -2.135
+      -2.055
     get_stored_silver2: &JOINT2_GET_STORED_S2_POS
-      -2.941
+      -2.802
     get_stored_silver1: &JOINT2_GET_STORED_S1_POS
-      -3.84
+      -3.833
     get_stored_gold: &JOINT2_GET_STORED_GOLD_POS
       -1.062
 
@@ -96,19 +96,19 @@ common:
     store_to_s1: &JOINT3_STORE_TO_S1_POS
       -1.024
     store_to_s2: &JOINT3_STORE_TO_S2_POS
-      -1.605
+      -1.723
     store_to_s3: &JOINT3_STORE_TO_S3_POS
-      -1.306
+      -1.467
     get_stored_silver3: &JOINT3_GET_STORED_SILVER3_POS
-      -1.512
+      -1.741
     get_stored_silver2: &JOINT3_GET_STORED_SILVER2_POS
-      -1.209
+      -1.57
     get_stored_silver1: &JOINT3_GET_STORED_SILVER1_POS
-      -0.187
+      -0.307
     ready_get_stored_gold: &JOINT3_READY_GET_STORED_GOLD_POS
       -1.67
     get_stored_gold: &JOINT3_GET_STORED_GOLD_POS
-      -1.815
+      -1.92
 
   joint4:
     home: &JOINT4_HOME_POS
@@ -131,25 +131,31 @@ common:
     get_stored_gold: &JOINT5_GET_STORED_GOLD_POS
       1.586
     store_to_s1: &JOINT5_STORE_TO_S1_POS
-      -0.227
+      -0.215
     store_to_s2: &JOINT5_STORE_TO_S2_POS
-      -0.522
+      -0.314
     store_to_s3: &JOINT5_STORE_TO_S3_POS
-      -1.368
+      -1.153
 
   joint6:
     home: &JOINT6_HOME_POS
       0.0
     l_90: &JOINT6_L_90
-      3.14
+      1.57
     r_90: &JOINT6_R_90
-      -3.14
+      -1.57
     get_small3: &JOINT6_SMALL3
       4.387
     get_small2: &JOINT6_SMALL2
       4.387
     get_small1: &JOINT6_SMALL1
       2.7
+    get_s3: &JOINT6_GET_S3
+      -0.9
+    get_s2: &JOINT6_GET_S2
+      -0.346
+    get_s1: &JOINT6_GET_S1
+      -0.65
 
   middle_pitch:
     ground_stone: &MIDDLE_PITCH_GROUND_STONE
@@ -191,19 +197,19 @@ common:
   gimbal:
     right_pos: &RIGHT_POS
       frame: base_link
-      position: [ 0.001 , -3., 0. ]
+      position: [ 0.00 , -3., 0. ]
     left_pos: &LEFT_POS
       frame: base_link
       position: [ 0.001, 3., 0. ]
     front_pos: &FRONT_POS
       frame: base_link
-      position: [ 2., 0.001, 0. ]
+      position: [ 3., -0.3, 0. ]
     back_pos: &BACK_POS
       frame: base_link
-      position: [ -2., 0.001, 0. ]
+      position: [ -3., -0.3, 0. ]
     follow_ee: &FOLLOW_EE
       frame: link5
-      position: [ 0.001, 0.001, 0. ]
+      position: [ 0.00, 0.00, 0. ]
 
   chassis_forward: &CHASSIS_FORWARD
     frame: base_link
@@ -214,6 +220,15 @@ common:
     common:
       timeout: 2.
 
+  chassis_fmove_left: &CHASSIS_MOVE_LEFT
+    frame: base_link
+    position: [ 0.0, 0.2 ]
+    yaw: 0.0
+    chassis_tolerance_position: 0.1
+    chassis_tolerance_angular: 0.2
+    common:
+      timeout: 2.
+
 
   gripper:
     open_main_gripper: &OPEN_MAIN_GRIPPER
@@ -255,17 +270,22 @@ common:
 
 
 steps_list:
-  GIMBAL_FRONT:
+  GIMBAL_F:
     - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
 
-  GIMBAL_RIGHT:
+  GIMBAL_R:
     - step: "gimbal look right"
       gimbal:
         <<: *RIGHT_POS
 
-  GIMBAL_FOLLOW_EE:
+  GIMBAL_B:
+    - step: "gimbal look back"
+      gimbal:
+        <<: *BACK_POS
+
+  GIMBAL_EE:
     - step: "gimbal follow ee"
       gimbal:
         <<: *FOLLOW_EE
@@ -275,6 +295,27 @@ steps_list:
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
   HOME:
+    - step: "pusher back"
+      silver_pusher:
+        target: 0.
+    - step: "rotate up"
+      silver_rotator:
+        target: 0.
+    - step: "lifter down"
+      silver_lifter:
+        target: 0.0
+    - step: "get gold"
+      gold_pusher:
+        target: 0.
+    - step: "lift gold"
+      gold_lifter:
+        target: 0.
+    - step: "gimbal look front"
+      gimbal:
+        <<: *RIGHT_POS
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
     - step: "arm home pos"
       arm:
         joints: [*JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
@@ -284,6 +325,9 @@ steps_list:
           <<: *NORMAL_TOLERANCE
 
   HOME_WITH_PITCH:
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
     - step: "arm home pos"
       arm:
         joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
@@ -299,17 +343,24 @@ steps_list:
     - step: "arm enter exchange pos"
       arm:
         <<: *LV4_EXCHANGE
-#    - step: "gimbal ready"
-
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE
   LV5_L_EXCHANGE:
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_L_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE
 
   LV5_R_EXCHANGE:
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_R_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE
 
 
 
@@ -324,6 +375,9 @@ steps_list:
         <<: *CLOSE_S_GRIPPER
 
   GET_SMALL_ISLAND:
+    - step: "gimbal look right"
+      gimbal:
+        <<: *RIGHT_POS
     - step: "ready get small island silver"
       arm:
         joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1]
@@ -377,9 +431,9 @@ steps_list:
     - step: "close main gripper"
       gripper:
         <<: *CLOSE_MAIN_GRIPPER
-#    - step: "add stone"
-#      stone_num:
-#        change: "+s1"
+    - step: "add stone"
+      stone_num:
+        change: "+s1"
     - step: "lift up arm"
       arm:
         joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S1_POS, *JOINT3_STORE_TO_S1_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S1_POS, *JOINT6_HOME_POS ]
@@ -422,9 +476,9 @@ steps_list:
     - step: "close main gripper"
       gripper:
         <<: *CLOSE_MAIN_GRIPPER
-#    - step: "add stone"
-#      stone_num:
-#        change: "+s2"
+    - step: "add stone"
+      stone_num:
+        change: "+s2"
     - step: "lift up arm"
       arm:
         joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ]
@@ -467,9 +521,9 @@ steps_list:
     - step: "close main gripper"
       gripper:
         <<: *CLOSE_MAIN_GRIPPER
-#    - step: "add stone"
-#      stone_num:
-#        change: "+s3"
+    - step: "add stone"
+      stone_num:
+        change: "+s3"
     - step: "lift up arm"
       arm:
         joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ]
@@ -522,7 +576,7 @@ steps_list:
   GET_STORED_SILVER1:
     - step: "ready to get silver from gripper1"
       arm:
-        joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+        joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1]
         common:
           <<: *NORMALLY
         tolerance:
@@ -533,7 +587,7 @@ steps_list:
         <<: *OPEN_MAIN_GRIPPER
     - step: "get silver"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -541,12 +595,12 @@ steps_list:
     - step: "close gripper1"
       gripper:
         <<: *CLOSE_S1
-#    - step: "minus stone"
-#      stone_num:
-#        change: "-s1"
+    - step: "minus stone"
+      stone_num:
+        change: "-s1"
     - step: "lift arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -559,10 +613,10 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-  GET_STORED_SILVER2:
+  GET_STORED_SILVER2: &GET_STORED_S2
     - step: "ready to get silver from gripper2"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -572,7 +626,7 @@ steps_list:
         <<: *OPEN_MAIN_GRIPPER
     - step: "store"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -580,35 +634,35 @@ steps_list:
     - step: "close gripper2"
       gripper:
         <<: *CLOSE_S2
-#    - step: "minus stone"
-#      stone_num:
-#        change: "-s2"
+    - step: "minus stone"
+      stone_num:
+        change: "-s2"
     - step: "lift arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
-  GET_STORED_SILVER3:
+  GET_STORED_SILVER3: &GET_STORED_S3
     - step: "lift arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ready to get silver from gripper3"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -618,7 +672,7 @@ steps_list:
         <<: *OPEN_MAIN_GRIPPER
     - step: "get ore"
       arm:
-        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -626,19 +680,19 @@ steps_list:
     - step: "close gripper3"
       gripper:
         <<: *CLOSE_S3
-#    - step: "minus stone"
-#      stone_num:
-#        change: "-s3"
+    - step: "minus stone"
+      stone_num:
+        change: "-s3"
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "left arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -665,9 +719,9 @@ steps_list:
     - step: "close gold gripper"
       gripper:
         <<: *CLOSE_GOLD
-#    - step: "add stone"
-#      stone_num:
-#        change: "+g"
+    - step: "add stone"
+      stone_num:
+        change: "+g"
     - step: "move out gold"
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -684,6 +738,9 @@ steps_list:
           <<: *NORMAL_TOLERANCE
 
   ARM_THREE_SILVER:
+    - step: "gimbal look right"
+      gimbal:
+        <<: *RIGHT_POS
     - step: "silver rotator up"
       silver_rotator:
         target: 1.57
@@ -692,25 +749,28 @@ steps_list:
         <<: *OPEN_S_GRIPPER
     - step: "ready get small island L silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "open s2"
+      gripper:
+        <<: *OPEN_S2
     - step: "open main gripper"
       gripper:
         <<: *OPEN_MAIN_GRIPPER
   ARM_THREE_SILVER0:
     - step: "get small island silver"
       arm:
-        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ]
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "pull up silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -732,9 +792,9 @@ steps_list:
     - step: "close main gripper"
       gripper:
         <<: *CLOSE_MAIN_GRIPPER
-    #    - step: "add stone"
-    #      stone_num:
-    #        change: "+s2"
+    - step: "add stone"
+      stone_num:
+        change: "+s2"
     - step: "lift up arm"
       arm:
         joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ]
@@ -784,12 +844,15 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "open s3"
+      gripper:
+        <<: *OPEN_S3
     - step: "close main gripper"
       gripper:
         <<: *CLOSE_MAIN_GRIPPER
-    #    - step: "add stone"
-    #      stone_num:
-    #        change: "+s3"
+    - step: "add stone"
+      stone_num:
+        change: "+s3"
     - step: "lift up arm"
       arm:
         joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ]
@@ -802,7 +865,7 @@ steps_list:
 
     - step: "ready get small island silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -812,21 +875,39 @@ steps_list:
         <<: *OPEN_MAIN_GRIPPER
     - step: "get small island silver"
       arm:
-        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ]
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "pull up silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ]
+        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
   GRIPPER_THREE_SILVER:
-#    - step: "gimbal look back"
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_S1
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_S2
+    - step: "open gripper"
+      gripper:
+        <<: *OPEN_S3
+    - step: "lift gimbal up"
+      arm:
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_HOME_POS, *JOINT3_ZERO_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "gimbal look right"
+      gimbal:
+        <<: *RIGHT_POS
     - step: "open gripper"
       gripper:
         <<: *OPEN_S1
@@ -857,6 +938,9 @@ steps_list:
     - step: "open silver gripper"
       gripper:
         <<: *OPEN_S_GRIPPER
+    - step: "gimbal look back"
+      gimbal:
+        <<: *BACK_POS
   GRIPPER_THREE_SILVER0:
     - step: "get silver"
       silver_pusher:
@@ -866,10 +950,6 @@ steps_list:
       silver_pusher:
         target: -0.015
         delay: 0.2
-#    - step: "rotate up"
-#      silver_rotator:
-#        target: 0.02
-#        delay: 0.5
     - step: "lifter up"
       silver_lifter:
         target: 0.2
@@ -898,9 +978,24 @@ steps_list:
     - step: "add stone1"
       stone_num:
         change: "+s3"
-#    - step: "gimbal look front"
+    - step: "gimbal look right"
+      gimbal:
+        <<: *RIGHT_POS
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "arm home pos"
+      arm:
+        joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
 
   MID_BIG_ISLAND:
+    - step: "gimbal look right"
+      gimbal:
+        <<: *RIGHT_POS
     - step: "pusher back"
       gold_pusher:
         target: 0.
@@ -915,6 +1010,9 @@ steps_list:
       gold_pusher:
         target: -0.443
         delay: 0.5
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
     - step: "lift gold"
       gold_lifter:
         target: 0.06
@@ -923,12 +1021,18 @@ steps_list:
       gold_pusher:
         target: -0.054
         delay: 0.6
-#    - step: "chassis left"
+    - step: "chassis left"
+      chassis:
+        <<: *CHASSIS_MOVE_LEFT
     - step: "add stone"
       stone_num:
         change: "+g"
 
+
   SIDE_BIG_ISLAND:
+    - step: "gimbal look right"
+      gimbal:
+        <<: *RIGHT_POS
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_HIGHEST
@@ -950,7 +1054,6 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-  #SIDE_BIG_ISLAND00:
     - step: "arm lift gold"
       arm:
         joints: [ 0.127,0.093,-1.38,-1.474,-1.571,-1.528 ]
@@ -958,7 +1061,6 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-  #SIDE_BIG_ISLAND000:
     - step: "arm pull back gold"
       arm:
         joints: [ 0.127,1.04,-1.99,-1.474,-1.321,-1.528 ]
@@ -966,53 +1068,631 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
+
+  BOTH_BIG_ISLAND:
+    - step: "gimbal look right"
+      gimbal:
+        <<: *RIGHT_POS
+    - step: "gimbal look right"
+      gimbal:
+        <<: *RIGHT_POS
+    - step: "pusher back"
+      gold_pusher:
+        target: 0.
+    - step: "lifter ready"
+      gold_lifter:
+        target: 0.00001
+    - step: "open gold gripper"
+      gripper:
+        <<: *OPEN_GOLD
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_HIGHEST
+    - step: "arm ready"
+      arm:
+        joints: [ 0.037,0.929,-1.963,-1.474,-1.434,-1.528 ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+  BOTH_BIG_ISLAND0:
+    - step: "get gold"
+      gold_pusher:
+        target: -0.443
+        delay: 0.3
+    - step: "lift gold"
+      gold_lifter:
+        target: 0.06
+    - step: "arm get gold"
+      arm:
+        joints: [ 0.037,0.093,-1.38,-1.474,-1.571,-1.528 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "arm lift gold"
+      arm:
+        joints: [ 0.127,0.093,-1.38,-1.474,-1.571,-1.528 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "pull back gold"
+      gold_pusher:
+        target: -0.054
+    - step: "add stone"
+      stone_num:
+        change: "+g"
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "arm pull back gold"
+      arm:
+        joints: [ 0.127,1.04,-1.99,-1.474,-1.321,-1.528 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "chassis left"
+      chassis:
+        <<: *CHASSIS_MOVE_LEFT
 
-  OPEN_MAIN:
+  OM:
     - step: "main open"
       gripper:
         <<: *OPEN_MAIN_GRIPPER
-  CLOSE_MAIN:
+  CM:
     - step: "main close"
       gripper:
         <<: *CLOSE_MAIN_GRIPPER
-  OPEN_S1:
+  OS1:
     - step: "s1 open"
       gripper:
         <<: *OPEN_S1
-  CLOSE_S1:
-    - step: "s1 c"
+  CS1:
+    - step: "s1 close"
       gripper:
         <<: *CLOSE_S1
-  OPEN_S2:
+  OS2:
     - step: "s2 open"
       gripper:
         <<: *OPEN_S2
-
-  CLOSE_S2:
+  CS2:
     - step: "s2 close"
       gripper:
         <<: *CLOSE_S2
-  OPEN_S3:
+  OS3:
     - step: "s3 open"
       gripper:
         <<: *OPEN_S3
-  CLOSE_S3:
+  CS3:
     - step: "s3 close"
       gripper:
         <<: *CLOSE_S3
-  OPEN_G:
+  OG:
     - step: "g1 open"
       gripper:
         <<: *OPEN_GOLD
-  CLOSE_G:
+  CG:
     - step: "g1 close"
       gripper:
         <<: *CLOSE_GOLD
-  OPEN_SG:
+  OSG:
     - step: "g1 close"
       gripper:
         <<: *OPEN_S_GRIPPER
-  CLOSE_SG:
-    - step: "g1 close"
+  CSG:
+    - step: "close silver gripper"
+      gripper:
+        <<: *CLOSE_S_GRIPPER
+
+  LV5_L_S1:
+    - step: "ready to get silver from gripper1"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+    - step: "get silver"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "close gripper1"
+      gripper:
+        <<: *CLOSE_S1
+    - step: "minus stone"
+      stone_num:
+        change: "-s1"
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "close silver gripper"
       gripper:
         <<: *CLOSE_S_GRIPPER
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV5_L_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE
+
+
+
+  LV5_L_S2:
+    - step: "ready to get silver from gripper2"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+    - step: "store"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "close gripper2"
+      gripper:
+        <<: *CLOSE_S2
+    - step: "minus stone"
+      stone_num:
+        change: "-s2"
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV5_L_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE
+
+
+
+  LV5_L_S3:
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ready to get silver from gripper3"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+    - step: "get ore"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "close gripper3"
+      gripper:
+        <<: *CLOSE_S3
+    - step: "minus stone"
+      stone_num:
+        change: "-s3"
+    - step: "left arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV5_L_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE
+  LV5_L_G:
+    - step: "ready to get stored gold"
+      arm:
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+    - step: "get stored gold"
+      arm:
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "close gold gripper"
+      gripper:
+        <<: *CLOSE_GOLD
+    - step: "add stone"
+      stone_num:
+        change: "-g"
+    - step: "move out gold"
+      arm:
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "lift up gold"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV5_L_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE
+  LV5_R_S1:
+    - step: "ready to get silver from gripper1"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+    - step: "get silver"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "close gripper1"
+      gripper:
+        <<: *CLOSE_S1
+    - step: "minus stone"
+      stone_num:
+        change: "-s1"
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "close silver gripper"
+      gripper:
+        <<: *CLOSE_S_GRIPPER
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV5_R_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE
+  LV5_R_S2:
+    - step: "ready to get silver from gripper2"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+    - step: "store"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "close gripper2"
+      gripper:
+        <<: *CLOSE_S2
+    - step: "minus stone"
+      stone_num:
+        change: "-s2"
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV5_R_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE
+  LV5_R_S3:
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ready to get silver from gripper3"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+    - step: "get ore"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "close gripper3"
+      gripper:
+        <<: *CLOSE_S3
+    - step: "minus stone"
+      stone_num:
+        change: "-s3"
+    - step: "left arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV5_R_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE
+  LV5_R_G:
+    - step: "ready to get stored gold"
+      arm:
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+    - step: "get stored gold"
+      arm:
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "close gold gripper"
+      gripper:
+        <<: *CLOSE_GOLD
+    - step: "add stone"
+      stone_num:
+        change: "-g"
+    - step: "move out gold"
+      arm:
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "lift up gold"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV5_R_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE
+  LV4_S1:
+    - step: "ready to get silver from gripper1"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+    - step: "get silver"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "close gripper1"
+      gripper:
+        <<: *CLOSE_S1
+    - step: "minus stone"
+      stone_num:
+        change: "-s1"
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "close silver gripper"
+      gripper:
+        <<: *CLOSE_S_GRIPPER
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV4_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE
+  LV4_S2:
+    - step: "ready to get silver from gripper2"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+    - step: "store"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "close gripper2"
+      gripper:
+        <<: *CLOSE_S2
+    - step: "minus stone"
+      stone_num:
+        change: "-s2"
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV4_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE
+  LV4_S3:
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "ready to get silver from gripper3"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+    - step: "get ore"
+      arm:
+        joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "close gripper3"
+      gripper:
+        <<: *CLOSE_S3
+    - step: "minus stone"
+      stone_num:
+        change: "-s3"
+    - step: "left arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV4_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE
+  LV4_G:
+    - step: "ready to get stored gold"
+      arm:
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+    - step: "get stored gold"
+      arm:
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "close gold gripper"
+      gripper:
+        <<: *CLOSE_GOLD
+    - step: "add stone"
+      stone_num:
+        change: "-g"
+    - step: "move out gold"
+      arm:
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "lift up gold"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV4_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FOLLOW_EE

From 2f41fc8cf25ca367cbe8e578847022c03e1d7391 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 25 Jul 2024 23:01:37 +0800
Subject: [PATCH 124/147] Add middle pitch motion before arm move.

---
 .../config/engineer2_steps_list.yaml          | 137 ++++++++++++------
 1 file changed, 93 insertions(+), 44 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index 4266efc..ae78c86 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -27,7 +27,7 @@ common:
     home: &JOINT1_HOME_POS
       0.1
     ready_get_ground: &JOINT1_READY_GET_GROUND_POS
-      0.5
+      0.005
     get_ground: &JOINT1_GET_GROUND_POS
       0.4
     exchange: &JOINT1_EXCHANGE_POS
@@ -51,6 +51,8 @@ common:
   joint2:
     home: &JOINT2_HOME_POS
       1.0
+    avoid_controller: &JOINT2_AVOID_CONTROLLER_POS
+      0.6
     exchange: &JOINT2_EXCHANGE_POS
       -1.57
     l_exchange: &JOINT2_L_EXCHANGE_POS
@@ -159,7 +161,7 @@ common:
 
   middle_pitch:
     ground_stone: &MIDDLE_PITCH_GROUND_STONE
-      1.57
+      1.3
     normal: &MIDDLE_PITCH_NORMAL
       0.0
     highest: &MIDDLE_PITCH_HIGHEST
@@ -208,8 +210,8 @@ common:
       frame: base_link
       position: [ -3., -0.3, 0. ]
     follow_ee: &FOLLOW_EE
-      frame: link5
-      position: [ 0.00, 0.00, 0. ]
+      frame: link6
+      position: [ 0.15, 0.00, 0. ]
 
   chassis_forward: &CHASSIS_FORWARD
     frame: base_link
@@ -298,9 +300,6 @@ steps_list:
     - step: "pusher back"
       silver_pusher:
         target: 0.
-    - step: "rotate up"
-      silver_rotator:
-        target: 0.
     - step: "lifter down"
       silver_lifter:
         target: 0.0
@@ -318,11 +317,10 @@ steps_list:
         <<: *FRONT_POS
     - step: "arm home pos"
       arm:
-        joints: [*JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
+        <<: *HOME
+    - step: "middle pitch down"
+      middle_pitch:
+        target: *MIDDLE_PITCH_GROUND_STONE
 
   HOME_WITH_PITCH:
     - step: "gimbal look front"
@@ -330,16 +328,15 @@ steps_list:
         <<: *FRONT_POS
     - step: "arm home pos"
       arm:
-        joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
-    - step: "middle pitch up"
+        <<: *HOME
+    - step: "middle pitch down"
       middle_pitch:
-        target: *MIDDLE_PITCH_NORMAL
+        target: *MIDDLE_PITCH_GROUND_STONE
 
   LV4_EXCHANGE:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "arm enter exchange pos"
       arm:
         <<: *LV4_EXCHANGE
@@ -347,6 +344,9 @@ steps_list:
       gimbal:
         <<: *FOLLOW_EE
   LV5_L_EXCHANGE:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_L_EXCHANGE
@@ -355,6 +355,9 @@ steps_list:
         <<: *FOLLOW_EE
 
   LV5_R_EXCHANGE:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_R_EXCHANGE
@@ -362,19 +365,10 @@ steps_list:
       gimbal:
         <<: *FOLLOW_EE
 
-
-
-  OPEN_SILVER_GRIPPER:
-    - step: "open silver gripper"
-      gripper:
-        <<: *OPEN_S_GRIPPER
-
-  CLOSE_SILVER_GRIPPER:
-    - step: "close silver gripper"
-      gripper:
-        <<: *CLOSE_S_GRIPPER
-
   GET_SMALL_ISLAND:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "gimbal look right"
       gimbal:
         <<: *RIGHT_POS
@@ -542,24 +536,28 @@ steps_list:
   GROUND_STONE:
     - step: "arm ready get ground stone"
       arm:
-        joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
+        joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_HOME_POS, *JOINT3_ZERO_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "ready to get ground stone"
-      middle_pitch:
-        target: *MIDDLE_PITCH_GROUND_STONE
-    - step: "open main gripper"
-      gripper:
-        <<: *OPEN_MAIN_GRIPPER
-    - step: "arm down get ground stone"
+    - step: "gimbal front"
+      gimbal:
+        <<: *FRONT_POS
+  GROUND_STONE0:
+    - step: "move arm to get ground stone"
       arm:
-        joints: [*JOINT1_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
+        joints: [ *JOINT1_READY_GET_GROUND_POS, *JOINT2_HOME_POS, *JOINT3_ZERO_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+    - step: "ready to get ground stone"
+      middle_pitch:
+        target: *MIDDLE_PITCH_GROUND_STONE
     - step: "arm pull up stone"
       arm:
         joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
@@ -567,13 +565,13 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "middle pitch home"
-      middle_pitch:
-        target: *MIDDLE_PITCH_NORMAL
 
 
 
   GET_STORED_SILVER1:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "ready to get silver from gripper1"
       arm:
         joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1]
@@ -614,6 +612,9 @@ steps_list:
           <<: *NORMAL_TOLERANCE
 
   GET_STORED_SILVER2: &GET_STORED_S2
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "ready to get silver from gripper2"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
@@ -653,6 +654,9 @@ steps_list:
           <<: *NORMAL_TOLERANCE
 
   GET_STORED_SILVER3: &GET_STORED_S3
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
@@ -738,6 +742,9 @@ steps_list:
           <<: *NORMAL_TOLERANCE
 
   ARM_THREE_SILVER:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "gimbal look right"
       gimbal:
         <<: *RIGHT_POS
@@ -900,7 +907,7 @@ steps_list:
         <<: *OPEN_S3
     - step: "lift gimbal up"
       arm:
-        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_HOME_POS, *JOINT3_ZERO_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_AVOID_CONTROLLER_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
         common:
           <<: *QUICKLY
         tolerance:
@@ -1030,6 +1037,9 @@ steps_list:
 
 
   SIDE_BIG_ISLAND:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "gimbal look right"
       gimbal:
         <<: *RIGHT_POS
@@ -1073,6 +1083,9 @@ steps_list:
         <<: *FRONT_POS
 
   BOTH_BIG_ISLAND:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "gimbal look right"
       gimbal:
         <<: *RIGHT_POS
@@ -1193,6 +1206,9 @@ steps_list:
         <<: *CLOSE_S_GRIPPER
 
   LV5_L_S1:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "ready to get silver from gripper1"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
@@ -1237,6 +1253,9 @@ steps_list:
 
 
   LV5_L_S2:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "ready to get silver from gripper2"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
@@ -1277,6 +1296,9 @@ steps_list:
 
 
   LV5_L_S3:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1321,6 +1343,9 @@ steps_list:
       gimbal:
         <<: *FOLLOW_EE
   LV5_L_G:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "ready to get stored gold"
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1365,6 +1390,9 @@ steps_list:
       gimbal:
         <<: *FOLLOW_EE
   LV5_R_S1:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "ready to get silver from gripper1"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
@@ -1406,6 +1434,9 @@ steps_list:
       gimbal:
         <<: *FOLLOW_EE
   LV5_R_S2:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "ready to get silver from gripper2"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
@@ -1443,6 +1474,9 @@ steps_list:
       gimbal:
         <<: *FOLLOW_EE
   LV5_R_S3:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1487,6 +1521,9 @@ steps_list:
       gimbal:
         <<: *FOLLOW_EE
   LV5_R_G:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "ready to get stored gold"
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1531,6 +1568,9 @@ steps_list:
       gimbal:
         <<: *FOLLOW_EE
   LV4_S1:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "ready to get silver from gripper1"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
@@ -1572,6 +1612,9 @@ steps_list:
       gimbal:
         <<: *FOLLOW_EE
   LV4_S2:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "ready to get silver from gripper2"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
@@ -1609,6 +1652,9 @@ steps_list:
       gimbal:
         <<: *FOLLOW_EE
   LV4_S3:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1653,6 +1699,9 @@ steps_list:
       gimbal:
         <<: *FOLLOW_EE
   LV4_G:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "ready to get stored gold"
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]

From c6a3a05b02474345074549bfc2124bde4176f937 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 25 Jul 2024 23:05:30 +0800
Subject: [PATCH 125/147] Modify home pos.

---
 engineer2_arm_config/launch/moveit.rviz       | 297 ++++++++++++++++--
 .../config/engineer2_steps_list.yaml          |   2 +-
 2 files changed, 277 insertions(+), 22 deletions(-)

diff --git a/engineer2_arm_config/launch/moveit.rviz b/engineer2_arm_config/launch/moveit.rviz
index 6a876dd..09ce292 100644
--- a/engineer2_arm_config/launch/moveit.rviz
+++ b/engineer2_arm_config/launch/moveit.rviz
@@ -5,8 +5,10 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /MotionPlanning1
+        - /TF1
+        - /TF1/Frames1
       Splitter Ratio: 0.5
-    Tree Height: 226
+    Tree Height: 1301
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -14,6 +16,10 @@ Panels:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
@@ -23,7 +29,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.03
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -35,30 +41,61 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Class: moveit_rviz_plugin/MotionPlanning
-      Enabled: true
+    - Acceleration_Scaling_Factor: 0.1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: false
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
       Name: MotionPlanning
       Planned Path:
-        Links: ~
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
         Loop Animation: true
         Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
         Show Robot Collision: false
         Show Robot Visual: true
         Show Trail: false
         State Display Time: 0.05 s
+        Trail Step Size: 1
         Trajectory Topic: move_group/display_planned_path
+        Use Sim Time: false
       Planning Metrics:
         Payload: 1
         Show Joint Torques: false
         Show Manipulability: false
         Show Manipulability Index: false
         Show Weight Limit: false
+        TextHeight: 0.07999999821186066
       Planning Request:
         Colliding Link Color: 255; 0; 0
         Goal State Alpha: 1
         Goal State Color: 250; 128; 0
         Interactive Marker Size: 0
         Joint Violation Color: 255; 0; 255
+        Planning Group: engineer_arm
         Query Goal State: true
         Query Start State: false
         Show Workspace: false
@@ -68,19 +105,237 @@ Visualization Manager:
       Robot Description: robot_description
       Scene Geometry:
         Scene Alpha: 1
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
         Show Scene Geometry: true
         Voxel Coloring: Z-Axis
         Voxel Rendering: Occupied Voxels
       Scene Robot:
         Attached Body Color: 150; 50; 150
-        Links: ~
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
         Robot Alpha: 0.5
-        Show Scene Robot: true
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: false
+      Velocity_Scaling_Factor: 0.1
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base_link:
+          Value: false
+        fake_link4:
+          Value: false
+        gold_lifter_link:
+          Value: false
+        gold_pusher_link:
+          Value: false
+        left_back_wheel:
+          Value: false
+        left_front_wheel:
+          Value: false
+        link1:
+          Value: false
+        link2:
+          Value: false
+        link3:
+          Value: false
+        link4:
+          Value: false
+        link5:
+          Value: false
+        link6:
+          Value: true
+        map:
+          Value: false
+        odom:
+          Value: false
+        pitch:
+          Value: false
+        right_back_wheel:
+          Value: false
+        right_front_wheel:
+          Value: false
+        silver_gripper_link:
+          Value: false
+        silver_lifter_link:
+          Value: false
+        silver_pusher_link:
+          Value: false
+        tools_link:
+          Value: false
+        yaw:
+          Value: false
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        base_link:
+          gold_pusher_link:
+            gold_lifter_link:
+              {}
+          left_back_wheel:
+            {}
+          left_front_wheel:
+            {}
+          link1:
+            link2:
+              link3:
+                fake_link4:
+                  {}
+                link4:
+                  link5:
+                    link6:
+                      tools_link:
+                        {}
+              yaw:
+                pitch:
+                  {}
+          right_back_wheel:
+            {}
+          right_front_wheel:
+            {}
+          silver_lifter_link:
+            silver_pusher_link:
+              silver_gripper_link:
+                {}
+        map:
+          odom:
+            {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fake_link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gold_lifter_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gold_pusher_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_back_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_front_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pitch:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_back_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_front_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        silver_gripper_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        silver_lifter_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        silver_pusher_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tools_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        yaw:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
       Value: true
+      Visual Enabled: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
+    Default Light: true
     Fixed Frame: base_link
+    Frame Rate: 30
   Name: root
   Tools:
     - Class: rviz/Interact
@@ -91,30 +346,30 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2.0
+      Distance: 1.3629440069198608
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.06
+        Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: -0.1
-        Y: 0.25
-        Z: 0.30
+        X: -0.27402463555336
+        Y: -0.39333611726760864
+        Z: 0.4662781059741974
       Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05
+      Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.01
-      Pitch: 0.5
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.419796109199524
       Target Frame: base_link
-      Yaw: -0.6232355833053589
+      Yaw: 3.614947557449341
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 848
+  Height: 1536
   Help:
     collapsed: false
   Hide Left Dock: false
@@ -123,9 +378,9 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000005a6fc0200000007fb000000100044006900730070006c006100790073010000003d000005a6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002f4000002ef0000017d00ffffff000007bf000005a600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
-  Width: 1291
-  X: 454
-  Y: 25
+  Width: 2488
+  X: 72
+  Y: 27
diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index ae78c86..ddeb557 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -82,7 +82,7 @@ common:
 
   joint3:
     home: &JOINT3_HOME_POS
-      0.9
+      -2.4
     zero: &JOINT3_ZERO_POS
       0.00000001
     l_exchange: &JOINT3_L_EXCHANGE_POS

From d71816f4d2733785984febf94a98140922d46965 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Fri, 26 Jul 2024 12:12:15 +0800
Subject: [PATCH 126/147] Modify get side big island omtion.

---
 .../config/engineer2_steps_list.yaml          | 27 +++++++++----------
 1 file changed, 13 insertions(+), 14 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index ddeb557..a32bafe 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -82,7 +82,7 @@ common:
 
   joint3:
     home: &JOINT3_HOME_POS
-      -2.4
+      -2.3
     zero: &JOINT3_ZERO_POS
       0.00000001
     l_exchange: &JOINT3_L_EXCHANGE_POS
@@ -161,7 +161,7 @@ common:
 
   middle_pitch:
     ground_stone: &MIDDLE_PITCH_GROUND_STONE
-      1.3
+      1.
     normal: &MIDDLE_PITCH_NORMAL
       0.0
     highest: &MIDDLE_PITCH_HIGHEST
@@ -1048,7 +1048,7 @@ steps_list:
         target: *MIDDLE_PITCH_HIGHEST
     - step: "arm ready"
       arm:
-        joints: [ 0.037,0.929,-1.963,-1.474,-1.434,-1.528 ]
+        joints: [ 0.037,0.9,-1.963,*JOINT4_L90_POS, 0., *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1059,21 +1059,21 @@ steps_list:
   SIDE_BIG_ISLAND0:
     - step: "arm get gold"
       arm:
-        joints: [ 0.037,0.093,-1.38,-1.474,-1.571,-1.528 ]
+        joints: [ 0.037,0.13,-1.38,*JOINT4_L90_POS,-0., *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "arm lift gold"
       arm:
-        joints: [ 0.127,0.093,-1.38,-1.474,-1.571,-1.528 ]
+        joints: [ 0.127,0.13,-1.38,*JOINT4_L90_POS,-0., *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "arm pull back gold"
       arm:
-        joints: [ 0.127,1.04,-1.99,-1.474,-1.321,-1.528 ]
+        joints: [ 0.127,1.0,-1.99,*JOINT4_L90_POS,-0.36, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1106,7 +1106,7 @@ steps_list:
         target: *MIDDLE_PITCH_HIGHEST
     - step: "arm ready"
       arm:
-        joints: [ 0.037,0.929,-1.963,-1.474,-1.434,-1.528 ]
+        joints: [ 0.037,0.9,-1.963,*JOINT4_L90_POS, 0.00001, *JOINT6_R_90 ]
         common:
           <<: *QUICKLY
         tolerance:
@@ -1118,20 +1118,19 @@ steps_list:
     - step: "get gold"
       gold_pusher:
         target: -0.443
-        delay: 0.3
-    - step: "lift gold"
-      gold_lifter:
-        target: 0.06
     - step: "arm get gold"
       arm:
-        joints: [ 0.037,0.093,-1.38,-1.474,-1.571,-1.528 ]
+        joints: [ 0.037,0.13,-1.38,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "lift gold"
+      gold_lifter:
+        target: 0.06
     - step: "arm lift gold"
       arm:
-        joints: [ 0.127,0.093,-1.38,-1.474,-1.571,-1.528 ]
+        joints: [ 0.127,0.13,-1.38,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1147,7 +1146,7 @@ steps_list:
         <<: *FRONT_POS
     - step: "arm pull back gold"
       arm:
-        joints: [ 0.127,1.04,-1.99,-1.474,-1.321,-1.528 ]
+        joints: [ 0.127,1.0,-1.99,*JOINT4_L90_POS,-0.36, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:

From 238a3ae943003cfb952fe21f95d2d9acd1fe5264 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Fri, 26 Jul 2024 23:16:35 +0800
Subject: [PATCH 127/147] Modify motion timeout time.

---
 .../config/engineer2_steps_list.yaml          | 56 +++++++++----------
 1 file changed, 28 insertions(+), 28 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index a32bafe..436df58 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -7,11 +7,11 @@ common:
     normally: &NORMALLY
       speed: 0.4
       accel: 0.3
-      timeout: 5
+      timeout: 6
     quickly: &QUICKLY
-      speed: 0.6
+      speed: 0.8
       accel: 0.6
-      timeout: 3.
+      timeout: 6.
 
   tolerance:
     small_tolerance: &SMALL_TOLERANCE
@@ -173,7 +173,7 @@ common:
       common:
         <<: *NORMALLY
       tolerance:
-        <<: *NORMAL_TOLERANCE
+        <<: *BIGGER_TOLERANCE
 
     lv4_exchange: &LV4_EXCHANGE
       joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_45, *JOINT6_HOME_POS ]
@@ -210,7 +210,7 @@ common:
       frame: base_link
       position: [ -3., -0.3, 0. ]
     follow_ee: &FOLLOW_EE
-      frame: link6
+      frame: base_link
       position: [ 0.15, 0.00, 0. ]
 
   chassis_forward: &CHASSIS_FORWARD
@@ -342,7 +342,7 @@ steps_list:
         <<: *LV4_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS
   LV5_L_EXCHANGE:
     - step: "middle pitch up"
       middle_pitch:
@@ -352,7 +352,7 @@ steps_list:
         <<: *LV5_L_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS
 
   LV5_R_EXCHANGE:
     - step: "middle pitch up"
@@ -363,7 +363,7 @@ steps_list:
         <<: *LV5_R_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS
 
   GET_SMALL_ISLAND:
     - step: "middle pitch up"
@@ -951,7 +951,7 @@ steps_list:
   GRIPPER_THREE_SILVER0:
     - step: "get silver"
       silver_pusher:
-        target: -0.03
+        target: -0.04
         delay: 0.5
     - step: "pusher back little"
       silver_pusher:
@@ -1212,7 +1212,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
@@ -1247,7 +1247,7 @@ steps_list:
         <<: *LV5_L_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS
 
 
 
@@ -1259,7 +1259,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "open main gripper"
@@ -1290,7 +1290,7 @@ steps_list:
         <<: *LV5_L_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS
 
 
 
@@ -1302,7 +1302,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ready to get silver from gripper3"
@@ -1340,7 +1340,7 @@ steps_list:
         <<: *LV5_L_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS
   LV5_L_G:
     - step: "middle pitch up"
       middle_pitch:
@@ -1349,7 +1349,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "open main gripper"
@@ -1387,7 +1387,7 @@ steps_list:
         <<: *LV5_L_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS
   LV5_R_S1:
     - step: "middle pitch up"
       middle_pitch:
@@ -1396,7 +1396,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
@@ -1431,7 +1431,7 @@ steps_list:
         <<: *LV5_R_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS
   LV5_R_S2:
     - step: "middle pitch up"
       middle_pitch:
@@ -1440,7 +1440,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "open main gripper"
@@ -1471,7 +1471,7 @@ steps_list:
         <<: *LV5_R_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS
   LV5_R_S3:
     - step: "middle pitch up"
       middle_pitch:
@@ -1480,7 +1480,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "ready to get silver from gripper3"
@@ -1518,7 +1518,7 @@ steps_list:
         <<: *LV5_R_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS
   LV5_R_G:
     - step: "middle pitch up"
       middle_pitch:
@@ -1565,7 +1565,7 @@ steps_list:
         <<: *LV5_R_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS
   LV4_S1:
     - step: "middle pitch up"
       middle_pitch:
@@ -1609,7 +1609,7 @@ steps_list:
         <<: *LV4_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS
   LV4_S2:
     - step: "middle pitch up"
       middle_pitch:
@@ -1649,7 +1649,7 @@ steps_list:
         <<: *LV4_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS
   LV4_S3:
     - step: "middle pitch up"
       middle_pitch:
@@ -1696,7 +1696,7 @@ steps_list:
         <<: *LV4_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS
   LV4_G:
     - step: "middle pitch up"
       middle_pitch:
@@ -1743,4 +1743,4 @@ steps_list:
         <<: *LV4_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FOLLOW_EE
+        <<: *RIGHT_POS

From c6da30d619ae50156045776b35fc91d1d22b7fe5 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Sat, 27 Jul 2024 03:06:39 +0800
Subject: [PATCH 128/147] Add get ground stone and fix get side big island.

---
 .../config/engineer2_steps_list.yaml          | 51 ++++++++++++-------
 1 file changed, 33 insertions(+), 18 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index 436df58..607bd3f 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -5,11 +5,11 @@ common:
       accel: 0.1
       timeout: 15.
     normally: &NORMALLY
-      speed: 0.4
+      speed: 0.6
       accel: 0.3
       timeout: 6
     quickly: &QUICKLY
-      speed: 0.8
+      speed: 1.0
       accel: 0.6
       timeout: 6.
 
@@ -26,10 +26,8 @@ common:
   joint1:
     home: &JOINT1_HOME_POS
       0.1
-    ready_get_ground: &JOINT1_READY_GET_GROUND_POS
+    ready_get_ground: &JOINT1_GET_GROUND_POS
       0.005
-    get_ground: &JOINT1_GET_GROUND_POS
-      0.4
     exchange: &JOINT1_EXCHANGE_POS
       0.35
     ready_get_small: &JOINT1_READY_GET_SMALL_POS
@@ -536,7 +534,7 @@ steps_list:
   GROUND_STONE:
     - step: "arm ready get ground stone"
       arm:
-        joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_HOME_POS, *JOINT3_ZERO_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
+        joints: [*JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, 1.112  , -0.655]
         common:
           <<: *NORMALLY
         tolerance:
@@ -547,7 +545,7 @@ steps_list:
   GROUND_STONE0:
     - step: "move arm to get ground stone"
       arm:
-        joints: [ *JOINT1_READY_GET_GROUND_POS, *JOINT2_HOME_POS, *JOINT3_ZERO_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
+        joints: [*JOINT1_GET_GROUND_POS, 0.94, -2.066, *JOINT4_ZERO_POS, 1.112  , -0.655]
         common:
           <<: *NORMALLY
         tolerance:
@@ -555,12 +553,12 @@ steps_list:
     - step: "open main gripper"
       gripper:
         <<: *OPEN_MAIN_GRIPPER
-    - step: "ready to get ground stone"
+    - step: "get ground stone"
       middle_pitch:
-        target: *MIDDLE_PITCH_GROUND_STONE
+        target: 0.49
     - step: "arm pull up stone"
       arm:
-        joints: [*JOINT1_READY_GET_GROUND_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS]
+        joints: [0.2, 0.94, -2.066, *JOINT4_ZERO_POS, 1.112  , -0.655]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1048,7 +1046,7 @@ steps_list:
         target: *MIDDLE_PITCH_HIGHEST
     - step: "arm ready"
       arm:
-        joints: [ 0.037,0.9,-1.963,*JOINT4_L90_POS, 0., *JOINT6_R_90 ]
+        joints: [ 0.037,0.9,-1.963,*JOINT4_L90_POS, 0.00001, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1059,18 +1057,21 @@ steps_list:
   SIDE_BIG_ISLAND0:
     - step: "arm get gold"
       arm:
-        joints: [ 0.037,0.13,-1.38,*JOINT4_L90_POS,-0., *JOINT6_R_90 ]
+        joints: [ 0.037,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "arm lift gold"
       arm:
-        joints: [ 0.127,0.13,-1.38,*JOINT4_L90_POS,-0., *JOINT6_R_90 ]
+        joints: [ 0.127,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
     - step: "arm pull back gold"
       arm:
         joints: [ 0.127,1.0,-1.99,*JOINT4_L90_POS,-0.36, *JOINT6_R_90 ]
@@ -1078,9 +1079,16 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "gimbal look front"
-      gimbal:
-        <<: *FRONT_POS
+    - step: "chassis left"
+      chassis:
+        <<: *CHASSIS_MOVE_LEFT
+    - step: "move stone towards down"
+      arm:
+        joints: [ 0.127, 1.0, -2.388, 0.0001, 1.57, -1.57 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
 
   BOTH_BIG_ISLAND:
     - step: "middle pitch up"
@@ -1120,7 +1128,7 @@ steps_list:
         target: -0.443
     - step: "arm get gold"
       arm:
-        joints: [ 0.037,0.13,-1.38,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
+        joints: [ 0.037,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1130,7 +1138,7 @@ steps_list:
         target: 0.06
     - step: "arm lift gold"
       arm:
-        joints: [ 0.127,0.13,-1.38,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
+        joints: [ 0.127,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1154,6 +1162,13 @@ steps_list:
     - step: "chassis left"
       chassis:
         <<: *CHASSIS_MOVE_LEFT
+    - step: "move stone towards down"
+      arm:
+        joints: [ 0.127, 1.0, -2.388, 0.0001, 1.57, -1.57 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
 
   OM:
     - step: "main open"

From c7a05eb0a4ed1cd02164034f4707e6aa06ae29d0 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 29 Jul 2024 19:08:51 +0800
Subject: [PATCH 129/147] Add default 0.05s delay for gpio motion.

---
 engineer_middleware/include/engineer_middleware/motion.h | 7 +++++++
 engineer_middleware/include/engineer_middleware/step.h   | 2 ++
 2 files changed, 9 insertions(+)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 0827a84..2846822 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -568,7 +568,14 @@ class GpioMotion : public PublishMotion<rm_msgs::GpioData>
     return PublishMotion::move();
   }
 
+  bool isFinish() override
+  {
+    return ((ros::Time::now() - start_time_).toSec() > delay_);
+  }
+
 private:
+  ros::Time start_time_;
+  double delay_{ 0.05 };
   bool state_;
   int pin_;
 };
diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h
index 7239b13..563c4e4 100644
--- a/engineer_middleware/include/engineer_middleware/step.h
+++ b/engineer_middleware/include/engineer_middleware/step.h
@@ -216,6 +216,8 @@ class Step
       success &= silver_pusher_motion_->isFinish();
     if (silver_rotator_motion_)
       success &= silver_rotator_motion_->isFinish();
+    if (gpio_motion_)
+      success &= gpio_motion_->isFinish();
     if (gold_pusher_motion_)
       success &= gold_pusher_motion_->isFinish();
     if (gold_lifter_motion_)

From 490045f7731b635ec4a9d384f82ccf5eb9a9257f Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 29 Jul 2024 20:50:04 +0800
Subject: [PATCH 130/147] Set higer speed and accel limit.

---
 engineer2_arm_config/config/joint_limits.yaml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/engineer2_arm_config/config/joint_limits.yaml b/engineer2_arm_config/config/joint_limits.yaml
index 6699247..72eee70 100644
--- a/engineer2_arm_config/config/joint_limits.yaml
+++ b/engineer2_arm_config/config/joint_limits.yaml
@@ -2,8 +2,8 @@
 
 # For beginners, we downscale velocity and acceleration limits.
 # You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
-default_velocity_scaling_factor: 0.1
-default_acceleration_scaling_factor: 0.1
+default_velocity_scaling_factor: 1.0
+default_acceleration_scaling_factor: 1.0
 
 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
 # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]

From 3414f33439245fb6656f6a063e1eae2db6def0bf Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 29 Jul 2024 20:50:36 +0800
Subject: [PATCH 131/147] Record.

---
 .../config/engineer2_steps_list.yaml          | 177 +++++++++++++++---
 1 file changed, 146 insertions(+), 31 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index 607bd3f..b851f0c 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -1,16 +1,16 @@
 common:
   speed:
     slowly: &SLOWLY
-      speed: 0.15
+      speed: 0.4
       accel: 0.1
       timeout: 15.
     normally: &NORMALLY
-      speed: 0.6
-      accel: 0.3
+      speed: 2.5
+      accel: 1.5
       timeout: 6
     quickly: &QUICKLY
-      speed: 1.0
-      accel: 0.6
+      speed: 3
+      accel: 2
       timeout: 6.
 
   tolerance:
@@ -19,7 +19,7 @@ common:
     normal_tolerance: &NORMAL_TOLERANCE
       tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.07 ]
     bigger_tolerance: &BIGGER_TOLERANCE
-      tolerance_joints: [ 0.015, 0.015, 0.03, 0.35, 0.2, 0.2, 0.1 ]
+      tolerance_joints: [ 0.015, 0.04, 0.03, 0.35, 0.2, 0.2, 0.1 ]
     max_tolerance: &MAX_TOLERANCE
       tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3]
 
@@ -54,9 +54,9 @@ common:
     exchange: &JOINT2_EXCHANGE_POS
       -1.57
     l_exchange: &JOINT2_L_EXCHANGE_POS
-      -0.984
+      -0.3
     r_exchange: &JOINT2_R_EXCHANGE_POS
-      -2.156
+      -2.5
     get_small_island_l: &JOINT2_GET_SMALL_ISLAND_L_POS
       -0.098
     get_small_island_m: &JOINT2_GET_SMALL_ISLAND_M_POS
@@ -84,9 +84,9 @@ common:
     zero: &JOINT3_ZERO_POS
       0.00000001
     l_exchange: &JOINT3_L_EXCHANGE_POS
-      -1.165
+      -0.7
     r_exchange: &JOINT3_R_EXCHANGE_POS
-      1.165
+      0.7
     get_small_island_l: &JOINT3_GET_SMALL_ISLAND_L_POS
                           -1.948
     get_small_island_m: &JOINT3_GET_SMALL_ISLAND_M_POS
@@ -180,14 +180,14 @@ common:
       tolerance:
         <<: *NORMAL_TOLERANCE
 
-    lv5_l_exchange: &LV5_L_EXCHANGE
+    lv5_r_exchange: &LV5_L_EXCHANGE
       joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_L_EXCHANGE_POS, *JOINT3_L_EXCHANGE_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
 
-    lv5_r_exchange: &LV5_R_EXCHANGE
+    lv5_l_exchange: &LV5_R_EXCHANGE
       joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90]
       common:
         <<: *NORMALLY
@@ -220,14 +220,23 @@ common:
     common:
       timeout: 2.
 
+  chassis_turn_180: &CHASSIS_TURN_180
+    frame: base_link
+    position: [ 0., 0. ]
+    yaw: 1.57
+    chassis_tolerance_position: 0.1
+    chassis_tolerance_angular: 0.3
+    common:
+      timeout: 4.
+
   chassis_fmove_left: &CHASSIS_MOVE_LEFT
     frame: base_link
-    position: [ 0.0, 0.2 ]
+    position: [ 0.0, 0.4 ]
     yaw: 0.0
     chassis_tolerance_position: 0.1
     chassis_tolerance_angular: 0.2
     common:
-      timeout: 2.
+      timeout: 3.
 
 
   gripper:
@@ -295,6 +304,9 @@ steps_list:
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
   HOME:
+    - step: "main close"
+      gripper:
+        <<: *CLOSE_MAIN_GRIPPER
     - step: "pusher back"
       silver_pusher:
         target: 0.
@@ -313,6 +325,9 @@ steps_list:
     - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
+    - step: "main close"
+      gripper:
+        <<: *CLOSE_MAIN_GRIPPER
     - step: "arm home pos"
       arm:
         <<: *HOME
@@ -320,13 +335,63 @@ steps_list:
       middle_pitch:
         target: *MIDDLE_PITCH_GROUND_STONE
 
+  CALIBRATION:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
+    - step: "main close"
+      gripper:
+        <<: *CLOSE_MAIN_GRIPPER
+    - step: "s1 close"
+      gripper:
+        <<: *CLOSE_S1
+    - step: "s2 close"
+      gripper:
+        <<: *CLOSE_S2
+    - step: "s3 close"
+      gripper:
+        <<: *CLOSE_S3
+    - step: "g1 close"
+      gripper:
+        <<: *CLOSE_GOLD
+    - step: "close silver gripper"
+      gripper:
+        <<: *CLOSE_S_GRIPPER
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "main close"
+      gripper:
+        <<: *CLOSE_MAIN_GRIPPER
+
   HOME_WITH_PITCH:
+    - step: "main close"
+      gripper:
+        <<: *CLOSE_MAIN_GRIPPER
+    - step: "s1 close"
+      gripper:
+        <<: *CLOSE_S1
+    - step: "s2 close"
+      gripper:
+        <<: *CLOSE_S2
+    - step: "s3 close"
+      gripper:
+        <<: *CLOSE_S3
+    - step: "g1 close"
+      gripper:
+        <<: *CLOSE_GOLD
+    - step: "close silver gripper"
+      gripper:
+        <<: *CLOSE_S_GRIPPER
     - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
     - step: "arm home pos"
       arm:
         <<: *HOME
+    - step: "main close"
+      gripper:
+        <<: *CLOSE_MAIN_GRIPPER
     - step: "middle pitch down"
       middle_pitch:
         target: *MIDDLE_PITCH_GROUND_STONE
@@ -532,9 +597,12 @@ steps_list:
           <<: *NORMAL_TOLERANCE
 
   GROUND_STONE:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
     - step: "arm ready get ground stone"
       arm:
-        joints: [*JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, 1.112  , -0.655]
+        joints: [*JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, 1.112  , -0.397]
         common:
           <<: *NORMALLY
         tolerance:
@@ -545,7 +613,7 @@ steps_list:
   GROUND_STONE0:
     - step: "move arm to get ground stone"
       arm:
-        joints: [*JOINT1_GET_GROUND_POS, 0.94, -2.066, *JOINT4_ZERO_POS, 1.112  , -0.655]
+        joints: [*JOINT1_GET_GROUND_POS, 0.943, -1.863, *JOINT4_ZERO_POS, 1.112  , -0.397]
         common:
           <<: *NORMALLY
         tolerance:
@@ -556,9 +624,10 @@ steps_list:
     - step: "get ground stone"
       middle_pitch:
         target: 0.49
+        delay: 0.5
     - step: "arm pull up stone"
       arm:
-        joints: [0.2, 0.94, -2.066, *JOINT4_ZERO_POS, 1.112  , -0.655]
+        joints: [0.2, 0.943, -1.863, *JOINT4_ZERO_POS, 1.112  , -0.397]
         common:
           <<: *NORMALLY
         tolerance:
@@ -931,6 +1000,9 @@ steps_list:
     - step: "pusher back"
       silver_pusher:
         target: -0.00001
+    - step: "turn 180"
+      chassis:
+        <<: *CHASSIS_TURN_180
     - step: "open gripper"
       gripper:
         <<: *OPEN_S1
@@ -1046,9 +1118,9 @@ steps_list:
         target: *MIDDLE_PITCH_HIGHEST
     - step: "arm ready"
       arm:
-        joints: [ 0.037,0.9,-1.963,*JOINT4_L90_POS, 0.00001, *JOINT6_R_90 ]
+        joints: [ 0.040,0.885,-2.107,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "open main gripper"
@@ -1057,14 +1129,14 @@ steps_list:
   SIDE_BIG_ISLAND0:
     - step: "arm get gold"
       arm:
-        joints: [ 0.037,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
+        joints: [ 0.040,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "arm lift gold"
       arm:
-        joints: [ 0.127,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
+        joints: [ 0.115,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1074,7 +1146,7 @@ steps_list:
         <<: *FRONT_POS
     - step: "arm pull back gold"
       arm:
-        joints: [ 0.127,1.0,-1.99,*JOINT4_L90_POS,-0.36, *JOINT6_R_90 ]
+        joints: [ 0.115,0.991,-2.142,*JOINT4_L90_POS,-0.335, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1084,7 +1156,7 @@ steps_list:
         <<: *CHASSIS_MOVE_LEFT
     - step: "move stone towards down"
       arm:
-        joints: [ 0.127, 1.0, -2.388, 0.0001, 1.57, -1.57 ]
+        joints: [ 0.120, 0.998, -2.388, 0.0001, 1.57, -1.57 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1097,6 +1169,9 @@ steps_list:
     - step: "gimbal look right"
       gimbal:
         <<: *RIGHT_POS
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
     - step: "gimbal look right"
       gimbal:
         <<: *RIGHT_POS
@@ -1114,7 +1189,7 @@ steps_list:
         target: *MIDDLE_PITCH_HIGHEST
     - step: "arm ready"
       arm:
-        joints: [ 0.037,0.9,-1.963,*JOINT4_L90_POS, 0.00001, *JOINT6_R_90 ]
+        joints: [ 0.040,0.885,-2.107,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
         common:
           <<: *QUICKLY
         tolerance:
@@ -1128,17 +1203,17 @@ steps_list:
         target: -0.443
     - step: "arm get gold"
       arm:
-        joints: [ 0.037,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
+        joints: [ 0.040,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "lift gold"
       gold_lifter:
-        target: 0.06
+        target: 0.055
     - step: "arm lift gold"
       arm:
-        joints: [ 0.127,0.12,-1.47,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
+        joints: [ 0.115,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1154,17 +1229,17 @@ steps_list:
         <<: *FRONT_POS
     - step: "arm pull back gold"
       arm:
-        joints: [ 0.127,1.0,-1.99,*JOINT4_L90_POS,-0.36, *JOINT6_R_90 ]
+        joints: [ 0.115,0.991,-2.142,*JOINT4_L90_POS,-0.335, *JOINT6_R_90 ]
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "chassis left"
       chassis:
         <<: *CHASSIS_MOVE_LEFT
     - step: "move stone towards down"
       arm:
-        joints: [ 0.127, 1.0, -2.388, 0.0001, 1.57, -1.57 ]
+        joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1219,6 +1294,46 @@ steps_list:
       gripper:
         <<: *CLOSE_S_GRIPPER
 
+  CA:
+    - step: "main close"
+      gripper:
+        <<: *CLOSE_MAIN_GRIPPER
+    - step: "s1 close"
+      gripper:
+        <<: *CLOSE_S1
+    - step: "s2 close"
+      gripper:
+        <<: *CLOSE_S2
+    - step: "s3 close"
+      gripper:
+        <<: *CLOSE_S3
+    - step: "g1 close"
+      gripper:
+        <<: *CLOSE_GOLD
+    - step: "close silver gripper"
+      gripper:
+        <<: *CLOSE_S_GRIPPER
+
+  OA:
+    - step: "main OPEN"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
+    - step: "s1 OPEN"
+      gripper:
+        <<: *OPEN_S1
+    - step: "s2 OPEN"
+      gripper:
+        <<: *OPEN_S2
+    - step: "s3 OPEN"
+      gripper:
+        <<: *OPEN_S3
+    - step: "g1 OPEN"
+      gripper:
+        <<: *OPEN_GOLD
+    - step: "OPEN silver gripper"
+      gripper:
+        <<: *OPEN_S_GRIPPER
+
   LV5_L_S1:
     - step: "middle pitch up"
       middle_pitch:

From 7a22e9afd7e2bc50878f774aedacc7d90d7089aa Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 29 Jul 2024 21:23:05 +0800
Subject: [PATCH 132/147] Add gpio motion default delay to 0.1s.

---
 engineer_middleware/include/engineer_middleware/motion.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 2846822..91057b1 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -575,7 +575,7 @@ class GpioMotion : public PublishMotion<rm_msgs::GpioData>
 
 private:
   ros::Time start_time_;
-  double delay_{ 0.05 };
+  double delay_{ 0.1 };
   bool state_;
   int pin_;
 };

From a4f1b5ba00864f64d4374dc2fadcb7ef22d1a72a Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 30 Jul 2024 05:25:05 +0800
Subject: [PATCH 133/147] Modify joint3 speed limit in moveit config file.

---
 engineer2_arm_config/config/joint_limits.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/engineer2_arm_config/config/joint_limits.yaml b/engineer2_arm_config/config/joint_limits.yaml
index 72eee70..75658e6 100644
--- a/engineer2_arm_config/config/joint_limits.yaml
+++ b/engineer2_arm_config/config/joint_limits.yaml
@@ -20,7 +20,7 @@ joint_limits:
     max_acceleration: 0
   joint3:
     has_velocity_limits: true
-    max_velocity: 3.76
+    max_velocity: 5.44
     has_acceleration_limits: false
     max_acceleration: 0
   joint4:

From 2db342b3f3887d65cec0f20614fe9386971b0336 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 30 Jul 2024 05:26:29 +0800
Subject: [PATCH 134/147] Fix unavailable gpio motion delay.

---
 engineer_middleware/include/engineer_middleware/motion.h | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index 91057b1..b2bdf94 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -528,6 +528,7 @@ class GpioMotion : public PublishMotion<rm_msgs::GpioData>
   GpioMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface)
     : PublishMotion<rm_msgs::GpioData>(motion, interface)
   {
+    delay_ = xmlRpcGetDouble(motion, "delay", 0.01);
     msg_.gpio_state.assign(8, false);
     msg_.gpio_name.assign(8, "no_registered");
     pin_ = motion["pin"];
@@ -564,6 +565,7 @@ class GpioMotion : public PublishMotion<rm_msgs::GpioData>
   }
   bool move() override
   {
+    start_time_ = ros::Time::now();
     msg_.gpio_state[pin_] = state_;
     return PublishMotion::move();
   }
@@ -575,7 +577,7 @@ class GpioMotion : public PublishMotion<rm_msgs::GpioData>
 
 private:
   ros::Time start_time_;
-  double delay_{ 0.1 };
+  double delay_;
   bool state_;
   int pin_;
 };

From 97e20a9f19ff06a7c9630fb6735488637ddad3e6 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 30 Jul 2024 06:39:31 +0800
Subject: [PATCH 135/147] Adjust side gold stone motion, add motion to exchange
 side gold at front of chassis.

---
 .../config/engineer2_steps_list.yaml          | 56 +++++++++++++++----
 1 file changed, 45 insertions(+), 11 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index b851f0c..fc615d9 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -167,7 +167,7 @@ common:
 
   arm:
     home: &HOME
-      joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
+      joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
       common:
         <<: *NORMALLY
       tolerance:
@@ -428,6 +428,38 @@ steps_list:
       gimbal:
         <<: *RIGHT_POS
 
+  LV5_R_CAR_FRONT_EXCHANGE:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
+    - step: "arm enter exchange pos"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, -0.774, *JOINT3_R_EXCHANGE_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FRONT_POS
+
+  LV5_L_CAR_FRONT_EXCHANGE:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
+    - step: "arm enter exchange pos"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, 1.001, *JOINT3_L_EXCHANGE_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *FRONT_POS
+
   GET_SMALL_ISLAND:
     - step: "middle pitch up"
       middle_pitch:
@@ -1116,9 +1148,10 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_HIGHEST
+        delay: 0.3
     - step: "arm ready"
       arm:
-        joints: [ 0.040,0.885,-2.107,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
+        joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_HOME_POS ]
         common:
           <<: *QUICKLY
         tolerance:
@@ -1129,14 +1162,14 @@ steps_list:
   SIDE_BIG_ISLAND0:
     - step: "arm get gold"
       arm:
-        joints: [ 0.040,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
+        joints: [ 0.040,0.162,-1.414,*JOINT4_R90_POS, 0.097, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "arm lift gold"
       arm:
-        joints: [ 0.115,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
+        joints: [ 0.115,0.162,-1.414,*JOINT4_R90_POS,0.097, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1146,17 +1179,17 @@ steps_list:
         <<: *FRONT_POS
     - step: "arm pull back gold"
       arm:
-        joints: [ 0.115,0.991,-2.142,*JOINT4_L90_POS,-0.335, *JOINT6_R_90 ]
+        joints: [ 0.115,0.988,-2.023,*JOINT4_R90_POS,0.233, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "chassis left"
       chassis:
         <<: *CHASSIS_MOVE_LEFT
     - step: "move stone towards down"
       arm:
-        joints: [ 0.120, 0.998, -2.388, 0.0001, 1.57, -1.57 ]
+        joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1187,9 +1220,10 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_HIGHEST
+        delay: 0.3
     - step: "arm ready"
       arm:
-        joints: [ 0.040,0.885,-2.107,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
+        joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_HOME_POS ]
         common:
           <<: *QUICKLY
         tolerance:
@@ -1203,7 +1237,7 @@ steps_list:
         target: -0.443
     - step: "arm get gold"
       arm:
-        joints: [ 0.040,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
+        joints: [ 0.040,0.162,-1.414,*JOINT4_R90_POS, 0.097, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1213,7 +1247,7 @@ steps_list:
         target: 0.055
     - step: "arm lift gold"
       arm:
-        joints: [ 0.115,0.138,-1.649,*JOINT4_L90_POS,-0.00001, *JOINT6_R_90 ]
+        joints: [ 0.115,0.162,-1.414,*JOINT4_R90_POS,0.097, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1229,7 +1263,7 @@ steps_list:
         <<: *FRONT_POS
     - step: "arm pull back gold"
       arm:
-        joints: [ 0.115,0.991,-2.142,*JOINT4_L90_POS,-0.335, *JOINT6_R_90 ]
+        joints: [ 0.115,0.988,-2.023,*JOINT4_R90_POS,0.233, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:

From 965a94fd3798f15847cd86ed191d21533aa17114 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 30 Jul 2024 22:50:48 +0800
Subject: [PATCH 136/147] Record.

---
 engineer_middleware/config/engineer2_steps_list.yaml | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index fc615d9..cdb467f 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -139,7 +139,7 @@ common:
 
   joint6:
     home: &JOINT6_HOME_POS
-      0.0
+      0.00001
     l_90: &JOINT6_L_90
       1.57
     r_90: &JOINT6_R_90
@@ -1162,14 +1162,14 @@ steps_list:
   SIDE_BIG_ISLAND0:
     - step: "arm get gold"
       arm:
-        joints: [ 0.040,0.162,-1.414,*JOINT4_R90_POS, 0.097, *JOINT6_HOME_POS ]
+        joints: [ 0.040,0.16,-1.42,*JOINT4_R90_POS, 0.097, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "arm lift gold"
       arm:
-        joints: [ 0.115,0.162,-1.414,*JOINT4_R90_POS,0.097, *JOINT6_HOME_POS ]
+        joints: [ 0.115,0.16,-1.42,*JOINT4_R90_POS,0.097, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1179,7 +1179,7 @@ steps_list:
         <<: *FRONT_POS
     - step: "arm pull back gold"
       arm:
-        joints: [ 0.115,0.988,-2.023,*JOINT4_R90_POS,0.233, *JOINT6_HOME_POS ]
+        joints: [ 0.114,0.988,-2.023,*JOINT4_R90_POS,0.232, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:

From 772ddf1e92ee1e70c216077a640443fe444d9d9b Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Wed, 31 Jul 2024 08:24:02 +0800
Subject: [PATCH 137/147] Enable max acceleration limit for engineer2.

---
 engineer2_arm_config/config/joint_limits.yaml | 30 +++++++++----------
 1 file changed, 15 insertions(+), 15 deletions(-)

diff --git a/engineer2_arm_config/config/joint_limits.yaml b/engineer2_arm_config/config/joint_limits.yaml
index 75658e6..7031779 100644
--- a/engineer2_arm_config/config/joint_limits.yaml
+++ b/engineer2_arm_config/config/joint_limits.yaml
@@ -2,39 +2,39 @@
 
 # For beginners, we downscale velocity and acceleration limits.
 # You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
-default_velocity_scaling_factor: 1.0
-default_acceleration_scaling_factor: 1.0
+default_velocity_scaling_factor: 0.1
+default_acceleration_scaling_factor: 0.1
 
 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
 # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
 joint_limits:
   joint1:
     has_velocity_limits: true
-    max_velocity: 2.25
-    has_acceleration_limits: false
-    max_acceleration: 0
+    max_velocity: 1.25
+    has_acceleration_limits: true
+    max_acceleration: 1
   joint2:
     has_velocity_limits: true
     max_velocity: 10.46
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10
   joint3:
     has_velocity_limits: true
     max_velocity: 5.44
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 15
   joint4:
     has_velocity_limits: true
     max_velocity: 20.94
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 15
   joint5:
     has_velocity_limits: true
     max_velocity: 10
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 15
   joint6:
     has_velocity_limits: true
     max_velocity: 10
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 15

From 48e274c7e8e02851b1a43e6def51b077c6cda8f3 Mon Sep 17 00:00:00 2001
From: 2194555 <3631676002@qq.com>
Date: Wed, 31 Jul 2024 22:30:52 +0800
Subject: [PATCH 138/147] Update.

---
 .../config/engineer_steps_list.yaml           | 32 +++++++++++++------
 1 file changed, 23 insertions(+), 9 deletions(-)

diff --git a/engineer_middleware/config/engineer_steps_list.yaml b/engineer_middleware/config/engineer_steps_list.yaml
index 92a1e58..257570e 100644
--- a/engineer_middleware/config/engineer_steps_list.yaml
+++ b/engineer_middleware/config/engineer_steps_list.yaml
@@ -42,7 +42,7 @@ common:
       bin_get_stone_left: &JOINT1_BIN_GET_STONE_LEFT
                      0.2
       bin_get_stone_right: &JOINT1_BIN_GET_STONE_RIGHT
-                            0.1198
+                            0.1248
       big_ready: &JOINT1_BIG_READY
                      0.005
       big_adjust_mid: &JOINT1_BIG_ADJUST_MID
@@ -80,9 +80,9 @@ common:
       small_store: &JOINT2_SMALL_STORE
                       0.060290
       bin_get_stone: &JOINT2_BIN_GET_STONE
-                            0.13
+                            0.1
       back_bin_position: &JOINT2_BACK_BIN_POSITION
-                             0.06451
+                             0.04551
       avoid_collision: &JOINT2_AVOID_COLLISION
                         0.1
       big_ready: &JOINT2_BIG_READY
@@ -113,7 +113,7 @@ common:
       small_turn: &JOINT3_SMALL_TURN
                    0.257
       bin_get_stone: &JOINT3_BIN_GET_STONE
-                          0.2608
+                          0.2528
       bin_get_lv4_stone: &JOINT3_BIN_GET_LV4_STONE
                            0.294414
       big_ready: &JOINT3_BIG_READY
@@ -160,7 +160,7 @@ common:
       right_90_position: &JOINT5_R90_POSITION
                            1.59
       bin_get_stone: &JOINT5_BIN_GET_STONE
-                           1.63912
+                           1.53912
       big_ready: &JOINT5_BIG_READY
                   -0.017
       big_ready_store: &JOINT5_BIG_READY_STORE
@@ -191,7 +191,7 @@ common:
       small_ready_store: &JOINT6_SMALL_READY_STORE
                    -1.633
       bin_get_stone: &JOINT6_BIN_GET_STONE
-                      -1.5936
+                      -1.6036
       big_ready: &JOINT6_BIG_READY
                        0.014
       big_adjust_mid: &JOINT6_BIG_ADJUST_MID
@@ -216,7 +216,7 @@ common:
       big_ready_store: &JOINT7_BIG_READY_STORE
                           -1.522
       small_ready: &JOINT7_SMALL_READY
-                         0.012
+                         -0.083
       small_ready_store: &JOINT7_SMALL_READY_STORE
                           1.722
       exchange: &JOINT7_EXCHANGE
@@ -224,7 +224,7 @@ common:
       lv4_ore: &JOINT7_LV4_ORE
                  -0.098300
       bin_get_left: &JOINT7_BIN_GET_LEFT
-                  -1.6973
+                  -1.5793
       to: &JOINT7_TAKE_ONE
         -0.844500
       bin_get_right: &JOINT7_BIN_GET_RIGHT
@@ -243,7 +243,12 @@ common:
     small_island_pos: &BIG_ISLAND_POS
       frame: gimbal_lifter
       position: [ 1., -1., 0.]
-
+    test: &GIMBAL_TEST
+      frame: link4
+      position: [1., 0., 0.]
+    test2: &GIMBAL_TEST2
+      frame: link7
+      position: [0., 0., -1.]
 
   gimbal_lifter:
     button_pos: &BUTTON_POS
@@ -363,6 +368,15 @@ common:
           2.6
 
 steps_list:
+  TEST1:
+    - step: "gimbal test1"
+      gimbal:
+        <<: *GIMBAL_TEST
+  TEST2:
+    - step: "gimbal test2"
+      gimbal:
+        <<: *GIMBAL_TEST2
+
   EXCHANGE_POS:
     - step: "gimbal look front"
       gimbal:

From 38a8c64924e8e637ae8d76875fb1b77d97ca8b05 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 1 Aug 2024 04:28:55 +0800
Subject: [PATCH 139/147] Modify gpio motion.

---
 .../include/engineer_middleware/motion.h               | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h
index b2bdf94..eec089f 100644
--- a/engineer_middleware/include/engineer_middleware/motion.h
+++ b/engineer_middleware/include/engineer_middleware/motion.h
@@ -529,8 +529,8 @@ class GpioMotion : public PublishMotion<rm_msgs::GpioData>
     : PublishMotion<rm_msgs::GpioData>(motion, interface)
   {
     delay_ = xmlRpcGetDouble(motion, "delay", 0.01);
-    msg_.gpio_state.assign(8, false);
-    msg_.gpio_name.assign(8, "no_registered");
+    msg_.gpio_state.assign(6, false);
+    msg_.gpio_name.assign(6, "no_registered");
     pin_ = motion["pin"];
     state_ = motion["state"];
     switch (pin_)
@@ -551,15 +551,15 @@ class GpioMotion : public PublishMotion<rm_msgs::GpioData>
         msg_.gpio_name[4] = "gold_gripper";
         break;
       case 5:
-        msg_.gpio_name[5] = "unused";
-        ROS_WARN_STREAM("GPIO port 6 is unused now!");
+        msg_.gpio_name[5] = "silver_pump";
         break;
       case 6:
         msg_.gpio_name[6] = "unused";
         ROS_WARN_STREAM("GPIO port 7 is unused now!");
         break;
       case 7:
-        msg_.gpio_name[7] = "silver_pump";
+        msg_.gpio_name[7] = "unused";
+        ROS_WARN_STREAM("GPIO port 7 is unused now!");
         break;
     }
   }

From fd21d7b26fef71d325a5225177a637dfa9a36ce3 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 1 Aug 2024 04:29:14 +0800
Subject: [PATCH 140/147] Record.

---
 .../config/engineer2_steps_list.yaml          | 138 +++++++++++-------
 1 file changed, 89 insertions(+), 49 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index cdb467f..8fa6918 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -1,16 +1,16 @@
 common:
   speed:
     slowly: &SLOWLY
-      speed: 0.4
+      speed: 0.3
       accel: 0.1
       timeout: 15.
     normally: &NORMALLY
-      speed: 2.5
-      accel: 1.5
+      speed: 0.6
+      accel: 0.6
       timeout: 6
     quickly: &QUICKLY
-      speed: 3
-      accel: 2
+      speed: 1
+      accel: 1
       timeout: 6.
 
   tolerance:
@@ -39,7 +39,7 @@ common:
     get_stored: &JOINT1_GET_STORED_POS
       0.478
     get_stored_gold: &JOINT1_GET_STORED_GOLD_POS
-      0.05
+      0.04
     ready_store: &JOINT1_READY_STORE_POS
       0.44
     store: &JOINT1_STORE_POS
@@ -76,7 +76,7 @@ common:
     get_stored_silver1: &JOINT2_GET_STORED_S1_POS
       -3.833
     get_stored_gold: &JOINT2_GET_STORED_GOLD_POS
-      -1.062
+      -1.257
 
   joint3:
     home: &JOINT3_HOME_POS
@@ -108,7 +108,7 @@ common:
     ready_get_stored_gold: &JOINT3_READY_GET_STORED_GOLD_POS
       -1.67
     get_stored_gold: &JOINT3_GET_STORED_GOLD_POS
-      -1.92
+      -1.75
 
   joint4:
     home: &JOINT4_HOME_POS
@@ -119,6 +119,8 @@ common:
       -1.57
     l90: &JOINT4_L90_POS
       1.57
+    get_stored_gold: &JOINT4_GET_STORED_GOLD_POS
+      -1.721
 
 
   joint5:
@@ -129,7 +131,7 @@ common:
     down_90: &JOINT5_DOWN_90
       1.57
     get_stored_gold: &JOINT5_GET_STORED_GOLD_POS
-      1.586
+      1.726
     store_to_s1: &JOINT5_STORE_TO_S1_POS
       -0.215
     store_to_s2: &JOINT5_STORE_TO_S2_POS
@@ -180,14 +182,14 @@ common:
       tolerance:
         <<: *NORMAL_TOLERANCE
 
-    lv5_r_exchange: &LV5_L_EXCHANGE
+    lv5_r_exchange: &LV5_R_EXCHANGE
       joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_L_EXCHANGE_POS, *JOINT3_L_EXCHANGE_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
 
-    lv5_l_exchange: &LV5_R_EXCHANGE
+    lv5_l_exchange: &LV5_L_EXCHANGE
       joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90]
       common:
         <<: *NORMALLY
@@ -271,10 +273,10 @@ common:
       pin: 4
       state: false
     open_silver_gripper: &OPEN_S_GRIPPER
-      pin: 7
+      pin: 5
       state: true
     close_silver_gripper: &CLOSE_S_GRIPPER
-      pin: 7
+      pin: 5
       state: false
 
 
@@ -400,6 +402,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+      delay: 0.3
     - step: "arm enter exchange pos"
       arm:
         <<: *LV4_EXCHANGE
@@ -410,6 +413,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_L_EXCHANGE
@@ -421,6 +425,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_R_EXCHANGE
@@ -428,42 +433,47 @@ steps_list:
       gimbal:
         <<: *RIGHT_POS
 
-  LV5_R_CAR_FRONT_EXCHANGE:
+  LV5_R_DROP_GOLD_EXCHANGE:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
         delay: 0.3
+    - step: "drop side gold"
+      gripper:
+        <<: *CLOSE_GOLD
+    - step: "add stone"
+      stone_num:
+        change: "-g"
     - step: "arm enter exchange pos"
       arm:
-        joints: [ *JOINT1_EXCHANGE_POS, -0.774, *JOINT3_R_EXCHANGE_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90 ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
+        <<: *LV5_R_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FRONT_POS
+        <<: *RIGHT_POS
 
-  LV5_L_CAR_FRONT_EXCHANGE:
+  LV5_L_DROP_GOLD_EXCHANGE:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
         delay: 0.3
+    - step: "drop side gold"
+      gripper:
+        <<: *CLOSE_GOLD
+    - step: "add stone"
+      stone_num:
+        change: "-g"
     - step: "arm enter exchange pos"
       arm:
-        joints: [ *JOINT1_EXCHANGE_POS, 1.001, *JOINT3_L_EXCHANGE_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ]
-        common:
-          <<: *NORMALLY
-        tolerance:
-          <<: *NORMAL_TOLERANCE
+        <<: *LV5_L_EXCHANGE
     - step: "gimbal ready"
       gimbal:
-        <<: *FRONT_POS
+        <<: *RIGHT_POS
 
   GET_SMALL_ISLAND:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "gimbal look right"
       gimbal:
         <<: *RIGHT_POS
@@ -632,6 +642,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "arm ready get ground stone"
       arm:
         joints: [*JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, 1.112  , -0.397]
@@ -671,6 +682,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "ready to get silver from gripper1"
       arm:
         joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1]
@@ -714,6 +726,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "ready to get silver from gripper2"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
@@ -756,6 +769,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
@@ -802,19 +816,23 @@ steps_list:
           <<: *NORMAL_TOLERANCE
 
   GET_STORED_GOLD:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
+    - step: "open main gripper"
+      gripper:
+        <<: *OPEN_MAIN_GRIPPER
     - step: "ready to get stored gold"
       arm:
-        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "open main gripper"
-      gripper:
-        <<: *OPEN_MAIN_GRIPPER
     - step: "get stored gold"
       arm:
-        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS]
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_GET_STORED_GOLD_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_L_90]
         common:
           <<: *NORMALLY
         tolerance:
@@ -822,19 +840,22 @@ steps_list:
     - step: "close gold gripper"
       gripper:
         <<: *CLOSE_GOLD
+    - step: "move gold gripper back"
+      gold_pusher:
+        target: 0.00001
     - step: "add stone"
       stone_num:
         change: "+g"
     - step: "move out gold"
       arm:
-        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "lift up gold"
       arm:
-        joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1118,7 +1139,7 @@ steps_list:
     - step: "get gold"
       gold_pusher:
         target: -0.443
-        delay: 0.5
+        delay: 1.2
     - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
@@ -1129,7 +1150,7 @@ steps_list:
     - step: "pull back gold"
       gold_pusher:
         target: -0.054
-        delay: 0.6
+        delay: 1
     - step: "chassis left"
       chassis:
         <<: *CHASSIS_MOVE_LEFT
@@ -1139,15 +1160,12 @@ steps_list:
 
 
   SIDE_BIG_ISLAND:
-    - step: "middle pitch up"
-      middle_pitch:
-        target: *MIDDLE_PITCH_NORMAL
     - step: "gimbal look right"
       gimbal:
         <<: *RIGHT_POS
     - step: "middle pitch up"
       middle_pitch:
-        target: *MIDDLE_PITCH_HIGHEST
+        target: *MIDDLE_PITCH_NORMAL
         delay: 0.3
     - step: "arm ready"
       arm:
@@ -1162,16 +1180,16 @@ steps_list:
   SIDE_BIG_ISLAND0:
     - step: "arm get gold"
       arm:
-        joints: [ 0.040,0.16,-1.42,*JOINT4_R90_POS, 0.097, *JOINT6_HOME_POS ]
+        joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "arm lift gold"
       arm:
-        joints: [ 0.115,0.16,-1.42,*JOINT4_R90_POS,0.097, *JOINT6_HOME_POS ]
+        joints: [ 0.114,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "gimbal look front"
@@ -1179,9 +1197,9 @@ steps_list:
         <<: *FRONT_POS
     - step: "arm pull back gold"
       arm:
-        joints: [ 0.114,0.988,-2.023,*JOINT4_R90_POS,0.232, *JOINT6_HOME_POS ]
+        joints: [ 0.114,0.988,-1.927, *JOINT4_R90_POS,0.344, *JOINT6_HOME_POS ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "chassis left"
@@ -1199,6 +1217,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "gimbal look right"
       gimbal:
         <<: *RIGHT_POS
@@ -1237,7 +1256,7 @@ steps_list:
         target: -0.443
     - step: "arm get gold"
       arm:
-        joints: [ 0.040,0.162,-1.414,*JOINT4_R90_POS, 0.097, *JOINT6_HOME_POS ]
+        joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1247,7 +1266,7 @@ steps_list:
         target: 0.055
     - step: "arm lift gold"
       arm:
-        joints: [ 0.115,0.162,-1.414,*JOINT4_R90_POS,0.097, *JOINT6_HOME_POS ]
+        joints: [ 0.114,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1263,9 +1282,9 @@ steps_list:
         <<: *FRONT_POS
     - step: "arm pull back gold"
       arm:
-        joints: [ 0.115,0.988,-2.023,*JOINT4_R90_POS,0.233, *JOINT6_HOME_POS ]
+        joints: [ 0.114,0.988,-1.927, *JOINT4_R90_POS,0.344, *JOINT6_HOME_POS ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "chassis left"
@@ -1372,6 +1391,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "ready to get silver from gripper1"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
@@ -1419,6 +1439,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "ready to get silver from gripper2"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
@@ -1462,6 +1483,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1509,6 +1531,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "ready to get stored gold"
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1529,6 +1552,9 @@ steps_list:
     - step: "close gold gripper"
       gripper:
         <<: *CLOSE_GOLD
+    - step: "move gold gripper back"
+      gold_pusher:
+        target: 0.00001
     - step: "add stone"
       stone_num:
         change: "-g"
@@ -1556,6 +1582,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "ready to get silver from gripper1"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
@@ -1600,6 +1627,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "ready to get silver from gripper2"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
@@ -1640,6 +1668,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1687,6 +1716,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "ready to get stored gold"
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1707,6 +1737,9 @@ steps_list:
     - step: "close gold gripper"
       gripper:
         <<: *CLOSE_GOLD
+    - step: "move gold gripper back"
+      gold_pusher:
+        target: 0.00001
     - step: "add stone"
       stone_num:
         change: "-g"
@@ -1734,6 +1767,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "ready to get silver from gripper1"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
@@ -1778,6 +1812,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "ready to get silver from gripper2"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
@@ -1818,6 +1853,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1865,6 +1901,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
     - step: "ready to get stored gold"
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1885,6 +1922,9 @@ steps_list:
     - step: "close gold gripper"
       gripper:
         <<: *CLOSE_GOLD
+    - step: "move gold gripper back"
+      gold_pusher:
+        target: 0.00001
     - step: "add stone"
       stone_num:
         change: "-g"

From b4ad7edd48bb98adc9c313c47884e66dd602a197 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 1 Aug 2024 08:53:06 +0800
Subject: [PATCH 141/147] Swap exchange direction of engineer2.

---
 ...al_motion_planner_planning_pipeline.launch.xml | 15 ---------------
 .../config/engineer2_steps_list.yaml              | 11 ++++++-----
 2 files changed, 6 insertions(+), 20 deletions(-)
 delete mode 100644 engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml

diff --git a/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
deleted file mode 100644
index c7c4cf5..0000000
--- a/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
+++ /dev/null
@@ -1,15 +0,0 @@
-<launch>
-
-  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
-  <arg name="planning_adapters" default="" />
-
-  <param name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />
-  <param name="request_adapters" value="$(arg planning_adapters)" />
-
-  <!-- Define default planner (for all groups) -->
-  <param name="default_planner_config" value="PTP" />
-
-  <!-- MoveGroup capabilities to load for this pipeline, append sequence capability -->
-  <param name="capabilities" value="pilz_industrial_motion_planner/MoveGroupSequenceAction
-                                    pilz_industrial_motion_planner/MoveGroupSequenceService" />
-</launch>
diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index 8fa6918..cd18ea2 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -182,14 +182,14 @@ common:
       tolerance:
         <<: *NORMAL_TOLERANCE
 
-    lv5_r_exchange: &LV5_R_EXCHANGE
+    lv5_r_exchange: &LV5_L_EXCHANGE
       joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_L_EXCHANGE_POS, *JOINT3_L_EXCHANGE_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90]
       common:
         <<: *NORMALLY
       tolerance:
         <<: *NORMAL_TOLERANCE
 
-    lv5_l_exchange: &LV5_L_EXCHANGE
+    lv5_l_exchange: &LV5_R_EXCHANGE
       joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90]
       common:
         <<: *NORMALLY
@@ -1151,12 +1151,13 @@ steps_list:
       gold_pusher:
         target: -0.054
         delay: 1
-    - step: "chassis left"
-      chassis:
-        <<: *CHASSIS_MOVE_LEFT
     - step: "add stone"
       stone_num:
         change: "+g"
+    - step: "chassis left"
+      chassis:
+        <<: *CHASSIS_MOVE_LEFT
+
 
 
   SIDE_BIG_ISLAND:

From 06de8595cf8f2e0f81d1eebd439035db9069423a Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Mon, 5 Aug 2024 23:26:59 +0800
Subject: [PATCH 142/147] Modify get small island motion.

---
 engineer2_arm_config/launch/gazebo.launch     |  34 ----
 engineer2_arm_config/launch/moveit.rviz       |  16 +-
 ...ntrol_moveit_controller_manager.launch.xml |   4 -
 .../launch/ros_controllers.launch             |  11 --
 .../config/engineer2_steps_list.yaml          | 185 ++++++++++++------
 5 files changed, 135 insertions(+), 115 deletions(-)
 delete mode 100644 engineer2_arm_config/launch/gazebo.launch
 delete mode 100644 engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml
 delete mode 100644 engineer2_arm_config/launch/ros_controllers.launch

diff --git a/engineer2_arm_config/launch/gazebo.launch b/engineer2_arm_config/launch/gazebo.launch
deleted file mode 100644
index 9f7e235..0000000
--- a/engineer2_arm_config/launch/gazebo.launch
+++ /dev/null
@@ -1,34 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-  <!-- Gazebo options -->
-  <arg name="gazebo_gui" default="true" doc="Start Gazebo GUI"/>
-  <arg name="paused" default="false" doc="Start Gazebo paused"/>
-  <arg name="world_name" default="worlds/empty.world" doc="Gazebo world file"/>
-  <arg name="world_pose" default="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" doc="Pose to spawn the robot at"/>
-  <arg name="initial_joint_positions" default=" -J joint1 0.0101 -J joint2 0 -J joint3 0 -J joint4 0 -J joint5 0 -J joint6 0" doc="Initial joint configuration of the robot"/>
-
-  <!-- Start Gazebo paused to allow the controllers to pickup the initial pose -->
-  <include file="$(find gazebo_ros)/launch/empty_world.launch" pass_all_args="true">
-    <arg name="paused" value="true"/>
-  </include>
-
-  <!-- Set the robot urdf on the parameter server -->
-  <param name="robot_description" command="xacro  '$(find rm_description)/urdf/engineer2/engineer2.urdf.xacro'" />
-
-  <!-- Unpause the simulation after loading the robot model -->
-  <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
-
-  <!-- Spawn the robot in Gazebo -->
-  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg unpause) $(arg world_pose) $(arg initial_joint_positions)"
-    respawn="false" output="screen" />
-
-  <!-- Load the controller parameters onto the parameter server -->
-  <rosparam file="$(find engineer2_arm_config)/config/gazebo_controllers.yaml" />
-  <include file="$(dirname)/ros_controllers.launch"/>
-
-  <!-- Spawn the Gazebo ROS controllers -->
-  <node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />
-
-  <!-- Given the published joint states, publish tf for the robot links -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
-</launch>
diff --git a/engineer2_arm_config/launch/moveit.rviz b/engineer2_arm_config/launch/moveit.rviz
index 09ce292..e70752e 100644
--- a/engineer2_arm_config/launch/moveit.rviz
+++ b/engineer2_arm_config/launch/moveit.rviz
@@ -354,17 +354,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: -0.27402463555336
-        Y: -0.39333611726760864
-        Z: 0.4662781059741974
+        X: -0.3085601031780243
+        Y: 0.24779054522514343
+        Z: 0.4662521481513977
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.419796109199524
+      Pitch: 1.5697963237762451
       Target Frame: base_link
-      Yaw: 3.614947557449341
+      Yaw: 3.154945135116577
     Saved: ~
 Window Geometry:
   Displays:
@@ -374,11 +374,7 @@ Window Geometry:
     collapsed: false
   Hide Left Dock: false
   Hide Right Dock: false
-  MotionPlanning:
-    collapsed: false
-  MotionPlanning - Trajectory Slider:
-    collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000005a6fc0200000007fb000000100044006900730070006c006100790073010000003d000005a6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002f4000002ef0000017d00ffffff000007bf000005a600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000005a6fc0200000007fb000000100044006900730070006c006100790073010000003d000005a6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002f4000002ef0000000000000000000007bf000005a600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
   Width: 2488
diff --git a/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml
deleted file mode 100644
index 9ebc91c..0000000
--- a/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml
+++ /dev/null
@@ -1,4 +0,0 @@
-<launch>
-	<!-- Define MoveIt controller manager plugin -->
-	<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager" />
-</launch>
diff --git a/engineer2_arm_config/launch/ros_controllers.launch b/engineer2_arm_config/launch/ros_controllers.launch
deleted file mode 100644
index f149647..0000000
--- a/engineer2_arm_config/launch/ros_controllers.launch
+++ /dev/null
@@ -1,11 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-
-  <!-- Load joint controller configurations from YAML file to parameter server -->
-  <rosparam file="$(find engineer2_arm_config)/config/ros_controllers.yaml" command="load"/>
-
-  <!-- Load the controllers -->
-  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-    output="screen" args="arm_trajectory_controller "/>
-
-</launch>
diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index cd18ea2..f280a96 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -33,7 +33,7 @@ common:
     ready_get_small: &JOINT1_READY_GET_SMALL_POS
       0.42
     get_small: &JOINT1_GET_SMALL_POS
-      0.3
+      0.34
     ready_get_stored: &JOINT1_READY_GET_STORED_POS
       0.54
     get_stored: &JOINT1_GET_STORED_POS
@@ -58,7 +58,7 @@ common:
     r_exchange: &JOINT2_R_EXCHANGE_POS
       -2.5
     get_small_island_l: &JOINT2_GET_SMALL_ISLAND_L_POS
-      -0.098
+      0.858
     get_small_island_m: &JOINT2_GET_SMALL_ISLAND_M_POS
                           -1.101
     get_small_island_r: &JOINT2_GET_SMALL_ISLAND_R_POS
@@ -88,7 +88,7 @@ common:
     r_exchange: &JOINT3_R_EXCHANGE_POS
       0.7
     get_small_island_l: &JOINT3_GET_SMALL_ISLAND_L_POS
-                          -1.948
+                          -1.643
     get_small_island_m: &JOINT3_GET_SMALL_ISLAND_M_POS
                           -1.833
     get_small_island_r: &JOINT3_GET_SMALL_ISLAND_R_POS
@@ -151,13 +151,13 @@ common:
     get_small2: &JOINT6_SMALL2
       4.387
     get_small1: &JOINT6_SMALL1
-      2.7
-    get_s3: &JOINT6_GET_S3
-      -0.9
-    get_s2: &JOINT6_GET_S2
-      -0.346
-    get_s1: &JOINT6_GET_S1
-      -0.65
+      -0.481
+    get_own_s3: &JOINT6_GET_S3
+                  0.6536
+    get_own_s2: &JOINT6_GET_S2
+                  1.22727
+    get_own_s1: &JOINT6_GET_S1
+                  0.9794
 
   middle_pitch:
     ground_stone: &MIDDLE_PITCH_GROUND_STONE
@@ -210,7 +210,7 @@ common:
       frame: base_link
       position: [ -3., -0.3, 0. ]
     follow_ee: &FOLLOW_EE
-      frame: base_link
+      frame: link6
       position: [ 0.15, 0.00, 0. ]
 
   chassis_forward: &CHASSIS_FORWARD
@@ -240,6 +240,15 @@ common:
     common:
       timeout: 3.
 
+  chassis_move_back: &CHASSIS_MOVE_BACK
+    frame: base_link
+    position: [ -0.4, 0.0 ]
+    yaw: 0.0
+    chassis_tolerance_position: 0.1
+    chassis_tolerance_angular: 0.2
+    common:
+      timeout: 3.
+
 
   gripper:
     open_main_gripper: &OPEN_MAIN_GRIPPER
@@ -474,9 +483,9 @@ steps_list:
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
         delay: 0.3
-    - step: "gimbal look right"
+    - step: "gimbal look front"
       gimbal:
-        <<: *RIGHT_POS
+        <<: *FRONT_POS
     - step: "ready get small island silver"
       arm:
         joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1]
@@ -487,6 +496,7 @@ steps_list:
     - step: "open main gripper"
       gripper:
         <<: *OPEN_MAIN_GRIPPER
+  GET_SMALL_ISLAND0:
     - step: "get small island silver"
       arm:
         joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ]
@@ -496,11 +506,22 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "pull up silver"
       arm:
-        joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "chassis back"
+      chassis:
+        <<: *CHASSIS_MOVE_BACK
+    - step: "move stone away"
+      arm:
+        joints: [ *JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, *JOINT5_DOWN_90  , 0.000001 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
 
   STORE_SILVER_TO_GRIPPER1:
     - step: "silver rotator up"
@@ -804,7 +825,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
         common:
-          <<: *NORMALLY
+          <<: *SLOWLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "left arm"
@@ -1025,6 +1046,9 @@ steps_list:
     - step: "open gripper"
       gripper:
         <<: *OPEN_S3
+    - step: "open silver gripper"
+      gripper:
+        <<: *OPEN_S_GRIPPER
     - step: "lift gimbal up"
       arm:
         joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_AVOID_CONTROLLER_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
@@ -1035,15 +1059,6 @@ steps_list:
     - step: "gimbal look right"
       gimbal:
         <<: *RIGHT_POS
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_S1
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_S2
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_S3
     - step: "rotate down"
       silver_rotator:
         target: 0.0001
@@ -1053,28 +1068,16 @@ steps_list:
     - step: "pusher back"
       silver_pusher:
         target: -0.00001
-    - step: "turn 180"
-      chassis:
-        <<: *CHASSIS_TURN_180
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_S1
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_S2
-    - step: "open gripper"
-      gripper:
-        <<: *OPEN_S3
-    - step: "open silver gripper"
-      gripper:
-        <<: *OPEN_S_GRIPPER
     - step: "gimbal look back"
       gimbal:
         <<: *BACK_POS
+    - step: "turn 180"
+      chassis:
+        <<: *CHASSIS_TURN_180
   GRIPPER_THREE_SILVER0:
     - step: "get silver"
       silver_pusher:
-        target: -0.04
+        target: -0.06
         delay: 0.5
     - step: "pusher back little"
       silver_pusher:
@@ -1086,8 +1089,8 @@ steps_list:
         delay: 0.5
     - step: "pusher back"
       silver_pusher:
-        target: 0.
-        delay: 0.1
+        target: 0.05
+        delay: 0.05
     - step: "rotate up"
       silver_rotator:
         target: 1.64
@@ -1139,7 +1142,7 @@ steps_list:
     - step: "get gold"
       gold_pusher:
         target: -0.443
-        delay: 1.2
+        delay: 2
     - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
@@ -1393,6 +1396,13 @@ steps_list:
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
         delay: 0.3
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
     - step: "ready to get silver from gripper1"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
@@ -1421,12 +1431,19 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "close silver gripper"
       gripper:
         <<: *CLOSE_S_GRIPPER
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_L_EXCHANGE
@@ -1441,6 +1458,13 @@ steps_list:
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
         delay: 0.3
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
     - step: "ready to get silver from gripper2"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
@@ -1471,6 +1495,13 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_L_EXCHANGE
@@ -1487,16 +1518,16 @@ steps_list:
         delay: 0.3
     - step: "lift arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *QUICKLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "ready to get silver from gripper3"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "open main gripper"
@@ -1519,7 +1550,14 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *QUICKLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "arm enter exchange pos"
@@ -1535,7 +1573,7 @@ steps_list:
         delay: 0.3
     - step: "ready to get stored gold"
       arm:
-        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ]
         common:
           <<: *QUICKLY
         tolerance:
@@ -1545,7 +1583,7 @@ steps_list:
         <<: *OPEN_MAIN_GRIPPER
     - step: "get stored gold"
       arm:
-        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_L_90 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1561,14 +1599,14 @@ steps_list:
         change: "-g"
     - step: "move out gold"
       arm:
-        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "lift up gold"
       arm:
-        joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1584,6 +1622,13 @@ steps_list:
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
         delay: 0.3
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
     - step: "ready to get silver from gripper1"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
@@ -1612,12 +1657,19 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "close silver gripper"
       gripper:
         <<: *CLOSE_S_GRIPPER
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_R_EXCHANGE
@@ -1629,6 +1681,13 @@ steps_list:
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
         delay: 0.3
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
     - step: "ready to get silver from gripper2"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
@@ -1659,6 +1718,13 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *QUICKLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_R_EXCHANGE
@@ -1672,11 +1738,11 @@ steps_list:
         delay: 0.3
     - step: "lift arm"
       arm:
-        joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
           <<: *QUICKLY
         tolerance:
-          <<: *NORMAL_TOLERANCE
+          <<: *BIGGER_TOLERANCE
     - step: "ready to get silver from gripper3"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
@@ -1704,7 +1770,14 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
         common:
-          <<: *NORMALLY
+          <<: *QUICKLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "lift arm"
+      arm:
+        joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *QUICKLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "arm enter exchange pos"

From 15273a073efeada4d5d327deade05b0940c5bccc Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 8 Aug 2024 16:06:47 +0800
Subject: [PATCH 143/147] Record.

---
 .../config/engineer2_steps_list.yaml          | 370 ++++++++++++------
 1 file changed, 241 insertions(+), 129 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index f280a96..ae3b0b5 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -5,12 +5,12 @@ common:
       accel: 0.1
       timeout: 15.
     normally: &NORMALLY
-      speed: 0.6
-      accel: 0.6
+      speed: 0.4
+      accel: 0.4
       timeout: 6
     quickly: &QUICKLY
-      speed: 1
-      accel: 1
+      speed: 0.6
+      accel: 0.5
       timeout: 6.
 
   tolerance:
@@ -33,7 +33,7 @@ common:
     ready_get_small: &JOINT1_READY_GET_SMALL_POS
       0.42
     get_small: &JOINT1_GET_SMALL_POS
-      0.34
+      0.325
     ready_get_stored: &JOINT1_READY_GET_STORED_POS
       0.54
     get_stored: &JOINT1_GET_STORED_POS
@@ -76,7 +76,9 @@ common:
     get_stored_silver1: &JOINT2_GET_STORED_S1_POS
       -3.833
     get_stored_gold: &JOINT2_GET_STORED_GOLD_POS
-      -1.257
+      -1.062
+    zero_pos: &JOINTN2_ZERO_POS
+      0.000001
 
   joint3:
     home: &JOINT3_HOME_POS
@@ -108,7 +110,7 @@ common:
     ready_get_stored_gold: &JOINT3_READY_GET_STORED_GOLD_POS
       -1.67
     get_stored_gold: &JOINT3_GET_STORED_GOLD_POS
-      -1.75
+      -1.868
 
   joint4:
     home: &JOINT4_HOME_POS
@@ -120,7 +122,7 @@ common:
     l90: &JOINT4_L90_POS
       1.57
     get_stored_gold: &JOINT4_GET_STORED_GOLD_POS
-      -1.721
+      -1.627
 
 
   joint5:
@@ -131,7 +133,7 @@ common:
     down_90: &JOINT5_DOWN_90
       1.57
     get_stored_gold: &JOINT5_GET_STORED_GOLD_POS
-      1.726
+      1.765
     store_to_s1: &JOINT5_STORE_TO_S1_POS
       -0.215
     store_to_s2: &JOINT5_STORE_TO_S2_POS
@@ -196,6 +198,20 @@ common:
       tolerance:
         <<: *NORMAL_TOLERANCE
 
+    lv5_r_straight_exchange: &LV5_R_STRAIGHT_EXCHANGE
+      joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90 ]
+      common:
+        <<: *NORMALLY
+      tolerance:
+        <<: *NORMAL_TOLERANCE
+
+    lv5_l_straight_exchange: &LV5_L_STRAIGHT_EXCHANGE
+      joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ]
+      common:
+        <<: *NORMALLY
+      tolerance:
+        <<: *NORMAL_TOLERANCE
+
   gimbal:
     right_pos: &RIGHT_POS
       frame: base_link
@@ -212,43 +228,9 @@ common:
     follow_ee: &FOLLOW_EE
       frame: link6
       position: [ 0.15, 0.00, 0. ]
-
-  chassis_forward: &CHASSIS_FORWARD
-    frame: base_link
-    position: [ 0.1, 0. ]
-    yaw: 0.0
-    chassis_tolerance_position: 0.1
-    chassis_tolerance_angular: 0.2
-    common:
-      timeout: 2.
-
-  chassis_turn_180: &CHASSIS_TURN_180
-    frame: base_link
-    position: [ 0., 0. ]
-    yaw: 1.57
-    chassis_tolerance_position: 0.1
-    chassis_tolerance_angular: 0.3
-    common:
-      timeout: 4.
-
-  chassis_fmove_left: &CHASSIS_MOVE_LEFT
-    frame: base_link
-    position: [ 0.0, 0.4 ]
-    yaw: 0.0
-    chassis_tolerance_position: 0.1
-    chassis_tolerance_angular: 0.2
-    common:
-      timeout: 3.
-
-  chassis_move_back: &CHASSIS_MOVE_BACK
-    frame: base_link
-    position: [ -0.4, 0.0 ]
-    yaw: 0.0
-    chassis_tolerance_position: 0.1
-    chassis_tolerance_angular: 0.2
-    common:
-      timeout: 3.
-
+    follow_gold_gripper: &FOLLOW_GOLD
+      frame: gold_lifter_link
+      position: [ 0.0, 0.0, 0.0 ]
 
   gripper:
     open_main_gripper: &OPEN_MAIN_GRIPPER
@@ -324,6 +306,9 @@ steps_list:
     - step: "lifter down"
       silver_lifter:
         target: 0.0
+    - step: "rotate up"
+      silver_rotator:
+        target: 1.64
     - step: "get gold"
       gold_pusher:
         target: 0.
@@ -376,24 +361,9 @@ steps_list:
         <<: *CLOSE_MAIN_GRIPPER
 
   HOME_WITH_PITCH:
-    - step: "main close"
-      gripper:
-        <<: *CLOSE_MAIN_GRIPPER
-    - step: "s1 close"
-      gripper:
-        <<: *CLOSE_S1
-    - step: "s2 close"
-      gripper:
-        <<: *CLOSE_S2
-    - step: "s3 close"
-      gripper:
-        <<: *CLOSE_S3
-    - step: "g1 close"
-      gripper:
-        <<: *CLOSE_GOLD
-    - step: "close silver gripper"
-      gripper:
-        <<: *CLOSE_S_GRIPPER
+    - step: "rotate up"
+      silver_rotator:
+        target: 1.64
     - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
@@ -422,7 +392,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.3
+        delay: 0.6
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_L_EXCHANGE
@@ -434,7 +404,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.3
+        delay: 0.6
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_R_EXCHANGE
@@ -442,6 +412,30 @@ steps_list:
       gimbal:
         <<: *RIGHT_POS
 
+  LV5_L_STRAIGHT_EXCHANGE:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV5_L_STRAIGHT_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *RIGHT_POS
+
+  LV5_R_STRAIGHT_EXCHANGE:
+    - step: "middle pitch up"
+      middle_pitch:
+        target: *MIDDLE_PITCH_NORMAL
+        delay: 0.3
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV5_R_STRAIGHT_EXCHANGE
+    - step: "gimbal ready"
+      gimbal:
+        <<: *RIGHT_POS
+
   LV5_R_DROP_GOLD_EXCHANGE:
     - step: "middle pitch up"
       middle_pitch:
@@ -511,9 +505,7 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
-    - step: "chassis back"
-      chassis:
-        <<: *CHASSIS_MOVE_BACK
+  GET_SMALL_ISLAND00:
     - step: "move stone away"
       arm:
         joints: [ *JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, *JOINT5_DOWN_90  , 0.000001 ]
@@ -1053,7 +1045,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_AVOID_CONTROLLER_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "gimbal look right"
@@ -1064,37 +1056,40 @@ steps_list:
         target: 0.0001
     - step: "lifter ready"
       silver_lifter:
-        target: 0.07
+        target: 0.075
     - step: "pusher back"
       silver_pusher:
         target: -0.00001
     - step: "gimbal look back"
       gimbal:
         <<: *BACK_POS
-    - step: "turn 180"
-      chassis:
-        <<: *CHASSIS_TURN_180
   GRIPPER_THREE_SILVER0:
     - step: "get silver"
       silver_pusher:
-        target: -0.06
-        delay: 0.5
+        target: -0.042
+  GRIPPER_THREE_SILVER00:
+    - step: "lifter ready"
+      silver_lifter:
+        target: 0.08
     - step: "pusher back little"
       silver_pusher:
-        target: -0.015
-        delay: 0.2
+        target: -0.02
+        delay: 0.5
+    - step: "rotate up"
+      silver_rotator:
+        target: 0.1
     - step: "lifter up"
       silver_lifter:
         target: 0.2
         delay: 0.5
     - step: "pusher back"
       silver_pusher:
-        target: 0.05
-        delay: 0.05
+        target: 0.0001
+  GRIPPER_THREE_SILVER000:
     - step: "rotate up"
       silver_rotator:
         target: 1.64
-        delay: 0.5
+        delay: 0.4
     - step: "lifter down"
       silver_lifter:
         target: 0.05
@@ -1141,15 +1136,41 @@ steps_list:
   MID_BIG_ISLAND0:
     - step: "get gold"
       gold_pusher:
-        target: -0.443
-        delay: 2
+        target: -0.5
+  MID_BIG_ISLAND00:
+    - step: "pull gold back"
+      gold_pusher:
+        target: -0.444
+        delay: 0.2
+    - step: "lift gold"
+      gold_lifter:
+        target: 0.062
+        delay: 0.6
+  MID_BIG_ISLAND000:
+    - step: "pull back gold"
+      gold_pusher:
+        target: -0.054
+        delay: 1
+    - step: "add stone"
+      stone_num:
+        change: "+g"
     - step: "gimbal look front"
       gimbal:
         <<: *FRONT_POS
+
+  MID_BIG_ISLAND1:
+    - step: "get gold"
+      gold_pusher:
+        target: -0.5
+        delay: 2.0
+    - step: "pull gold back"
+      gold_pusher:
+        target: -0.444
+        delay: 0.1
     - step: "lift gold"
       gold_lifter:
-        target: 0.06
-        delay: 0.5
+        target: 0.062
+        delay: 0.6
     - step: "pull back gold"
       gold_pusher:
         target: -0.054
@@ -1157,9 +1178,9 @@ steps_list:
     - step: "add stone"
       stone_num:
         change: "+g"
-    - step: "chassis left"
-      chassis:
-        <<: *CHASSIS_MOVE_LEFT
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
 
 
 
@@ -1173,14 +1194,41 @@ steps_list:
         delay: 0.3
     - step: "arm ready"
       arm:
-        joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_HOME_POS ]
+        joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_L_90 ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "open main gripper"
       gripper:
           <<: *OPEN_MAIN_GRIPPER
+  SIDE_BIG_ISLAND1:
+    - step: "gimbal look ee"
+      gimbal:
+        <<: *FOLLOW_EE
+    - step: "arm get gold"
+      arm:
+        joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_L_90 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+  SIDE_BIG_ISLAND11:
+    - step: "gimbal look right"
+      gimbal:
+        <<: *RIGHT_POS
+  SIDE_BIG_ISLAND111:
+    - step: "move stone towards down"
+      arm:
+        joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
+
   SIDE_BIG_ISLAND0:
     - step: "arm get gold"
       arm:
@@ -1206,9 +1254,6 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *BIGGER_TOLERANCE
-    - step: "chassis left"
-      chassis:
-        <<: *CHASSIS_MOVE_LEFT
     - step: "move stone towards down"
       arm:
         joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ]
@@ -1246,18 +1291,19 @@ steps_list:
         delay: 0.3
     - step: "arm ready"
       arm:
-        joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_HOME_POS ]
+        joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_L_90 ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "open main gripper"
       gripper:
         <<: *OPEN_MAIN_GRIPPER
+
   BOTH_BIG_ISLAND0:
     - step: "get gold"
       gold_pusher:
-        target: -0.443
+        target: -0.5
     - step: "arm get gold"
       arm:
         joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ]
@@ -1265,9 +1311,17 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
+  BOTH_BIG_ISLAND00:
+    - step: "gimbal look right"
+      gimbal:
+        <<: *RIGHT_POS
+    - step: "pull gold"
+      gold_pusher:
+        target: -0.44
+        delay: 0.3
     - step: "lift gold"
       gold_lifter:
-        target: 0.055
+        target: 0.062
     - step: "arm lift gold"
       arm:
         joints: [ 0.114,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ]
@@ -1277,23 +1331,67 @@ steps_list:
           <<: *NORMAL_TOLERANCE
     - step: "pull back gold"
       gold_pusher:
-        target: -0.054
+        target: -0.09
     - step: "add stone"
       stone_num:
         change: "+g"
-    - step: "gimbal look front"
-      gimbal:
-        <<: *FRONT_POS
     - step: "arm pull back gold"
       arm:
-        joints: [ 0.114,0.988,-1.927, *JOINT4_R90_POS,0.344, *JOINT6_HOME_POS ]
+        joints: [ 0.104,0.988,-1.927, *JOINT4_R90_POS,0.344, *JOINT6_HOME_POS ]
         common:
           <<: *SLOWLY
         tolerance:
           <<: *BIGGER_TOLERANCE
-    - step: "chassis left"
-      chassis:
-        <<: *CHASSIS_MOVE_LEFT
+  BOTH_BIG_ISLAND000:
+    - step: "gimbal look right"
+      gimbal:
+        <<: *FRONT_POS
+    - step: "pull back gold"
+      gold_pusher:
+        target: -0.054
+    - step: "move stone towards down"
+      arm:
+        joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+  BOTH_BIG_ISLAND1:
+    - step: "get gold"
+      gold_pusher:
+        target: -0.5
+    - step: "arm get gold"
+      arm:
+        joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_L_90 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "pull gold"
+      gold_pusher:
+        target: -0.444
+        delay: 0.2
+    - step: "lift gold"
+      gold_lifter:
+        target: 0.062
+    - step: "gimbal look ee"
+      gimbal:
+        <<: *FOLLOW_EE
+  BOTH_BIG_ISLAND11:
+    - step: "gimbal look right"
+      gimbal:
+        <<: *RIGHT_POS
+  BOTH_BIG_ISLAND111:
+    - step: "pull back gold"
+      gold_pusher:
+        target: -0.054
+    - step: "add stone"
+      stone_num:
+        change: "+g"
+    - step: "gimbal look front"
+      gimbal:
+        <<: *FRONT_POS
     - step: "move stone towards down"
       arm:
         joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ]
@@ -1400,14 +1498,14 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "ready to get silver from gripper1"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
@@ -1431,7 +1529,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "close silver gripper"
@@ -1441,9 +1539,12 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
+    - step: "rotate down"
+      silver_rotator:
+        target: 0.0001
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_L_EXCHANGE
@@ -1462,14 +1563,14 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "ready to get silver from gripper2"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "open main gripper"
@@ -1499,7 +1600,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "arm enter exchange pos"
@@ -1520,14 +1621,14 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "ready to get silver from gripper3"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "open main gripper"
@@ -1550,14 +1651,14 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "arm enter exchange pos"
@@ -1567,6 +1668,9 @@ steps_list:
       gimbal:
         <<: *RIGHT_POS
   LV5_L_G:
+    - step: "rotate down"
+      silver_rotator:
+        target: 0.000001
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
@@ -1575,7 +1679,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "open main gripper"
@@ -1594,12 +1698,13 @@ steps_list:
     - step: "move gold gripper back"
       gold_pusher:
         target: 0.00001
+        delay: 0.3
     - step: "add stone"
       stone_num:
         change: "-g"
     - step: "move out gold"
       arm:
-        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ]
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_L_90 ]
         common:
           <<: *NORMALLY
         tolerance:
@@ -1626,14 +1731,14 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "ready to get silver from gripper1"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
 
@@ -1657,7 +1762,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "close silver gripper"
@@ -1667,9 +1772,12 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
+    - step: "rotate down"
+      silver_rotator:
+        target: 0.0001
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_R_EXCHANGE
@@ -1685,14 +1793,14 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "ready to get silver from gripper2"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
     - step: "open main gripper"
@@ -1722,7 +1830,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "arm enter exchange pos"
@@ -1740,7 +1848,7 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "ready to get silver from gripper3"
@@ -1770,14 +1878,14 @@ steps_list:
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
         common:
-          <<: *QUICKLY
+          <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
     - step: "arm enter exchange pos"
@@ -1787,6 +1895,9 @@ steps_list:
       gimbal:
         <<: *RIGHT_POS
   LV5_R_G:
+    - step: "rotate down"
+      silver_rotator:
+        target: 0.000001
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
@@ -1814,12 +1925,13 @@ steps_list:
     - step: "move gold gripper back"
       gold_pusher:
         target: 0.00001
+        delay: 0.3
     - step: "add stone"
       stone_num:
         change: "-g"
     - step: "move out gold"
       arm:
-        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ]
         common:
           <<: *NORMALLY
         tolerance:

From 3fe3719b3b97a16c454cf6599ed242a314391acd Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Thu, 8 Aug 2024 17:33:16 +0800
Subject: [PATCH 144/147] Improve gripper 3 silver.

---
 engineer_middleware/config/engineer2_steps_list.yaml | 12 +++++-------
 1 file changed, 5 insertions(+), 7 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index ae3b0b5..fec9248 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -1082,10 +1082,10 @@ steps_list:
       silver_lifter:
         target: 0.2
         delay: 0.5
+  GRIPPER_THREE_SILVER000:
     - step: "pusher back"
       silver_pusher:
         target: 0.0001
-  GRIPPER_THREE_SILVER000:
     - step: "rotate up"
       silver_rotator:
         target: 1.64
@@ -1093,7 +1093,7 @@ steps_list:
     - step: "lifter down"
       silver_lifter:
         target: 0.05
-        delay: 0.15
+        delay: 0.3
     - step: "lifter down"
       silver_lifter:
         target: 0.0
@@ -1136,16 +1136,14 @@ steps_list:
   MID_BIG_ISLAND0:
     - step: "get gold"
       gold_pusher:
-        target: -0.5
+        target: -0.438
   MID_BIG_ISLAND00:
     - step: "pull gold back"
       gold_pusher:
-        target: -0.444
-        delay: 0.2
+        target: -0.42
     - step: "lift gold"
       gold_lifter:
-        target: 0.062
-        delay: 0.6
+        target: 0.055
   MID_BIG_ISLAND000:
     - step: "pull back gold"
       gold_pusher:

From 570501d5b86c56371b9e2aca311e43f79d878e3d Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Fri, 9 Aug 2024 10:23:49 +0800
Subject: [PATCH 145/147] Add improved direction change motion, place stone on
 arm to a stable place.

---
 .../config/engineer2_steps_list.yaml          | 196 ++++++++++++++----
 1 file changed, 160 insertions(+), 36 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index fec9248..6521b06 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -6,7 +6,7 @@ common:
       timeout: 15.
     normally: &NORMALLY
       speed: 0.4
-      accel: 0.4
+      accel: 0.3
       timeout: 6
     quickly: &QUICKLY
       speed: 0.6
@@ -392,7 +392,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.6
+        delay: 1.0
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_L_EXCHANGE
@@ -400,11 +400,64 @@ steps_list:
       gimbal:
         <<: *RIGHT_POS
 
+  L2R_EXCHANGE:
+    - step: "arm move back"
+      arm:
+        joints: [*JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "reverse joint2 and joint3"
+      arm:
+        joints: [*JOINT1_EXCHANGE_POS, -0.846, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "rotate joint2 to r exchange pos"
+      arm:
+        joints: [*JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV5_R_EXCHANGE
+
+  R2L_EXCHANGE:
+    - step: "fold arm"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "rotate joint2 to l exchange pos"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, -0.846, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "reverse joint2 and joint3"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *BIGGER_TOLERANCE
+    - step: "arm enter exchange pos"
+      arm:
+        <<: *LV5_L_EXCHANGE
+
+
   LV5_R_EXCHANGE:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.6
+        delay: 1.0
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_R_EXCHANGE
@@ -506,13 +559,24 @@ steps_list:
         tolerance:
           <<: *NORMAL_TOLERANCE
   GET_SMALL_ISLAND00:
-    - step: "move stone away"
+    - step: "ready stuck stone"
+      arm:
+        joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "stuck stone"
       arm:
-        joints: [ *JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, *JOINT5_DOWN_90  , 0.000001 ]
+        joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "get ground stone"
+      middle_pitch:
+        target: 0.49
+        delay: 0.5
 
 
   STORE_SILVER_TO_GRIPPER1:
@@ -688,6 +752,23 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+  GROUND_STONE00:
+    - step: "ready stuck stone"
+      arm:
+        joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "stuck stone"
+      arm:
+        joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+
+
 
 
 
@@ -1136,19 +1217,18 @@ steps_list:
   MID_BIG_ISLAND0:
     - step: "get gold"
       gold_pusher:
-        target: -0.438
+        target: -0.44
   MID_BIG_ISLAND00:
     - step: "pull gold back"
       gold_pusher:
         target: -0.42
     - step: "lift gold"
       gold_lifter:
-        target: 0.055
+        target: 0.058
   MID_BIG_ISLAND000:
     - step: "pull back gold"
       gold_pusher:
         target: -0.054
-        delay: 1
     - step: "add stone"
       stone_num:
         change: "+g"
@@ -1159,20 +1239,18 @@ steps_list:
   MID_BIG_ISLAND1:
     - step: "get gold"
       gold_pusher:
-        target: -0.5
-        delay: 2.0
+        target: -0.44
+  MID_BIG_ISLAND11:
     - step: "pull gold back"
       gold_pusher:
-        target: -0.444
-        delay: 0.1
+        target: -0.42
     - step: "lift gold"
       gold_lifter:
         target: 0.062
-        delay: 0.6
+        delay: 0.7
     - step: "pull back gold"
       gold_pusher:
         target: -0.054
-        delay: 1
     - step: "add stone"
       stone_num:
         change: "+g"
@@ -1252,13 +1330,25 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *BIGGER_TOLERANCE
-    - step: "move stone towards down"
+  SIDE_BIG_ISLAND00:
+    - step: "ready stuck stone"
       arm:
-        joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ]
+        joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "stuck stone"
+      arm:
+        joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "get ground stone"
+      middle_pitch:
+        target: 0.49
+        delay: 0.5
 
   BOTH_BIG_ISLAND:
     - step: "middle pitch up"
@@ -1301,7 +1391,7 @@ steps_list:
   BOTH_BIG_ISLAND0:
     - step: "get gold"
       gold_pusher:
-        target: -0.5
+        target: -0.44
     - step: "arm get gold"
       arm:
         joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ]
@@ -1315,11 +1405,10 @@ steps_list:
         <<: *RIGHT_POS
     - step: "pull gold"
       gold_pusher:
-        target: -0.44
-        delay: 0.3
+        target: -0.42
     - step: "lift gold"
       gold_lifter:
-        target: 0.062
+        target: 0.058
     - step: "arm lift gold"
       arm:
         joints: [ 0.114,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ]
@@ -1327,9 +1416,10 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+  BOTH_BIG_ISLAND000:
     - step: "pull back gold"
       gold_pusher:
-        target: -0.09
+        target: -0.054
     - step: "add stone"
       stone_num:
         change: "+g"
@@ -1340,20 +1430,33 @@ steps_list:
           <<: *SLOWLY
         tolerance:
           <<: *BIGGER_TOLERANCE
-  BOTH_BIG_ISLAND000:
+  BOTH_BIG_ISLAND0000:
     - step: "gimbal look right"
       gimbal:
         <<: *FRONT_POS
     - step: "pull back gold"
       gold_pusher:
         target: -0.054
-    - step: "move stone towards down"
+  BOTH_BIG_ISLAND00000:
+    - step: "ready stuck stone"
       arm:
-        joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ]
+        joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11 ]
         common:
           <<: *NORMALLY
         tolerance:
           <<: *NORMAL_TOLERANCE
+    - step: "stuck stone"
+      arm:
+        joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11 ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
+    - step: "get ground stone"
+      middle_pitch:
+        target: 0.49
+        delay: 0.5
+
 
   BOTH_BIG_ISLAND1:
     - step: "get gold"
@@ -1491,7 +1594,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.3
+        delay: 0.6
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1543,6 +1646,13 @@ steps_list:
     - step: "rotate down"
       silver_rotator:
         target: 0.0001
+    - step: "arm move back"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_L_EXCHANGE
@@ -1556,7 +1666,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.3
+        delay: 0.6
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1601,6 +1711,13 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
+    - step: "arm move back"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_L_EXCHANGE
@@ -1614,7 +1731,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.3
+        delay: 0.6
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1659,6 +1776,13 @@ steps_list:
           <<: *NORMALLY
         tolerance:
           <<: *BIGGER_TOLERANCE
+    - step: "arm move back"
+      arm:
+        joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
+        common:
+          <<: *NORMALLY
+        tolerance:
+          <<: *NORMAL_TOLERANCE
     - step: "arm enter exchange pos"
       arm:
         <<: *LV5_L_EXCHANGE
@@ -1672,7 +1796,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.3
+        delay: 0.6
     - step: "ready to get stored gold"
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ]
@@ -1724,7 +1848,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.3
+        delay: 0.6
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1786,7 +1910,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.3
+        delay: 0.6
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1841,7 +1965,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.3
+        delay: 0.6
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1899,7 +2023,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.3
+        delay: 0.6
     - step: "ready to get stored gold"
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -1951,7 +2075,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.3
+        delay: 0.6
     - step: "ready to get silver from gripper1"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ]
@@ -1996,7 +2120,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.3
+        delay: 0.6
     - step: "ready to get silver from gripper2"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ]
@@ -2037,7 +2161,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.3
+        delay: 0.6
     - step: "lift arm"
       arm:
         joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]
@@ -2085,7 +2209,7 @@ steps_list:
     - step: "middle pitch up"
       middle_pitch:
         target: *MIDDLE_PITCH_NORMAL
-        delay: 0.3
+        delay: 0.6
     - step: "ready to get stored gold"
       arm:
         joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ]

From cee21d47b030da345b8f8d8e371a4badd02dfa79 Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Fri, 9 Aug 2024 10:24:44 +0800
Subject: [PATCH 146/147] Disable chassis interface temporarily to avoid crazy
 chassis motion.

---
 engineer2_arm_config/launch/moveit.rviz       | 84 +++++++++----------
 .../include/engineer_middleware/middleware.h  |  5 +-
 2 files changed, 45 insertions(+), 44 deletions(-)

diff --git a/engineer2_arm_config/launch/moveit.rviz b/engineer2_arm_config/launch/moveit.rviz
index e70752e..2d517dd 100644
--- a/engineer2_arm_config/launch/moveit.rviz
+++ b/engineer2_arm_config/launch/moveit.rviz
@@ -4,6 +4,7 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /Global Options1
         - /MotionPlanning1
         - /TF1
         - /TF1/Frames1
@@ -131,7 +132,7 @@ Visualization Manager:
       Frames:
         All Enabled: false
         base_link:
-          Value: false
+          Value: true
         fake_link4:
           Value: false
         gold_lifter_link:
@@ -151,15 +152,15 @@ Visualization Manager:
         link4:
           Value: false
         link5:
-          Value: false
+          Value: true
         link6:
           Value: true
         map:
-          Value: false
+          Value: true
         odom:
-          Value: false
+          Value: true
         pitch:
-          Value: false
+          Value: true
         right_back_wheel:
           Value: false
         right_front_wheel:
@@ -181,38 +182,37 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        base_link:
-          gold_pusher_link:
-            gold_lifter_link:
-              {}
-          left_back_wheel:
-            {}
-          left_front_wheel:
-            {}
-          link1:
-            link2:
-              link3:
-                fake_link4:
-                  {}
-                link4:
-                  link5:
-                    link6:
-                      tools_link:
-                        {}
-              yaw:
-                pitch:
-                  {}
-          right_back_wheel:
-            {}
-          right_front_wheel:
-            {}
-          silver_lifter_link:
-            silver_pusher_link:
-              silver_gripper_link:
-                {}
         map:
           odom:
-            {}
+            base_link:
+              gold_pusher_link:
+                gold_lifter_link:
+                  {}
+              left_back_wheel:
+                {}
+              left_front_wheel:
+                {}
+              link1:
+                link2:
+                  link3:
+                    fake_link4:
+                      {}
+                    link4:
+                      link5:
+                        link6:
+                          tools_link:
+                            {}
+                  yaw:
+                    pitch:
+                      {}
+              right_back_wheel:
+                {}
+              right_front_wheel:
+                {}
+              silver_lifter_link:
+                silver_pusher_link:
+                  silver_gripper_link:
+                    {}
       Update Interval: 0
       Value: true
     - Alpha: 1
@@ -334,7 +334,7 @@ Visualization Manager:
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: base_link
+    Fixed Frame: odom
     Frame Rate: 30
   Name: root
   Tools:
@@ -346,7 +346,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.3629440069198608
+      Distance: 1.3873012065887451
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -354,17 +354,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: -0.3085601031780243
-        Y: 0.24779054522514343
-        Z: 0.4662521481513977
+        X: 0.22694315016269684
+        Y: -0.3219256103038788
+        Z: 0.15961049497127533
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.5697963237762451
+      Pitch: 1.3847957849502563
       Target Frame: base_link
-      Yaw: 3.154945135116577
+      Yaw: 4.408130168914795
     Saved: ~
 Window Geometry:
   Displays:
diff --git a/engineer_middleware/include/engineer_middleware/middleware.h b/engineer_middleware/include/engineer_middleware/middleware.h
index 4d89de4..199935f 100644
--- a/engineer_middleware/include/engineer_middleware/middleware.h
+++ b/engineer_middleware/include/engineer_middleware/middleware.h
@@ -72,8 +72,9 @@ class Middleware
   }
   void run(ros::Duration period)
   {
-    if (is_middleware_control_)
-      chassis_interface_.run(period);
+    // TODO chassis run crazily in motion
+    //    if (is_middleware_control_)
+    //      chassis_interface_.run(period);
   }
 
 private:

From c8a14d62707d80239f30680ececceda24a878fff Mon Sep 17 00:00:00 2001
From: cc0h <7921166012@qq.com>
Date: Tue, 20 Aug 2024 21:30:14 +0800
Subject: [PATCH 147/147] Record.

---
 engineer_middleware/config/engineer2_steps_list.yaml | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml
index 6521b06..ce4df3c 100644
--- a/engineer_middleware/config/engineer2_steps_list.yaml
+++ b/engineer_middleware/config/engineer2_steps_list.yaml
@@ -1217,11 +1217,11 @@ steps_list:
   MID_BIG_ISLAND0:
     - step: "get gold"
       gold_pusher:
-        target: -0.44
+        target: -0.46
   MID_BIG_ISLAND00:
     - step: "pull gold back"
       gold_pusher:
-        target: -0.42
+        target: -0.43
     - step: "lift gold"
       gold_lifter:
         target: 0.058
@@ -1391,7 +1391,7 @@ steps_list:
   BOTH_BIG_ISLAND0:
     - step: "get gold"
       gold_pusher:
-        target: -0.44
+        target: -0.46
     - step: "arm get gold"
       arm:
         joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ]
@@ -1405,7 +1405,7 @@ steps_list:
         <<: *RIGHT_POS
     - step: "pull gold"
       gold_pusher:
-        target: -0.42
+        target: -0.43
     - step: "lift gold"
       gold_lifter:
         target: 0.058