diff --git a/.vscode/settings.json b/.vscode/settings.json index 31b6015..ab9d225 100644 --- 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 - } -} \ No newline at end of file + }, + "cSpell.words": [ + "RCLCPP" + ] +} diff --git a/README.md b/README.md index 2ba664e..08ff6e8 100644 --- 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 index 8f73e98..df55ef5 100644 --- 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 index 8442670..9beb06c 100644 --- 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,6 +17,9 @@ <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> @@ -23,6 +27,7 @@ <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"> @@ -30,9 +35,13 @@ <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 index 4552bc0..c2aa624 100644 --- 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 index eb7f80b..c06be16 100644 --- 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 index c3c11fe..041d1a9 100644 --- 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 index a6abf6d..0e9e883 100644 --- 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 new file mode 100644 index 0000000..1f86b48 --- /dev/null +++ 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 index 4f2f809..d8faec8 100644 --- 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 index 7c921f3..b164e45 100644 --- 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 index 4b12f6e..604855e 100644 --- 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> diff --git a/ar_hardware_interface/src/ar_hardware_interface.cpp b/ar_hardware_interface/src/ar_hardware_interface.cpp index f9370f1..4a9d743 100644 --- a/ar_hardware_interface/src/ar_hardware_interface.cpp +++ b/ar_hardware_interface/src/ar_hardware_interface.cpp @@ -18,9 +18,13 @@ hardware_interface::CallbackReturn ARHardwareInterface::on_init( // init motor driver std::string serial_port = info_.hardware_parameters.at("serial_port"); std::string ar_model = info_.hardware_parameters.at("ar_model"); + std::string velocity_control_p = + info_.hardware_parameters.at("velocity_control_enabled"); + bool velocity_control_enabled = + velocity_control_p == "True" || velocity_control_p == "true"; int baud_rate = 9600; - bool success = - driver_.init(ar_model, serial_port, baud_rate, info_.joints.size()); + bool success = driver_.init(ar_model, serial_port, baud_rate, + info_.joints.size(), velocity_control_enabled); if (!success) { return hardware_interface::CallbackReturn::ERROR; } @@ -30,8 +34,10 @@ hardware_interface::CallbackReturn ARHardwareInterface::on_init( void ARHardwareInterface::init_variables() { // resize vectors int num_joints = info_.joints.size(); - actuator_commands_.resize(num_joints); + actuator_pos_commands_.resize(num_joints); + actuator_vel_commands_.resize(num_joints); actuator_positions_.resize(num_joints); + actuator_velocities_.resize(num_joints); joint_positions_.resize(num_joints); joint_velocities_.resize(num_joints); joint_efforts_.resize(num_joints); @@ -68,14 +74,6 @@ hardware_interface::CallbackReturn ARHardwareInterface::on_activate( calibrated_ = true; } - // init position commands at current positions - driver_.getJointPositions(actuator_positions_); - for (size_t i = 0; i < info_.joints.size(); ++i) { - // apply offsets, convert from deg to rad for moveit - joint_positions_[i] = degToRad(actuator_positions_[i] + joint_offsets_[i]); - joint_position_commands_[i] = joint_positions_[i]; - } - return hardware_interface::CallbackReturn::SUCCESS; } @@ -92,6 +90,8 @@ ARHardwareInterface::export_state_interfaces() { for (size_t i = 0; i < info_.joints.size(); ++i) { state_interfaces.emplace_back(info_.joints[i].name, "position", &joint_positions_[i]); + state_interfaces.emplace_back(info_.joints[i].name, "velocity", + &joint_velocities_[i]); } return state_interfaces; } @@ -102,6 +102,8 @@ ARHardwareInterface::export_command_interfaces() { for (size_t i = 0; i < info_.joints.size(); ++i) { command_interfaces.emplace_back(info_.joints[i].name, "position", &joint_position_commands_[i]); + command_interfaces.emplace_back(info_.joints[i].name, "velocity", + &joint_velocity_commands_[i]); } return command_interfaces; } @@ -109,18 +111,12 @@ ARHardwareInterface::export_command_interfaces() { hardware_interface::return_type ARHardwareInterface::read( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { driver_.getJointPositions(actuator_positions_); + driver_.getJointVelocities(actuator_velocities_); for (size_t i = 0; i < info_.joints.size(); ++i) { // apply offsets, convert from deg to rad for moveit joint_positions_[i] = degToRad(actuator_positions_[i] + joint_offsets_[i]); + joint_velocities_[i] = degToRad(actuator_velocities_[i]); } - std::string logInfo = "Joint Pos: "; - for (size_t i = 0; i < info_.joints.size(); i++) { - std::stringstream jointPositionStm; - jointPositionStm << std::fixed << std::setprecision(2) - << radToDeg(joint_positions_[i]); - logInfo += info_.joints[i].name + ": " + jointPositionStm.str() + " | "; - } - RCLCPP_DEBUG_THROTTLE(logger_, clock_, 500, logInfo.c_str()); return hardware_interface::return_type::OK; } @@ -128,24 +124,19 @@ hardware_interface::return_type ARHardwareInterface::write( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { for (size_t i = 0; i < info_.joints.size(); ++i) { // convert from rad to deg, apply offsets - actuator_commands_[i] = + actuator_pos_commands_[i] = radToDeg(joint_position_commands_[i]) - joint_offsets_[i]; + actuator_vel_commands_[i] = radToDeg(joint_velocity_commands_[i]); } - std::string logInfo = "Joint Cmd: "; - for (size_t i = 0; i < info_.joints.size(); i++) { - std::stringstream jointPositionStm; - jointPositionStm << std::fixed << std::setprecision(2) - << radToDeg(joint_position_commands_[i]); - logInfo += info_.joints[i].name + ": " + jointPositionStm.str() + " | "; - } - RCLCPP_DEBUG_THROTTLE(logger_, clock_, 500, logInfo.c_str()); - driver_.update(actuator_commands_, actuator_positions_); + driver_.update(actuator_pos_commands_, actuator_vel_commands_, + actuator_positions_, actuator_velocities_); if (driver_.isEStopped()) { std::string logWarn = "Hardware in EStop state. To reset the EStop " "reactivate the hardware component using 'ros2 " "run ar_hardware_interface reset_estop.sh'."; RCLCPP_WARN(logger_, logWarn.c_str()); + return hardware_interface::return_type::ERROR; } return hardware_interface::return_type::OK; diff --git a/ar_hardware_interface/src/arduino_nano_driver.cpp b/ar_hardware_interface/src/arduino_nano_driver.cpp index 6c99069..606a0a2 100644 --- a/ar_hardware_interface/src/arduino_nano_driver.cpp +++ b/ar_hardware_interface/src/arduino_nano_driver.cpp @@ -61,7 +61,7 @@ bool ArduinoNanoDriver::transmit(std::string msg, std::string& err) { if (!ec) { return true; } else { - err = "Error in transmit"; + err = ec.message(); return false; } } diff --git a/ar_hardware_interface/src/teensy_driver.cpp b/ar_hardware_interface/src/teensy_driver.cpp index 9866643..222716e 100644 --- a/ar_hardware_interface/src/teensy_driver.cpp +++ b/ar_hardware_interface/src/teensy_driver.cpp @@ -1,14 +1,15 @@ #include "ar_hardware_interface/teensy_driver.hpp" #include <chrono> +#include <stdexcept> #include <thread> -#define FW_VERSION "0.1.0" +#define FW_VERSION "2.0.0" namespace ar_hardware_interface { bool TeensyDriver::init(std::string ar_model, std::string port, int baudrate, - int num_joints) { + int num_joints, bool velocity_control_enabled) { // @TODO read version from config version_ = FW_VERSION; ar_model_ = ar_model; @@ -30,7 +31,7 @@ bool TeensyDriver::init(std::string ar_model, std::string port, int baudrate, } initialised_ = false; - std::string msg = "STA" + version_ + +"B" + ar_model_ + "\n"; + std::string msg = "STA" + version_ + "B" + ar_model_ + "\n"; while (!initialised_) { RCLCPP_INFO(logger_, "Waiting for response from Teensy on port %s", @@ -44,34 +45,52 @@ bool TeensyDriver::init(std::string ar_model, std::string port, int baudrate, // initialise joint and encoder calibration num_joints_ = num_joints; joint_positions_deg_.resize(num_joints_); + joint_velocities_deg_.resize(num_joints_); enc_calibrations_.resize(num_joints_); + velocity_control_enabled_ = velocity_control_enabled; is_estopped_ = false; return true; } TeensyDriver::TeensyDriver() : serial_port_(io_service_) {} -void TeensyDriver::setStepperSpeed(std::vector<double>& max_speed, - std::vector<double>& max_accel) { - std::string outMsg = "SS"; - for (int i = 0, charIdx = 0; i < num_joints_; ++i, charIdx += 2) { - outMsg += 'A' + charIdx; - outMsg += std::to_string(max_speed[i]); - outMsg += 'A' + charIdx + 1; - outMsg += std::to_string(max_accel[i]); - } - outMsg += "\n"; - exchange(outMsg); -} - // Update between hardware interface and hardware driver void TeensyDriver::update(std::vector<double>& pos_commands, - std::vector<double>& joint_positions) { + std::vector<double>& vel_commands, + std::vector<double>& joint_positions, + std::vector<double>& joint_velocities) { + // log pos_commands + std::string logInfo = "Joint Pos Cmd: "; + for (int i = 0; i < num_joints_; i++) { + std::stringstream jointPositionStm; + jointPositionStm << std::fixed << std::setprecision(2) << pos_commands[i]; + logInfo += std::to_string(i) + ": " + jointPositionStm.str() + " | "; + } + RCLCPP_DEBUG_THROTTLE(logger_, clock_, 500, logInfo.c_str()); + + // log vel_commands + logInfo = "Joint Vel Cmd: "; + for (int i = 0; i < num_joints_; i++) { + std::stringstream jointVelocityStm; + jointVelocityStm << std::fixed << std::setprecision(2) << vel_commands[i]; + logInfo += std::to_string(i) + ": " + jointVelocityStm.str() + " | "; + } + RCLCPP_DEBUG_THROTTLE(logger_, clock_, 500, logInfo.c_str()); + + std::string outMsg = ""; // construct update message - std::string outMsg = "MT"; - for (int i = 0; i < num_joints_; ++i) { - outMsg += 'A' + i; - outMsg += std::to_string(pos_commands[i]); + if (velocity_control_enabled_) { + outMsg += "MV"; + for (int i = 0; i < num_joints_; ++i) { + outMsg += 'A' + i; + outMsg += std::to_string(vel_commands[i]); + } + } else { + outMsg += "MT"; + for (int i = 0; i < num_joints_; ++i) { + outMsg += 'A' + i; + outMsg += std::to_string(pos_commands[i]); + } } outMsg += "\n"; @@ -79,6 +98,27 @@ void TeensyDriver::update(std::vector<double>& pos_commands, exchange(outMsg); joint_positions = joint_positions_deg_; + joint_velocities = joint_velocities_deg_; + + // print joint_positions + logInfo = "Joint Pos: "; + for (int i = 0; i < num_joints_; i++) { + std::stringstream jointPositionStm; + jointPositionStm << std::fixed << std::setprecision(2) + << joint_positions[i]; + logInfo += std::to_string(i) + ": " + jointPositionStm.str() + " | "; + } + RCLCPP_DEBUG_THROTTLE(logger_, clock_, 500, logInfo.c_str()); + + // print joint_velocities + logInfo = "Joint Vel: "; + for (int i = 0; i < num_joints_; i++) { + std::stringstream jointVelocityStm; + jointVelocityStm << std::fixed << std::setprecision(2) + << joint_velocities[i]; + logInfo += std::to_string(i) + ": " + jointVelocityStm.str() + " | "; + } + RCLCPP_DEBUG_THROTTLE(logger_, clock_, 500, logInfo.c_str()); } bool TeensyDriver::calibrateJoints() { @@ -99,11 +139,15 @@ bool TeensyDriver::resetEStop() { return !is_estopped_; } -bool TeensyDriver::isEStopped() { - return is_estopped_; +bool TeensyDriver::isEStopped() { return is_estopped_; } + +void TeensyDriver::getJointVelocities(std::vector<double>& joint_velocities) { + // get current joint velocities + std::string msg = "JV\n"; + exchange(msg); + joint_velocities = joint_velocities_deg_; } -// Send specific commands bool TeensyDriver::sendCommand(std::string outMsg) { return exchange(outMsg); } // Send msg to board and collect data @@ -116,30 +160,40 @@ bool TeensyDriver::exchange(std::string outMsg) { return false; } - receive(inMsg); - // parse msg - std::string header = inMsg.substr(0, 2); - if (header == "ST") { - // init acknowledgement - checkInit(inMsg); - } else if (header == "JC") { - if (!succeeded(inMsg)) { - return false; + while (true) { + receive(inMsg); + std::string header = inMsg.substr(0, 2); + + if (header == "DB") { + // debug message + RCLCPP_DEBUG(logger_, "Debug message: %s", inMsg.c_str()); + } else { + if (header == "ST") { + // init acknowledgement + checkInit(inMsg); + } else if (header == "JC") { + // encoder calibration values + updateEncoderCalibrations(inMsg); + } else if (header == "JP") { + // encoder steps + updateJointPositions(inMsg); + } else if (header == "JV") { + // encoder steps + updateJointVelocities(inMsg); + } else if (header == "ES") { + // estop status + updateEStopStatus(inMsg); + } else if (header == "ER") { + // error message + RCLCPP_INFO(logger_, "ERROR message: %s", inMsg.c_str()); + return false; + } else { + // unknown header + RCLCPP_WARN(logger_, "Unknown header %s", header.c_str()); + return false; + } + return true; } - // encoder calibration values - updateEncoderCalibrations(inMsg); - } else if (header == "JP") { - // encoder steps - updateJointPositions(inMsg); - } else if (header == "ES") { - // estop status - updateEStopStatus(inMsg); - } else if (header == "DB") { - // debug message - RCLCPP_DEBUG(logger_, "Debug message: %s", inMsg.c_str()); - } else { - // unknown header - RCLCPP_WARN(logger_, "Unknown header %s", header.c_str()); } return true; } @@ -153,7 +207,7 @@ bool TeensyDriver::transmit(std::string msg, std::string& err) { if (!ec) { return true; } else { - err = "Error in transmit"; + err = ec.message(); return false; } } @@ -198,56 +252,51 @@ void TeensyDriver::checkInit(std::string msg) { } } -void TeensyDriver::updateEncoderCalibrations(std::string msg) { - // Skip the first 5 letters as they are the header: JCRES - size_t idx1 = msg.find("A", 5) + 1; - size_t idx2 = msg.find("B", 5) + 1; - size_t idx3 = msg.find("C", 5) + 1; - size_t idx4 = msg.find("D", 5) + 1; - size_t idx5 = msg.find("E", 5) + 1; - size_t idx6 = msg.find("F", 5) + 1; - enc_calibrations_[0] = std::stoi(msg.substr(idx1, idx2 - idx1)); - enc_calibrations_[1] = std::stoi(msg.substr(idx2, idx3 - idx2)); - enc_calibrations_[2] = std::stoi(msg.substr(idx3, idx4 - idx3)); - enc_calibrations_[3] = std::stoi(msg.substr(idx4, idx5 - idx4)); - enc_calibrations_[4] = std::stoi(msg.substr(idx5, idx6 - idx5)); - enc_calibrations_[5] = std::stoi(msg.substr(idx6)); - - // @TODO update config file - RCLCPP_INFO(logger_, "Successfully updated encoder calibrations"); +void TeensyDriver::updateJointPositions(const std::string msg) { + parseValuesToVector(msg, joint_positions_deg_); } -void TeensyDriver::updateJointPositions(std::string msg) { - size_t idx1 = msg.find("A", 2) + 1; - size_t idx2 = msg.find("B", 2) + 1; - size_t idx3 = msg.find("C", 2) + 1; - size_t idx4 = msg.find("D", 2) + 1; - size_t idx5 = msg.find("E", 2) + 1; - size_t idx6 = msg.find("F", 2) + 1; - joint_positions_deg_[0] = std::stod(msg.substr(idx1, idx2 - idx1)); - joint_positions_deg_[1] = std::stod(msg.substr(idx2, idx3 - idx2)); - joint_positions_deg_[2] = std::stod(msg.substr(idx3, idx4 - idx3)); - joint_positions_deg_[3] = std::stod(msg.substr(idx4, idx5 - idx4)); - joint_positions_deg_[4] = std::stod(msg.substr(idx5, idx6 - idx5)); - joint_positions_deg_[5] = std::stod(msg.substr(idx6)); +void TeensyDriver::updateJointVelocities(const std::string msg) { + parseValuesToVector(msg, joint_velocities_deg_); } void TeensyDriver::updateEStopStatus(std::string msg) { is_estopped_ = msg.substr(2) == "1" ? true : false; } -bool TeensyDriver::succeeded(std::string msg) { - size_t res_idx = msg.find("RES", 2) + 3; - if (res_idx != std::string::npos && msg[res_idx] == '0') { - const std::string errmsg_code = "MSG"; - size_t idx = msg.find(errmsg_code, 5); - if (idx != std::string::npos) { - RCLCPP_ERROR(logger_, msg.substr(idx + errmsg_code.size()).c_str()); +void TeensyDriver::updateEncoderCalibrations(const std::string msg) { + parseValuesToVector(msg, enc_calibrations_); +} + +template <typename T> +void TeensyDriver::parseValuesToVector(const std::string msg, + std::vector<T>& values) { + values.clear(); + size_t prevIdx = msg.find('A', 2) + 1; + + for (size_t i = 1;; ++i) { + char currentIdentifier = 'A' + i; + size_t currentIdx = msg.find(currentIdentifier, 2); + + try { + if (currentIdx == std::string::npos) { + if constexpr (std::is_same<T, int>::value) { + values.push_back(std::stoi(msg.substr(prevIdx))); + } else if constexpr (std::is_same<T, double>::value) { + values.push_back(std::stod(msg.substr(prevIdx))); + } + break; + } + if constexpr (std::is_same<T, int>::value) { + values.push_back(std::stoi(msg.substr(prevIdx, currentIdx - prevIdx))); + } else if constexpr (std::is_same<T, double>::value) { + values.push_back(std::stod(msg.substr(prevIdx, currentIdx - prevIdx))); + } + } catch (const std::invalid_argument&) { + RCLCPP_WARN(logger_, "Invalid argument, can't parse %s", msg.c_str()); } - return false; + prevIdx = currentIdx + 1; } - - return true; } } // namespace ar_hardware_interface diff --git a/ar_hardware_interface/urdf/ar.urdf.xacro b/ar_hardware_interface/urdf/ar.urdf.xacro index cda6121..c3d3abb 100644 --- a/ar_hardware_interface/urdf/ar.urdf.xacro +++ b/ar_hardware_interface/urdf/ar.urdf.xacro @@ -22,6 +22,7 @@ calibrate="$(arg calibrate)" 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:if value="$(arg include_gripper)"> diff --git a/ar_microcontrollers/AR4_teensy/AR4_teensy.ino b/ar_microcontrollers/AR4_teensy/AR4_teensy.ino index 2d832e7..521083a 100644 --- a/ar_microcontrollers/AR4_teensy/AR4_teensy.ino +++ b/ar_microcontrollers/AR4_teensy/AR4_teensy.ino @@ -1,4 +1,5 @@ #include <AccelStepper.h> +#include <Bounce2.h> #include <Encoder.h> #include <avr/pgmspace.h> #include <math.h> @@ -6,7 +7,7 @@ #include <map> // Firmware version -const char* VERSION = "0.1.0"; +const char* VERSION = "2.0.0"; // Model of the AR4, i.e. mk1, mk2, mk3 String MODEL = ""; @@ -54,17 +55,19 @@ int JOINT_LIMIT_MAX_MK3[] = {170, 90, 52, 180, 105, 180}; // ROS Driver Params /////////////////////////////////////////////////////////////////////////////// -// roughly equals 0, 0, 0, 0, 0, 0 degrees -std::map<String, const int*> REST_ENC_POSITIONS; -const int REST_ENC_POSITIONS_MK1[] = {75556, 23333, 49444, 70489, 11477, 34311}; -const int REST_ENC_POSITIONS_MK2[] = {75556, 23333, 49444, 70489, 11477, 34311}; -const int REST_ENC_POSITIONS_MK3[] = {75556, 23333, 49444, 89600, 11477, 40000}; +// roughly equals 0, 0, 0, 0, 0, 0 degrees without any user-defined offsets. +std::map<String, const int*> REST_MOTOR_STEPS; +const int REST_MOTOR_STEPS_MK1[] = {7555, 2333, 4944, 7049, 2295, 3431}; +const int REST_MOTOR_STEPS_MK2[] = {7555, 2333, 4944, 7049, 2295, 3431}; +const int REST_MOTOR_STEPS_MK3[] = {7555, 2333, 4944, 8960, 2295, 4000}; enum SM { STATE_TRAJ, STATE_ERR }; SM STATE = STATE_TRAJ; const int NUM_JOINTS = 6; AccelStepper stepperJoints[NUM_JOINTS]; +Bounce2::Button limitSwitches[NUM_JOINTS]; +const int DEBOUCE_INTERVAL = 10; // ms // calibration settings const int LIMIT_SWITCH_HIGH[] = { @@ -74,20 +77,17 @@ const int CAL_DIR[] = {-1, -1, 1, const int CAL_SPEED = 500; // motor steps per second const int CAL_SPEED_MULT[] = { 1, 1, 1, 2, 1, 1}; // multiplier to account for motor steps/rev +// num of encoder steps in range of motion of joint +int ENC_RANGE_STEPS[NUM_JOINTS]; // speed and acceleration settings -float JOINT_MAX_SPEED[] = {30.0, 30.0, 30.0, 30.0, 30.0, 30.0}; // deg/s -float JOINT_MAX_ACCEL[] = {10.0, 10.0, 10.0, 10.0, 10.0, 10.0}; // deg/s^2 +float JOINT_MAX_SPEED[] = {60.0, 60.0, 60.0, 60.0, 60.0, 60.0}; // deg/s +float JOINT_MAX_ACCEL[] = {30.0, 30.0, 30.0, 30.0, 30.0, 30.0}; // deg/s^2 char JOINT_NAMES[] = {'A', 'B', 'C', 'D', 'E', 'F'}; -// num of encoder steps in range of motion of joint -int ENC_RANGE_STEPS[NUM_JOINTS]; - bool estop_pressed = false; -void estopPressed() { - estop_pressed = true; -} +void estopPressed() { estop_pressed = true; } void resetEstop() { // if ESTOP button is pressed still, do not reset the flag! @@ -101,6 +101,7 @@ void resetEstop() { // _stepInterval and _n to 0, which is required to avoid a jerk resume // when Estop is reset after interruption of an accelerated motion stepperJoints[i].setCurrentPosition(stepperJoints[i].currentPosition()); + stepperJoints[i].setSpeed(0); } estop_pressed = false; @@ -129,11 +130,9 @@ void setup() { JOINT_LIMIT_MAX["mk2"] = JOINT_LIMIT_MAX_MK2; JOINT_LIMIT_MAX["mk3"] = JOINT_LIMIT_MAX_MK3; - REST_ENC_POSITIONS["mk1"] = REST_ENC_POSITIONS_MK1; - REST_ENC_POSITIONS["mk2"] = REST_ENC_POSITIONS_MK2; - REST_ENC_POSITIONS["mk3"] = REST_ENC_POSITIONS_MK3; - - Serial.begin(9600); + REST_MOTOR_STEPS["mk1"] = REST_MOTOR_STEPS_MK1; + REST_MOTOR_STEPS["mk2"] = REST_MOTOR_STEPS_MK2; + REST_MOTOR_STEPS["mk3"] = REST_MOTOR_STEPS_MK3; for (int i = 0; i < NUM_JOINTS; ++i) { pinMode(STEP_PINS[i], OUTPUT); @@ -141,8 +140,17 @@ void setup() { pinMode(LIMIT_PINS[i], INPUT); } + for (int i = 0; i < NUM_JOINTS; ++i) { + limitSwitches[i] = Bounce2::Button(); + limitSwitches[i].attach(LIMIT_PINS[i], INPUT); + limitSwitches[i].interval(DEBOUCE_INTERVAL); + limitSwitches[i].setPressedState(LIMIT_SWITCH_HIGH[i]); + } + pinMode(ESTOP_PIN, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(ESTOP_PIN), estopPressed, FALLING); + + Serial.begin(9600); } void setupSteppersMK1() { @@ -228,6 +236,11 @@ bool initStateTraj(String inData) { return false; } +template <typename T> +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + void readMotorSteps(int* motorSteps) { for (int i = 0; i < NUM_JOINTS; ++i) { motorSteps[i] = encPos[i].read() / ENC_MULT[i]; @@ -255,35 +268,100 @@ String JointPosToString(double* jointPos) { return out; } -void updateStepperSpeed(String inData) { - int idxSpeedJ1 = inData.indexOf('A'); - int idxAccelJ1 = inData.indexOf('B'); - int idxSpeedJ2 = inData.indexOf('C'); - int idxAccelJ2 = inData.indexOf('D'); - int idxSpeedJ3 = inData.indexOf('E'); - int idxAccelJ3 = inData.indexOf('F'); - int idxSpeedJ4 = inData.indexOf('G'); - int idxAccelJ4 = inData.indexOf('H'); - int idxSpeedJ5 = inData.indexOf('I'); - int idxAccelJ5 = inData.indexOf('J'); - int idxSpeedJ6 = inData.indexOf('K'); - int idxAccelJ6 = inData.indexOf('L'); - - JOINT_MAX_SPEED[0] = inData.substring(idxSpeedJ1 + 1, idxAccelJ1).toFloat(); - JOINT_MAX_ACCEL[0] = inData.substring(idxAccelJ1 + 1, idxSpeedJ2).toFloat(); - JOINT_MAX_SPEED[1] = inData.substring(idxSpeedJ2 + 1, idxAccelJ2).toFloat(); - JOINT_MAX_ACCEL[1] = inData.substring(idxAccelJ2 + 1, idxSpeedJ3).toFloat(); - JOINT_MAX_SPEED[2] = inData.substring(idxSpeedJ3 + 1, idxAccelJ3).toFloat(); - JOINT_MAX_ACCEL[2] = inData.substring(idxAccelJ3 + 1, idxSpeedJ4).toFloat(); - JOINT_MAX_SPEED[3] = inData.substring(idxSpeedJ4 + 1, idxAccelJ4).toFloat(); - JOINT_MAX_ACCEL[3] = inData.substring(idxAccelJ4 + 1, idxSpeedJ5).toFloat(); - JOINT_MAX_SPEED[4] = inData.substring(idxSpeedJ5 + 1, idxAccelJ5).toFloat(); - JOINT_MAX_ACCEL[4] = inData.substring(idxAccelJ5 + 1, idxSpeedJ6).toFloat(); - JOINT_MAX_SPEED[5] = inData.substring(idxSpeedJ6 + 1, idxAccelJ6).toFloat(); - JOINT_MAX_ACCEL[5] = inData.substring(idxAccelJ6 + 1).toFloat(); +String JointVelToString(double* lastVelocity) { + String out; + + for (int i = 0; i < NUM_JOINTS; ++i) { + out += JOINT_NAMES[i]; + out += String(lastVelocity[i], 6); + } + + return out; +} + +void ParseMessage(String& inData, double* cmdJointPos) { + for (int i = 0; i < NUM_JOINTS; i++) { + bool lastJoint = i == NUM_JOINTS - 1; + int msgIdxJ_S, msgIdxJ_E = 0; + msgIdxJ_S = inData.indexOf(JOINT_NAMES[i]); + msgIdxJ_E = (lastJoint) ? -1 : inData.indexOf(JOINT_NAMES[i + 1]); + if (msgIdxJ_S == -1) { + Serial.printf("ER: panic, missing joint %c\n", JOINT_NAMES[i]); + return; + } + if (msgIdxJ_E != -1) { + cmdJointPos[i] = inData.substring(msgIdxJ_S + 1, msgIdxJ_E).toFloat(); + } else { + cmdJointPos[i] = inData.substring(msgIdxJ_S + 1).toFloat(); + } + } } -bool calibrateJoints(int* calJoints) { +void MoveVelocity(String inData) { + double cmdJointVel[NUM_JOINTS]; + ParseMessage(inData, cmdJointVel); + + for (int i = 0; i < NUM_JOINTS; i++) { + if (abs(cmdJointVel[i]) > JOINT_MAX_SPEED[i]) { + Serial.printf("DB: joint %c speed %f > %f, clipping.\n", JOINT_NAMES[i], + cmdJointVel[i], JOINT_MAX_SPEED[i]); + cmdJointVel[i] = sgn(cmdJointVel[i]) * JOINT_MAX_SPEED[i]; + } + cmdJointVel[i] *= MOTOR_STEPS_PER_DEG[MODEL][i]; + stepperJoints[i].setMaxSpeed(abs(cmdJointVel[i])); + stepperJoints[i].setSpeed(cmdJointVel[i]); + stepperJoints[i].move(sgn(cmdJointVel[i]) * __LONG_MAX__); + } +} + +void MoveTo(const int* cmdSteps, int* motorSteps) { + setAllMaxSpeeds(); + for (int i = 0; i < NUM_JOINTS; ++i) { + int diffEncSteps = cmdSteps[i] - motorSteps[i]; + if (abs(diffEncSteps) > 2) { + int diffMotSteps = diffEncSteps * ENC_DIR[i]; + stepperJoints[i].move(diffMotSteps); + } + } +} + +void MoveTo(String inData, int* motorSteps) { + double cmdJointPos[NUM_JOINTS] = {0}; + ParseMessage(inData, cmdJointPos); + + for (int i = 0; i < NUM_JOINTS; i++) { + if (abs(cmdJointPos[i] > 380.0)) { + Serial.printf("ER: panic, joint %c value %f out of range\n", + JOINT_NAMES[i], cmdJointPos[i]); + return; + } + } + + // get current joint position + double curJointPos[NUM_JOINTS]; + encStepsToJointPos(motorSteps, curJointPos); + + // update target joint position + int cmdEncSteps[NUM_JOINTS] = {0}; + jointPosToEncSteps(cmdJointPos, cmdEncSteps); + + MoveTo(cmdEncSteps, motorSteps); +} + +void setAllMaxSpeeds() { + for (int i = 0; i < NUM_JOINTS; ++i) { + stepperJoints[i].setMaxSpeed(JOINT_MAX_SPEED[i] * + MOTOR_STEPS_PER_DEG[MODEL][i]); + } +} + +void updateAllLimitSwitches() { + for (int i = 0; i < NUM_JOINTS; ++i) { + limitSwitches[i].update(); + } +} + +bool moveToLimitSwitches(int* calJoints) { // check which joints to calibrate bool calAllDone = false; bool calJointsDone[NUM_JOINTS]; @@ -291,18 +369,18 @@ bool calibrateJoints(int* calJoints) { calJointsDone[i] = !calJoints[i]; } - // first pass of calibration, fast speed for (int i = 0; i < NUM_JOINTS; i++) { stepperJoints[i].setSpeed(CAL_SPEED * CAL_SPEED_MULT[i] * CAL_DIR[i]); } unsigned long startTime = millis(); while (!calAllDone) { + updateAllLimitSwitches(); calAllDone = true; for (int i = 0; i < NUM_JOINTS; ++i) { // if joint is not calibrated yet if (!calJointsDone[i]) { // check limit switches - if (!reachedLimitSwitch(i)) { + if (!limitSwitches[i].isPressed()) { // limit switch not reached, continue moving safeRunSpeed(stepperJoints[i]); calAllDone = false; @@ -322,46 +400,62 @@ bool calibrateJoints(int* calJoints) { return true; } -void moveAwayFromLimitSwitch() { +bool moveAwayFromLimitSwitch(int* calJoints) { for (int i = 0; i < NUM_JOINTS; i++) { - stepperJoints[i].setSpeed(CAL_SPEED * CAL_SPEED_MULT[i] * CAL_DIR[i] * -1); + if (calJoints[i]) { + stepperJoints[i].setSpeed(CAL_SPEED * CAL_SPEED_MULT[i] * CAL_DIR[i] * + -1); + } } - for (int j = 0; j < 10000000; j++) { + + bool limitSwitchesActive = true; + unsigned long startTime = millis(); + while (limitSwitchesActive || millis() - startTime < 4000) { + limitSwitchesActive = false; + updateAllLimitSwitches(); for (int i = 0; i < NUM_JOINTS; ++i) { - safeRunSpeed(stepperJoints[i]); + if (calJoints[i]) { + if (limitSwitches[i].isPressed()) { + limitSwitchesActive = true; + } + safeRunSpeed(stepperJoints[i]); + } + } + + if (millis() - startTime > 10000) { + return false; } } + for (int i = 0; i < NUM_JOINTS; i++) { - stepperJoints[i].setSpeed(0); + stepperJoints[i].setSpeed(0); // redundancy } - delay(2000); - return; + delay(1000); + return true; } -bool reachedLimitSwitch(int joint) { - int pin = LIMIT_PINS[joint]; - // check multiple times to deal with noise - // possibly EMI from motor cables? - for (int i = 0; i < 5; ++i) { - if (digitalRead(pin) != LIMIT_SWITCH_HIGH[joint]) { - return false; - } +bool moveLimitedAwayFromLimitSwitch(int* calJoints) { + // move the ones that already hit a limit away from it before start of + // calibration + int limitedJoints[NUM_JOINTS] = {0}; + updateAllLimitSwitches(); + for (int i = 0; i < NUM_JOINTS; i++) { + limitedJoints[i] = (calJoints[i] && limitSwitches[i].isPressed()); } - return true; + return moveAwayFromLimitSwitch(limitedJoints); } -void doCalibrationRoutine() { +bool doCalibrationRoutine(String& outputMsg) { // calibrate all joints int calJoints[] = {1, 1, 1, 1, 1, 1}; - int success = calibrateJoints(calJoints); - if (!success) { - // set all speeds to zero - for (int i = 0; i < NUM_JOINTS; ++i) { - stepperJoints[i].setSpeed(0); - } - String msg = String("JCRES0MSG") + "Failed to calibrate joints."; - Serial.println(msg); - return; + if (!moveLimitedAwayFromLimitSwitch(calJoints)) { + outputMsg = "ER: Failed to move away from limit switches at the start."; + return false; + } + + if (!moveToLimitSwitches(calJoints)) { + outputMsg = "ER: Failed to move to limit switches."; + return false; } // record encoder steps @@ -376,53 +470,74 @@ void doCalibrationRoutine() { // move away from the limit switches a bit so that if the next command // is in the wrong direction, the limit switches will not be run over // immediately and become damaged. - moveAwayFromLimitSwitch(); + if (!moveAwayFromLimitSwitch(calJoints)) { + outputMsg = "ER: Failed to move away from limit switches."; + return false; + } + + // restore original max speed + // + for (int i = 0; i < NUM_JOINTS; ++i) { + stepperJoints[i].setMaxSpeed(JOINT_MAX_SPEED[i] * + MOTOR_STEPS_PER_DEG[MODEL][i]); + } // return to original position int curMotorSteps[NUM_JOINTS]; readMotorSteps(curMotorSteps); - // repeatedly set pos, spd, acc 3x due to some joints occasionally not - // moving - for (int j = 0; j < 3; j++) { - for (int i = 0; i < NUM_JOINTS; ++i) { - stepperJoints[i].setAcceleration(JOINT_MAX_ACCEL[i] * - MOTOR_STEPS_PER_DEG[MODEL][i]); - stepperJoints[i].setMaxSpeed(JOINT_MAX_SPEED[i] * - MOTOR_STEPS_PER_DEG[MODEL][i]); - float target_pos = - (REST_ENC_POSITIONS[MODEL][i] / ENC_MULT[i] - curMotorSteps[i]) * - ENC_DIR[i]; - stepperJoints[i].move(target_pos); - safeRun(stepperJoints[i]); + MoveTo(REST_MOTOR_STEPS[MODEL], curMotorSteps); + for (int i = 0; i < NUM_JOINTS; ++i) { + if (estop_pressed) { + return false; } + stepperJoints[i].runToPosition(); } - bool restPosReached = false; - unsigned long startTime = millis(); - while (!restPosReached && millis() - startTime < 10000) { - restPosReached = true; - for (int i = 0; i < NUM_JOINTS; ++i) { - if (abs(stepperJoints[i].distanceToGo()) > 5) { - safeRun(stepperJoints[i]); - restPosReached = false; - } + + // calibration done, send calibration values + // N.B. calibration values aren't used right now + outputMsg = String("JC") + "A" + calSteps[0] + "B" + calSteps[1] + "C" + + calSteps[2] + "D" + calSteps[3] + "E" + calSteps[4] + "F" + + calSteps[5]; + return true; +} + +void updateMotorVelocities(int* motorSteps, int* lastMotorSteps, + int* checksteps, unsigned long* lastVelocityCalc, + double* lastVelocity) { + for (int i = 0; i < NUM_JOINTS; i++) { + // for really small velocities we still get + // artifacts, but quite manageable now! + + if (micros() - lastVelocityCalc[i] < 5000) { + // we want to trigger calculation only after x ms but + // immediately when steps change after that + checksteps[i] = motorSteps[i]; + continue; } - } - if (!restPosReached) { - // set all speeds to zero - for (int i = 0; i < NUM_JOINTS; ++i) { - stepperJoints[i].setSpeed(0); - safeRun(stepperJoints[i]); + + // have to add some sort of outlier-filter here , maybe moving average .. + if (abs(stepperJoints[i].speed() / MOTOR_STEPS_PER_DEG[MODEL][i]) < 5) { + // NB! trying to fix artifacts at low velocity + if (abs(motorSteps[i] - checksteps[i]) > 0) { + lastVelocity[i] = + stepperJoints[i].speed() / MOTOR_STEPS_PER_DEG[MODEL][i]; + lastMotorSteps[i] = motorSteps[i]; + lastVelocityCalc[i] = micros(); + } else if (stepperJoints[i].speed() == 0) { + lastVelocity[i] = 0; + } + } else { + unsigned long currentMicros = micros(); + double delta = (currentMicros - lastVelocityCalc[i]); + if (abs(motorSteps[i] - checksteps[i]) > 0) { + // calculate TRUE motor velocity + lastVelocity[i] = ENC_DIR[i] * (motorSteps[i] - lastMotorSteps[i]) / + MOTOR_STEPS_PER_DEG[MODEL][i] / (delta / 1000000.0); + lastMotorSteps[i] = motorSteps[i]; + lastVelocityCalc[i] = currentMicros; + } } - String msg = String("JCRES0MSG") + "Failed to return to rest position."; - Serial.println(msg); - return; } - - // calibration done, send calibration values - String msg = String("JCRES1") + "A" + calSteps[0] + "B" + calSteps[1] + "C" + - calSteps[2] + "D" + calSteps[3] + "E" + calSteps[4] + "F" + - calSteps[5]; - Serial.println(msg); } void stateTRAJ() { @@ -432,12 +547,16 @@ void stateTRAJ() { // initialise joint steps double curJointPos[NUM_JOINTS]; int curMotorSteps[NUM_JOINTS]; + int lastMotorSteps[NUM_JOINTS]; + int checksteps[NUM_JOINTS]; + double lastVelocity[NUM_JOINTS]; + unsigned long lastVelocityCalc[NUM_JOINTS]; + readMotorSteps(curMotorSteps); - double cmdJointPos[NUM_JOINTS]; - int cmdEncSteps[NUM_JOINTS]; for (int i = 0; i < NUM_JOINTS; ++i) { - cmdEncSteps[i] = curMotorSteps[i]; + lastVelocityCalc[i] = micros(); + lastMotorSteps[i] = curMotorSteps[i]; } // start loop @@ -449,6 +568,12 @@ void stateTRAJ() { inData += received; } + if (MODEL != "") { + readMotorSteps(curMotorSteps); + updateMotorVelocities(curMotorSteps, lastMotorSteps, checksteps, + lastVelocityCalc, lastVelocity); + } + // process message when new line character is received if (received == '\n') { String function = inData.substring(0, 2); @@ -464,66 +589,48 @@ void stateTRAJ() { } if (function == "MT") { - // update trajectory information - readMotorSteps(curMotorSteps); + // clear speed counter + for (int i = 0; i < NUM_JOINTS; i++) { + if (stepperJoints[i].speed() == 0) { + lastVelocityCalc[i] = micros(); + } + } + + MoveTo(inData, curMotorSteps); // update the host about estop state String msg = String("ES") + estop_pressed; Serial.println(msg); - // get new position commands - int msgIdxJ1 = inData.indexOf('A'); - int msgIdxJ2 = inData.indexOf('B'); - int msgIdxJ3 = inData.indexOf('C'); - int msgIdxJ4 = inData.indexOf('D'); - int msgIdxJ5 = inData.indexOf('E'); - int msgIdxJ6 = inData.indexOf('F'); - cmdJointPos[0] = inData.substring(msgIdxJ1 + 1, msgIdxJ2).toFloat(); - cmdJointPos[1] = inData.substring(msgIdxJ2 + 1, msgIdxJ3).toFloat(); - cmdJointPos[2] = inData.substring(msgIdxJ3 + 1, msgIdxJ4).toFloat(); - cmdJointPos[3] = inData.substring(msgIdxJ4 + 1, msgIdxJ5).toFloat(); - cmdJointPos[4] = inData.substring(msgIdxJ5 + 1, msgIdxJ6).toFloat(); - cmdJointPos[5] = inData.substring(msgIdxJ6 + 1).toFloat(); - jointPosToEncSteps(cmdJointPos, cmdEncSteps); - - // update target joint positions - readMotorSteps(curMotorSteps); - for (int i = 0; i < NUM_JOINTS; ++i) { - int diffEncSteps = cmdEncSteps[i] - curMotorSteps[i]; - if (abs(diffEncSteps) > 2) { - int diffMotSteps = diffEncSteps * ENC_DIR[i]; - // if (diffMotSteps < MOTOR_STEPS_PER_REV[i]) { - // // for the last rev of motor, introduce artificial - // decceleration - // // to help prevent overshoot - // diffMotSteps = diffMotSteps / 2; - // } - stepperJoints[i].move(diffMotSteps); - safeRun(stepperJoints[i]); + } else if (function == "MV") { + // clear speed counter + for (int i = 0; i < NUM_JOINTS; i++) { + if (stepperJoints[i].speed() == 0) { + lastVelocityCalc[i] = micros(); } } - } else if (function == "JC") { - doCalibrationRoutine(); + + MoveVelocity(inData); + + // update the host about estop state + String msg = String("ES") + estop_pressed; + Serial.println(msg); + } else if (function == "JP") { readMotorSteps(curMotorSteps); encStepsToJointPos(curMotorSteps, curJointPos); String msg = String("JP") + JointPosToString(curJointPos); Serial.println(msg); - } else if (function == "SS") { - updateStepperSpeed(inData); - // set motor speed and acceleration - for (int i = 0; i < NUM_JOINTS; ++i) { - stepperJoints[i].setAcceleration(JOINT_MAX_ACCEL[i] * - MOTOR_STEPS_PER_DEG[MODEL][i]); - stepperJoints[i].setMaxSpeed(JOINT_MAX_SPEED[i] * - MOTOR_STEPS_PER_DEG[MODEL][i]); + } else if (function == "JV") { + String msg = String("JV") + JointVelToString(lastVelocity); + Serial.println(msg); + } else if (function == "JC") { + String msg; + if (!doCalibrationRoutine(msg)) { + for (int i = 0; i < NUM_JOINTS; ++i) { + stepperJoints[i].setSpeed(0); + } } - // read current joint positions - readMotorSteps(curMotorSteps); - encStepsToJointPos(curMotorSteps, curJointPos); - - // update host with joint positions - String msg = String("JP") + JointPosToString(curJointPos); Serial.println(msg); } else if (function == "RE") { resetEstop(); @@ -534,6 +641,7 @@ void stateTRAJ() { inData = ""; // clear message } + for (int i = 0; i < NUM_JOINTS; ++i) { safeRun(stepperJoints[i]); } @@ -547,7 +655,7 @@ void stateERR() { } while (STATE == STATE_ERR) { - Serial.println("DB: Unrecoverable error state entered. Please reset."); + Serial.println("ER: Unrecoverable error state entered. Please reset."); delay(1000); } } diff --git a/ar_moveit_config/config/joint_limits.yaml b/ar_moveit_config/config/joint_limits.yaml index 3cd511d..e461643 100644 --- a/ar_moveit_config/config/joint_limits.yaml +++ b/ar_moveit_config/config/joint_limits.yaml @@ -1,38 +1,60 @@ # These limits are used by MoveIt and augment/override the definitions in # ur_description. +# You can confirm parameters are loaded despite warning, by: +# ros2 param get /move_group robot_description_planning.joint_limits.joint_1.max_deceleration joint_limits: joint_1: has_acceleration_limits: true - max_acceleration: 0.08727 # 5 deg/s^2 + max_acceleration: 0.5236 # 30 deg/s^2 + has_deceleration_limits: true + max_deceleration: 0.5236 # 30 deg/s^2 has_velocity_limits: true - max_velocity: 0.2618 # 15 deg/s + max_velocity: 1.0472 # 60 deg/s joint_2: has_acceleration_limits: true - max_acceleration: 0.08727 # 5 deg/s^2 + max_acceleration: 0.5236 # 30 deg/s^2 + has_deceleration_limits: true + max_deceleration: 0.5236 # 30 deg/s^2 has_velocity_limits: true - max_velocity: 0.2618 # 15 deg/s + max_velocity: 1.0472 # 60 deg/s joint_3: has_acceleration_limits: true - max_acceleration: 0.08727 # 5 deg/s^2 + max_acceleration: 0.5236 # 30 deg/s^2 + has_deceleration_limits: true + max_deceleration: 0.5236 # 30 deg/s^2 has_velocity_limits: true - max_velocity: 0.2618 # 15 deg/s + max_velocity: 1.0472 # 60 deg/s joint_4: has_acceleration_limits: true - max_acceleration: 0.08727 # 5 deg/s^2 + max_acceleration: 0.5236 # 30 deg/s^2 + has_deceleration_limits: true + max_deceleration: 0.5236 # 30 deg/s^2 has_velocity_limits: true - max_velocity: 0.2618 # 15 deg/s + max_velocity: 1.0472 # 60 deg/s joint_5: has_acceleration_limits: true - max_acceleration: 0.174533 # 10 deg/s^2 + max_acceleration: 0.5236 # 30 deg/s^2 + has_deceleration_limits: true + max_deceleration: 0.5236 # 30 deg/s^2 has_velocity_limits: true - max_velocity: 0.261799 # 30 deg/s + max_velocity: 1.0472 # 60 deg/s joint_6: has_acceleration_limits: true - max_acceleration: 0.174533 # 10 deg/s^2 + max_acceleration: 0.5236 # 30 deg/s^2 + has_deceleration_limits: true + max_deceleration: 0.5236 # 30 deg/s^2 has_velocity_limits: true - max_velocity: 0.261799 # 30 deg/s + max_velocity: 1.0472 # 60 deg/s gripper_jaw1_joint: has_acceleration_limits: true - max_acceleration: 0.01 # m/s^2 + max_acceleration: 0.1 # m/s^2 + has_deceleration_limits: true + max_deceleration: 0.1 # m/s^2 has_velocity_limits: true - max_velocity: 0.02 # m/s + max_velocity: 0.05 # m/s + +cartesian_limits: + max_trans_vel: 0.05 # m/s + max_trans_acc: 0.01 # m/s^2 + max_trans_dec: -0.01 # m/s^2 + max_rot_vel: 0.08727 # 5 deg/s diff --git a/ar_moveit_config/config/ompl_planning.yaml b/ar_moveit_config/config/ompl_planning.yaml index 6dce493..675bf2c 100644 --- a/ar_moveit_config/config/ompl_planning.yaml +++ b/ar_moveit_config/config/ompl_planning.yaml @@ -1,5 +1,4 @@ -planning_plugins: - - ompl_interface/OMPLPlanner +planning_plugin: ompl_interface/OMPLPlanner request_adapters: >- default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames @@ -62,6 +61,7 @@ planner_configs: PRMstarkConfigDefault: type: geometric::PRMstar ar_manipulator: + default_planner_config: RRTConnectkConfigDefault planner_configs: - SBLkConfigDefault - ESTkConfigDefault @@ -78,6 +78,7 @@ ar_manipulator: #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) longest_valid_segment_fraction: 0.01 ar_gripper: + default_planner_config: RRTConnectkConfigDefault planner_configs: - SBLkConfigDefault - ESTkConfigDefault @@ -90,3 +91,5 @@ ar_gripper: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault + +start_state_max_bounds_error: 0.1 diff --git a/ar_moveit_config/config/pilz_planning.yaml b/ar_moveit_config/config/pilz_planning.yaml new file mode 100644 index 0000000..e19e28b --- /dev/null +++ b/ar_moveit_config/config/pilz_planning.yaml @@ -0,0 +1,14 @@ +planning_plugin: pilz_industrial_motion_planner/CommandPlanner +request_adapters: >- + default_planner_request_adapters/FixWorkspaceBounds + default_planner_request_adapters/FixStartStateBounds + default_planner_request_adapters/FixStartStateCollision + default_planner_request_adapters/FixStartStatePathConstraints +capabilities: >- + pilz_industrial_motion_planner/MoveGroupSequenceAction + pilz_industrial_motion_planner/MoveGroupSequenceService + +ar_manipulator: + default_planner_config: PTP + +start_state_max_bounds_error: 0.1 diff --git a/ar_moveit_config/launch/ar_moveit.launch.py b/ar_moveit_config/launch/ar_moveit.launch.py index 5c5fd54..6ec896c 100644 --- a/ar_moveit_config/launch/ar_moveit.launch.py +++ b/ar_moveit_config/launch/ar_moveit.launch.py @@ -134,17 +134,16 @@ def generate_launch_description(): } # Planning Configuration - ompl_planning_pipeline_config = { - "default_planning_pipeline": "ompl", - "planning_pipelines": ["ompl"], - "ompl": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "start_state_max_bounds_error": 0.1, - } - } ompl_planning_yaml = load_yaml("ar_moveit_config", "config/ompl_planning.yaml") - ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml) + pilz_planning_yaml = load_yaml("ar_moveit_config", + "config/pilz_planning.yaml") + planning_pipeline_config = { + "default_planning_pipeline": "ompl", + "planning_pipelines": ["ompl", "pilz"], + "ompl": ompl_planning_yaml, + "pilz": pilz_planning_yaml, + } # Trajectory Execution Configuration controllers_yaml = load_yaml("ar_moveit_config", "config/controllers.yaml") @@ -169,10 +168,15 @@ def generate_launch_description(): "publish_state_updates": True, "publish_transforms_updates": True, "publish_robot_description": True, - "publish_robot_description_semantic": True + "publish_robot_description_semantic": True, # Two above added due to https://github.com/moveit/moveit2_tutorials/issues/528 } + # Starts Pilz Industrial Motion Planner MoveGroupSequenceAction and MoveGroupSequenceService servers + move_group_capabilities = { + "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService" + } + # Start the actual move_group node/action server move_group_node = Node( package="moveit_ros_move_group", @@ -183,10 +187,11 @@ def generate_launch_description(): robot_description_semantic, robot_description_kinematics, robot_description_planning, - ompl_planning_pipeline_config, + planning_pipeline_config, trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, + move_group_capabilities, { "use_sim_time": use_sim_time }, @@ -205,9 +210,10 @@ def generate_launch_description(): parameters=[ robot_description, robot_description_semantic, - ompl_planning_pipeline_config, + planning_pipeline_config, robot_description_kinematics, robot_description_planning, + {"use_sim_time": use_sim_time}, ], ) diff --git a/ar_moveit_config/launch/demo.launch.py b/ar_moveit_config/launch/demo.launch.py index 0166a40..0e0e0fb 100644 --- a/ar_moveit_config/launch/demo.launch.py +++ b/ar_moveit_config/launch/demo.launch.py @@ -78,19 +78,16 @@ def generate_launch_description(): } # Planning Configuration - ompl_planning_pipeline_config = { - "default_planning_pipeline": "ompl", - "planning_pipelines": ["ompl"], - "ompl": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": - """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", - "start_state_max_bounds_error": 0.1, - } - } ompl_planning_yaml = load_yaml("ar_moveit_config", "config/ompl_planning.yaml") - ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml) + pilz_planning_yaml = load_yaml("ar_moveit_config", + "config/pilz_planning.yaml") + planning_pipeline_config = { + "default_planning_pipeline": "ompl", + "planning_pipelines": ["ompl", "pilz"], + "ompl": ompl_planning_yaml, + "pilz": pilz_planning_yaml, + } # Trajectory Execution Configuration controllers_yaml = load_yaml("ar_moveit_config", "config/controllers.yaml") @@ -116,6 +113,11 @@ def generate_launch_description(): "publish_transforms_updates": True, } + # Starts Pilz Industrial Motion Planner MoveGroupSequenceAction and MoveGroupSequenceService servers + move_group_capabilities = { + "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService" + } + # Start the actual move_group node/action server run_move_group_node = Node( package="moveit_ros_move_group", @@ -126,10 +128,11 @@ def generate_launch_description(): robot_description_semantic, robot_description_kinematics, robot_description_planning, - ompl_planning_pipeline_config, + planning_pipeline_config, trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, + move_group_capabilities, ], ) @@ -149,7 +152,7 @@ def generate_launch_description(): robot_description_semantic, robot_description_kinematics, robot_description_planning, - ompl_planning_pipeline_config, + planning_pipeline_config, trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, diff --git a/ar_moveit_config/package.xml b/ar_moveit_config/package.xml index cb7ae35..b9c7e8f 100644 --- a/ar_moveit_config/package.xml +++ b/ar_moveit_config/package.xml @@ -1,6 +1,6 @@ <package format="3"> <name>ar_moveit_config</name> - <version>0.1.0</version> + <version>2.0.0</version> <description> Configuration and launch files for using the AR4 robot with MoveIt 2 </description> diff --git a/ar_moveit_config/srdf/ar_macro.srdf.xacro b/ar_moveit_config/srdf/ar_macro.srdf.xacro index 4da8e4d..ce9e88d 100644 --- a/ar_moveit_config/srdf/ar_macro.srdf.xacro +++ b/ar_moveit_config/srdf/ar_macro.srdf.xacro @@ -25,7 +25,7 @@ <group_state name="upright" group="ar_manipulator"> <joint name="joint_1" value="0" /> <joint name="joint_2" value="-0.087" /> - <joint name="joint_3" value="-1.34" /> + <joint name="joint_3" value="-1.4" /> <joint name="joint_4" value="0" /> <joint name="joint_5" value="0" /> <joint name="joint_6" value="0" /> diff --git a/ar_moveit_config/urdf/fake_ar.urdf.xacro b/ar_moveit_config/urdf/fake_ar.urdf.xacro index 8aba322..536b7a4 100644 --- a/ar_moveit_config/urdf/fake_ar.urdf.xacro +++ b/ar_moveit_config/urdf/fake_ar.urdf.xacro @@ -21,6 +21,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 name="ARGripperFakeSystem"