Use Velocity-based Position Control and Bounce2 switches by default, …

…add PILZ planner (#28) * pilz motion planner * velocity control * upgrade verions to 2.0 * make estop work with velocity control * clean up
ycheng517 · Nov 30, 2024 · 7f42062 · 7f42062
1 parent 1bdf7e0
commit 7f42062
Show file tree

Hide file tree

Showing 25 changed files with 653 additions and 344 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -78,7 +78,9 @@
       "typeindex": "cpp",
       "typeinfo": "cpp",
       "variant": "cpp",
-      "*.ipp": "cpp"
+      "*.ipp": "cpp",
+      "callback": "cpp",
+      "image": "cpp"
     },
     "ros.distro": "iron",
     "C_Cpp.default.includePath": [
@@ -97,5 +99,8 @@
       "build": true,
       "install": true,
       "log": true
-    }
-}
+    },
+    "cSpell.words": [
+      "RCLCPP"
+    ]
+}
diff --git a/README.md b/README.md
@@ -183,3 +183,18 @@ You can now plan in RViz and control the simulated arm.
 ### Hand-Eye Calibration
 
 See [ar4_hand_eye_calibration](https://github.com/ycheng517/ar4_hand_eye_calibration)
+
+## Tuning and Tweaks
+
+### Tuning Joint Offsets
+
+If for some reason your robot's joint positions appear misaligned after moving
+to the home position, you can adjust the joint offsets in the
+[joint_offsets/](./ar_hardware_interface/config/joint_offsets/) config folder.
+Select and modify the YAML file corresponding to your AR model to fine-tune the joint positions.
+
+### Switching to Position Control
+
+By default this repo uses velocity-based joint trajectory control. It allows the arm to move a lot faster and the arm movement is also a lot smoother. If for any
+reason you'd like to use the simpler classic position-only control mode, you can
+set `velocity_control_enabled: false` in [driver.yaml](./ar_hardware_interface/config/driver.yaml). Note that you'll need to reduce velocity and acceleration scaling in order for larger motions to succeed.
diff --git a/ar_description/package.xml b/ar_description/package.xml
@@ -1,6 +1,6 @@
 <package format="3">
   <name>ar_description</name>
-  <version>1.0.0</version>
+  <version>2.0.0</version>
   <description>
     URDF Description package for Annin Robotics robotic arms
   </description>

diff --git a/ar_description/urdf/ar.ros2_control.xacro b/ar_description/urdf/ar.ros2_control.xacro
@@ -8,6 +8,7 @@
     calibrate
     robot_parameters_file
     joint_offset_parameters_file
+    driver_parameters_file
   ">
 
     <xacro:property name="robot_parameters"
@@ -16,23 +17,31 @@
     <xacro:property name="joint_offset_parameters"
       value="${xacro.load_yaml(joint_offset_parameters_file)}"/>
 
+    <xacro:property name="driver_parameters"
+      value="${xacro.load_yaml(driver_parameters_file)}"/>
+
     <ros2_control name="${ar_model}" type="system">
 
       <hardware>
         <plugin>${plugin_name}</plugin>
         <param name="ar_model">${ar_model}</param>
         <param name="serial_port">${serial_port}</param>
         <param name="calibrate">${calibrate}</param>
+        <param name="velocity_control_enabled">${driver_parameters['velocity_control_enabled']}</param>
       </hardware>
 
       <joint name="joint_1">
         <command_interface name="position">
           <param name="min">${robot_parameters['j1_limit_min']}</param>
           <param name="max">${robot_parameters['j1_limit_max']}</param>
         </command_interface>
+        <command_interface name="velocity" />
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
         <param name="position_offset">${joint_offset_parameters['joint_1']}</param>
       </joint>
 
@@ -41,9 +50,13 @@
           <param name="min">${robot_parameters['j2_limit_min']}</param>
           <param name="max">${robot_parameters['j2_limit_max']}</param>
         </command_interface>
+        <command_interface name="velocity" />
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
         <param name="position_offset">${joint_offset_parameters['joint_2']}</param>
       </joint>
 
@@ -52,9 +65,13 @@
           <param name="min">${robot_parameters['j3_limit_min']}</param>
           <param name="max">${robot_parameters['j3_limit_max']}</param>
         </command_interface>
+        <command_interface name="velocity" />
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
         <param name="position_offset">${joint_offset_parameters['joint_3']}</param>
       </joint>
 
@@ -63,9 +80,13 @@
           <param name="min">${robot_parameters['j4_limit_min']}</param>
           <param name="max">${robot_parameters['j4_limit_max']}</param>
         </command_interface>
+        <command_interface name="velocity" />
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
         <param name="position_offset">${joint_offset_parameters['joint_4']}</param>
       </joint>
 
@@ -74,9 +95,13 @@
           <param name="min">${robot_parameters['j5_limit_min']}</param>
           <param name="max">${robot_parameters['j5_limit_max']}</param>
         </command_interface>
+        <command_interface name="velocity" />
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
         <param name="position_offset">${joint_offset_parameters['joint_5']}</param>
       </joint>
 
@@ -85,9 +110,13 @@
           <param name="min">${robot_parameters['j6_limit_min']}</param>
           <param name="max">${robot_parameters['j6_limit_max']}</param>
         </command_interface>
+        <command_interface name="velocity" />
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
         <param name="position_offset">${joint_offset_parameters['joint_6']}</param>
       </joint>
 

diff --git a/ar_description/urdf/ar_gazebo.urdf.xacro b/ar_description/urdf/ar_gazebo.urdf.xacro
@@ -23,6 +23,7 @@
     calibrate="False"
     robot_parameters_file="$(find ar_description)/config/$(arg ar_model).yaml"
     joint_offset_parameters_file="$(find ar_hardware_interface)/config/joint_offsets/$(arg ar_model).yaml"
+    driver_parameters_file="$(find ar_hardware_interface)/config/driver.yaml"
   />
 
   <xacro:ar_gripper_ros2_control

diff --git a/ar_description/urdf/ar_macro.xacro b/ar_description/urdf/ar_macro.xacro
@@ -63,7 +63,12 @@
       <parent link="base_link"/>
       <child link="link_1"/>
       <axis xyz="0 0 1"/>
-      <limit lower="${robot_parameters['j1_limit_min']}" upper="${robot_parameters['j1_limit_max']}" effort="0" velocity="0.5"/>
+      <limit
+        lower="${robot_parameters['j1_limit_min']}"
+        upper="${robot_parameters['j1_limit_max']}"
+        effort="-1"
+        velocity="1.0472"
+      />
     </joint>
     <link name="link_2">
       <inertial>
@@ -92,7 +97,12 @@
       <parent link="link_1"/>
       <child link="link_2"/>
       <axis xyz="0 0 -1"/>
-      <limit lower="${robot_parameters['j2_limit_min']}" upper="${robot_parameters['j2_limit_max']}" effort="0.5" velocity="0.5"/>
+      <limit 
+        lower="${robot_parameters['j2_limit_min']}"
+        upper="${robot_parameters['j2_limit_max']}"
+        effort="-1" 
+        velocity="1.0472"
+      />
     </joint>
     <link name="link_3">
       <inertial>
@@ -121,7 +131,12 @@
       <parent link="link_2"/>
       <child link="link_3"/>
       <axis xyz="0 0 -1"/>
-      <limit lower="${robot_parameters['j3_limit_min']}" upper="${robot_parameters['j3_limit_max']}" effort="0" velocity="0.5"/>
+      <limit
+        lower="${robot_parameters['j3_limit_min']}"
+        upper="${robot_parameters['j3_limit_max']}"
+        effort="-1"
+        velocity="1.0472"
+      />
     </joint>
     <link name="link_4">
       <inertial>
@@ -150,7 +165,12 @@
       <parent link="link_3"/>
       <child link="link_4"/>
       <axis xyz="0 0 -1"/>
-      <limit lower="${robot_parameters['j4_limit_min']}" upper="${robot_parameters['j4_limit_max']}" effort="0" velocity="0.5"/>
+      <limit
+        lower="${robot_parameters['j4_limit_min']}"
+        upper="${robot_parameters['j4_limit_max']}"
+        effort="-1"
+        velocity="1.0472"
+      />
     </joint>
     <link name="link_5">
       <inertial>
@@ -179,7 +199,12 @@
       <parent link="link_4"/>
       <child link="link_5"/>
       <axis xyz="1 0 0"/>
-      <limit lower="${robot_parameters['j5_limit_min']}" upper="${robot_parameters['j5_limit_max']}" effort="0" velocity="1.0"/>
+      <limit
+        lower="${robot_parameters['j5_limit_min']}"
+        upper="${robot_parameters['j5_limit_max']}"
+        effort="-1"
+        velocity="1.0472"
+      />
     </joint>
     <link name="link_6">
       <inertial>
@@ -208,7 +233,12 @@
       <parent link="link_5"/>
       <child link="link_6"/>
       <axis xyz="0 0 1"/>
-      <limit lower="${robot_parameters['j6_limit_min']}" upper="${robot_parameters['j6_limit_max']}" effort="0" velocity="1.0"/>
+      <limit
+        lower="${robot_parameters['j6_limit_min']}"
+        upper="${robot_parameters['j6_limit_max']}"
+        effort="-1"
+        velocity="1.0472"
+      />
     </joint>
 
     <!-- center of the end effector mounting surface on link_6 -->

diff --git a/ar_gazebo/package.xml b/ar_gazebo/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ar_gazebo</name>
-  <version>0.1.0</version>
+  <version>2.0.0</version>
   <description>Gazebo simulation the AR4 robot</description>
   <author>Yifei Cheng</author>
   <maintainer email="ycheng517@gmail.com">Yifei Cheng</maintainer>

diff --git a/ar_hardware_interface/config/controllers.yaml b/ar_hardware_interface/config/controllers.yaml
@@ -22,8 +22,13 @@ joint_trajectory_controller:
       - joint_6
     command_interfaces:
       - position
+      - velocity
     state_interfaces:
       - position
+      - velocity
+
+    action_monitor_rate: 20.0
+
     allow_partial_joints_goal: false
     constraints:
       stopped_velocity_tolerance: 0.02
@@ -35,6 +40,24 @@ joint_trajectory_controller:
       joint_5: { trajectory: 0.2, goal: 0.02 }
       joint_6: { trajectory: 0.2, goal: 0.02 }
 
+    gains: # Required because we're controlling a velocity interface
+      joint_1: {p: 10.0,  d: 1.0, i: 1.0, i_clamp: 1.0}  # Smaller 'p' term, since ff term does most of the work
+      joint_2: {p: 10.0,  d: 1.0, i: 1.0, i_clamp: 1.0}
+      joint_3: {p: 10.0,  d: 1.0, i: 1.0, i_clamp: 1.0}
+      joint_4: {p: 10.0,  d: 1.0, i: 1.0, i_clamp: 1.0}
+      joint_5: {p: 10.0,  d: 1.0, i: 1.0, i_clamp: 1.0}
+      joint_6: {p: 10.0,  d: 1.0, i: 1.0, i_clamp: 1.0}
+
+    velocity_ff:
+      joint_1: 1.0
+      joint_2: 1.0
+      joint_3: 1.0
+      joint_4: 1.0
+      joint_5: 1.0
+      joint_6: 1.0
+
+    state_interfaces: ["position", "velocity"]
+
 gripper_controller:
   ros__parameters:
     joint: gripper_jaw1_joint
diff --git a/ar_hardware_interface/config/driver.yaml b/ar_hardware_interface/config/driver.yaml
@@ -0,0 +1 @@
+velocity_control_enabled: true
diff --git a/ar_hardware_interface/include/ar_hardware_interface/ar_hardware_interface.hpp b/ar_hardware_interface/include/ar_hardware_interface/ar_hardware_interface.hpp
@@ -36,8 +36,10 @@ class ARHardwareInterface : public hardware_interface::SystemInterface {
 
   // Motor driver
   TeensyDriver driver_;
-  std::vector<double> actuator_commands_;
+  std::vector<double> actuator_pos_commands_;
+  std::vector<double> actuator_vel_commands_;
   std::vector<double> actuator_positions_;
+  std::vector<double> actuator_velocities_;
 
   // Shared memory
   std::vector<double> joint_offsets_;

diff --git a/ar_hardware_interface/include/ar_hardware_interface/teensy_driver.hpp b/ar_hardware_interface/include/ar_hardware_interface/teensy_driver.hpp
@@ -15,12 +15,13 @@ namespace ar_hardware_interface {
 class TeensyDriver {
  public:
   bool init(std::string ar_model, std::string port, int baudrate,
-            int num_joints);
-  void setStepperSpeed(std::vector<double>& max_speed,
-                       std::vector<double>& max_accel);
+            int num_joints, bool velocity_control_enabled);
   void update(std::vector<double>& pos_commands,
-              std::vector<double>& joint_states);
+              std::vector<double>& vel_commands,
+              std::vector<double>& joint_states,
+              std::vector<double>& joint_velocities);
   void getJointPositions(std::vector<double>& joint_positions);
+  void getJointVelocities(std::vector<double>& joint_velocities);
   bool calibrateJoints();
   bool resetEStop();
   bool isEStopped();
@@ -35,8 +36,11 @@ class TeensyDriver {
   boost::asio::serial_port serial_port_;
   int num_joints_;
   std::vector<double> joint_positions_deg_;
+  std::vector<double> joint_velocities_deg_;
   std::vector<int> enc_calibrations_;
+  bool velocity_control_enabled_ = true;
   bool is_estopped_;
+
   rclcpp::Logger logger_ = rclcpp::get_logger("teensy_driver");
   rclcpp::Clock clock_ = rclcpp::Clock(RCL_ROS_TIME);
 
@@ -47,10 +51,13 @@ class TeensyDriver {
   bool sendCommand(std::string outMsg);  // send arbitrary commands
 
   void checkInit(std::string msg);
+
+  template <typename T>
+  void parseValuesToVector(const std::string msg, std::vector<T>& values);
   void updateEncoderCalibrations(std::string msg);
   void updateJointPositions(std::string msg);
+  void updateJointVelocities(std::string msg);
   void updateEStopStatus(std::string msg);
-  bool succeeded(std::string msg);
 };
 
 }  // namespace ar_hardware_interface

diff --git a/ar_hardware_interface/package.xml b/ar_hardware_interface/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ar_hardware_interface</name>
-  <version>0.1.0</version>
+  <version>2.0.0</version>
   <description>
     The ar_hardware_interface package provides the hardware interface for
     running an AR4 robot using the ros2_control software framework.
@@ -29,10 +29,8 @@
   <exec_depend>joint_state_publisher_gui</exec_depend>
   <exec_depend>joint_trajectory_controller</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
-  <exec_depend>ros2_aruco</exec_depend>
   <exec_depend>ros2controlcli</exec_depend>
   <exec_depend>ros2launch</exec_depend>
-  <exec_depend>rviz2</exec_depend>
   <exec_depend>urdf</exec_depend>
   <exec_depend>xacro</exec_depend>