diff --git a/.gitignore b/.gitignore index d617dd4..64d83b0 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ build __pycache__ *cpython*.so -python/data \ No newline at end of file +python/data +build* \ No newline at end of file diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 4eda22d..f5ec868 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -6,12 +6,16 @@ "${workspaceFolder}/**", "${workspaceFolder}/include", "${HOME}/miniforge3/envs/arx-py310/include", + "${HOME}/miniforge3/envs/arx-py310/include/eigen3", + "${HOME}/miniforge3/envs/arx-py310/include/python3.10", + "${HOME}/miniforge3/envs/arx-py310/include/kdl_parser", ], "defines": [], "compilerPath": "/usr/bin/gcc", "cStandard": "c17", "cppStandard": "gnu++17", - "intelliSenseMode": "linux-gcc-arm64" + "intelliSenseMode": "linux-gcc-arm64", + "compileCommands": "${workspaceFolder}/build/compile_commands.json" } ], "version": 4 diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c6f1e3..7d72e83 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,8 +15,8 @@ find_package(fmt REQUIRED) find_package(spdlog REQUIRED) find_package(Boost REQUIRED COMPONENTS container) -set(ARX_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) -add_definitions(-DARX_DIRECTORY="${ARX_DIRECTORY}") # TODO: remove this +add_compile_definitions(SDK_ROOT="${CMAKE_BINARY_DIR}/..") +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) if(CMAKE_SYSTEM_PROCESSOR MATCHES "arm.*|arm|aarch64") set(LIB_DIR ${CMAKE_CURRENT_SOURCE_DIR}/lib/aarch64) @@ -34,6 +34,7 @@ include_directories( add_library(ArxJointController SHARED src/app/joint_controller.cpp + src/app/controller_base.cpp src/utils.cpp ) target_link_libraries(ArxJointController @@ -48,6 +49,7 @@ target_link_libraries(ArxJointController ) add_library(ArxCartesianController SHARED src/app/cartesian_controller.cpp + src/app/controller_base.cpp src/utils.cpp ) target_link_libraries(ArxCartesianController diff --git a/README.md b/README.md index 4e53439..6ee20a1 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ ## Update (2024.08.22) - Enable one-step waypoint scheduling (see `python/examples`). - Support EtherCAT-CAN adapter (follow the instructions in [EtherCAT-CAN setup](README.md#ethercat-can-setup)). -- Support arbitrary DoF robot arm (not only 6DoF); thoguh other DoF numbers are not tested yet. +- Support arbitrary DoF robot arm (not only 6DoF). - Allow setting up robot and controller configurations as arguments (see `config.h` and `test_joint_control.py`). When updating the sdk to your codebase, please first remove the entire `build` folder and run the building process again. @@ -11,10 +11,10 @@ When updating the sdk to your codebase, please first remove the entire `build` f ## Features - Run without ROS - No `sudo` requirement for building the library (all dependencies are managed under conda environment, thanks to [Cheng Chi](https://cheng-chi.github.io/)) -- Simple python interface with complete type hints (see `python/arx5_interface.pyi`) +- Simple python interface with complete type hints (see `python/arx5_interface.pyi`, please include it into your vscode config `"python.analysis.extraPaths"`) - Joint controller runs at 500Hz in the background (motor communication delay ~0.4ms) - Cartesian space controller with keyboard and SpaceMouse tele-operation and teach-replay (thanks to [Cheng Chi](https://cheng-chi.github.io/)) -- Control multiple arms in the same process through C++ multi-threading +- Control multiple arms in the same process through C++ multi-threading (much better than Python multi-processing) ## Build & Install We set up a conda environment for all the cmake dependencies, so no system package is required. If you want to run `cmake` and `make` after modifying the C++ source files, please make sure you are under the created conda environment (`arx-py310` etc.). diff --git a/examples/test_cartesian_controller.cpp b/examples/test_cartesian_controller.cpp index 066dc09..be75f96 100644 --- a/examples/test_cartesian_controller.cpp +++ b/examples/test_cartesian_controller.cpp @@ -5,7 +5,7 @@ using namespace arx; -Arx5CartesianController *arx5_cartesian_controller = new Arx5CartesianController("X5", "can0", "../models/arx5.urdf"); +Arx5CartesianController *arx5_cartesian_controller = new Arx5CartesianController("L5", "can0"); void signal_handler(int signal) { @@ -18,14 +18,18 @@ int main() { EEFState cmd; int loop_cnt = 0; - int dof = arx5_cartesian_controller->get_robot_config().joint_dof; + RobotConfig robot_config = arx5_cartesian_controller->get_robot_config(); + int dof = robot_config.joint_dof; Gain gain{dof}; - Arx5Solver solver("../models/arx5.urdf", dof); + Arx5Solver solver("../models/arx5.urdf", dof, robot_config.joint_pos_min, robot_config.joint_pos_max); arx5_cartesian_controller->reset_to_home(); + arx5_cartesian_controller->set_log_level(spdlog::level::debug); gain.kd = (arx5_cartesian_controller->get_controller_config()).default_kd / 10; std::signal(SIGINT, signal_handler); arx5_cartesian_controller->set_gain(gain); + double current_time = arx5_cartesian_controller->get_timestamp(); cmd.pose_6d = arx5_cartesian_controller->get_home_pose(); + cmd.timestamp = current_time + 0.1; arx5_cartesian_controller->set_eef_cmd(cmd); while (true) { @@ -37,7 +41,7 @@ int main() // eef_state.gripper_pos); printf("raw joint pos: %.2f, %.2f, %.2f, %.2f, %.2f, %.2f\n", // joint_state.pos[0], joint_state.pos[1], joint_state.pos[2], // joint_state.pos[3], joint_state.pos[4], joint_state.pos[5]); - std::tuple result = solver.inverse_kinematics(eef_state.pose_6d, joint_state.pos); + std::tuple result = solver.inverse_kinematics(eef_state.pose_6d, joint_state.pos); if (std::get<0>(result)) { VecDoF ik_joint_pos = std::get<1>(result); diff --git a/examples/test_joint_controller.cpp b/examples/test_joint_controller.cpp index 285bd83..db7d756 100644 --- a/examples/test_joint_controller.cpp +++ b/examples/test_joint_controller.cpp @@ -5,7 +5,7 @@ using namespace arx; -Arx5JointController *arx5_joint_controller = new Arx5JointController("X5", "can0"); +Arx5JointController *arx5_joint_controller = new Arx5JointController("L5", "can0"); void signal_handler(int signal) { @@ -17,18 +17,17 @@ void signal_handler(int signal) int main() { std::signal(SIGINT, signal_handler); - arx5_joint_controller->enable_background_send_recv(); - arx5_joint_controller->enable_gravity_compensation("../models/arx5.urdf"); int loop_cnt = 0; while (true) { - JointState state = arx5_joint_controller->get_state(); - Pose6d pose6 = arx5_joint_controller->get_tool_pose(); + JointState state = arx5_joint_controller->get_joint_state(); + EEFState eef_state = arx5_joint_controller->get_eef_state(); + Pose6d pose6 = eef_state.pose_6d; std::cout << "Gripper: " << state.gripper_pos << ", joint pos: " << state.pos.transpose() << ", Pose: " << pose6.transpose() << std::endl; usleep(10000); // 10ms loop_cnt++; - if (loop_cnt % 200 == 0) + if (loop_cnt % 500 == 0) { arx5_joint_controller->reset_to_home(); arx5_joint_controller->set_to_damping(); diff --git a/include/app/cartesian_controller.h b/include/app/cartesian_controller.h index f226baa..356ebf5 100644 --- a/include/app/cartesian_controller.h +++ b/include/app/cartesian_controller.h @@ -1,82 +1,37 @@ #ifndef CARTESIAN_CONTROLLER_H #define CARTESIAN_CONTROLLER_H + #include "app/common.h" #include "app/config.h" +#include "app/controller_base.h" #include "app/solver.h" #include "hardware/arx_can.h" #include "utils.h" + #include #include #include #include #include +#include #include #include + namespace arx { - -class Arx5CartesianController +class Arx5CartesianController : public Arx5ControllerBase { public: - Arx5CartesianController(RobotConfig robot_config, ControllerConfig controller_config, std::string interface_name, - std::string urdf_path); - Arx5CartesianController(std::string model, std::string interface_name, std::string urdf_path); - ~Arx5CartesianController(); + Arx5CartesianController(RobotConfig robot_config, ControllerConfig controller_config, std::string interface_name); + Arx5CartesianController(std::string model, std::string interface_name); void set_eef_cmd(EEFState new_cmd); - std::tuple get_eef_cmd(); - std::tuple get_joint_cmd(); - EEFState get_eef_state(); - JointState get_joint_state(); - double get_timestamp(); - void set_gain(Gain new_gain); - Gain get_gain(); - void set_log_level(spdlog::level::level_enum level); - RobotConfig get_robot_config(); - ControllerConfig get_controller_config(); - - void reset_to_home(); - Pose6d get_home_pose(); - void set_to_damping(); - - private: - RobotConfig _robot_config; - ControllerConfig _controller_config; - double _clipping_output_threshold = 0.001; - int _moving_window_size = 1; // 1 for no filtering + void set_eef_traj(std::vector new_traj); + EEFState get_eef_cmd(); - EEFState _input_eef_cmd; - EEFState _output_eef_cmd; - EEFState _interp_start_eef_cmd; - JointState _input_joint_cmd{_robot_config.joint_dof}; - JointState _output_joint_cmd{_robot_config.joint_dof}; - JointState _joint_state{_robot_config.joint_dof}; - Gain _gain{_robot_config.joint_dof}; - - ArxCan _can_handle; - std::shared_ptr _logger; - std::thread _background_send_recv_thread; - std::shared_ptr _solver; - - void _init_robot(); - void _background_send_recv(); - void _update_output_cmd(); - void _calc_joint_cmd(); - void _over_current_protection(); - void _check_joint_state_sanity(); - void _enter_emergency_state(); - bool _send_recv(); - int _over_current_cnt = 0; - bool _background_send_recv_running = false; - bool _destroy_background_threads = false; - bool _enable_gravity_compensation = true; - std::mutex _cmd_mutex; - std::mutex _state_mutex; - int _start_time_us; - - MovingAverageXd _joint_pos_filter{_robot_config.joint_dof, _moving_window_size}; - MovingAverageXd _joint_torque_filter{_robot_config.joint_dof, _moving_window_size}; + std::tuple multi_trial_ik(Eigen::Matrix target_pose_6d, + Eigen::VectorXd current_joint_pos, int additional_trial_num = 5); }; - } // namespace arx -#endif \ No newline at end of file + +#endif // CARTESIAN_CONTROLLER \ No newline at end of file diff --git a/include/app/common.h b/include/app/common.h index 1962849..b0f7427 100644 --- a/include/app/common.h +++ b/include/app/common.h @@ -30,8 +30,8 @@ struct JointState VecDoF vel; VecDoF torque; double gripper_pos = 0.0f; // m; 0 for close, GRIPPER_WIDTH for fully open - double gripper_vel = 0.0f; // s^{-1} - double gripper_torque = 0.0f; // Nm + double gripper_vel = 0.0f; // s^{-1}; currently not supported + double gripper_torque = 0.0f; // Nm; currently not supported JointState(int dof) : pos(VecDoF::Zero(dof)), vel(VecDoF::Zero(dof)), torque(VecDoF::Zero(dof)) { } @@ -39,14 +39,23 @@ struct JointState : pos(pos), vel(vel), torque(torque), gripper_pos(gripper_pos) { } + JointState operator+(const JointState &other) const { return JointState(pos + other.pos, vel + other.vel, torque + other.torque, gripper_pos + other.gripper_pos); } + JointState operator-(const JointState &other) const + { + return JointState(pos - other.pos, vel - other.vel, torque - other.torque, gripper_pos - other.gripper_pos); + } JointState operator*(const double &scalar) const { return JointState(pos * scalar, vel * scalar, torque * scalar, gripper_pos * scalar); } + JointState operator/(const double &scalar) const + { + return JointState(pos / scalar, vel / scalar, torque / scalar, gripper_pos / scalar); + } // For pybind11 to update values VecDoF &get_pos_ref() { @@ -60,6 +69,7 @@ struct JointState { return torque; } + }; struct Gain diff --git a/include/app/config.h b/include/app/config.h index 81dbbf4..a455f87 100755 --- a/include/app/config.h +++ b/include/app/config.h @@ -23,8 +23,9 @@ class RobotConfig double gripper_vel_max; // m/s double gripper_torque_max; - double gripper_width; // m, fully opened: GRIPPER_WIDTH, fully closed: 0 - double gripper_open_readout; // fully-opened gripper motor readout + double gripper_width; // m, fully opened: gripper_width, fully closed: 0 + double gripper_open_readout; // fully-opened gripper motor readout. Should be calibrated using + // python/examples/calibrate.py int joint_dof; std::vector motor_id; std::vector motor_type; @@ -40,17 +41,20 @@ class RobotConfig std::string base_link_name; std::string eef_link_name; + std::string urdf_path; + RobotConfig(std::string robot_model, VecDoF joint_pos_min, VecDoF joint_pos_max, VecDoF joint_vel_max, VecDoF joint_torque_max, Pose6d ee_vel_max, double gripper_vel_max, double gripper_torque_max, double gripper_width, double gripper_open_readout, int joint_dof, std::vector motor_id, std::vector motor_type, int gripper_motor_id, MotorType gripper_motor_type, - Eigen::Vector3d gravity_vector, std::string base_link_name, std::string eef_link_name) + Eigen::Vector3d gravity_vector, std::string base_link_name, std::string eef_link_name, + std::string urdf_path) : robot_model(robot_model), joint_pos_min(joint_pos_min), joint_pos_max(joint_pos_max), joint_vel_max(joint_vel_max), joint_torque_max(joint_torque_max), ee_vel_max(ee_vel_max), gripper_vel_max(gripper_vel_max), gripper_torque_max(gripper_torque_max), gripper_width(gripper_width), gripper_open_readout(gripper_open_readout), joint_dof(joint_dof), motor_id(motor_id), motor_type(motor_type), gripper_motor_id(gripper_motor_id), gripper_motor_type(gripper_motor_type), gravity_vector(gravity_vector), - base_link_name(base_link_name), eef_link_name(eef_link_name) + base_link_name(base_link_name), eef_link_name(eef_link_name), urdf_path(urdf_path) { } }; @@ -73,7 +77,12 @@ class RobotConfigFactory } else { - throw std::runtime_error("Unknown robot model. Currently available: X5, L5, X7Left, X7Right"); + std::string available_models; + for (auto &config : configurations) + { + available_models += config.first + " "; + } + throw std::runtime_error("Unknown robot model. Currently available: " + available_models); } } @@ -83,11 +92,11 @@ class RobotConfigFactory configurations["X5"] = std::make_shared( "X5", // robot_model (VecDoF(6) << -3.14, -0.05, -0.1, -1.6, -1.57, -2).finished(), // joint_pos_min - (VecDoF(6) << 2.618, 3.14, 3.24, 1.55, 1.57, 2).finished(), // joint_pos_max - (VecDoF(6) << 3.0, 2.0, 2.0, 2.0, 3.0, 3.0).finished(), // joint_vel_max + (VecDoF(6) << 2.618, 3.50, 3.20, 1.55, 1.57, 2).finished(), // joint_pos_max + (VecDoF(6) << 5.0, 5.0, 5.5, 5.5, 5.0, 5.0).finished(), // joint_vel_max (VecDoF(6) << 30.0, 40.0, 30.0, 15.0, 10.0, 10.0).finished(), // joint_torque_max (Pose6d() << 0.6, 0.6, 0.6, 1.8, 1.8, 1.8).finished(), // ee_vel_max - 0.1, // gripper_vel_max + 0.3, // gripper_vel_max 1.5, // gripper_torque_max 0.088, // gripper_width 5.03, // gripper_open_readout @@ -99,16 +108,39 @@ class RobotConfigFactory MotorType::DM_J4310, // gripper_motor_type (Eigen::Vector3d() << 0, 0, -9.807).finished(), // gravity_vector "base_link", // base_link_name - "eef_link" // eef_link_name + "eef_link", // eef_link_name + std::string(SDK_ROOT) + "/models/X5.urdf" // urdf_path + ); + configurations["X5_umi"] = std::make_shared( + "X5_umi", // robot_model + (VecDoF(6) << -3.14, -0.05, -0.1, -1.6, -1.57, -2).finished(), // joint_pos_min + (VecDoF(6) << 2.618, 3.50, 3.20, 1.55, 1.57, 2).finished(), // joint_pos_max + (VecDoF(6) << 5.0, 5.0, 5.5, 5.5, 5.0, 5.0).finished(), // joint_vel_max + (VecDoF(6) << 30.0, 40.0, 30.0, 15.0, 10.0, 10.0).finished(), // joint_torque_max + (Pose6d() << 0.6, 0.6, 0.6, 1.8, 1.8, 1.8).finished(), // ee_vel_max + 0.3, // gripper_vel_max + 1.5, // gripper_torque_max + 0.086, // gripper_width + 4.90, // gripper_open_readout + 6, // joint_dof + std::vector{1, 2, 4, 5, 6, 7}, // motor_id + std::vector{MotorType::EC_A4310, MotorType::EC_A4310, MotorType::EC_A4310, MotorType::DM_J4310, + MotorType::DM_J4310, MotorType::DM_J4310}, // motor_type + 8, // gripper_motor_id + MotorType::DM_J4310, // gripper_motor_type + (Eigen::Vector3d() << 0, 0, -9.807).finished(), // gravity_vector + "base_link", // base_link_name + "eef_link", // eef_link_name + std::string(SDK_ROOT) + "/models/X5_umi.urdf" // urdf_path ); configurations["L5"] = std::make_shared( "L5", // robot_model (VecDoF(6) << -3.14, -0.05, -0.1, -1.6, -1.57, -2).finished(), // joint_pos_min - (VecDoF(6) << 2.618, 3.14, 3.24, 1.55, 1.57, 2).finished(), // joint_pos_max - (VecDoF(6) << 3.0, 2.0, 2.0, 2.0, 3.0, 3.0).finished(), // joint_vel_max + (VecDoF(6) << 2.618, 3.50, 3.20, 1.55, 1.57, 2).finished(), // joint_pos_max + (VecDoF(6) << 5.0, 5.0, 5.5, 5.5, 5.0, 5.0).finished(), // joint_vel_max (VecDoF(6) << 30.0, 40.0, 30.0, 15.0, 10.0, 10.0).finished(), // joint_torque_max (Pose6d() << 0.6, 0.6, 0.6, 1.8, 1.8, 1.8).finished(), // ee_vel_max - 0.1, // gripper_vel_max + 0.3, // gripper_vel_max 1.5, // gripper_torque_max 0.088, // gripper_width 5.03, // gripper_open_readout @@ -120,16 +152,39 @@ class RobotConfigFactory MotorType::DM_J4310, // gripper_motor_type (Eigen::Vector3d() << 0, 0, -9.807).finished(), // gravity_vector "base_link", // base_link_name - "eef_link" // eef_link_name + "eef_link", // eef_link_name + std::string(SDK_ROOT) + "/models/L5.urdf" // urdf_path + ); + configurations["L5_umi"] = std::make_shared( + "L5_umi", // robot_model + (VecDoF(6) << -3.14, -0.05, -0.1, -1.6, -1.57, -2).finished(), // joint_pos_min + (VecDoF(6) << 2.618, 3.50, 3.20, 1.55, 1.57, 2).finished(), // joint_pos_max + (VecDoF(6) << 5.0, 5.0, 5.5, 5.5, 5.0, 5.0).finished(), // joint_vel_max + (VecDoF(6) << 30.0, 40.0, 30.0, 15.0, 10.0, 10.0).finished(), // joint_torque_max + (Pose6d() << 0.6, 0.6, 0.6, 1.8, 1.8, 1.8).finished(), // ee_vel_max + 0.3, // gripper_vel_max + 1.5, // gripper_torque_max + 0.086, // gripper_width + 4.90, // gripper_open_readout + 6, // joint_dof + std::vector{1, 2, 4, 5, 6, 7}, // motor_id + std::vector{MotorType::DM_J4340, MotorType::DM_J4340, MotorType::DM_J4340, MotorType::DM_J4310, + MotorType::DM_J4310, MotorType::DM_J4310}, // motor_type + 8, // gripper_motor_id + MotorType::DM_J4310, // gripper_motor_type + (Eigen::Vector3d() << 0, 0, -9.807).finished(), // gravity_vector + "base_link", // base_link_name + "eef_link", // eef_link_name + std::string(SDK_ROOT) + "/models/L5_umi.urdf" // urdf_path ); - configurations["X7Left"] = std::make_shared( - "X7Left", // robot_model + configurations["X7_left"] = std::make_shared( + "X7_left", // robot_model (VecDoF(7) << -2.09439, -1.5, -1.5, -1.5, -1.2, -0.3, -0.7854).finished(), // joint_pos_min (VecDoF(7) << 2.09439, 0.3, 1.5, 0.3, 1.2, 0.7854, 0.7854).finished(), // joint_pos_max - (VecDoF(7) << 2.0, 3.0, 2.0, 2.0, 2.0, 3.0, 3.0).finished(), // joint_vel_max + (VecDoF(7) << 3.0, 5.0, 5.0, 5.5, 5.5, 5.0, 5.0).finished(), // joint_vel_max (VecDoF(7) << 30.0, 30.0, 40.0, 30.0, 15.0, 10.0, 10.0).finished(), // joint_torque_max (Pose6d() << 0.6, 0.6, 0.6, 1.8, 1.8, 1.8).finished(), // ee_vel_max - 0.1, // gripper_vel_max + 0.3, // gripper_vel_max 1.5, // gripper_torque_max 0.088, // gripper_width 5.03, // gripper_open_readout @@ -141,16 +196,17 @@ class RobotConfigFactory MotorType::DM_J4310, // gripper_motor_type (Eigen::Vector3d() << 0, 0, -9.807).finished(), // gravity_vector "base_link", // base_link_name - "eef_link" // eef_link_name + "eef_link", // eef_link_name + std::string(SDK_ROOT) + "/models/X7_left.urdf" // urdf_path ); - configurations["X7Right"] = std::make_shared( - "X7Right", // robot_model + configurations["X7_right"] = std::make_shared( + "X7_right", // robot_model (VecDoF(7) << -2.09439, -0.3, -1.5, -0.3, -1.2, -0.7854, -0.7854).finished(), // joint_pos_min (VecDoF(7) << 2.09439, 1.5, 1.5, 1.5, 1.2, 0.3, 0.7854).finished(), // joint_pos_max - (VecDoF(7) << 2.0, 3.0, 2.0, 2.0, 2.0, 3.0, 3.0).finished(), // joint_vel_max + (VecDoF(7) << 3.0, 5.0, 5.0, 5.5, 5.5, 5.0, 5.0).finished(), // joint_vel_max (VecDoF(7) << 30.0, 30.0, 40.0, 30.0, 15.0, 10.0, 10.0).finished(), // joint_torque_max (Pose6d() << 0.6, 0.6, 0.6, 1.8, 1.8, 1.8).finished(), // ee_vel_max - 0.1, // gripper_vel_max + 0.3, // gripper_vel_max 1.5, // gripper_torque_max 0.088, // gripper_width 5.03, // gripper_open_readout @@ -162,7 +218,8 @@ class RobotConfigFactory MotorType::DM_J4310, // gripper_motor_type (Eigen::Vector3d() << 0, 0, -9.807).finished(), // gravity_vector "base_link", // base_link_name - "eef_link" // eef_link_name + "eef_link", // eef_link_name + std::string(SDK_ROOT) + "/models/X7_right.urdf" // urdf_path ); } @@ -183,12 +240,26 @@ class ControllerConfig double default_gripper_kd; int over_current_cnt_max; double controller_dt; + bool gravity_compensation; + bool background_send_recv; + bool shutdown_to_passive; + // true: will set the arm to damping then passive mode when pressing `ctrl-C`. (recommended); + // pressing `ctrl-\` will directly kill the program so this process will be skipped + // false: will keep the arm in the air when shutting down the controller (both `ctrl-\` and `ctrl-C`). + // X5 cannot be kept in the air. + std::string interpolation_method; // "linear" or "cubic" (cubic is not well supported yet) + double default_preview_time; // The default value for preview time if the command has 0 timestamp ControllerConfig(std::string controller_type, VecDoF default_kp, VecDoF default_kd, double default_gripper_kp, - double default_gripper_kd, int over_current_cnt_max, double controller_dt) + double default_gripper_kd, int over_current_cnt_max, double controller_dt, + bool gravity_compensation, bool background_send_recv, bool shutdown_to_passive, + std::string interpolation_method, double default_preview_time) : controller_type(controller_type), default_kp(default_kp), default_kd(default_kd), default_gripper_kp(default_gripper_kp), default_gripper_kd(default_gripper_kd), - over_current_cnt_max(over_current_cnt_max), controller_dt(controller_dt) + over_current_cnt_max(over_current_cnt_max), controller_dt(controller_dt), + gravity_compensation(gravity_compensation), background_send_recv(background_send_recv), + shutdown_to_passive(shutdown_to_passive), interpolation_method(interpolation_method), + default_preview_time(default_preview_time) { } }; @@ -227,7 +298,12 @@ class ControllerConfigFactory 5.0, // default_gripper_kp 0.2, // default_gripper_kd 20, // over_current_cnt_max - 0.002 // controller_dt + 0.002, // controller_dt + true, // gravity_compensation + true, // background_send_recv + true, // shutdown_to_passive + "linear", // interpolation_method + 0.0 // default_preview_time ); configurations["joint_controller_6"] = std::make_shared( "joint_controller", // controller_type @@ -236,7 +312,12 @@ class ControllerConfigFactory 5.0, // default_gripper_kp 0.2, // default_gripper_kd 20, // over_current_cnt_max - 0.002 // controller_dt + 0.002, // controller_dt + true, // gravity_compensation + true, // background_send_recv + true, // shutdown_to_passive + "linear", // interpolation_method + 0.0 // default_preview_time ); configurations["cartesian_controller_7"] = std::make_shared( "cartesian_controller", // controller_type @@ -245,16 +326,26 @@ class ControllerConfigFactory 5.0, // default_gripper_kp 0.2, // default_gripper_kd 20, // over_current_cnt_max - 0.005 // controller_dt + 0.002, // controller_dt + true, // gravity_compensation + true, // background_send_recv + true, // shutdown_to_passive + "linear", // interpolation_method + 0.1 // default_preview_time ); configurations["cartesian_controller_6"] = std::make_shared( - "cartesian_controller", // controller_type - (VecDoF(6) << 300.0, 300.0, 300.0, 80.0, 50.0, 40.0).finished(), // default_kp - (VecDoF(6) << 5.0, 5.0, 5.0, 1.0, 1.0, 1.0).finished(), // default_kd - 5.0, // default_gripper_kp - 0.2, // default_gripper_kd - 20, // over_current_cnt_max - 0.005 // controller_dt + "cartesian_controller", // controller_type + (VecDoF(6) << 200.0, 200.0, 200.0, 120.0, 80.0, 60.0).finished(), // default_kp + (VecDoF(6) << 5.0, 5.0, 5.0, 1.0, 1.0, 1.0).finished(), // default_kd + 5.0, // default_gripper_kp + 0.2, // default_gripper_kd + 20, // over_current_cnt_max + 0.002, // controller_dt + true, // gravity_compensation + true, // background_send_recv + true, // shutdown_to_passive + "linear", // interpolation_method + 0.1 // default_preview_time ); } std::unordered_map> configurations; diff --git a/include/app/controller_base.h b/include/app/controller_base.h new file mode 100644 index 0000000..b5cab2f --- /dev/null +++ b/include/app/controller_base.h @@ -0,0 +1,76 @@ +#ifndef CONTROLLER_BASE_H +#define CONTROLLER_BASE_H +#include "app/common.h" +#include "app/config.h" +#include "app/solver.h" +#include "hardware/arx_can.h" +#include "utils.h" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace arx +{ +class Arx5ControllerBase // parent class for the other two controllers +{ + public: + Arx5ControllerBase(RobotConfig robot_config, ControllerConfig controller_config, std::string interface_name); + ~Arx5ControllerBase(); + JointState get_joint_cmd(); + JointState get_joint_state(); + EEFState get_eef_state(); + Pose6d get_home_pose(); + void set_gain(Gain new_gain); + Gain get_gain(); + + double get_timestamp(); + RobotConfig get_robot_config(); + ControllerConfig get_controller_config(); + void set_log_level(spdlog::level::level_enum level); + + void reset_to_home(); + void set_to_damping(); + + protected: + RobotConfig _robot_config; + ControllerConfig _controller_config; + + int _over_current_cnt = 0; + JointState _output_joint_cmd{_robot_config.joint_dof}; + + JointState _joint_state{_robot_config.joint_dof}; + Gain _gain{_robot_config.joint_dof}; + // bool _prev_gripper_updated = false; // Declaring here leads to segfault + + ArxCan _can_handle; + std::shared_ptr _logger; + std::thread _background_send_recv_thread; + + bool _prev_gripper_updated = false; // To suppress the warning message + bool _background_send_recv_running = false; + bool _destroy_background_threads = false; + + std::mutex _cmd_mutex; + std::mutex _state_mutex; + + long int _start_time_us; + std::shared_ptr _solver; + JointStateInterpolator _interpolator{_robot_config.joint_dof, _controller_config.interpolation_method}; + void _init_robot(); + void _update_joint_state(); + void _update_output_cmd(); + void _send_recv(); + void _recv(); + void _check_joint_state_sanity(); + void _over_current_protection(); + void _background_send_recv(); + void _enter_emergency_state(); +}; +} // namespace arx + +#endif \ No newline at end of file diff --git a/include/app/joint_controller.h b/include/app/joint_controller.h index 2141ee6..48cca71 100644 --- a/include/app/joint_controller.h +++ b/include/app/joint_controller.h @@ -2,6 +2,7 @@ #define JOINT_CONTROLLER_H #include "app/common.h" #include "app/config.h" +#include "app/controller_base.h" #include "app/solver.h" #include "hardware/arx_can.h" #include "utils.h" @@ -17,65 +18,22 @@ namespace arx { -class Arx5JointController +class Arx5JointController : public Arx5ControllerBase { public: Arx5JointController(RobotConfig robot_config, ControllerConfig controller_config, std::string interface_name); Arx5JointController(std::string model, std::string interface_name); - ~Arx5JointController(); - - void send_recv_once(); - void enable_background_send_recv(); - void disable_background_send_recv(); - void enable_gravity_compensation(std::string urdf_path); - void disable_gravity_compensation(); void set_joint_cmd(JointState new_cmd); - std::tuple get_joint_cmd(); - JointState get_state(); - Pose6d get_tool_pose(); - void set_gain(Gain new_gain); - Gain get_gain(); + void set_joint_traj(std::vector new_traj); - void reset_to_home(); - void set_to_damping(); + // Only works when background_send_recv is disabled + void send_recv_once(); + void recv_once(); void calibrate_gripper(); void calibrate_joint(int joint_id); - double get_timestamp(); - RobotConfig get_robot_config(); - ControllerConfig get_controller_config(); - - void set_log_level(spdlog::level::level_enum level); - - private: - RobotConfig _robot_config; - ControllerConfig _controller_config; - void _init_robot(); - void _background_send_recv(); - bool _send_recv(); - void _over_current_protection(); - void _check_joint_state_sanity(); - void _enter_emergency_state(); - int _over_current_cnt = 0; - JointState _output_joint_cmd{_robot_config.joint_dof}; - JointState _intermediate_joint_cmd{_robot_config.joint_dof}; // _output_joint_cmd without gravity compensation - JointState _input_joint_cmd{_robot_config.joint_dof}; - JointState _interp_start_joint_cmd{_robot_config.joint_dof}; - JointState _joint_state{_robot_config.joint_dof}; - Gain _gain{_robot_config.joint_dof}; - ArxCan _can_handle; - std::shared_ptr _logger; - std::thread _background_send_recv_thread; - bool _background_send_recv_running = false; - bool _destroy_background_threads = false; - std::mutex _cmd_mutex; - std::mutex _state_mutex; - void _update_output_cmd(); - int _start_time_us; - bool _enable_gravity_compensation = false; - std::shared_ptr _solver; }; } // namespace arx diff --git a/include/app/solver.h b/include/app/solver.h index 440a06f..bc617ab 100644 --- a/include/app/solver.h +++ b/include/app/solver.h @@ -2,25 +2,15 @@ #define SOLVER_H #include -#include #include -#include #include #include #include -#include -#include #include -#include #include -#include -#include -#include -#include -#include -#include #include #include +#include #include #include #include @@ -28,6 +18,9 @@ #include #include #include + +#include + namespace arx { @@ -35,32 +28,61 @@ class Arx5Solver { public: - Arx5Solver(std::string urdf_path, int joint_dof); - Arx5Solver(std::string urdf_path, int joint_dof, std::string base_link, std::string eef_link, - Eigen::Vector3d gravity_vector); + Arx5Solver(std::string urdf_path, int joint_dof, Eigen::VectorXd joint_pos_min, Eigen::VectorXd joint_pos_max); + Arx5Solver(std::string urdf_path, int joint_dof, Eigen::VectorXd joint_pos_min, Eigen::VectorXd joint_pos_max, + std::string base_link, std::string eef_link, Eigen::Vector3d gravity_vector); ~Arx5Solver() = default; Eigen::VectorXd inverse_dynamics(Eigen::VectorXd joint_pos, Eigen::VectorXd joint_vel, Eigen::VectorXd joint_acc); - std::tuple inverse_kinematics(Eigen::Matrix target_pose_6d, - Eigen::VectorXd current_joint_pos); + + // Inverse kinematics will return now ik_status based on the definition in `` + // ik_status=0 means no error + // Call `get_ik_status_name(ik_status)` to get the error message + std::tuple multi_trial_ik(Eigen::Matrix target_pose_6d, + Eigen::VectorXd current_joint_pos, int additional_trial_num = 5); + // Inverse kinematics will return now ik_status based on the definition in `` + // ik_status=0 means no error + // Call `get_ik_status_name(ik_status)` to get the error message + std::tuple inverse_kinematics(Eigen::Matrix target_pose_6d, + Eigen::VectorXd current_joint_pos); + std::string get_ik_status_name(int ik_status); Eigen::Matrix forward_kinematics(Eigen::VectorXd joint_pos); + enum + { + E_EXCEED_JOITN_LIMIT = -9, + }; + private: // parameters for ik solver - const double _EPS = 1E-5; - const int _MAXITER = 500; - const double _EPS_JOINTS = 1E-15; + const double _EPS = 1E-4; + const int _MAXITER = 50; + const double _EPS_JOINTS = 1E-10; const int _JOINT_DOF; + const Eigen::VectorXd _JOINT_POS_MIN; + const Eigen::VectorXd _JOINT_POS_MAX; + + const std::unordered_map _IK_STATUS_MAP = { + {KDL::SolverI::E_DEGRADED, "E_DEGRADED"}, + {KDL::SolverI::E_NOERROR, "E_NOERROR"}, + {KDL::SolverI::E_NO_CONVERGE, "E_NO_CONVERGE"}, + {KDL::SolverI::E_UNDEFINED, "E_UNDEFINED"}, + {KDL::SolverI::E_NOT_UP_TO_DATE, "E_NOT_UP_TO_DATE"}, + {KDL::SolverI::E_SIZE_MISMATCH, "E_SIZE_MISMATCH"}, + {KDL::SolverI::E_MAX_ITERATIONS_EXCEEDED, "E_MAX_ITERATIONS_EXCEEDED"}, + {KDL::SolverI::E_OUT_OF_RANGE, "E_OUT_OF_RANGE"}, + {KDL::SolverI::E_NOT_IMPLEMENTED, "E_NOT_IMPLEMENTED"}, + {KDL::SolverI::E_SVD_FAILED, "E_SVD_FAILED"}, + {E_EXCEED_JOITN_LIMIT, "E_EXCEED_JOITN_LIMIT"}}; + + // These variables should be class members and will be used when solvers are called. KDL::Tree _tree; KDL::Chain _chain; - KDL::Chain _chain_without_end_effector; + KDL::Chain _chain_without_fixed_joints; std::shared_ptr _ik_solver; std::shared_ptr _fk_solver; - std::shared_ptr _fk_vel_solver; - std::shared_ptr _ik_acc_solver; - std::shared_ptr _jac_dot_solver; std::shared_ptr _id_solver; }; } // namespace arx diff --git a/include/utils.h b/include/utils.h index 188fb43..c016f02 100644 --- a/include/utils.h +++ b/include/utils.h @@ -2,7 +2,8 @@ #define UTILS_H #include "app/common.h" #include - +#include +#include namespace arx { class MovingAverageXd @@ -23,8 +24,34 @@ class MovingAverageXd Eigen::MatrixXd _window; }; +class JointStateInterpolator +{ + public: + JointStateInterpolator(int dof, std::string method); + ~JointStateInterpolator() = default; + void init(JointState start_state, JointState end_state); + void init_fixed(JointState start_state); + void append_waypoint(double current_time, JointState end_state); + void append_traj(double current_time, std::vector traj); + void override_waypoint(double current_time, JointState end_state); + void override_traj(double current_time, std::vector traj); + JointState interpolate(double time); + std::string to_string(); + bool is_initialized(); + + private: + int _dof; + bool _initialized = false; + std::string _method; + std::vector _traj; +}; + +void calc_joint_vel(std::vector &traj, double avg_window_s = 0.05); // std::string vec2str(const Eigen::VectorXd& vec, int precision = 3); +std::string joint_traj2str(const std::vector &traj, int precision = 3); +std::string state2str(const JointState &state, int precision = 3); + } // namespace arx std::string vec2str(const Eigen::VectorXd &vec, int precision = 3); diff --git a/lib/aarch64/libhardware.so b/lib/aarch64/libhardware.so index 84912e5..9b97b56 100755 Binary files a/lib/aarch64/libhardware.so and b/lib/aarch64/libhardware.so differ diff --git a/lib/aarch64/libsolver.so b/lib/aarch64/libsolver.so index d70ade6..ad503a6 100755 Binary files a/lib/aarch64/libsolver.so and b/lib/aarch64/libsolver.so differ diff --git a/lib/x86_64/libhardware.so b/lib/x86_64/libhardware.so index e6794b0..03717d1 100755 Binary files a/lib/x86_64/libhardware.so and b/lib/x86_64/libhardware.so differ diff --git a/lib/x86_64/libsolver.so b/lib/x86_64/libsolver.so old mode 100755 new mode 100644 index ba769aa..738533a Binary files a/lib/x86_64/libsolver.so and b/lib/x86_64/libsolver.so differ diff --git a/models/L5.urdf b/models/L5.urdf new file mode 100644 index 0000000..b12372a --- /dev/null +++ b/models/L5.urdf @@ -0,0 +1,242 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/models/L5_umi.urdf b/models/L5_umi.urdf new file mode 100644 index 0000000..a08bed5 --- /dev/null +++ b/models/L5_umi.urdf @@ -0,0 +1,239 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/models/arx5.urdf b/models/X5.urdf similarity index 93% rename from models/arx5.urdf rename to models/X5.urdf index 692247b..101e3ab 100644 --- a/models/arx5.urdf +++ b/models/X5.urdf @@ -2,7 +2,7 @@ - + - + @@ -52,7 +52,7 @@ - + @@ -77,7 +77,7 @@ - + @@ -86,7 +86,7 @@ - + @@ -111,7 +111,7 @@ - + @@ -145,7 +145,7 @@ - + @@ -182,7 +182,7 @@ - + @@ -206,14 +206,14 @@ - + - + @@ -222,7 +222,7 @@ - + diff --git a/models/arx5_no_gripper.urdf b/models/X5_no_gripper.urdf similarity index 100% rename from models/arx5_no_gripper.urdf rename to models/X5_no_gripper.urdf diff --git a/models/arx5_gopro.urdf b/models/X5_umi.urdf similarity index 93% rename from models/arx5_gopro.urdf rename to models/X5_umi.urdf index 4d42824..99ffa20 100644 --- a/models/arx5_gopro.urdf +++ b/models/X5_umi.urdf @@ -1,5 +1,5 @@ - + - + @@ -27,7 +27,7 @@ - + @@ -40,7 +40,7 @@ - + @@ -49,7 +49,7 @@ - + @@ -74,7 +74,7 @@ - + @@ -83,7 +83,7 @@ - + @@ -108,7 +108,7 @@ - + @@ -142,7 +142,7 @@ - + @@ -179,7 +179,7 @@ - + diff --git a/models/arx7_left.urdf b/models/X7_left.urdf similarity index 100% rename from models/arx7_left.urdf rename to models/X7_left.urdf diff --git a/models/arx7_left_new.urdf b/models/X7_left_new.urdf similarity index 100% rename from models/arx7_left_new.urdf rename to models/X7_left_new.urdf diff --git a/models/arx7_right.urdf b/models/X7_right.urdf similarity index 100% rename from models/arx7_right.urdf rename to models/X7_right.urdf diff --git a/models/arx7_right_new.urdf b/models/X7_right_new.urdf similarity index 100% rename from models/arx7_right_new.urdf rename to models/X7_right_new.urdf diff --git a/models/meshes/base_link.STL b/models/meshes/X5/base_link.STL similarity index 100% rename from models/meshes/base_link.STL rename to models/meshes/X5/base_link.STL diff --git a/models/meshes/link1.STL b/models/meshes/X5/link1.STL similarity index 100% rename from models/meshes/link1.STL rename to models/meshes/X5/link1.STL diff --git a/models/meshes/link2.STL b/models/meshes/X5/link2.STL similarity index 100% rename from models/meshes/link2.STL rename to models/meshes/X5/link2.STL diff --git a/models/meshes/link3.STL b/models/meshes/X5/link3.STL similarity index 100% rename from models/meshes/link3.STL rename to models/meshes/X5/link3.STL diff --git a/models/meshes/link4.STL b/models/meshes/X5/link4.STL similarity index 100% rename from models/meshes/link4.STL rename to models/meshes/X5/link4.STL diff --git a/models/meshes/link5.STL b/models/meshes/X5/link5.STL similarity index 100% rename from models/meshes/link5.STL rename to models/meshes/X5/link5.STL diff --git a/models/meshes/link6.STL b/models/meshes/X5/link6.STL similarity index 100% rename from models/meshes/link6.STL rename to models/meshes/X5/link6.STL diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 671f15c..dd8531a 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -14,6 +14,7 @@ pybind11_add_module(arx5_interface arx5_pybind.cpp ${CMAKE_CURRENT_SOURCE_DIR}/../src/app/joint_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/../src/app/cartesian_controller.cpp +${CMAKE_CURRENT_SOURCE_DIR}/../src/app/controller_base.cpp ${CMAKE_CURRENT_SOURCE_DIR}/../src/utils.cpp ) diff --git a/python/arx5_interface.pyi b/python/arx5_interface.pyi index fc89631..801a860 100644 --- a/python/arx5_interface.pyi +++ b/python/arx5_interface.pyi @@ -30,6 +30,7 @@ class RobotConfig: gravity_vector: np.ndarray base_link_name: str eef_link_name: str + urdf_path: str class ControllerConfig: """Does not have a constructor, use ControllerConfigFactory.get_instance().get_config(...) instead.""" @@ -41,6 +42,11 @@ class ControllerConfig: default_gripper_kd: float over_current_cnt_max: int controller_dt: float + gravity_compensation: bool + background_send_recv: bool + shutdown_to_passive: bool + interpolation_method: str + default_preview_time: float class RobotConfigFactory: @classmethod @@ -102,7 +108,11 @@ class JointState: class Arx5JointController: @overload - def __init__(self, model: str, interface_name: str) -> None: ... + def __init__( + self, + model: str, + interface_name: str, + ) -> None: ... @overload def __init__( self, @@ -111,14 +121,14 @@ class Arx5JointController: interface_name: str, ) -> None: ... def send_recv_once(self) -> None: ... - def enable_background_send_recv(self) -> None: ... - def disable_background_send_recv(self) -> None: ... - def enable_gravity_compensation(self, urdf_path) -> None: ... - def disable_gravity_compensation(self) -> None: ... + def recv_once(self) -> None: ... def set_joint_cmd(self, cmd: JointState) -> None: ... - def get_joint_cmd(self) -> tuple[JointState, JointState]: ... + def set_joint_traj(self, traj: list[JointState]) -> None: ... + def get_joint_cmd(self) -> JointState: ... def get_timestamp(self) -> float: ... - def get_state(self) -> JointState: ... + def get_joint_state(self) -> JointState: ... + def get_eef_state(self) -> EEFState: ... + def get_home_pose(self) -> np.ndarray: ... def set_gain(self, gain: Gain) -> None: ... def get_gain(self) -> Gain: ... def get_robot_config(self) -> RobotConfig: ... @@ -146,18 +156,18 @@ class EEFState: class Arx5CartesianController: @overload - def __init__(self, model: str, interface_name: str, urdf_path: str) -> None: ... + def __init__(self, model: str, interface_name: str) -> None: ... @overload def __init__( self, robot_config: RobotConfig, controller_config: ControllerConfig, interface_name: str, - urdf_path: str, ) -> None: ... def set_eef_cmd(self, cmd: EEFState) -> None: ... - def get_eef_cmd(self) -> tuple[EEFState, EEFState]: ... - def get_joint_cmd(self) -> tuple[JointState, JointState]: ... + def set_eef_traj(self, traj: list[EEFState]) -> None: ... + def get_joint_cmd(self) -> JointState: ... + def get_eef_cmd(self) -> EEFState: ... def get_eef_state(self) -> EEFState: ... def get_joint_state(self) -> JointState: ... def get_timestamp(self) -> float: ... @@ -172,12 +182,20 @@ class Arx5CartesianController: class Arx5Solver: @overload - def __init__(self, urdf_path: str, joint_dof: int) -> None: ... + def __init__( + self, + urdf_path: str, + joint_dof: int, + joint_pos_min: npt.NDArray[np.float64], + joint_pos_max: npt.NDArray[np.float64], + ) -> None: ... @overload def __init__( self, urdf_path: str, joint_dof: int, + joint_pos_min: npt.NDArray[np.float64], + joint_pos_max: npt.NDArray[np.float64], base_link: str, eef_link: str, gravity_vector: npt.NDArray[np.float64], @@ -192,7 +210,14 @@ class Arx5Solver: self, target_pose_6d: npt.NDArray[np.float64], current_joint_pos: npt.NDArray[np.float64], - ) -> Tuple[bool, npt.NDArray[np.float64]]: ... + ) -> Tuple[int, npt.NDArray[np.float64]]: ... + def multi_trial_ik( + self, + target_pose_6d: npt.NDArray[np.float64], + current_joint_pos: npt.NDArray[np.float64], + additional_trial_num: int, + ) -> Tuple[int, npt.NDArray[np.float64]]: ... + def get_ik_status_name(self, status: int) -> str: ... def forward_kinematics( self, joint_pos: npt.NDArray[np.float64] ) -> npt.NDArray[np.float64]: ... diff --git a/python/arx5_pybind.cpp b/python/arx5_pybind.cpp index 0a0b2c8..0c0f489 100644 --- a/python/arx5_pybind.cpp +++ b/python/arx5_pybind.cpp @@ -1,6 +1,7 @@ #include "app/cartesian_controller.h" #include "app/common.h" #include "app/config.h" +#include "app/controller_base.h" #include "app/joint_controller.h" #include "hardware/arx_can.h" #include "spdlog/spdlog.h" @@ -58,13 +59,13 @@ PYBIND11_MODULE(arx5_interface, m) .def(py::init()) .def(py::init()) .def("send_recv_once", &Arx5JointController::send_recv_once) - .def("enable_background_send_recv", &Arx5JointController::enable_background_send_recv) - .def("disable_background_send_recv", &Arx5JointController::disable_background_send_recv) - .def("enable_gravity_compensation", &Arx5JointController::enable_gravity_compensation) - .def("disable_gravity_compensation", &Arx5JointController::disable_gravity_compensation) - .def("get_state", &Arx5JointController::get_state) + .def("recv_once", &Arx5JointController::recv_once) + .def("get_joint_state", &Arx5JointController::get_joint_state) .def("get_timestamp", &Arx5JointController::get_timestamp) .def("set_joint_cmd", &Arx5JointController::set_joint_cmd) + .def("set_joint_traj", &Arx5JointController::set_joint_traj) + .def("get_home_pose", &Arx5JointController::get_home_pose) + .def("get_eef_state", &Arx5JointController::get_eef_state) .def("get_joint_cmd", &Arx5JointController::get_joint_cmd) .def("set_gain", &Arx5JointController::set_gain) .def("get_gain", &Arx5JointController::get_gain) @@ -76,11 +77,12 @@ PYBIND11_MODULE(arx5_interface, m) .def("calibrate_joint", &Arx5JointController::calibrate_joint) .def("calibrate_gripper", &Arx5JointController::calibrate_gripper); py::class_(m, "Arx5CartesianController") - .def(py::init()) - .def(py::init()) + .def(py::init()) + .def(py::init()) .def("set_eef_cmd", &Arx5CartesianController::set_eef_cmd) - .def("get_eef_cmd", &Arx5CartesianController::get_eef_cmd) + .def("set_eef_traj", &Arx5CartesianController::set_eef_traj) .def("get_joint_cmd", &Arx5CartesianController::get_joint_cmd) + .def("get_eef_cmd", &Arx5CartesianController::get_eef_cmd) .def("get_eef_state", &Arx5CartesianController::get_eef_state) .def("get_joint_state", &Arx5CartesianController::get_joint_state) .def("get_timestamp", &Arx5CartesianController::get_timestamp) @@ -91,13 +93,17 @@ PYBIND11_MODULE(arx5_interface, m) .def("get_robot_config", &Arx5CartesianController::get_robot_config) .def("get_controller_config", &Arx5CartesianController::get_controller_config) .def("reset_to_home", &Arx5CartesianController::reset_to_home) + .def("multi_trial_ik", &Arx5CartesianController::multi_trial_ik) .def("set_to_damping", &Arx5CartesianController::set_to_damping); py::class_(m, "Arx5Solver") - .def(py::init()) - .def(py::init()) + .def(py::init()) + .def(py::init()) .def("inverse_dynamics", &Arx5Solver::inverse_dynamics) .def("forward_kinematics", &Arx5Solver::forward_kinematics) - .def("inverse_kinematics", &Arx5Solver::inverse_kinematics); + .def("inverse_kinematics", &Arx5Solver::inverse_kinematics) + .def("get_ik_status_name", &Arx5Solver::get_ik_status_name) + .def("multi_trial_ik", &Arx5Solver::multi_trial_ik); py::class_(m, "RobotConfig") .def_readwrite("robot_model", &RobotConfig::robot_model) .def_readwrite("joint_pos_min", &RobotConfig::joint_pos_min) @@ -116,7 +122,9 @@ PYBIND11_MODULE(arx5_interface, m) .def_readwrite("gripper_motor_type", &RobotConfig::gripper_motor_type) .def_readwrite("gravity_vector", &RobotConfig::gravity_vector) .def_readwrite("base_link_name", &RobotConfig::base_link_name) - .def_readwrite("eef_link_name", &RobotConfig::eef_link_name); + .def_readwrite("eef_link_name", &RobotConfig::eef_link_name) + .def_readwrite("urdf_path", &RobotConfig::urdf_path); + py::class_(m, "ControllerConfig") .def_readwrite("controller_type", &ControllerConfig::controller_type) .def_readwrite("default_kp", &ControllerConfig::default_kp) @@ -124,6 +132,11 @@ PYBIND11_MODULE(arx5_interface, m) .def_readwrite("default_gripper_kp", &ControllerConfig::default_gripper_kp) .def_readwrite("default_gripper_kd", &ControllerConfig::default_gripper_kd) .def_readwrite("over_current_cnt_max", &ControllerConfig::over_current_cnt_max) + .def_readwrite("background_send_recv", &ControllerConfig::background_send_recv) + .def_readwrite("gravity_compensation", &ControllerConfig::gravity_compensation) + .def_readwrite("shutdown_to_passive", &ControllerConfig::shutdown_to_passive) + .def_readwrite("interpolation_method", &ControllerConfig::interpolation_method) + .def_readwrite("default_preview_time", &ControllerConfig::default_preview_time) .def_readwrite("controller_dt", &ControllerConfig::controller_dt); py::class_(m, "RobotConfigFactory") .def_static("get_instance", &RobotConfigFactory::get_instance, py::return_value_policy::reference) diff --git a/python/communication/zmq_server.py b/python/communication/zmq_server.py index 7007495..5401943 100644 --- a/python/communication/zmq_server.py +++ b/python/communication/zmq_server.py @@ -31,15 +31,11 @@ def __init__( zmq_port: int, model: str, interface: str, - urdf_path: str, no_cmd_timeout: float = 60.0, ): self.model = model self.interface = interface - self.urdf_path = urdf_path - self.arx5_cartesian_controller = arx5.Arx5CartesianController( - model, interface, urdf_path - ) + self.arx5_cartesian_controller = arx5.Arx5CartesianController(model, interface) print(f"Arx5Server is initialized with {model} on {interface}") self.context = zmq.Context() self.socket = self.context.socket(zmq.REP) @@ -63,7 +59,7 @@ def run(self): if self.arx5_cartesian_controller is None: print(f"Reestablishing high level controller") self.arx5_cartesian_controller = arx5.Arx5CartesianController( - self.model, self.interface, self.urdf_path + self.model, self.interface ) else: @@ -94,7 +90,11 @@ def run(self): continue if msg["cmd"] == "GET_STATE": # print(f"Received GET_STATE message") + eef_pose_cmd = self.arx5_cartesian_controller.get_eef_cmd() eef_state = self.arx5_cartesian_controller.get_eef_state() + + print(f"{eef_pose_cmd}") + print(f"{eef_state.pose_6d()}") low_state = self.arx5_cartesian_controller.get_joint_state() reply_msg = { "cmd": "GET_STATE", @@ -226,12 +226,10 @@ def __del__(self): @click.command() @click.argument("model") # ARX arm model: X5 or L5 @click.argument("interface") # can bus name (can0 etc.) -@click.option("--urdf_path", "-u", default="../models/arx5.urdf", help="URDF file path") -def main(model: str, interface: str, urdf_path: str): +def main(model: str, interface: str): server = Arx5Server( model=model, interface=interface, - urdf_path=urdf_path, zmq_ip="0.0.0.0", zmq_port=8765, ) diff --git a/python/examples/calibrate.py b/python/examples/calibrate.py index 321e1d3..e332fa6 100644 --- a/python/examples/calibrate.py +++ b/python/examples/calibrate.py @@ -16,14 +16,14 @@ def calibrate_joint(model: str, interface: str, joint_id: int): joint_controller = arx5.Arx5JointController(model, interface) gain = arx5.Gain(joint_controller.get_robot_config().joint_dof) joint_controller.set_gain(gain) - joint_controller.enable_background_send_recv() joint_controller.calibrate_joint(joint_id) while True: - state = joint_controller.get_state() + state = joint_controller.get_joint_state() pos = state.pos() print(", ".join([f"{x:.3f}" for x in pos])) time.sleep(0.1) + @click.command() @click.argument("model") # ARX arm model: X5 or L5 @click.argument("interface") # can bus name (can0 etc.) diff --git a/python/examples/cartesian_waypoint_scheduling.py b/python/examples/cartesian_waypoint_scheduling.py index 915bf6e..b9d8495 100644 --- a/python/examples/cartesian_waypoint_scheduling.py +++ b/python/examples/cartesian_waypoint_scheduling.py @@ -14,30 +14,81 @@ @click.command() @click.argument("model") # ARX arm model: X5 or L5 @click.argument("interface") # can bus name (can0 etc.) -@click.option("--urdf_path", "-u", default="../models/arx5.urdf", help="URDF file path") -def main(model: str, interface: str, urdf_path: str): - controller = arx5.Arx5CartesianController(model, interface, urdf_path) - +def main(model: str, interface: str): + controller = arx5.Arx5CartesianController(model, interface) + np.set_printoptions(precision=4, suppress=True) controller.set_log_level(arx5.LogLevel.DEBUG) home_pose = controller.get_home_pose() controller.reset_to_home() cartesian_waypoints = home_pose + np.array( [ - [0.0, 0.0, 0.3, 0.0, 0.0, 0.0], - [0.3, 0.0, 0.3, 0.0, 0.0, 0.0], - [0.3, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.2, 0.0, 0.0, 0.0], + [0.2, 0.0, 0.2, 0.0, 0.0, 0.0], + [0.2, 0.0, 0.0, 0.0, 0.0, 0.0], ] ) - waypoint_interval_s = 2.0 - for waypoint in cartesian_waypoints: + waypoint_interval_s = 1.0 + + interpolate_interval_s = 0.1 + interpolated_waypoints = [] + for i in range(len(cartesian_waypoints) - 1): + start = cartesian_waypoints[i] + end = cartesian_waypoints[i + 1] + num_interpolations = int(waypoint_interval_s / interpolate_interval_s) + for j in range(num_interpolations): + interpolated_waypoint = start + (end - start) * j / num_interpolations + interpolated_waypoints.append(interpolated_waypoint) + + pose_error = np.zeros(6) + pose_error_cnt = 0 + # Set waypoints one-by-one (only position control) + for waypoint in interpolated_waypoints: eef_state = controller.get_eef_state() eef_cmd = arx5.EEFState(waypoint, 0.0) - eef_cmd.timestamp = eef_state.timestamp + waypoint_interval_s + eef_cmd.timestamp = eef_state.timestamp + interpolate_interval_s controller.set_eef_cmd(eef_cmd) - time.sleep(waypoint_interval_s) + current_time = time.time() + while time.time() < current_time + interpolate_interval_s: + # You can do whatever you want here while the robot is moving + eef_state = controller.get_eef_state() + eef_cmd = controller.get_eef_cmd() + # joint_vel = controller.get_joint_cmd().vel() + # print(f"Vel cmd: {joint_vel}") + pose_error += eef_cmd.pose_6d() - eef_state.pose_6d() + pose_error_cnt += 1 + time.sleep(0.05) + single_update_pose_err = pose_error / pose_error_cnt + controller.reset_to_home() + + # Directly set a eef trajectory (with velocity automatically updated) + eef_traj = [] + current_timestamp = controller.get_eef_state().timestamp + for k, waypoint in enumerate(interpolated_waypoints): + eef_cmd = arx5.EEFState(waypoint, 0.0) + eef_cmd.timestamp = current_timestamp + interpolate_interval_s * (k + 1) + eef_traj.append(eef_cmd) + controller.set_eef_traj(eef_traj) + pose_error = np.zeros(6) + pose_error_cnt = 0 + current_time = time.time() + while time.time() < current_time + interpolate_interval_s * len( + interpolated_waypoints + ): + # You can do whatever you want here while the robot is moving + eef_state = controller.get_eef_state() + eef_cmd = controller.get_eef_cmd() + # joint_vel = controller.get_joint_cmd().vel().copy() + # print(f"Vel cmd: {joint_vel}") + pose_error += eef_cmd.pose_6d() - eef_state.pose_6d() + pose_error_cnt += 1 + time.sleep(0.05) + traj_update_pose_err = pose_error / pose_error_cnt controller.reset_to_home() + print(f"Single point update: average pose error {single_update_pose_err}") + print(f"EEF trajectory update: average pose error {traj_update_pose_err}") + # Since the input trajectory is already smooth enough, the tracking error difference is not significant if __name__ == "__main__": diff --git a/python/examples/joint_waypoint_scheduling.py b/python/examples/joint_waypoint_scheduling.py index 37ebfb5..adc0c85 100644 --- a/python/examples/joint_waypoint_scheduling.py +++ b/python/examples/joint_waypoint_scheduling.py @@ -14,24 +14,34 @@ @click.command() @click.argument("model") # ARX arm model: X5 or L5 @click.argument("interface") # can bus name (can0 etc.) -@click.option("--urdf_path", "-u", default="../models/arx5.urdf", help="URDF file path") -def main(model: str, interface: str, urdf_path: str): +def main(model: str, interface: str): controller = arx5.Arx5JointController(model, interface) - controller.enable_background_send_recv() - controller.enable_gravity_compensation(urdf_path) controller.set_log_level(arx5.LogLevel.DEBUG) controller.reset_to_home() joint_waypoints = np.array( [[1.0, 2.0, 2.0, 1.5, 1.5, -1.57], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] ) waypoint_interval_s = 5.0 + + # Single waypoint scheduling for waypoint in joint_waypoints: - joint_state = controller.get_state() + joint_state = controller.get_joint_state() joint_cmd = arx5.JointState(waypoint, np.zeros(6), np.zeros(6), 0.0) joint_cmd.timestamp = joint_state.timestamp + waypoint_interval_s controller.set_joint_cmd(joint_cmd) time.sleep(waypoint_interval_s) + # Sending joint trajectory + joint_traj = [] + init_timestamp = controller.get_joint_state().timestamp + for waypoint in joint_waypoints: + joint_traj.append(arx5.JointState(waypoint, np.zeros(6), np.zeros(6), 0.0)) + joint_traj[-1].timestamp = init_timestamp + waypoint_interval_s * len( + joint_traj + ) + controller.set_joint_traj(joint_traj) + time.sleep(waypoint_interval_s * len(joint_traj)) + time.sleep(1.0) controller.reset_to_home() diff --git a/python/examples/keyboard_teleop.py b/python/examples/keyboard_teleop.py index b5906f1..1188cab 100644 --- a/python/examples/keyboard_teleop.py +++ b/python/examples/keyboard_teleop.py @@ -19,14 +19,15 @@ def start_keyboard_teleop(controller: Arx5CartesianController): - ori_speed = 0.6 - pos_speed = 0.2 + ori_speed = 1.0 + pos_speed = 0.4 gripper_speed = 0.04 target_pose_6d = controller.get_home_pose() target_gripper_pos = 0.0 - - window_size = 20 + cmd_dt = 0.01 + preview_time = 0.1 + window_size = 5 keyboard_queue = Queue(window_size) robot_config = controller.get_robot_config() controller_config = controller.get_controller_config() @@ -130,33 +131,30 @@ def get_filtered_keyboard_output(key_pressed: dict): else: gripper_cmd = 0 - target_pose_6d[:3] += state[:3] * pos_speed * controller_config.controller_dt - target_pose_6d[3:] += state[3:] * ori_speed * controller_config.controller_dt - target_gripper_pos += ( - gripper_cmd * gripper_speed * controller_config.controller_dt - ) + target_pose_6d[:3] += state[:3] * pos_speed * cmd_dt + target_pose_6d[3:] += state[3:] * ori_speed * cmd_dt + target_gripper_pos += gripper_cmd * gripper_speed * cmd_dt if target_gripper_pos >= robot_config.gripper_width: target_gripper_pos = robot_config.gripper_width elif target_gripper_pos <= 0: target_gripper_pos = 0 loop_cnt += 1 - while ( - time.monotonic() < start_time + loop_cnt * controller_config.controller_dt - ): + while time.monotonic() < start_time + loop_cnt * cmd_dt: pass + current_timestamp = controller.get_timestamp() eef_cmd = EEFState() eef_cmd.pose_6d()[:] = target_pose_6d eef_cmd.gripper_pos = target_gripper_pos + eef_cmd.timestamp = current_timestamp + preview_time controller.set_eef_cmd(eef_cmd) @click.command() @click.argument("model") # ARX arm model: X5 or L5 @click.argument("interface") # can bus name (can0 etc.) -@click.option("--urdf_path", "-u", default="../models/arx5.urdf", help="URDF file path") -def main(model: str, interface: str, urdf_path: str): - controller = Arx5CartesianController(model, interface, urdf_path) +def main(model: str, interface: str): + controller = Arx5CartesianController(model, interface) controller.reset_to_home() robot_config = controller.get_robot_config() diff --git a/python/examples/spacemouse_teleop.py b/python/examples/spacemouse_teleop.py index 6f34bfd..976d8ec 100644 --- a/python/examples/spacemouse_teleop.py +++ b/python/examples/spacemouse_teleop.py @@ -7,7 +7,15 @@ ROOT_DIR = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) sys.path.append(ROOT_DIR) os.chdir(ROOT_DIR) -from arx5_interface import Arx5CartesianController, EEFState, Gain, LogLevel +from arx5_interface import ( + Arx5CartesianController, + ControllerConfig, + ControllerConfigFactory, + EEFState, + Gain, + LogLevel, + RobotConfigFactory, +) from peripherals.spacemouse_shared_memory import Spacemouse from multiprocessing.managers import SharedMemoryManager @@ -18,22 +26,49 @@ def start_teleop_recording(controller: Arx5CartesianController): ori_speed = 1.5 - pos_speed = 0.6 + pos_speed = 0.8 gripper_speed = 0.04 + # For earlier spacemouse versions (wired version), the readout might be not zero even after it is released + # If you are using the wireless 3Dconnexion spacemouse, you can set the deadzone_threshold to 0.0 for better sensitivity + deadzone_threshold = 0.0 target_pose_6d = controller.get_home_pose() target_gripper_pos = 0.0 - window_size = 5 + window_size = 3 + cmd_dt = 0.01 + preview_time = 0.05 # Each trajectory command is 0.15s ahead of the current time + + UPDATE_TRAJ = True + # False: only override single points with position control + # True: send a trajectory command every update_dt. Will include velocity. This is + + pose_x_min = target_pose_6d[0] spacemouse_queue = Queue(window_size) robot_config = controller.get_robot_config() - controller_config = controller.get_controller_config() - + + avg_error = np.zeros(6) + avg_cnt = 0 + prev_eef_cmd = EEFState() + eef_cmd = EEFState() + with SharedMemoryManager() as shm_manager: - with Spacemouse(shm_manager=shm_manager, deadzone=0.1, max_value=500) as sm: + with Spacemouse( + shm_manager=shm_manager, deadzone=deadzone_threshold, max_value=500 + ) as sm: def get_filtered_spacemouse_output(sm: Spacemouse): state = sm.get_motion_state_transformed() + # Remove the deadzone and normalize the output + positive_idx = state >= deadzone_threshold + negative_idx = state <= -deadzone_threshold + state[positive_idx] = (state[positive_idx] - deadzone_threshold) / ( + 1 - deadzone_threshold + ) + state[negative_idx] = (state[negative_idx] + deadzone_threshold) / ( + 1 - deadzone_threshold + ) + if ( spacemouse_queue.maxsize > 0 and spacemouse_queue._qsize() == spacemouse_queue.maxsize @@ -51,6 +86,8 @@ def get_filtered_spacemouse_output(sm: Spacemouse): if state.any() or button_left or button_right: print(f"Start tracking!") break + eef_cmd = controller.get_eef_cmd() + prev_eef_cmd = eef_cmd start_time = time.monotonic() loop_cnt = 0 while True: @@ -64,12 +101,18 @@ def get_filtered_spacemouse_output(sm: Spacemouse): button_left = sm.is_button_pressed(0) button_right = sm.is_button_pressed(1) if button_left and button_right: + print(f"Avg 6D pose error: {avg_error / avg_cnt}") + # Traj with vel Avg 6D pose error: [ 0.0004 0.0002 -0.0016 0.0002 0.0032 0.0005] + # Single point without vel: [-0.0002 -0.0006 -0.0026 0.0027 0.0042 -0.0017] + # Traj without vel Avg 6D pose error: [ 0.0005 0.0008 -0.005 -0.0024 0.0073 -0.0001] + controller.reset_to_home() config = controller.get_robot_config() target_pose_6d = controller.get_home_pose() target_gripper_pos = 0.0 loop_cnt = 0 start_time = time.monotonic() + continue elif button_left and not button_right: gripper_cmd = 1 @@ -77,39 +120,54 @@ def get_filtered_spacemouse_output(sm: Spacemouse): gripper_cmd = -1 else: gripper_cmd = 0 - - target_pose_6d[:3] += ( - state[:3] * pos_speed * controller_config.controller_dt - ) - target_pose_6d[3:] += ( - state[3:] * ori_speed * controller_config.controller_dt - ) - target_gripper_pos += ( - gripper_cmd * gripper_speed * controller_config.controller_dt - ) + # print(state, target_gripper_pos) + target_pose_6d[:3] += state[:3] * pos_speed * cmd_dt + target_pose_6d[3:] += state[3:] * ori_speed * cmd_dt + target_gripper_pos += gripper_cmd * gripper_speed * cmd_dt if target_gripper_pos >= robot_config.gripper_width: target_gripper_pos = robot_config.gripper_width elif target_gripper_pos <= 0: target_gripper_pos = 0 loop_cnt += 1 - while ( - time.monotonic() - < start_time + loop_cnt * controller_config.controller_dt - ): + while time.monotonic() < start_time + loop_cnt * cmd_dt: pass - - eef_cmd = EEFState() + current_timestamp = controller.get_timestamp() + prev_eef_cmd = eef_cmd + if target_pose_6d[0] < pose_x_min: + target_pose_6d[0] = pose_x_min eef_cmd.pose_6d()[:] = target_pose_6d eef_cmd.gripper_pos = target_gripper_pos - controller.set_eef_cmd(eef_cmd) + eef_cmd.timestamp = current_timestamp + preview_time + + if UPDATE_TRAJ: + # This will calculate the velocity automatically + controller.set_eef_traj([eef_cmd]) + + # Or sending single eef_cmd: + else: + # Only position control + controller.set_eef_cmd(eef_cmd) + + output_eef_cmd = controller.get_eef_cmd() + eef_state = controller.get_eef_state() + avg_error += output_eef_cmd.pose_6d() - eef_state.pose_6d() + avg_cnt += 1 + + print(f"6DPose Error: {output_eef_cmd.pose_6d() - eef_state.pose_6d()}") @click.command() @click.argument("model") # ARX arm model: X5 or L5 @click.argument("interface") # can bus name (can0 etc.) -@click.option("--urdf_path", "-u", default="../models/arx5.urdf", help="URDF file path") -def main(model: str, interface: str, urdf_path: str): - controller = Arx5CartesianController(model, interface, urdf_path) +def main(model: str, interface: str): + + robot_config = RobotConfigFactory.get_instance().get_config(model) + controller_config = ControllerConfigFactory.get_instance().get_config( + "cartesian_controller", robot_config.joint_dof + ) + # controller_config.interpolation_method = "cubic" + controller_config.default_kp = controller_config.default_kp + controller = Arx5CartesianController(robot_config, controller_config, interface) controller.reset_to_home() robot_config = controller.get_robot_config() diff --git a/python/examples/teach_replay.py b/python/examples/teach_replay.py index 4935373..cb23c16 100644 --- a/python/examples/teach_replay.py +++ b/python/examples/teach_replay.py @@ -105,9 +105,8 @@ def start_high_level_replay(controller: Arx5CartesianController, data_file: str) @click.command() @click.argument("model") # ARX arm model: X5 or L5 @click.argument("interface") # can bus name (can0 etc.) -@click.option("--urdf_path", "-u", default="../models/arx5.urdf", help="URDF file path") -def main(model: str, interface: str, urdf_path: str): - controller = Arx5CartesianController(model, interface, urdf_path) +def main(model: str, interface: str): + controller = Arx5CartesianController(model, interface) np.set_printoptions(precision=4, suppress=True) os.makedirs("data", exist_ok=True) diff --git a/python/examples/test_bimanual.py b/python/examples/test_bimanual.py index b8f2ba1..551398f 100644 --- a/python/examples/test_bimanual.py +++ b/python/examples/test_bimanual.py @@ -25,18 +25,11 @@ def main(): robot_config = arx5_0.get_robot_config() controller_config = arx5_0.get_controller_config() - arx5_0.enable_background_send_recv() arx5_0.reset_to_home() - arx5_0.enable_gravity_compensation("../models/arx5.urdf") - arx5_1.enable_background_send_recv() arx5_1.reset_to_home() - arx5_1.enable_gravity_compensation("../models/arx5.urdf") target_joint_poses = np.array([1.0, 2.0, 2.0, 1.5, 1.5, -1.57]) step_num = 1500 # 3s - USE_TIMER = True - if not USE_TIMER: - arx5_0.disable_background_send_recv() for i in range(step_num): cmd = arx5.JointState(robot_config.joint_dof) @@ -45,11 +38,8 @@ def main(): cmd.gripper_pos = easeInOutQuad((i / (step_num - 1))) * 0.08 arx5_0.set_joint_cmd(cmd) arx5_1.set_joint_cmd(cmd) - if not USE_TIMER: - arx5_0.send_recv_once() - arx5_1.send_recv_once() - JointState = arx5_0.get_state() - JointState = arx5_1.get_state() + JointState = arx5_0.get_joint_state() + JointState = arx5_1.get_joint_state() arm_dof_pos = JointState.pos().copy() arm_dof_vel = JointState.vel().copy() # print(arm_dof_pos, arm_dof_vel) @@ -64,12 +54,9 @@ def main(): cmd.gripper_pos = easeInOutQuad((1 - i / (step_num - 1))) * 0.08 arx5_0.set_joint_cmd(cmd) arx5_1.set_joint_cmd(cmd) - if not USE_TIMER: - arx5_0.send_recv_once() - arx5_1.send_recv_once() time.sleep(controller_config.controller_dt) - JointState = arx5_0.get_state() - JointState = arx5_1.get_state() + JointState = arx5_0.get_joint_state() + JointState = arx5_1.get_joint_state() # print(f"gripper: {JointState.gripper_pos:.05f}") diff --git a/python/examples/test_joint_control.py b/python/examples/test_joint_control.py index 7025cb0..1bfbda8 100644 --- a/python/examples/test_joint_control.py +++ b/python/examples/test_joint_control.py @@ -23,8 +23,7 @@ def easeInOutQuad(t): @click.command() @click.argument("model") # ARX arm model: X5 or L5 @click.argument("interface") # can bus name (can0 etc.) -@click.option("--urdf_path", "-u", default="../models/arx5.urdf", help="URDF file path") -def main(model: str, interface: str, urdf_path: str): +def main(model: str, interface: str): # To initialize robot with different configurations, # you can create RobotConfig and ControllerConfig by yourself and modify based on it @@ -35,6 +34,15 @@ def main(model: str, interface: str, urdf_path: str): # Modify the default configuration here # controller_config.controller_dt = 0.01 # etc. + USE_MULTITHREADING = True + if USE_MULTITHREADING: + # Will create another thread that communicates with the arm, so each send_recv_once() will take no time + # for the main thread to execute. Otherwise (without background send/recv), send_recv_once() will block the + # main thread until the arm responds (usually 2ms). + controller_config.background_send_recv = True + else: + controller_config.background_send_recv = False + arx5_joint_controller = arx5.Arx5JointController( robot_config, controller_config, interface ) @@ -48,14 +56,8 @@ def main(model: str, interface: str, urdf_path: str): controller_config = arx5_joint_controller.get_controller_config() step_num = 1500 - USE_MULTITHREADING = True - if USE_MULTITHREADING: - # Will create another thread that communicates with the arm, so each send_recv_once() will take no time - # for the main thread to execute. Otherwise (without background send/recv), send_recv_once() will block the - # main thread until the arm responds (usually 2ms). - arx5_joint_controller.enable_background_send_recv() + arx5_joint_controller.reset_to_home() - arx5_joint_controller.enable_gravity_compensation(urdf_path) target_joint_poses = np.array([1.0, 2.0, 2.0, 1.5, 1.5, -1.57]) @@ -69,7 +71,7 @@ def main(model: str, interface: str, urdf_path: str): arx5_joint_controller.send_recv_once() else: time.sleep(controller_config.controller_dt) - JointState = arx5_joint_controller.get_state() + JointState = arx5_joint_controller.get_joint_state() arm_dof_pos = JointState.pos().copy() arm_dof_vel = JointState.vel().copy() # print(arm_dof_pos, arm_dof_vel) @@ -86,7 +88,7 @@ def main(model: str, interface: str, urdf_path: str): arx5_joint_controller.send_recv_once() else: time.sleep(controller_config.controller_dt) - JointState = arx5_joint_controller.get_state() + JointState = arx5_joint_controller.get_joint_state() # print(f"gripper: {JointState.gripper_pos:.05f}") arx5_joint_controller.reset_to_home() diff --git a/python/examples/test_solver.py b/python/examples/test_solver.py index 55ac7df..a64e908 100644 --- a/python/examples/test_solver.py +++ b/python/examples/test_solver.py @@ -9,17 +9,20 @@ import numpy as np -joint_dof = 7 +joint_dof = 6 +robot_config = arx5.RobotConfigFactory.get_instance().get_config("X5") solver = arx5.Arx5Solver( - # "../models/arx5_gopro.urdf", - "../models/arx7_left.urdf", + "../models/arx5.urdf", + # "../models/arx7_left.urdf", joint_dof, - "base_link", - "eef_link", - np.array([0, 0, -9.807]), + robot_config.joint_pos_min, + robot_config.joint_pos_max, + # "base_link", + # "eef_link", + # np.array([0, 0, -9.807], dtype=np.float64), ) -# solver = arx5.Arx5Solver("../models/arx5_gopro.urdf", joint_dof) +# solver = arx5.Arx5Solver("../models/arx5_umi.urdf", joint_dof) # joint_dof = 7 # solver = arx5.Arx5Solver( @@ -32,18 +35,59 @@ print("Done initialization") + +# joint_pos = np.array([-0.000, 2.487, -0.449, 2.937, -0.000, -0.000]) + joint_pos = np.zeros(joint_dof) -joint_pos[2] = np.pi / 2 -joint_pos[3] = -np.pi / 2 +# joint_pos = np.zeros(joint_dof) +# joint_pos[2] = np.pi / 2 +# joint_pos[3] = -np.pi / 2 joint_vel = np.zeros(joint_dof) joint_acc = np.zeros(joint_dof) fk_pose = solver.forward_kinematics(joint_pos) print(f"{fk_pose=}") -succeed, ik_joint_pos = solver.inverse_kinematics(fk_pose, joint_pos) +home_pose = solver.forward_kinematics(joint_pos) +init_pose = home_pose + np.array([0.05, 0, 0, 0, 0, 0]) +ik_status, init_joint_pos = solver.inverse_kinematics(init_pose, np.zeros(6)) + +print(f"{init_joint_pos=}") + +ik_status, ik_joint_pos = solver.inverse_kinematics( + home_pose, + np.array( + [ + -3.69121061e-06, + 8.25553071e-01, + 1.94861662e-02, + 0.5, # 0.76 + 1.47200640e-06, + -2.36060781e-06, + ] + ), +) + +ik_status, multi_trial_ik_joint_pos = solver.multi_trial_ik( + home_pose, + np.array( + [ + -3.69121061e-06, + 8.25553071e-01, + 1.94861662e-02, + 0.5, # 0.76 + 1.47200640e-06, + -2.36060781e-06, + ] + ), + 5, +) + + +# ik_status, ik_joint_pos = solver.inverse_kinematics(home_pose, init_joint_pos) + -print(f"{ik_joint_pos=}") +print(f"{ik_joint_pos=}, {multi_trial_ik_joint_pos=}") start_time = time.monotonic() id_torque = solver.inverse_dynamics(joint_pos, joint_vel, joint_acc) diff --git a/python/examples/test_upside_down.py b/python/examples/test_upside_down.py new file mode 100644 index 0000000..49a070d --- /dev/null +++ b/python/examples/test_upside_down.py @@ -0,0 +1,43 @@ +import time + +import os +import sys + +ROOT_DIR = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +sys.path.append(ROOT_DIR) +os.chdir(ROOT_DIR) +import arx5_interface as arx5 +import click +import numpy as np + +""" +This script is to test the solver when the gravity vector is not the default value (pointing down) +To run this script successfully, please hang the robot arm upside down, so the gravity vector becomes [0, 0, 9.81]. +""" + + +@click.command() +@click.argument("model") # ARX arm model: X5 or L5 +@click.argument("interface") # can bus name (can0 etc.) +def main(model: str, interface: str): + + robot_config = arx5.RobotConfigFactory.get_instance().get_config(model) + robot_config.gravity_vector = np.array([0, 0, 9.81]) + controller_config = arx5.ControllerConfigFactory.get_instance().get_config( + "joint_controller", robot_config.joint_dof + ) + + arx5_joint_controller = arx5.Arx5JointController( + robot_config, controller_config, interface + ) + + gain = arx5.Gain(robot_config.joint_dof) + gain.kd()[:] = 0.1 + + arx5_joint_controller.set_gain(gain) + + time.sleep(1000) + + +if __name__ == "__main__": + main() diff --git a/python/examples/test_x7.py b/python/examples/test_x7.py index 2eb5cc0..a90debf 100644 --- a/python/examples/test_x7.py +++ b/python/examples/test_x7.py @@ -21,19 +21,22 @@ def easeInOutQuad(t): def main(): np.set_printoptions(precision=3, suppress=True) x7_left = arx5.Arx5JointController("X7Left", "can0") + x7_left_config = x7_left.get_robot_config() # x7_left = arx5.Arx5JointController("X7Left", "can1") # x7_left.set_log_level(arx5.LogLevel.DEBUG) - x7_left.enable_background_send_recv() # x7_left.reset_to_home() gain = x7_left.get_gain() gain.kp()[:] = 0 x7_left.set_gain(gain) - x7_left_solver = arx5.Arx5Solver("../models/arx7_left.urdf", 7) + x7_left_solver = arx5.Arx5Solver( + "../models/arx7_left.urdf", + 7, + x7_left_config.joint_pos_min, + x7_left_config.joint_pos_max, + ) - x7_left.enable_gravity_compensation("../models/arx7_left.urdf") - while True: - joint_state = x7_left.get_state() + joint_state = x7_left.get_joint_state() ee_pose = x7_left_solver.forward_kinematics(joint_state.pos()) print(ee_pose, joint_state.pos()) time.sleep(0.1) diff --git a/python/shared_memory/shared_memory_queue.py b/python/shared_memory/shared_memory_queue.py index 7b181a6..6ab3c10 100644 --- a/python/shared_memory/shared_memory_queue.py +++ b/python/shared_memory/shared_memory_queue.py @@ -1,6 +1,6 @@ from typing import Dict, List, Union import numbers -from queue import (Empty, Full) +from queue import Empty, Full from multiprocessing.managers import SharedMemoryManager import numpy as np from shared_memory.shared_memory_util import ArraySpec, SharedAtomicCounter @@ -12,14 +12,15 @@ class SharedMemoryQueue: A Lock-Free FIFO Shared Memory Data Structure. Stores a sequence of dict of numpy arrays. """ + class CallbackGuard: def __init__(self, callback, data): self.callback = callback self.data = data - + def __enter__(self): return self.data - + def __exit__(self, type, value, traceback): self.callback() del self.data @@ -49,9 +50,10 @@ def __init__( array = SharedNDArray.create_from_shape( mem_mgr=shm_manager, shape=(buffer_size,) + tuple(spec.shape), - dtype=spec.dtype) + dtype=spec.dtype, + ) shared_arrays[key] = array - + self.buffer_size = buffer_size self.array_specs = array_specs self.write_counter = write_counter @@ -73,18 +75,14 @@ def create_from_examples( if isinstance(value, np.ndarray): shape = value.shape dtype = value.dtype - assert dtype != np.dtype('O') + assert dtype != np.dtype("O") elif isinstance(value, numbers.Number): shape = tuple() dtype = np.dtype(type(value)) else: - raise TypeError(f'Unsupported type {type(value)}') + raise TypeError(f"Unsupported type {type(value)}") - spec = ArraySpec( - name=key, - shape=shape, - dtype=dtype - ) + spec = ArraySpec(name=key, shape=shape, dtype=dtype) specs.append(spec) obj = cls( @@ -94,17 +92,17 @@ def create_from_examples( use_atomic_counter=use_atomic_counter, ) return obj - + def qsize(self): read_count = self.read_counter.load() write_count = self.write_counter.load() n_data = write_count - read_count return n_data - + def empty(self): n_data = self.qsize() return n_data <= 0 - + def clear(self): self.read_counter.store(self.write_counter.load()) @@ -114,7 +112,7 @@ def put(self, data: Dict[str, Union[np.ndarray, float]]): n_data = write_count - read_count if n_data >= self.buffer_size: raise Full() - + next_idx = write_count % self.buffer_size # write to shared memory @@ -161,11 +159,11 @@ def get(self, out=None) -> Dict[str, np.ndarray]: for key, value in self.shared_arrays.items(): arr = value.get() np.copyto(out[key], arr[next_idx]) - + # update idx self.read_counter.add(1) return out - + def get_next_view(self) -> Dict[str, np.ndarray]: """ Get reference to the next element to write @@ -176,7 +174,7 @@ def get_next_view(self) -> Dict[str, np.ndarray]: n_data = write_count - read_count if n_data >= self.buffer_size: raise Full() - + next_idx = write_count % self.buffer_size out = dict() for key, value in self.shared_arrays.items(): @@ -195,7 +193,7 @@ def put_next_view(self, data: Dict[str, Union[np.ndarray, numbers.Number]]): n_data = write_count - read_count if n_data >= self.buffer_size: raise Full() - + next_idx = write_count % self.buffer_size # write to shared memory for key, value in data.items(): @@ -210,23 +208,20 @@ def put_next_view(self, data: Dict[str, Union[np.ndarray, numbers.Number]]): # update idx self.write_counter.add(1) - def get_view(self) -> CallbackGuard: write_count = self.write_counter.load() read_count = self.read_counter.load() n_data = write_count - read_count if n_data <= 0: raise Empty() - + next_idx = read_count % self.buffer_size data = dict() for key, value in self.shared_arrays.items(): arr = value.get() data[key] = arr[next_idx] - - return self.CallbackGuard( - callback=lambda: self.read_counter.add(1), - data=data) + + return self.CallbackGuard(callback=lambda: self.read_counter.add(1), data=data) def get_k(self, k, out=None) -> Dict[str, np.ndarray]: write_count = self.write_counter.load() diff --git a/python/shared_memory/shared_memory_ring_buffer.py b/python/shared_memory/shared_memory_ring_buffer.py index f6ceb26..d3832db 100644 --- a/python/shared_memory/shared_memory_ring_buffer.py +++ b/python/shared_memory/shared_memory_ring_buffer.py @@ -137,11 +137,11 @@ def put(self, data: Dict[str, Union[np.ndarray, float]], wait: bool = True): # get_last_k can safely read k elements from any count location. # Sanity check: when get_max_k == 1, the element pointed by next_idx # should be rewritten at minimum self.get_time_budget seconds later. - timestamp_lookahead_idx = (next_idx + self.get_max_k - 1) % self.buffer_size + timestamp_preview_idx = (next_idx + self.get_max_k - 1) % self.buffer_size # print( - # f"count: {count}, type: {type(count)}, {timestamp_lookahead_idx=}, {self.buffer_size=}({type(self.buffer_size)})" + # f"count: {count}, type: {type(count)}, {timestamp_preview_idx=}, {self.buffer_size=}({type(self.buffer_size)})" # ) - old_timestamp = self.timestamp_array.get()[timestamp_lookahead_idx] + old_timestamp = self.timestamp_array.get()[timestamp_preview_idx] t = time.monotonic() if (t - old_timestamp) < self.get_time_budget: deltat = t - old_timestamp diff --git a/src/app/cartesian_controller.cpp b/src/app/cartesian_controller.cpp index bf9f1a9..1a4077b 100644 --- a/src/app/cartesian_controller.cpp +++ b/src/app/cartesian_controller.cpp @@ -6,663 +6,174 @@ #include #include -namespace arx -{ +using namespace arx; Arx5CartesianController::Arx5CartesianController(RobotConfig robot_config, ControllerConfig controller_config, - std::string interface_name, std::string urdf_path) - : _can_handle(interface_name), - _logger(spdlog::stdout_color_mt(robot_config.robot_model + std::string("_") + interface_name)), - _robot_config(robot_config), _controller_config(controller_config) + std::string interface_name) + : Arx5ControllerBase(robot_config, controller_config, interface_name) { - _logger->set_pattern("[%H:%M:%S %n %^%l%$] %v"); - _solver = std::make_shared(urdf_path, _robot_config.joint_dof, _robot_config.base_link_name, - _robot_config.eef_link_name, _robot_config.gravity_vector); - _init_robot(); - _background_send_recv_thread = std::thread(&Arx5CartesianController::_background_send_recv, this); - _logger->info("Background send_recv task is running at ID: {}", syscall(SYS_gettid)); + if (!controller_config.background_send_recv) + throw std::runtime_error( + "controller_config.background_send_recv should be set to true when running cartesian controller."); } -Arx5CartesianController::Arx5CartesianController(std::string model, std::string interface_name, std::string urdf_path) +Arx5CartesianController::Arx5CartesianController(std::string model, std::string interface_name) : Arx5CartesianController::Arx5CartesianController( RobotConfigFactory::get_instance().get_config(model), ControllerConfigFactory::get_instance().get_config( "cartesian_controller", RobotConfigFactory::get_instance().get_config(model).joint_dof), - interface_name, urdf_path) -{ -} - -Arx5CartesianController::~Arx5CartesianController() -{ - Gain damping_gain{_robot_config.joint_dof}; - damping_gain.kd = _controller_config.default_kd; - // damping_gain.kd[0] *= 3; - // damping_gain.kd[1] *= 3; - // damping_gain.kd[2] *= 3; - // damping_gain.kd[3] *= 1.5; - _logger->info("Set to damping before exit"); - set_gain(damping_gain); - _input_joint_cmd.vel = VecDoF::Zero(_robot_config.joint_dof); - _input_joint_cmd.torque = VecDoF::Zero(_robot_config.joint_dof); - _enable_gravity_compensation = false; - sleep_ms(2000); - _destroy_background_threads = true; - _background_send_recv_thread.join(); - _logger->info("background send_recv task joined"); - spdlog::drop(_logger->name()); - _logger.reset(); - _solver.reset(); -} - -void Arx5CartesianController::_init_robot() + interface_name) { - for (int i = 0; i < _robot_config.joint_dof; ++i) - { - if (_robot_config.motor_type[i] == MotorType::DM_J4310 || _robot_config.motor_type[i] == MotorType::DM_J4340) - { - int id = _robot_config.motor_id[i]; - _can_handle.enable_DM_motor(id); - sleep_us(1000); - } - } - if (_robot_config.gripper_motor_type == MotorType::DM_J4310) - { - _can_handle.enable_DM_motor(_robot_config.gripper_motor_id); - sleep_us(1000); - } - - Gain gain{_robot_config.joint_dof}; - gain.kd = _controller_config.default_kd; - _input_joint_cmd = JointState(_robot_config.joint_dof); // initialize joint command to zero - - set_gain(gain); // set to damping by default - for (int i = 0; i <= 10; ++i) - { - // make sure all the motor positions are updated - _send_recv(); - sleep_ms(5); - } - // Check whether any motor has non-zero position - if (_joint_state.pos == VecDoF::Zero(_robot_config.joint_dof)) - { - _logger->error("None of the motors are initialized. Please check the connection or power of the arm."); - throw std::runtime_error( - "None of the motors are initialized. Please check the connection or power of the arm."); - } - _input_eef_cmd = get_eef_state(); - _output_eef_cmd = get_eef_state(); - _interp_start_eef_cmd = get_eef_state(); - _input_eef_cmd.timestamp = 0; - _output_eef_cmd.timestamp = 0; - _interp_start_eef_cmd.timestamp = 0; - - _background_send_recv_running = true; } void Arx5CartesianController::set_eef_cmd(EEFState new_cmd) { - std::lock_guard lock(_cmd_mutex); - if (new_cmd.gripper_vel != 0 || new_cmd.gripper_torque != 0) - { - _logger->warn("Gripper vel and torque control is not supported yet."); - new_cmd.gripper_vel = 0; - new_cmd.gripper_torque = 0; - } - if (new_cmd.timestamp != 0 && new_cmd.timestamp < get_timestamp()) - { - _logger->warn("EEF command timestamp ({:.4f}s) is not 0 but in the past (current timestamp: {:.4f}s). New EEF " - "command is ignored.", - new_cmd.timestamp, get_timestamp()); - return; - } - _input_eef_cmd = new_cmd; - _interp_start_eef_cmd = _output_eef_cmd; -} - -std::tuple Arx5CartesianController::get_eef_cmd() -{ - std::lock_guard lock(_cmd_mutex); - return std::make_tuple(_input_eef_cmd, _output_eef_cmd); -} - -EEFState Arx5CartesianController::get_eef_state() -{ - std::lock_guard lock(_state_mutex); - EEFState eef_state; - eef_state.timestamp = _joint_state.timestamp; - eef_state.pose_6d = _solver->forward_kinematics(_joint_state.pos); - eef_state.gripper_pos = _joint_state.gripper_pos; - eef_state.gripper_vel = _joint_state.gripper_vel; - eef_state.gripper_torque = _joint_state.gripper_torque; - return eef_state; -} - -JointState Arx5CartesianController::get_joint_state() -{ - std::lock_guard lock(_state_mutex); - return _joint_state; -} - -std::tuple Arx5CartesianController::get_joint_cmd() -{ - std::lock_guard lock(_cmd_mutex); - return std::make_tuple(_input_joint_cmd, _output_joint_cmd); -} - -double Arx5CartesianController::get_timestamp() -{ - return double(get_time_us() - _start_time_us) / 1e6; -} - -Gain Arx5CartesianController::get_gain() -{ - std::lock_guard lock(_cmd_mutex); - return _gain; -} - -void Arx5CartesianController::set_gain(Gain new_gain) -{ - // Make sure the robot doesn't jump when setting kp to non-zero - if (_gain.kp.isZero() && !new_gain.kp.isZero()) - { - double max_pos_error = (_joint_state.pos - _output_joint_cmd.pos).cwiseAbs().maxCoeff(); - double pos_error_threshold = 0.2; - double kp_threshold = 1; - if (max_pos_error > pos_error_threshold && new_gain.kp.maxCoeff() > kp_threshold) - { - _logger->error("Cannot set kp too large when the joint pos cmd is far from current pos."); - _logger->error( - "Target max kp: {}, kp threshold: {}. Current pos: {}, cmd pos: {}, position error threshold: {}", - new_gain.kp.maxCoeff(), kp_threshold, vec2str(_joint_state.pos), vec2str(_output_joint_cmd.pos), - pos_error_threshold); - _background_send_recv_running = false; - throw std::runtime_error("Cannot set kp to non-zero when the joint pos cmd is far from current pos."); - } - } - _gain = new_gain; -} - -RobotConfig Arx5CartesianController::get_robot_config() -{ - return _robot_config; -} - -ControllerConfig Arx5CartesianController::get_controller_config() -{ - return _controller_config; -} - -Pose6d Arx5CartesianController::get_home_pose() -{ - return _solver->forward_kinematics(VecDoF::Zero(_robot_config.joint_dof)); -} - -void Arx5CartesianController::reset_to_home() -{ - JointState joint_cmd{_robot_config.joint_dof}; - EEFState eef_cmd; - Gain gain{_robot_config.joint_dof}; - JointState init_state = get_joint_state(); - Gain init_gain = get_gain(); - Gain target_gain{_robot_config.joint_dof}; - if (init_gain.kp.isZero()) - { - _logger->info("Current kp is zero. Setting to default kp kd"); - target_gain = Gain(_controller_config.default_kp, _controller_config.default_kd, - _controller_config.default_gripper_kp, _controller_config.default_gripper_kd); - } - else - { - target_gain = init_gain; - } - - JointState target_state{_robot_config.joint_dof}; - - // calculate the maximum joint position error - double max_pos_error = (init_state.pos - VecDoF::Zero(_robot_config.joint_dof)).cwiseAbs().maxCoeff(); - max_pos_error = std::max(max_pos_error, init_state.gripper_pos * 2 / _robot_config.gripper_width); - // interpolate from current kp kd to default kp kd in max(max_pos_error*2, 0.5)s - // and keep the target for 0.5s - double step_num = std::max(max_pos_error * 2, 0.5) / _controller_config.controller_dt; - _logger->info("Start reset to home in {:.3f}s, max_pos_error: {:.3f}", - std::max(max_pos_error * 2, double(0.5)) + 0.5, max_pos_error); - - bool prev_running = _background_send_recv_running; - _background_send_recv_running = true; - for (int i = 0; i <= step_num; ++i) - { - double alpha = double(i) / step_num; - gain = init_gain * (1 - alpha) + target_gain * alpha; - joint_cmd = init_state * (1 - alpha) + target_state * alpha; - set_gain(gain); - eef_cmd.pose_6d = _solver->forward_kinematics(joint_cmd.pos); - eef_cmd.gripper_pos = joint_cmd.gripper_pos; - set_eef_cmd(eef_cmd); - sleep_ms(5); - } - sleep_ms(500); - _logger->info("Finish reset to home"); - _background_send_recv_running = prev_running; -} - -void Arx5CartesianController::set_to_damping() -{ - JointState joint_cmd{_robot_config.joint_dof}; - EEFState eef_cmd; - JointState joint_state{_robot_config.joint_dof}; - Gain gain{_robot_config.joint_dof}; - JointState init_state = get_joint_state(); - Gain init_gain = get_gain(); - Gain target_gain{_robot_config.joint_dof}; - target_gain.kd = _controller_config.default_kd; - _logger->info("Start set to damping"); - - joint_state = get_joint_state(); - eef_cmd.pose_6d = _solver->forward_kinematics(joint_state.pos); - eef_cmd.gripper_pos = joint_state.gripper_pos; - set_gain(target_gain); - set_eef_cmd(eef_cmd); - - sleep_ms(500); - _logger->info("Finish set to damping"); -} - -void Arx5CartesianController::set_log_level(spdlog::level::level_enum log_level) -{ - _logger->set_level(log_level); -} - -void Arx5CartesianController::_update_output_cmd() -{ - std::lock_guard guard_cmd(_cmd_mutex); - - JointState prev_output_cmd = _output_joint_cmd; + JointState current_joint_state = get_joint_state(); - _output_joint_cmd = _input_joint_cmd; + // The following line only works under c++17 + // auto [success, target_joint_pos] = _solver->inverse_kinematics(new_cmd.pose_6d, current_joint_state.pos); - // Joint velocity clipping - double dt = _controller_config.controller_dt; - for (int i = 0; i < 6; ++i) - { - if (_gain.kp[i] > 0) - { - double delta_pos = _input_joint_cmd.pos[i] - prev_output_cmd.pos[i]; - double max_vel = _robot_config.joint_vel_max[i]; - if (std::abs(delta_pos) > max_vel * dt) - { - _output_joint_cmd.pos[i] = prev_output_cmd.pos[i] + max_vel * dt * delta_pos / std::abs(delta_pos); - _logger->debug("Joint {} pos {:.3f} pos cmd clipped: {:.3f} to {:.3f}", i, _joint_state.pos[i], - _input_joint_cmd.pos[i], _output_joint_cmd.pos[i]); - } - } - else - { - _output_joint_cmd.pos[i] = _joint_state.pos[i]; - } - if (_gain.gripper_kp > 0) - { - double gripper_delta_pos = _input_joint_cmd.gripper_pos - prev_output_cmd.gripper_pos; - if (std::abs(gripper_delta_pos) / dt > _robot_config.gripper_vel_max) - { - _output_joint_cmd.gripper_pos = prev_output_cmd.gripper_pos + _robot_config.gripper_vel_max * dt * - gripper_delta_pos / - std::abs(gripper_delta_pos); - if (std::abs(_input_joint_cmd.gripper_pos - _output_joint_cmd.gripper_pos) >= 0.001) - _logger->debug("Gripper pos cmd clipped: {:.3f} to {:.3f}", _input_joint_cmd.gripper_pos, - _output_joint_cmd.gripper_pos); - } - } - else - { - _output_joint_cmd.gripper_pos = _joint_state.gripper_pos; - } - } + std::tuple ik_results; + ik_results = multi_trial_ik(new_cmd.pose_6d, _joint_state.pos); + int ik_status = std::get<0>(ik_results); - // Joint pos clipping - for (int i = 0; i < 6; ++i) - { - if (_output_joint_cmd.pos[i] < _robot_config.joint_pos_min[i]) - { - _logger->debug("Joint {} pos {:.3f} pos cmd clipped from {:.3f} to min {:.3f}", i, _joint_state.pos[i], - _output_joint_cmd.pos[i], _robot_config.joint_pos_min[i]); - _output_joint_cmd.pos[i] = _robot_config.joint_pos_min[i]; - } - else if (_output_joint_cmd.pos[i] > _robot_config.joint_pos_max[i]) - { - _logger->debug("Joint {} pos {:.3f} pos cmd clipped from {:.3f} to max {:.3f}", i, _joint_state.pos[i], - _output_joint_cmd.pos[i], _robot_config.joint_pos_max[i]); - _output_joint_cmd.pos[i] = _robot_config.joint_pos_max[i]; - } - } - // Gripper pos clipping - if (_output_joint_cmd.gripper_pos < 0) - { - if (_output_joint_cmd.gripper_pos < -0.005) - _logger->debug("Gripper pos cmd clipped from {:.3f} to min: {:.3f}", _output_joint_cmd.gripper_pos, 0.0); - _output_joint_cmd.gripper_pos = 0; - } - else if (_output_joint_cmd.gripper_pos > _robot_config.gripper_width) - { - if (_output_joint_cmd.gripper_pos > _robot_config.gripper_width + 0.005) - _logger->debug("Gripper pos cmd clipped from {:.3f} to max: {:.3f}", _output_joint_cmd.gripper_pos, - _robot_config.gripper_width); - _output_joint_cmd.gripper_pos = _robot_config.gripper_width; - } - if (std::abs(_joint_state.gripper_torque) > _robot_config.gripper_torque_max / 2) - { - double sign = _joint_state.gripper_torque > 0 ? 1 : -1; - // -1 for closing blocked, 1 for opening blocked - double delta_pos = _output_joint_cmd.gripper_pos - prev_output_cmd.gripper_pos; - // negative for closing, positive for opening - if (delta_pos * sign > 0) - { - _logger->debug("Gripper torque is too large, gripper pos cmd is not updated"); - _output_joint_cmd.gripper_pos = prev_output_cmd.gripper_pos; - } - } + if (new_cmd.timestamp == 0) + new_cmd.timestamp = get_timestamp() + _controller_config.default_preview_time; - // Torque clipping - for (int i = 0; i < 6; ++i) - { - if (_output_joint_cmd.torque[i] > _robot_config.joint_torque_max[i]) - { - _logger->debug("Joint {} torque cmd clipped from {:.3f} to max {:.3f}", i, _output_joint_cmd.torque[i], - _robot_config.joint_torque_max[i]); - _output_joint_cmd.torque[i] = _robot_config.joint_torque_max[i]; - } - else if (_output_joint_cmd.torque[i] < -_robot_config.joint_torque_max[i]) - { - _logger->debug("Joint {} torque cmd clipped from {:.3f} to min {:.3f}", i, _output_joint_cmd.torque[i], - -_robot_config.joint_torque_max[i]); - _output_joint_cmd.torque[i] = -_robot_config.joint_torque_max[i]; - } - } -} + JointState target_joint_state{_robot_config.joint_dof}; + target_joint_state.pos = std::get<1>(ik_results); + target_joint_state.gripper_pos = new_cmd.gripper_pos; + target_joint_state.timestamp = new_cmd.timestamp; -void Arx5CartesianController::_over_current_protection() -{ - bool over_current = false; - for (int i = 0; i < 6; ++i) - { - if (std::abs(_joint_state.torque[i]) > _robot_config.joint_torque_max[i]) - { - over_current = true; - _logger->error("Over current detected once on joint {}, current: {:.3f}", i, _joint_state.torque[i]); - break; - } - } - if (std::abs(_joint_state.gripper_torque) > _robot_config.gripper_torque_max) - { - over_current = true; - _logger->error("Over current detected once on gripper, current: {:.3f}", _joint_state.gripper_torque); - } - if (over_current) - { - _over_current_cnt++; - if (_over_current_cnt > _controller_config.over_current_cnt_max) - { - _logger->error("Over current detected, robot is set to damping. Please restart the program."); - _enter_emergency_state(); - } - } - else - { - _over_current_cnt = 0; - } -} + double current_time = get_timestamp(); + // TODO: include velocity + std::lock_guard guard(_cmd_mutex); + _interpolator.override_waypoint(get_timestamp(), target_joint_state); -void Arx5CartesianController::_check_joint_state_sanity() -{ - for (int i = 0; i < 6; ++i) + if (ik_status != 0) { - if (std::abs(_joint_state.pos[i]) > _robot_config.joint_pos_max[i] + 3.14 || - std::abs(_joint_state.pos[i]) < _robot_config.joint_pos_min[i] - 3.14) - { - _logger->error("Joint {} pos data error: {:.3f}. Please restart the program.", i, _joint_state.pos[i]); - _enter_emergency_state(); - } - if (std::abs(_input_joint_cmd.pos[i]) > _robot_config.joint_pos_max[i] + 3.14 || - std::abs(_input_joint_cmd.pos[i]) < _robot_config.joint_pos_min[i] - 3.14) - { - _logger->error("Joint {} command data error: {:.3f}. Please restart the program.", i, - _input_joint_cmd.pos[i]); - _enter_emergency_state(); - } - if (std::abs(_joint_state.torque[i]) > 100 * _robot_config.joint_torque_max[i]) - { - _logger->error("Joint {} torque data error: {:.3f}. Please restart the program.", i, - _joint_state.torque[i]); - _enter_emergency_state(); - } - } - // Gripper should be around 0~_robot_config.gripper_width - double gripper_width_tolerance = 0.005; // m - if (_joint_state.gripper_pos < -gripper_width_tolerance || - _joint_state.gripper_pos > _robot_config.gripper_width + gripper_width_tolerance) - { - _logger->error("Gripper position error: got {:.3f} but should be in 0~{:.3f} (m). Please close the gripper " - "before turning the arm on or recalibrate gripper home and width.", - _joint_state.gripper_pos, _robot_config.gripper_width); - _enter_emergency_state(); + _logger->warn("Inverse kinematics failed: {} ({})", _solver->get_ik_status_name(ik_status), ik_status); } } -void Arx5CartesianController::_enter_emergency_state() -{ - Gain damping_gain{_robot_config.joint_dof}; - damping_gain.kd = _controller_config.default_kd; - damping_gain.kd[1] *= 3; - damping_gain.kd[2] *= 3; - damping_gain.kd[3] *= 1.5; - set_gain(damping_gain); - _input_joint_cmd.vel = VecDoF::Zero(_robot_config.joint_dof); - _input_joint_cmd.torque = VecDoF::Zero(_robot_config.joint_dof); - _logger->error("Emergency state entered. Please restart the program."); - while (true) - { - std::lock_guard guard_cmd(_cmd_mutex); - set_gain(damping_gain); - _input_joint_cmd.vel = VecDoF::Zero(_robot_config.joint_dof); - _input_joint_cmd.torque = VecDoF::Zero(_robot_config.joint_dof); - _send_recv(); - sleep_ms(5); - } -} -bool Arx5CartesianController::_send_recv() +void Arx5CartesianController::set_eef_traj(std::vector new_traj) { - // TODO: in the motor documentation, there shouldn't be these torque constants. Torque will go directly into the - // motors - const double torque_constant_EC_A4310 = 1.4; // Nm/A - const double torque_constant_DM_J4310 = 0.424; - const double torque_constant_DM_J4340 = 1.0; - int start_time_us = get_time_us(); + double start_time = get_timestamp(); + std::vector joint_traj; + double avg_window_s = 0.05; + joint_traj.push_back(_interpolator.interpolate(start_time - 2 * avg_window_s)); + joint_traj.push_back(_interpolator.interpolate(start_time - avg_window_s)); + joint_traj.push_back(_interpolator.interpolate(start_time)); - _update_output_cmd(); - int update_cmd_time_us = get_time_us(); - int communicate_sleep_us = 150; - - for (int i = 0; i < _robot_config.joint_dof; i++) - { - int start_send_motor_time_us = get_time_us(); - if (_robot_config.motor_type[i] == MotorType::EC_A4310) - { - _can_handle.send_EC_motor_cmd(_robot_config.motor_id[i], _gain.kp[i], _gain.kd[i], _output_joint_cmd.pos[i], - _output_joint_cmd.vel[i], - _output_joint_cmd.torque[i] / torque_constant_EC_A4310); - } - else if (_robot_config.motor_type[i] == MotorType::DM_J4310) - { - - _can_handle.send_DM_motor_cmd(_robot_config.motor_id[i], _gain.kp[i], _gain.kd[i], _output_joint_cmd.pos[i], - _output_joint_cmd.vel[i], - _output_joint_cmd.torque[i] / torque_constant_DM_J4310); - } - else if (_robot_config.motor_type[i] == MotorType::DM_J4340) - { - _can_handle.send_DM_motor_cmd(_robot_config.motor_id[i], _gain.kp[i], _gain.kd[i], _output_joint_cmd.pos[i], - _output_joint_cmd.vel[i], - _output_joint_cmd.torque[i] / torque_constant_DM_J4340); - } - else - { - _logger->error("Motor type not supported."); - return false; - } - int finish_send_motor_time_us = get_time_us(); - sleep_us(communicate_sleep_us - (finish_send_motor_time_us - start_send_motor_time_us)); - } - - // Send gripper command (gripper is using DM motor) - if (_robot_config.gripper_motor_type == MotorType::DM_J4310) + double prev_timestamp = 0; + for (auto eef_state : new_traj) { - int start_send_motor_time_us = get_time_us(); - - double gripper_motor_pos = - _output_joint_cmd.gripper_pos / _robot_config.gripper_width * _robot_config.gripper_open_readout; - _can_handle.send_DM_motor_cmd(_robot_config.gripper_motor_id, _gain.gripper_kp, _gain.gripper_kd, - gripper_motor_pos, 0, 0); - int finish_send_motor_time_us = get_time_us(); - sleep_us(communicate_sleep_us - (finish_send_motor_time_us - start_send_motor_time_us)); - } - - int start_get_motor_msg_time_us = get_time_us(); - std::array motor_msg = _can_handle.get_motor_msg(); - int get_motor_msg_time_us = get_time_us(); + if (eef_state.timestamp <= start_time) + continue; + if (eef_state.timestamp == 0) + throw std::invalid_argument("EEFState timestamp must be set for all waypoints"); + if (eef_state.timestamp <= prev_timestamp) + throw std::invalid_argument("EEFState timestamps must be in ascending order"); + JointState current_joint_state = get_joint_state(); + std::tuple ik_results; + ik_results = multi_trial_ik(eef_state.pose_6d, current_joint_state.pos); + int ik_status = std::get<0>(ik_results); - // _logger->trace("update_cmd: {} us, send_motor_0: {} us, send_motor_1: {} us, send_motor_2: {} us, send_motor_3: - // {} us, send_motor_4: {} us, send_motor_5: {} us, send_motor_6: {} us, get_motor_msg: {} us", - // update_cmd_time_us - start_time_us, send_motor_0_time_us - start_send_motor_0_time_us, - // send_motor_1_time_us - start_send_motor_1_time_us, send_motor_2_time_us - - // start_send_motor_2_time_us, send_motor_3_time_us - start_send_motor_3_time_us, - // send_motor_4_time_us - start_send_motor_4_time_us, send_motor_5_time_us - - // start_send_motor_5_time_us, send_motor_6_time_us - start_send_motor_6_time_us, - // get_motor_msg_time_us - start_get_motor_msg_time_us); + JointState target_joint_state{_robot_config.joint_dof}; + target_joint_state.pos = std::get<1>(ik_results); + target_joint_state.gripper_pos = eef_state.gripper_pos; + target_joint_state.timestamp = eef_state.timestamp; - std::lock_guard guard_state(_state_mutex); - for (int i = 0; i < _robot_config.joint_dof; i++) - { - _joint_state.pos[i] = motor_msg[_robot_config.motor_id[i]].angle_actual_rad; - _joint_state.vel[i] = motor_msg[_robot_config.motor_id[i]].speed_actual_rad; + joint_traj.push_back(target_joint_state); + prev_timestamp = eef_state.timestamp; - // Torque: matching the values (there must be something wrong) - if (_robot_config.motor_type[i] == MotorType::EC_A4310) - { - _joint_state.torque[i] = motor_msg[_robot_config.motor_id[i]].current_actual_float * - torque_constant_EC_A4310 * torque_constant_EC_A4310; - // Why there are two torque_constant_EC_A4310? - } - else if (_robot_config.motor_type[i] == MotorType::DM_J4310) + if (ik_status != 0) { - _joint_state.torque[i] = - motor_msg[_robot_config.motor_id[i]].current_actual_float * torque_constant_DM_J4310; - } - else if (_robot_config.motor_type[i] == MotorType::DM_J4340) - { - _joint_state.torque[i] = - motor_msg[_robot_config.motor_id[i]].current_actual_float * torque_constant_DM_J4340; + _logger->warn("Inverse kinematics failed: {} ({})", _solver->get_ik_status_name(ik_status), ik_status); } } - _joint_state.gripper_pos = motor_msg[_robot_config.gripper_motor_id].angle_actual_rad / - _robot_config.gripper_open_readout * _robot_config.gripper_width; - - _joint_state.gripper_vel = motor_msg[_robot_config.gripper_motor_id].speed_actual_rad / - _robot_config.gripper_open_readout * _robot_config.gripper_width; + double ik_end_time = get_timestamp(); - _joint_state.gripper_torque = - motor_msg[_robot_config.gripper_motor_id].current_actual_float * torque_constant_DM_J4310; - _joint_state.timestamp = get_timestamp(); - return true; -} - -void Arx5CartesianController::_calc_joint_cmd() -{ - JointState joint_cmd{_robot_config.joint_dof}; - JointState joint_state = get_joint_state(); - std::tuple ik_results; - - { - std::lock_guard guard_cmd(_cmd_mutex); - // Calculate output eef command (according to the interpolation) - if (_input_eef_cmd.timestamp == 0) // No interpolation. Directly update the target - { - _output_eef_cmd = _input_eef_cmd; - _output_eef_cmd.timestamp = get_timestamp(); - } - else // Interpolate the current timestamp between _interp_start_eef_cmd and _input_eef_cmd - { - // _logger->debug("Received non-zero eef_cmd!"); - double current_timestamp = get_timestamp(); - assert(current_timestamp >= _interp_start_eef_cmd.timestamp); - assert(_input_eef_cmd.timestamp > _interp_start_eef_cmd.timestamp); - if (current_timestamp > _input_eef_cmd.timestamp) - // Current timestamp has already exceed the interpolation target: hold at this target pose - { - _output_eef_cmd = _input_eef_cmd; - _output_eef_cmd.timestamp = current_timestamp; - } - else // Apply interpolation - { - double alpha = (current_timestamp - _interp_start_eef_cmd.timestamp) / - (_input_eef_cmd.timestamp - _interp_start_eef_cmd.timestamp); - assert(alpha >= 0 && alpha <= 1); - _output_eef_cmd.pose_6d = _interp_start_eef_cmd.pose_6d * (1 - alpha) + _input_eef_cmd.pose_6d * alpha; - _output_eef_cmd.gripper_pos = - _interp_start_eef_cmd.gripper_pos * (1 - alpha) + _input_eef_cmd.gripper_pos * alpha; - _output_eef_cmd.timestamp = current_timestamp; - } - } + // Include velocity: first and last point based on current state, others based on neighboring points + calc_joint_vel(joint_traj, avg_window_s); - if (_output_eef_cmd.pose_6d.isZero() || _output_eef_cmd.pose_6d.norm() < 0.01) - { - _logger->error("EEF command should not be set close to zero. To start from the home pose, please call " - "get_home_pose()."); - _enter_emergency_state(); - } - ik_results = _solver->inverse_kinematics(_output_eef_cmd.pose_6d, joint_state.pos); - joint_cmd.gripper_pos = _output_eef_cmd.gripper_pos; - } - bool success = std::get<0>(ik_results); - VecDoF joint_pos = std::get<1>(ik_results); + double current_time = get_timestamp(); + std::lock_guard guard(_cmd_mutex); - VecDoF clipped_joint_pos = joint_pos.cwiseMax(_robot_config.joint_pos_min).cwiseMin(_robot_config.joint_pos_max); + _interpolator.override_traj(current_time, joint_traj); - if (success) - { - joint_cmd.pos = _joint_pos_filter.filter(clipped_joint_pos); - if (_enable_gravity_compensation) - { - // Use the torque of the current joint positions - VecDoF joint_torque = _solver->inverse_dynamics(_joint_state.pos, VecDoF::Zero(_robot_config.joint_dof), - VecDoF::Zero(_robot_config.joint_dof)); - joint_cmd.torque = _joint_torque_filter.filter(joint_torque); - } - _input_joint_cmd = joint_cmd; - } + double end_override_traj_time = get_timestamp(); + // _logger->debug("IK time: {:.3f}ms, calc vel time: {:.3f}ms, override_traj time: {:.3f}ms", + // (ik_end_time - start_time) * 1000, (current_time - ik_end_time) * 1000, + // (end_override_traj_time - ik_end_time) * 1000); } -void Arx5CartesianController::_background_send_recv() +EEFState Arx5CartesianController::get_eef_cmd() { - while (!_destroy_background_threads) + JointState joint_cmd = get_joint_cmd(); + EEFState eef_cmd; + eef_cmd.pose_6d = _solver->forward_kinematics(joint_cmd.pos); + eef_cmd.gripper_pos = joint_cmd.gripper_pos; + eef_cmd.timestamp = joint_cmd.timestamp; + return eef_cmd; +} + +std::tuple Arx5CartesianController::multi_trial_ik(Eigen::Matrix target_pose_6d, + Eigen::VectorXd current_joint_pos, + int additional_trial_num) +{ + if (additional_trial_num < 0) + throw std::invalid_argument("Number of additional trials must be non-negative"); + // Solve IK with at least 2 init joint positions: current joint position and home joint position + if (current_joint_pos.size() != _robot_config.joint_dof || target_pose_6d.size() != 6) + throw std::invalid_argument( + "Inverse kinematics input expected size 6, " + std::to_string(_robot_config.joint_dof) + " but got " + + std::to_string(target_pose_6d.size()) + ", " + std::to_string(current_joint_pos.size())); + Eigen::MatrixXd init_joint_positions = Eigen::MatrixXd::Zero(additional_trial_num + 2, _robot_config.joint_dof); + init_joint_positions.row(0) = current_joint_pos; + init_joint_positions.row(1) = Eigen::VectorXd::Zero(_robot_config.joint_dof); + for (int i = 0; i < additional_trial_num; i++) + { + init_joint_positions.row(i + 2) = Eigen::VectorXd::Random(_robot_config.joint_dof); + // Map the random values into the joint limits + for (int j = 0; j < _robot_config.joint_dof; j++) + { + init_joint_positions(i + 2, j) = + _robot_config.joint_pos_min[j] + (init_joint_positions(i + 2, j) + 1) / 2 * + (_robot_config.joint_pos_max[j] - _robot_config.joint_pos_min[j]); + } + } + Eigen::MatrixXd target_joint_positions = Eigen::MatrixXd::Zero(additional_trial_num + 2, _robot_config.joint_dof); + std::vector all_ik_status(additional_trial_num + 2, 0); + std::vector distances(additional_trial_num + 2, 100000); // L2 distances, initialize to infinity + + for (int i = 0; i < additional_trial_num + 2; i++) + { + std::tuple result; + result = _solver->inverse_kinematics(target_pose_6d, init_joint_positions.row(i)); + int ik_status = std::get<0>(result); + Eigen::VectorXd target_joint_pos = std::get<1>(result); + bool in_joint_limit = ((_robot_config.joint_pos_max - target_joint_pos).array() > 0).all() && + ((_robot_config.joint_pos_min - target_joint_pos).array() < 0).all(); + if (ik_status == 0 && !in_joint_limit) + { + ik_status = -9; // E_EXCEED_JOINT_LIMIT + } + all_ik_status[i] = ik_status; + // Check whether the target joint position is within the joint limits + distances[i] = (target_joint_pos - current_joint_pos).norm(); + target_joint_positions.row(i) = target_joint_pos; + } + bool final_success = std::any_of(all_ik_status.begin(), all_ik_status.end(), [](bool success) { return success; }); + int min_idx = std::distance(distances.begin(), std::min_element(distances.begin(), distances.end())); + int min_ik_status = all_ik_status[min_idx]; + Eigen::VectorXd min_target_joint_pos = target_joint_positions.row(min_idx); + // clip the target joint position to the joint limits + for (int i = 0; i < _robot_config.joint_dof; i++) { - int start_time_us = get_time_us(); - if (_background_send_recv_running) - { - _over_current_protection(); - _check_joint_state_sanity(); - _calc_joint_cmd(); - _send_recv(); - } - int elapsed_time_us = get_time_us() - start_time_us; - int sleep_time_us = int(_controller_config.controller_dt * 1e6) - elapsed_time_us; - if (sleep_time_us > 0) - { - std::this_thread::sleep_for(std::chrono::microseconds(sleep_time_us)); - } - else if (sleep_time_us < -500) - { - _logger->debug("Background send_recv task is running too slow, time: {} us", elapsed_time_us); - } + min_target_joint_pos[i] = + std::max(_robot_config.joint_pos_min[i], std::min(_robot_config.joint_pos_max[i], min_target_joint_pos[i])); } + return std::make_tuple(min_ik_status, min_target_joint_pos); } -} // namespace arx \ No newline at end of file diff --git a/src/app/controller_base.cpp b/src/app/controller_base.cpp new file mode 100644 index 0000000..971e94e --- /dev/null +++ b/src/app/controller_base.cpp @@ -0,0 +1,673 @@ +#include "app/controller_base.h" +#include "app/common.h" +#include "utils.h" +#include +#include +#include +#include +using namespace arx; + +Arx5ControllerBase::Arx5ControllerBase(RobotConfig robot_config, ControllerConfig controller_config, + std::string interface_name) + : _can_handle(interface_name), + _logger(spdlog::stdout_color_mt(robot_config.robot_model + std::string("_") + interface_name)), + _robot_config(robot_config), _controller_config(controller_config) +{ + _start_time_us = get_time_us(); + _logger->set_pattern("[%H:%M:%S %n %^%l%$] %v"); + _solver = std::make_shared( + _robot_config.urdf_path, _robot_config.joint_dof, _robot_config.joint_pos_min, _robot_config.joint_pos_max, + _robot_config.base_link_name, _robot_config.eef_link_name, _robot_config.gravity_vector); + if (_robot_config.robot_model == "X5" && !_controller_config.shutdown_to_passive) + { + _logger->warn("When shutting down X5 robot arms, the motors have to be set to passive. " + "_controller_config.shutdown_to_passive is set to `true`"); + _controller_config.shutdown_to_passive = true; + } + _init_robot(); + _background_send_recv_thread = std::thread(&Arx5ControllerBase::_background_send_recv, this); + _background_send_recv_running = _controller_config.background_send_recv; + _logger->info("Background send_recv task is running at ID: {}", syscall(SYS_gettid)); +} + +Arx5ControllerBase::~Arx5ControllerBase() +{ + if (_controller_config.shutdown_to_passive) + { + _logger->info("Set to damping before exit"); + Gain damping_gain{_robot_config.joint_dof}; + damping_gain.kd = _controller_config.default_kd; + + // Increase damping if needed + // damping_gain.kd[0] *= 3; + // damping_gain.kd[1] *= 3; + // damping_gain.kd[2] *= 3; + // damping_gain.kd[3] *= 1.5; + + set_gain(damping_gain); + { + std::lock_guard guard(_cmd_mutex); + _output_joint_cmd.vel = VecDoF::Zero(_robot_config.joint_dof); + _output_joint_cmd.torque = VecDoF::Zero(_robot_config.joint_dof); + _interpolator.init_fixed(_output_joint_cmd); + } + _background_send_recv_running = true; + _controller_config.gravity_compensation = false; + sleep_ms(2000); + } + else + { + _logger->info("Disconnect motors without setting to damping"); + } + + _destroy_background_threads = true; + _background_send_recv_thread.join(); + _logger->info("background send_recv task joined"); + spdlog::drop(_logger->name()); + _logger.reset(); + _solver.reset(); +} + +JointState Arx5ControllerBase::get_joint_cmd() +{ + std::lock_guard guard(_cmd_mutex); + return _output_joint_cmd; +} + +JointState Arx5ControllerBase::get_joint_state() +{ + std::lock_guard guard(_state_mutex); + return _joint_state; +} + +EEFState Arx5ControllerBase::get_eef_state() +{ + EEFState eef_state; + JointState joint_state = get_joint_state(); + Pose6d tool_pose = _solver->forward_kinematics(joint_state.pos); + eef_state.pose_6d = tool_pose; + eef_state.timestamp = joint_state.timestamp; + eef_state.gripper_pos = joint_state.gripper_pos; + eef_state.gripper_vel = joint_state.gripper_vel; + eef_state.gripper_torque = joint_state.gripper_torque; + return eef_state; +} + +void Arx5ControllerBase::set_gain(Gain new_gain) +{ + // Make sure the robot doesn't jump when setting kp to non-zero + if (_gain.kp.isZero() && !new_gain.kp.isZero()) + { + JointState joint_state = get_joint_state(); + JointState joint_cmd = get_joint_cmd(); + double max_pos_error = (joint_state.pos - joint_cmd.pos).cwiseAbs().maxCoeff(); + double pos_error_threshold = 0.2; + double kp_threshold = 1; + if (max_pos_error > pos_error_threshold && new_gain.kp.maxCoeff() > kp_threshold) + { + _logger->error("Cannot set kp too large when the joint pos cmd is far from current pos."); + _logger->error( + "Target max kp: {}, kp threshold: {}. Current pos: {}, cmd pos: {}, position error threshold: {}", + new_gain.kp.maxCoeff(), kp_threshold, vec2str(joint_state.pos), vec2str(joint_cmd.pos), + pos_error_threshold); + _background_send_recv_running = false; + throw std::runtime_error("Cannot set kp to non-zero when the joint pos cmd is far from current pos."); + } + } + { + std::lock_guard guard(_cmd_mutex); + _gain = new_gain; + } +} + +Gain Arx5ControllerBase::get_gain() +{ + std::lock_guard guard(_cmd_mutex); + return _gain; +} + +double Arx5ControllerBase::get_timestamp() +{ + return double(get_time_us() - _start_time_us) / 1e6; +} +RobotConfig Arx5ControllerBase::get_robot_config() +{ + return _robot_config; +} +ControllerConfig Arx5ControllerBase::get_controller_config() +{ + return _controller_config; +} +void Arx5ControllerBase::set_log_level(spdlog::level::level_enum level) +{ + _logger->set_level(level); +} + +void Arx5ControllerBase::reset_to_home() +{ + JointState init_state = get_joint_state(); + Gain init_gain = get_gain(); + double init_gripper_kp = _gain.gripper_kp; + double init_gripper_kd = _gain.gripper_kd; + Gain target_gain{_robot_config.joint_dof}; + if (init_gain.kp.isZero()) + { + _logger->info("Current kp is zero. Setting to default kp kd"); + target_gain = Gain(_controller_config.default_kp, _controller_config.default_kd, + _controller_config.default_gripper_kp, _controller_config.default_gripper_kd); + } + else + { + target_gain = init_gain; + } + + JointState target_state{_robot_config.joint_dof}; + + // calculate the maximum joint position error + double max_pos_error = (init_state.pos - VecDoF::Zero(_robot_config.joint_dof)).cwiseAbs().maxCoeff(); + max_pos_error = std::max(max_pos_error, init_state.gripper_pos * 2 / _robot_config.gripper_width); + // interpolate from current kp kd to default kp kd in max(max_pos_error, 0.5)s + // and keep the target for max(max_pos_error, 0.5)s + double wait_time = std::max(max_pos_error, 0.5); + int step_num = int(wait_time / _controller_config.controller_dt); + _logger->info("Start reset to home in {:.3f}s, max_pos_error: {:.3f}", std::max(max_pos_error, double(0.5)) + 0.5, + max_pos_error); + + bool prev_running = _background_send_recv_running; + _background_send_recv_running = true; + target_state.timestamp = get_timestamp() + wait_time; + target_state.pos[2] = 0.03; // avoiding clash + + { + std::lock_guard guard(_cmd_mutex); + JointState start_state{_robot_config.joint_dof}; + start_state.pos = init_state.pos; + start_state.gripper_pos = init_state.gripper_pos; + start_state.timestamp = get_timestamp(); + _interpolator.init(start_state, target_state); + } + Gain new_gain{_robot_config.joint_dof}; + for (int i = 0; i <= step_num; i++) + { + double alpha = double(i) / step_num; + new_gain = init_gain * (1 - alpha) + target_gain * alpha; + set_gain(new_gain); + sleep_us(int(_controller_config.controller_dt * 1e6)); + } + + target_state.pos[2] = 0.0; + target_state.timestamp = get_timestamp() + 0.5; + { + std::lock_guard guard(_cmd_mutex); + _interpolator.override_waypoint(get_timestamp(), target_state); + } + _logger->info("Finish reset to home"); + _background_send_recv_running = prev_running; +} + +void Arx5ControllerBase::set_to_damping() +{ + Gain damping_gain{_robot_config.joint_dof}; + damping_gain.kd = _controller_config.default_kd; + set_gain(damping_gain); + sleep_ms(10); + JointState joint_state = get_joint_state(); + { + std::lock_guard guard(_cmd_mutex); + joint_state.vel = VecDoF::Zero(_robot_config.joint_dof); + joint_state.torque = VecDoF::Zero(_robot_config.joint_dof); + _interpolator.init_fixed(joint_state); + } +} + +// ---------------------- Private functions ---------------------- + +void Arx5ControllerBase::_init_robot() +{ + // Background send receive is disabled during initialization + int init_rounds = 10; // Make sure the states of each motor is fully initialized + for (int j = 0; j < init_rounds; j++) + { + _recv(); + _check_joint_state_sanity(); + _over_current_protection(); + } + + Gain gain{_robot_config.joint_dof}; + gain.kd = _controller_config.default_kd; + + JointState init_joint_state = get_joint_state(); + init_joint_state.vel = VecDoF::Zero(_robot_config.joint_dof); + init_joint_state.torque = VecDoF::Zero(_robot_config.joint_dof); + set_gain(gain); // set to damping by default + + // std::lock_guard guard(_state_mutex); + // Check whether any motor has non-zero position + if (_joint_state.pos == VecDoF::Zero(_robot_config.joint_dof)) + { + _logger->error("None of the motors are initialized. Please check the connection or power of the arm."); + throw std::runtime_error( + "None of the motors are initialized. Please check the connection or power of the arm."); + } + { + std::lock_guard guard(_cmd_mutex); + _output_joint_cmd = init_joint_state; + _interpolator.init_fixed(init_joint_state); + } + + for (int j = 0; j < init_rounds; j++) + { + _send_recv(); + _check_joint_state_sanity(); + _over_current_protection(); + } +} + +void Arx5ControllerBase::_check_joint_state_sanity() +{ + std::lock_guard guard(_state_mutex); + + for (int i = 0; i < _robot_config.joint_dof; ++i) + { + if (std::abs(_joint_state.pos[i]) > _robot_config.joint_pos_max[i] + 3.14 || + std::abs(_joint_state.pos[i]) < _robot_config.joint_pos_min[i] - 3.14) + { + _logger->error("Joint {} pos data error: {:.3f}. Please restart the program.", i, _joint_state.pos[i]); + _enter_emergency_state(); + } + + if (_interpolator.is_initialized()) + { + std::lock_guard guard(_cmd_mutex); + JointState interpolator_cmd = _interpolator.interpolate(get_timestamp()); + if (std::abs(interpolator_cmd.pos[i]) > _robot_config.joint_pos_max[i] + 3.14 || + std::abs(interpolator_cmd.pos[i]) < _robot_config.joint_pos_min[i] - 3.14) + { + _logger->error("Joint {} interpolated command data error: {:.3f}. Please restart the program.", i, + interpolator_cmd.pos[i]); + _enter_emergency_state(); + } + } + if (std::abs(_joint_state.torque[i]) > 100 * _robot_config.joint_torque_max[i]) + { + _logger->error("Joint {} torque data error: {:.3f}. Please restart the program.", i, + _joint_state.torque[i]); + _enter_emergency_state(); + } + } + // Gripper should be around 0~_robot_config.gripper_width + double gripper_width_tolerance = 0.005; // m + if (_joint_state.gripper_pos < -gripper_width_tolerance || + _joint_state.gripper_pos > _robot_config.gripper_width + gripper_width_tolerance) + { + _logger->error("Gripper position error: got {:.3f} but should be in 0~{:.3f} (m). Please close the gripper " + "before turning the arm on or recalibrate gripper home and width.", + _joint_state.gripper_pos, _robot_config.gripper_width); + _enter_emergency_state(); + } +} + +void Arx5ControllerBase::_over_current_protection() +{ + bool over_current = false; + for (int i = 0; i < _robot_config.joint_dof; ++i) + { + if (std::abs(_joint_state.torque[i]) > _robot_config.joint_torque_max[i]) + { + over_current = true; + _logger->error("Over current detected once on joint {}, current: {:.3f}", i, _joint_state.torque[i]); + break; + } + } + if (std::abs(_joint_state.gripper_torque) > _robot_config.gripper_torque_max) + { + over_current = true; + _logger->error("Over current detected once on gripper, current: {:.3f}", _joint_state.gripper_torque); + } + if (over_current) + { + _over_current_cnt++; + if (_over_current_cnt > _controller_config.over_current_cnt_max) + { + _logger->error("Over current detected, robot is set to damping. Please restart the " + "program."); + _enter_emergency_state(); + } + } + else + { + _over_current_cnt = 0; + } +} + +void Arx5ControllerBase::_enter_emergency_state() +{ + Gain damping_gain{_robot_config.joint_dof}; + damping_gain.kd = _controller_config.default_kd; + damping_gain.kd[1] *= 3; + damping_gain.kd[2] *= 3; + damping_gain.kd[3] *= 1.5; + _logger->error("Emergency state entered. Please restart the program."); + while (true) + { + std::lock_guard guard(_cmd_mutex); + set_gain(damping_gain); + _output_joint_cmd.vel = VecDoF::Zero(_robot_config.joint_dof); + _output_joint_cmd.torque = VecDoF::Zero(_robot_config.joint_dof); + + _interpolator.init_fixed(_output_joint_cmd); + _send_recv(); + sleep_ms(5); + } +} + +void Arx5ControllerBase::_update_joint_state() +{ + // TODO: in the motor documentation, there shouldn't be these torque constants. Torque will go directly into the + // motors + const double torque_constant_EC_A4310 = 1.4; // Nm/A + const double torque_constant_DM_J4310 = 0.424; + const double torque_constant_DM_J4340 = 1.0; + std::array motor_msg = _can_handle.get_motor_msg(); + std::lock_guard guard(_state_mutex); + + for (int i = 0; i < _robot_config.joint_dof; i++) + { + _joint_state.pos[i] = motor_msg[_robot_config.motor_id[i]].angle_actual_rad; + _joint_state.vel[i] = motor_msg[_robot_config.motor_id[i]].speed_actual_rad; + + // Torque: matching the values (there must be something wrong) + if (_robot_config.motor_type[i] == MotorType::EC_A4310) + { + _joint_state.torque[i] = motor_msg[_robot_config.motor_id[i]].current_actual_float * + torque_constant_EC_A4310 * torque_constant_EC_A4310; + // Why are there two torque_constant_EC_A4310? + } + else if (_robot_config.motor_type[i] == MotorType::DM_J4310) + { + _joint_state.torque[i] = + motor_msg[_robot_config.motor_id[i]].current_actual_float * torque_constant_DM_J4310; + } + else if (_robot_config.motor_type[i] == MotorType::DM_J4340) + { + _joint_state.torque[i] = + motor_msg[_robot_config.motor_id[i]].current_actual_float * torque_constant_DM_J4340; + } + } + + _joint_state.gripper_pos = motor_msg[_robot_config.gripper_motor_id].angle_actual_rad / + _robot_config.gripper_open_readout * _robot_config.gripper_width; + + _joint_state.gripper_vel = motor_msg[_robot_config.gripper_motor_id].speed_actual_rad / + _robot_config.gripper_open_readout * _robot_config.gripper_width; + + _joint_state.gripper_torque = + motor_msg[_robot_config.gripper_motor_id].current_actual_float * torque_constant_DM_J4310; + _joint_state.timestamp = get_timestamp(); +} + +void Arx5ControllerBase::_update_output_cmd() +{ + JointState prev_output_cmd = _output_joint_cmd; + + // TODO: deal with non-zero velocity and torque for joint control + double timestamp = get_timestamp(); + { + std::lock_guard guard(_cmd_mutex); + _output_joint_cmd = _interpolator.interpolate(timestamp); + } + + std::lock_guard guard(_state_mutex); + if (_controller_config.gravity_compensation) + { + _output_joint_cmd.torque += _solver->inverse_dynamics(_joint_state.pos, VecDoF::Zero(_robot_config.joint_dof), + VecDoF::Zero(_robot_config.joint_dof)); + } + + // Joint pos clipping + for (int i = 0; i < _robot_config.joint_dof; ++i) + { + if (_output_joint_cmd.pos[i] < _robot_config.joint_pos_min[i]) + { + _logger->debug("Joint {} pos {:.3f} pos cmd clipped from {:.3f} to min {:.3f}", i, _joint_state.pos[i], + _output_joint_cmd.pos[i], _robot_config.joint_pos_min[i]); + _output_joint_cmd.pos[i] = _robot_config.joint_pos_min[i]; + } + else if (_output_joint_cmd.pos[i] > _robot_config.joint_pos_max[i]) + { + _logger->debug("Joint {} pos {:.3f} pos cmd clipped from {:.3f} to max {:.3f}", i, _joint_state.pos[i], + _output_joint_cmd.pos[i], _robot_config.joint_pos_max[i]); + _output_joint_cmd.pos[i] = _robot_config.joint_pos_max[i]; + } + } + // Joint velocity clipping + double dt = _controller_config.controller_dt; + for (int i = 0; i < _robot_config.joint_dof; ++i) + { + if (_gain.kp[i] > 0) + { + + double delta_pos = _output_joint_cmd.pos[i] - prev_output_cmd.pos[i]; + double max_vel = _robot_config.joint_vel_max[i]; + if (std::abs(delta_pos) > max_vel * dt) + { + double new_pos = prev_output_cmd.pos[i] + max_vel * dt * delta_pos / std::abs(delta_pos); + if (new_pos > _robot_config.joint_pos_max[i]) + new_pos = _robot_config.joint_pos_max[i]; + if (new_pos < _robot_config.joint_pos_min[i]) + new_pos = _robot_config.joint_pos_min[i]; + _logger->debug("Joint velocity reaches limit: Joint {} pos {:.3f} pos cmd clipped: {:.3f} to {:.3f}", i, + _joint_state.pos[i], _output_joint_cmd.pos[i], new_pos); + _output_joint_cmd.pos[i] = new_pos; + } + } + else + { + _output_joint_cmd.pos[i] = _joint_state.pos[i]; + } + + // Gripper pos clipping + if (_gain.gripper_kp > 0) + { + double gripper_delta_pos = _output_joint_cmd.gripper_pos - prev_output_cmd.gripper_pos; + if (std::abs(gripper_delta_pos) / dt > _robot_config.gripper_vel_max) + { + double new_gripper_pos = prev_output_cmd.gripper_pos + _robot_config.gripper_vel_max * dt * + gripper_delta_pos / + std::abs(gripper_delta_pos); + if (std::abs(_output_joint_cmd.gripper_pos - _output_joint_cmd.gripper_pos) >= 0.001) + _logger->debug("Gripper pos cmd clipped: {:.3f} to {:.3f}", _output_joint_cmd.gripper_pos, + _output_joint_cmd.gripper_pos); + _output_joint_cmd.gripper_pos = new_gripper_pos; + } + } + else + { + _output_joint_cmd.gripper_pos = _joint_state.gripper_pos; + } + } + + // Gripper pos clipping + if (_output_joint_cmd.gripper_pos < 0) + { + if (_output_joint_cmd.gripper_pos < -0.005) + _logger->debug("Gripper pos cmd clipped from {:.3f} to min: {:.3f}", _output_joint_cmd.gripper_pos, 0.0); + _output_joint_cmd.gripper_pos = 0; + } + else if (_output_joint_cmd.gripper_pos > _robot_config.gripper_width) + { + if (_output_joint_cmd.gripper_pos > _robot_config.gripper_width + 0.005) + _logger->debug("Gripper pos cmd clipped from {:.3f} to max: {:.3f}", _output_joint_cmd.gripper_pos, + _robot_config.gripper_width); + _output_joint_cmd.gripper_pos = _robot_config.gripper_width; + } + if (std::abs(_joint_state.gripper_torque) > _robot_config.gripper_torque_max / 2) + { + double sign = _joint_state.gripper_torque > 0 ? 1 : -1; // -1 for closing blocked, 1 for opening blocked + double delta_pos = + _output_joint_cmd.gripper_pos - prev_output_cmd.gripper_pos; // negative for closing, positive for opening + if (delta_pos * sign > 0) + { + if (_prev_gripper_updated) + _logger->warn("Gripper torque is too large, gripper pos cmd is not updated"); + _output_joint_cmd.gripper_pos = prev_output_cmd.gripper_pos; + _prev_gripper_updated = false; + } + else + _prev_gripper_updated = true; + } + + // Torque clipping + for (int i = 0; i < _robot_config.joint_dof; ++i) + { + if (_output_joint_cmd.torque[i] > _robot_config.joint_torque_max[i]) + { + _logger->debug("Joint {} torque cmd clipped from {:.3f} to max {:.3f}", i, _output_joint_cmd.torque[i], + _robot_config.joint_torque_max[i]); + _output_joint_cmd.torque[i] = _robot_config.joint_torque_max[i]; + } + else if (_output_joint_cmd.torque[i] < -_robot_config.joint_torque_max[i]) + { + _logger->debug("Joint {} torque cmd clipped from {:.3f} to min {:.3f}", i, _output_joint_cmd.torque[i], + -_robot_config.joint_torque_max[i]); + _output_joint_cmd.torque[i] = -_robot_config.joint_torque_max[i]; + } + } +} + +void Arx5ControllerBase::_send_recv() +{ + // TODO: in the motor documentation, there shouldn't be these torque constants. Torque will go directly into the + // motors + const double torque_constant_EC_A4310 = 1.4; // Nm/A + const double torque_constant_DM_J4310 = 0.424; + const double torque_constant_DM_J4340 = 1.0; + int start_time_us = get_time_us(); + + _update_output_cmd(); + int update_cmd_time_us = get_time_us(); + int communicate_sleep_us = 150; + + for (int i = 0; i < _robot_config.joint_dof; i++) + { + int start_send_motor_time_us = get_time_us(); + { + std::lock_guard guard(_cmd_mutex); + if (_robot_config.motor_type[i] == MotorType::EC_A4310) + { + _can_handle.send_EC_motor_cmd(_robot_config.motor_id[i], _gain.kp[i], _gain.kd[i], + _output_joint_cmd.pos[i], _output_joint_cmd.vel[i], + _output_joint_cmd.torque[i] / torque_constant_EC_A4310); + } + else if (_robot_config.motor_type[i] == MotorType::DM_J4310) + { + + _can_handle.send_DM_motor_cmd(_robot_config.motor_id[i], _gain.kp[i], _gain.kd[i], + _output_joint_cmd.pos[i], _output_joint_cmd.vel[i], + _output_joint_cmd.torque[i] / torque_constant_DM_J4310); + } + else if (_robot_config.motor_type[i] == MotorType::DM_J4340) + { + _can_handle.send_DM_motor_cmd(_robot_config.motor_id[i], _gain.kp[i], _gain.kd[i], + _output_joint_cmd.pos[i], _output_joint_cmd.vel[i], + _output_joint_cmd.torque[i] / torque_constant_DM_J4340); + } + else + { + _logger->error("Motor type not supported."); + return; + } + } + int finish_send_motor_time_us = get_time_us(); + sleep_us(communicate_sleep_us - (finish_send_motor_time_us - start_send_motor_time_us)); + } + + // Send gripper command (gripper is using DM motor) + if (_robot_config.gripper_motor_type == MotorType::DM_J4310) + { + int start_send_motor_time_us = get_time_us(); + + double gripper_motor_pos = + _output_joint_cmd.gripper_pos / _robot_config.gripper_width * _robot_config.gripper_open_readout; + _can_handle.send_DM_motor_cmd(_robot_config.gripper_motor_id, _gain.gripper_kp, _gain.gripper_kd, + gripper_motor_pos, 0, 0); + int finish_send_motor_time_us = get_time_us(); + sleep_us(communicate_sleep_us - (finish_send_motor_time_us - start_send_motor_time_us)); + } + + // _logger->trace("update_cmd: {} us, send_motor_0: {} us, send_motor_1: {} us, send_motor_2: {} us, send_motor_3: + // {} us, send_motor_4: {} us, send_motor_5: {} us, send_motor_6: {} us, get_motor_msg: {} us", + // update_cmd_time_us - start_time_us, send_motor_0_time_us - start_send_motor_0_time_us, + // send_motor_1_time_us - start_send_motor_1_time_us, send_motor_2_time_us - + // start_send_motor_2_time_us, send_motor_3_time_us - start_send_motor_3_time_us, + // send_motor_4_time_us - start_send_motor_4_time_us, send_motor_5_time_us - + // start_send_motor_5_time_us, send_motor_6_time_us - start_send_motor_6_time_us, + // get_motor_msg_time_us - start_get_motor_msg_time_us); + + _update_joint_state(); +} + +void Arx5ControllerBase::_recv() +{ + int communicate_sleep_us = 300; + for (int i = 0; i < _robot_config.joint_dof; i++) + { + int start_send_motor_time_us = get_time_us(); + if (_robot_config.motor_type[i] == MotorType::EC_A4310) + { + _logger->error("EC_A4310 motor type is not supported yet in _recv()."); + // assert(false); + } + else if (_robot_config.motor_type[i] == MotorType::DM_J4310 || + _robot_config.motor_type[i] == MotorType::DM_J4340 || + _robot_config.motor_type[i] == MotorType::DM_J8009) + { + _can_handle.enable_DM_motor(_robot_config.motor_id[i]); + } + else + { + _logger->error("Motor type not supported."); + assert(false); + } + int finish_send_motor_time_us = get_time_us(); + sleep_us(communicate_sleep_us - (finish_send_motor_time_us - start_send_motor_time_us)); + } + if (_robot_config.gripper_motor_type == MotorType::DM_J4310) + { + int start_send_motor_time_us = get_time_us(); + _can_handle.enable_DM_motor(_robot_config.gripper_motor_id); + int finish_send_motor_time_us = get_time_us(); + sleep_us(communicate_sleep_us - (finish_send_motor_time_us - start_send_motor_time_us)); + } + sleep_ms(1); // Wait until all the messages are updated + _update_joint_state(); +} + +void Arx5ControllerBase::_background_send_recv() +{ + while (!_destroy_background_threads) + { + int start_time_us = get_time_us(); + if (_background_send_recv_running) + { + _over_current_protection(); + _check_joint_state_sanity(); + _send_recv(); + } + int elapsed_time_us = get_time_us() - start_time_us; + int sleep_time_us = int(_controller_config.controller_dt * 1e6) - elapsed_time_us; + if (sleep_time_us > 0) + { + std::this_thread::sleep_for(std::chrono::microseconds(sleep_time_us)); + } + else if (sleep_time_us < -500) + { + _logger->debug("Background send_recv task is running too slow, time: {} us", elapsed_time_us); + } + } +} + +Pose6d Arx5ControllerBase::get_home_pose() +{ + return _solver->forward_kinematics(VecDoF::Zero(_robot_config.joint_dof)); +} diff --git a/src/app/joint_controller.cpp b/src/app/joint_controller.cpp index ac1a8a9..46e1fd9 100644 --- a/src/app/joint_controller.cpp +++ b/src/app/joint_controller.cpp @@ -8,14 +8,8 @@ using namespace arx; Arx5JointController::Arx5JointController(RobotConfig robot_config, ControllerConfig controller_config, std::string interface_name) - : _can_handle(interface_name), - _logger(spdlog::stdout_color_mt(robot_config.robot_model + std::string("_") + interface_name)), - _robot_config(robot_config), _controller_config(controller_config) + : Arx5ControllerBase(robot_config, controller_config, interface_name) { - _logger->set_pattern("[%H:%M:%S %n %^%l%$] %v"); - _init_robot(); - _background_send_recv_thread = std::thread(&Arx5JointController::_background_send_recv, this); - _logger->info("Background send_recv task is running at ID: {}", syscall(SYS_gettid)); } Arx5JointController::Arx5JointController(std::string model, std::string interface_name) @@ -24,632 +18,72 @@ Arx5JointController::Arx5JointController(std::string model, std::string interfac ControllerConfigFactory::get_instance().get_config( "joint_controller", RobotConfigFactory::get_instance().get_config(model).joint_dof), interface_name) - -{ -} - -Arx5JointController::~Arx5JointController() { - Gain damping_gain{_robot_config.joint_dof}; - damping_gain.kd = _controller_config.default_kd; - // damping_gain.kd[0] *= 3; - // damping_gain.kd[1] *= 3; - // damping_gain.kd[2] *= 3; - // damping_gain.kd[3] *= 1.5; - _logger->info("Set to damping before exit"); - set_gain(damping_gain); - set_joint_cmd(JointState(_robot_config.joint_dof)); - _background_send_recv_running = true; - _enable_gravity_compensation = false; - sleep_ms(2000); - _destroy_background_threads = true; - _background_send_recv_thread.join(); - _logger->info("background send_recv task joined"); - spdlog::drop(_logger->name()); - _logger.reset(); - _solver.reset(); } -void Arx5JointController::_init_robot() +void Arx5JointController::set_joint_cmd(JointState new_cmd) { + JointState current_joint_state = get_joint_state(); + double current_time = get_timestamp(); + if (new_cmd.timestamp == 0) + new_cmd.timestamp = current_time + _controller_config.default_preview_time; - for (int i = 0; i < _robot_config.joint_dof; ++i) - { - if (_robot_config.motor_type[i] == MotorType::DM_J4310 || _robot_config.motor_type[i] == MotorType::DM_J4340) - { - int id = _robot_config.motor_id[i]; - _can_handle.enable_DM_motor(id); - sleep_us(1000); - } - } - if (_robot_config.gripper_motor_type == MotorType::DM_J4310) - { - _can_handle.enable_DM_motor(_robot_config.gripper_motor_id); - sleep_us(1000); - } - - Gain gain{_robot_config.joint_dof}; - gain.kd = _controller_config.default_kd; - set_joint_cmd(JointState(_robot_config.joint_dof)); // initialize joint command to zero - set_gain(gain); // set to damping by default - for (int i = 0; i <= 10; ++i) - { - // make sure all the motor positions are updated - _send_recv(); - sleep_ms(5); - } - // Check whether any motor has non-zero position - if (_joint_state.pos == VecDoF::Zero(_robot_config.joint_dof)) - { - _logger->error("None of the motors are initialized. Please check the connection or power of the arm."); - throw std::runtime_error( - "None of the motors are initialized. Please check the connection or power of the arm."); - } - _input_joint_cmd = get_state(); - _input_joint_cmd.torque = VecDoF::Zero(_robot_config.joint_dof); - _input_joint_cmd.vel = VecDoF::Zero(_robot_config.joint_dof); - _input_joint_cmd.timestamp = 0; - _output_joint_cmd = _input_joint_cmd; - _intermediate_joint_cmd = _input_joint_cmd; - _interp_start_joint_cmd = _input_joint_cmd; + std::lock_guard guard(_cmd_mutex); + if (abs(new_cmd.timestamp - current_time) < 1e-3) + // If the new timestamp is close enough (<1ms) to the current time + // Will override the entire interpolator object + _interpolator.init_fixed(new_cmd); + else + _interpolator.override_waypoint(get_timestamp(), new_cmd); } -JointState Arx5JointController::get_state() +void Arx5JointController::set_joint_traj(std::vector new_traj) { - std::lock_guard guard(_state_mutex); - return _joint_state; -} + double start_time = get_timestamp(); + std::vector joint_traj; + double avg_window_s = 0.05; + joint_traj.push_back(_interpolator.interpolate(start_time - 2 * avg_window_s)); + joint_traj.push_back(_interpolator.interpolate(start_time - avg_window_s)); + joint_traj.push_back(_interpolator.interpolate(start_time)); -Pose6d Arx5JointController::get_tool_pose() -{ - if (_solver == nullptr) + double prev_timestamp = 0; + for (auto joint_state : new_traj) { - throw std::runtime_error("Solver is not initialized, cannot run forward kinematics."); + if (joint_state.timestamp <= start_time) + continue; + if (joint_state.timestamp == 0) + throw std::invalid_argument("JointState timestamp must be set for all waypoints"); + if (joint_state.timestamp <= prev_timestamp) + throw std::invalid_argument("JointState timestamps must be in ascending order"); + joint_traj.push_back(joint_state); + prev_timestamp = joint_state.timestamp; } - return _solver->forward_kinematics(_joint_state.pos); -} - -RobotConfig Arx5JointController::get_robot_config() -{ - return _robot_config; -} + calc_joint_vel(joint_traj, avg_window_s); -ControllerConfig Arx5JointController::get_controller_config() -{ - return _controller_config; + std::lock_guard guard(_cmd_mutex); + _interpolator.override_traj(get_timestamp(), joint_traj); } -void Arx5JointController::send_recv_once() +void Arx5JointController::recv_once() { if (_background_send_recv_running) { - std::cout << "send_recv task is already running in background. send_recv_once is ignored." << std::endl; + _logger->warn("send_recv task is already running in background. recv_once() is ignored."); return; } - _send_recv(); - _over_current_protection(); -} - -void Arx5JointController::_update_output_cmd() -{ - std::lock_guard guard_cmd(_cmd_mutex); - - JointState prev_output_cmd = _output_joint_cmd; - - // Calculate output joint command (according to the interpolation) - if (_input_joint_cmd.timestamp == 0) // No interpolation. Directly update the target - { - _output_joint_cmd = _input_joint_cmd; - _output_joint_cmd.timestamp = get_timestamp(); - } - else // Interpolate the current timestamp between _interp_start_joint_cmd and _input_joint_cmd - { - double current_timestamp = get_timestamp(); - assert(current_timestamp >= _interp_start_joint_cmd.timestamp); - assert(_input_joint_cmd.timestamp > _interp_start_joint_cmd.timestamp); - if (current_timestamp > _input_joint_cmd.timestamp) - // Current timestamp has already exceed the interpolation target: hold at this target pose - { - _output_joint_cmd = _input_joint_cmd; - _output_joint_cmd.timestamp = current_timestamp; - } - else // Apply interpolation - { - double alpha = (current_timestamp - _interp_start_joint_cmd.timestamp) / - (_input_joint_cmd.timestamp - _interp_start_joint_cmd.timestamp); - assert(alpha >= 0 && alpha <= 1); - _output_joint_cmd.pos = _interp_start_joint_cmd.pos * (1 - alpha) + _input_joint_cmd.pos * alpha; - _output_joint_cmd.vel = - _interp_start_joint_cmd.vel + alpha * (_input_joint_cmd.vel - _interp_start_joint_cmd.vel); - _output_joint_cmd.torque = - _interp_start_joint_cmd.torque + alpha * (_input_joint_cmd.torque - _interp_start_joint_cmd.torque); - _output_joint_cmd.gripper_pos = - _interp_start_joint_cmd.gripper_pos + - alpha * (_input_joint_cmd.gripper_pos - _interp_start_joint_cmd.gripper_pos); - _output_joint_cmd.timestamp = current_timestamp; - // _logger->debug("alpha: {:.3f}, start_pos[0]: {:.3f}, cmd_pos[0]: {:.3f}, output_pos[0]: {:.3f}", alpha, - // _interp_start_joint_cmd.pos[0], _input_joint_cmd.pos[0], _output_joint_cmd.pos[0]); - } - } - _intermediate_joint_cmd = _output_joint_cmd; - if (_enable_gravity_compensation && _solver != nullptr) - { - VecDoF gravity_torque = _solver->inverse_dynamics(_joint_state.pos, VecDoF::Zero(_robot_config.joint_dof), - VecDoF::Zero(_robot_config.joint_dof)); - _output_joint_cmd.torque += gravity_torque; - } - - // Joint velocity clipping - double dt = _controller_config.controller_dt; - for (int i = 0; i < _robot_config.joint_dof; ++i) - { - if (_gain.kp[i] > 0) - { - - double delta_pos = _output_joint_cmd.pos[i] - prev_output_cmd.pos[i]; - double max_vel = _robot_config.joint_vel_max[i]; - if (std::abs(delta_pos) > max_vel * dt) - { - _output_joint_cmd.pos[i] = prev_output_cmd.pos[i] + max_vel * dt * delta_pos / std::abs(delta_pos); - _logger->debug("Joint {} pos {:.3f} pos cmd clipped: {:.3f} to {:.3f}", i, _joint_state.pos[i], - _output_joint_cmd.pos[i], _output_joint_cmd.pos[i]); - } - } - else - { - _output_joint_cmd.pos[i] = _joint_state.pos[i]; - } - - // Gripper pos clipping - if (_gain.gripper_kp > 0) - { - double gripper_delta_pos = _output_joint_cmd.gripper_pos - prev_output_cmd.gripper_pos; - if (std::abs(gripper_delta_pos) / dt > _robot_config.gripper_vel_max) - { - _output_joint_cmd.gripper_pos = prev_output_cmd.gripper_pos + _robot_config.gripper_vel_max * dt * - gripper_delta_pos / - std::abs(gripper_delta_pos); - if (std::abs(_output_joint_cmd.gripper_pos - _output_joint_cmd.gripper_pos) >= 0.001) - _logger->debug("Gripper pos cmd clipped: {:.3f} to {:.3f}", _output_joint_cmd.gripper_pos, - _output_joint_cmd.gripper_pos); - } - } - else - { - _output_joint_cmd.gripper_pos = _joint_state.gripper_pos; - } - } - - // Joint pos clipping - for (int i = 0; i < _robot_config.joint_dof; ++i) - { - if (_output_joint_cmd.pos[i] < _robot_config.joint_pos_min[i]) - { - _logger->debug("Joint {} pos {:.3f} pos cmd clipped from {:.3f} to min {:.3f}", i, _joint_state.pos[i], - _output_joint_cmd.pos[i], _robot_config.joint_pos_min[i]); - _output_joint_cmd.pos[i] = _robot_config.joint_pos_min[i]; - } - else if (_output_joint_cmd.pos[i] > _robot_config.joint_pos_max[i]) - { - _logger->debug("Joint {} pos {:.3f} pos cmd clipped from {:.3f} to max {:.3f}", i, _joint_state.pos[i], - _output_joint_cmd.pos[i], _robot_config.joint_pos_max[i]); - _output_joint_cmd.pos[i] = _robot_config.joint_pos_max[i]; - } - } - // Gripper pos clipping - if (_output_joint_cmd.gripper_pos < 0) - { - if (_output_joint_cmd.gripper_pos < -0.005) - _logger->debug("Gripper pos cmd clipped from {:.3f} to min: {:.3f}", _output_joint_cmd.gripper_pos, 0.0); - _output_joint_cmd.gripper_pos = 0; - } - else if (_output_joint_cmd.gripper_pos > _robot_config.gripper_width) - { - if (_output_joint_cmd.gripper_pos > _robot_config.gripper_width + 0.005) - _logger->debug("Gripper pos cmd clipped from {:.3f} to max: {:.3f}", _output_joint_cmd.gripper_pos, - _robot_config.gripper_width); - _output_joint_cmd.gripper_pos = _robot_config.gripper_width; - } - if (std::abs(_joint_state.gripper_torque) > _robot_config.gripper_torque_max / 2) - { - double sign = _joint_state.gripper_torque > 0 ? 1 : -1; // -1 for closing blocked, 1 for opening blocked - double delta_pos = - _output_joint_cmd.gripper_pos - prev_output_cmd.gripper_pos; // negative for closing, positive for opening - if (delta_pos * sign > 0) - { - _logger->debug("Gripper torque is too large, gripper pos cmd is not updated"); - _output_joint_cmd.gripper_pos = prev_output_cmd.gripper_pos; - } - } - - // Torque clipping - for (int i = 0; i < _robot_config.joint_dof; ++i) - { - if (_output_joint_cmd.torque[i] > _robot_config.joint_torque_max[i]) - { - _logger->debug("Joint {} torque cmd clipped from {:.3f} to max {:.3f}", i, _output_joint_cmd.torque[i], - _robot_config.joint_torque_max[i]); - _output_joint_cmd.torque[i] = _robot_config.joint_torque_max[i]; - } - else if (_output_joint_cmd.torque[i] < -_robot_config.joint_torque_max[i]) - { - _logger->debug("Joint {} torque cmd clipped from {:.3f} to min {:.3f}", i, _output_joint_cmd.torque[i], - -_robot_config.joint_torque_max[i]); - _output_joint_cmd.torque[i] = -_robot_config.joint_torque_max[i]; - } - } -} - -double Arx5JointController::get_timestamp() -{ - return double(get_time_us() - _start_time_us) / 1e6; + _recv(); } -bool Arx5JointController::_send_recv() -{ - // TODO: in the motor documentation, there shouldn't be these torque constants. Torque will go directly into the - // motors - const double torque_constant_EC_A4310 = 1.4; // Nm/A - const double torque_constant_DM_J4310 = 0.424; - const double torque_constant_DM_J4340 = 1.0; - int start_time_us = get_time_us(); - - _update_output_cmd(); - int update_cmd_time_us = get_time_us(); - int communicate_sleep_us = 150; - - for (int i = 0; i < _robot_config.joint_dof; i++) - { - int start_send_motor_time_us = get_time_us(); - if (_robot_config.motor_type[i] == MotorType::EC_A4310) - { - _can_handle.send_EC_motor_cmd(_robot_config.motor_id[i], _gain.kp[i], _gain.kd[i], _output_joint_cmd.pos[i], - _output_joint_cmd.vel[i], - _output_joint_cmd.torque[i] / torque_constant_EC_A4310); - } - else if (_robot_config.motor_type[i] == MotorType::DM_J4310) - { - - _can_handle.send_DM_motor_cmd(_robot_config.motor_id[i], _gain.kp[i], _gain.kd[i], _output_joint_cmd.pos[i], - _output_joint_cmd.vel[i], - _output_joint_cmd.torque[i] / torque_constant_DM_J4310); - } - else if (_robot_config.motor_type[i] == MotorType::DM_J4340) - { - _can_handle.send_DM_motor_cmd(_robot_config.motor_id[i], _gain.kp[i], _gain.kd[i], _output_joint_cmd.pos[i], - _output_joint_cmd.vel[i], - _output_joint_cmd.torque[i] / torque_constant_DM_J4340); - } - else - { - _logger->error("Motor type not supported."); - return false; - } - int finish_send_motor_time_us = get_time_us(); - sleep_us(communicate_sleep_us - (finish_send_motor_time_us - start_send_motor_time_us)); - } - - // Send gripper command (gripper is using DM motor) - if (_robot_config.gripper_motor_type == MotorType::DM_J4310) - { - int start_send_motor_time_us = get_time_us(); - - double gripper_motor_pos = - _output_joint_cmd.gripper_pos / _robot_config.gripper_width * _robot_config.gripper_open_readout; - _can_handle.send_DM_motor_cmd(_robot_config.gripper_motor_id, _gain.gripper_kp, _gain.gripper_kd, - gripper_motor_pos, 0, 0); - int finish_send_motor_time_us = get_time_us(); - sleep_us(communicate_sleep_us - (finish_send_motor_time_us - start_send_motor_time_us)); - } - - int start_get_motor_msg_time_us = get_time_us(); - std::array motor_msg = _can_handle.get_motor_msg(); - int get_motor_msg_time_us = get_time_us(); - - // _logger->trace("update_cmd: {} us, send_motor_0: {} us, send_motor_1: {} us, send_motor_2: {} us, send_motor_3: - // {} us, send_motor_4: {} us, send_motor_5: {} us, send_motor_6: {} us, get_motor_msg: {} us", - // update_cmd_time_us - start_time_us, send_motor_0_time_us - start_send_motor_0_time_us, - // send_motor_1_time_us - start_send_motor_1_time_us, send_motor_2_time_us - - // start_send_motor_2_time_us, send_motor_3_time_us - start_send_motor_3_time_us, - // send_motor_4_time_us - start_send_motor_4_time_us, send_motor_5_time_us - - // start_send_motor_5_time_us, send_motor_6_time_us - start_send_motor_6_time_us, - // get_motor_msg_time_us - start_get_motor_msg_time_us); - - std::lock_guard guard_state(_state_mutex); - for (int i = 0; i < _robot_config.joint_dof; i++) - { - _joint_state.pos[i] = motor_msg[_robot_config.motor_id[i]].angle_actual_rad; - _joint_state.vel[i] = motor_msg[_robot_config.motor_id[i]].speed_actual_rad; - - // Torque: matching the values (there must be something wrong) - if (_robot_config.motor_type[i] == MotorType::EC_A4310) - { - _joint_state.torque[i] = motor_msg[_robot_config.motor_id[i]].current_actual_float * - torque_constant_EC_A4310 * torque_constant_EC_A4310; - // Why there are two torque_constant_EC_A4310? - } - else if (_robot_config.motor_type[i] == MotorType::DM_J4310) - { - _joint_state.torque[i] = - motor_msg[_robot_config.motor_id[i]].current_actual_float * torque_constant_DM_J4310; - } - else if (_robot_config.motor_type[i] == MotorType::DM_J4340) - { - _joint_state.torque[i] = - motor_msg[_robot_config.motor_id[i]].current_actual_float * torque_constant_DM_J4340; - } - } - - _joint_state.gripper_pos = motor_msg[_robot_config.gripper_motor_id].angle_actual_rad / - _robot_config.gripper_open_readout * _robot_config.gripper_width; - - _joint_state.gripper_vel = motor_msg[_robot_config.gripper_motor_id].speed_actual_rad / - _robot_config.gripper_open_readout * _robot_config.gripper_width; - - _joint_state.gripper_torque = - motor_msg[_robot_config.gripper_motor_id].current_actual_float * torque_constant_DM_J4310; - _joint_state.timestamp = get_timestamp(); - return true; -} - -void Arx5JointController::_check_joint_state_sanity() -{ - for (int i = 0; i < _robot_config.joint_dof; ++i) - { - if (std::abs(_joint_state.pos[i]) > _robot_config.joint_pos_max[i] + 3.14 || - std::abs(_joint_state.pos[i]) < _robot_config.joint_pos_min[i] - 3.14) - { - _logger->error("Joint {} pos data error: {:.3f}. Please restart the program.", i, _joint_state.pos[i]); - _enter_emergency_state(); - } - if (std::abs(_input_joint_cmd.pos[i]) > _robot_config.joint_pos_max[i] + 3.14 || - std::abs(_input_joint_cmd.pos[i]) < _robot_config.joint_pos_min[i] - 3.14) - { - _logger->error("Joint {} command data error: {:.3f}. Please restart the program.", i, - _input_joint_cmd.pos[i]); - _enter_emergency_state(); - } - if (std::abs(_joint_state.torque[i]) > 100 * _robot_config.joint_torque_max[i]) - { - _logger->error("Joint {} torque data error: {:.3f}. Please restart the program.", i, - _joint_state.torque[i]); - _enter_emergency_state(); - } - } - // Gripper should be around 0~_robot_config.gripper_width - double gripper_width_tolerance = 0.005; // m - if (_joint_state.gripper_pos < -gripper_width_tolerance || - _joint_state.gripper_pos > _robot_config.gripper_width + gripper_width_tolerance) - { - _logger->error("Gripper position error: got {:.3f} but should be in 0~{:.3f} (m). Please close the gripper " - "before turning the arm on or recalibrate gripper home and width.", - _joint_state.gripper_pos, _robot_config.gripper_width); - _enter_emergency_state(); - } -} - -void Arx5JointController::_over_current_protection() -{ - bool over_current = false; - for (int i = 0; i < _robot_config.joint_dof; ++i) - { - if (std::abs(_joint_state.torque[i]) > _robot_config.joint_torque_max[i]) - { - over_current = true; - _logger->error("Over current detected once on joint {}, current: {:.3f}", i, _joint_state.torque[i]); - break; - } - } - if (std::abs(_joint_state.gripper_torque) > _robot_config.gripper_torque_max) - { - over_current = true; - _logger->error("Over current detected once on gripper, current: {:.3f}", _joint_state.gripper_torque); - } - if (over_current) - { - _over_current_cnt++; - if (_over_current_cnt > _controller_config.over_current_cnt_max) - { - _logger->error("Over current detected, robot is set to damping. Please restart the " - "program."); - _enter_emergency_state(); - } - } - else - { - _over_current_cnt = 0; - } -} - -void Arx5JointController::_enter_emergency_state() -{ - Gain damping_gain{_robot_config.joint_dof}; - damping_gain.kd = _controller_config.default_kd; - damping_gain.kd[1] *= 3; - damping_gain.kd[2] *= 3; - damping_gain.kd[3] *= 1.5; - set_gain(damping_gain); - _input_joint_cmd.vel = VecDoF::Zero(_robot_config.joint_dof); - _input_joint_cmd.torque = VecDoF::Zero(_robot_config.joint_dof); - - while (true) - { - std::lock_guard guard_cmd(_cmd_mutex); - set_gain(damping_gain); - _input_joint_cmd.vel = VecDoF::Zero(_robot_config.joint_dof); - _input_joint_cmd.torque = VecDoF::Zero(_robot_config.joint_dof); - _send_recv(); - sleep_ms(5); - } -} - -void Arx5JointController::_background_send_recv() -{ - while (!_destroy_background_threads) - { - int start_time_us = get_time_us(); - if (_background_send_recv_running) - { - _over_current_protection(); - _check_joint_state_sanity(); - _send_recv(); - } - int elapsed_time_us = get_time_us() - start_time_us; - int sleep_time_us = int(_controller_config.controller_dt * 1e6) - elapsed_time_us; - if (sleep_time_us > 0) - { - std::this_thread::sleep_for(std::chrono::microseconds(sleep_time_us)); - } - else if (sleep_time_us < -500) - { - _logger->debug("Background send_recv task is running too slow, time: {} us", elapsed_time_us); - } - } -} - -void Arx5JointController::set_joint_cmd(JointState new_cmd) +void Arx5JointController::send_recv_once() { - std::lock_guard guard(_cmd_mutex); - if (new_cmd.gripper_vel != 0 || new_cmd.gripper_torque != 0) - { - _logger->warn("Gripper vel and torque control is not supported yet."); - new_cmd.gripper_vel = 0; - new_cmd.gripper_torque = 0; - } - if (new_cmd.timestamp != 0 && new_cmd.timestamp < get_timestamp()) + if (_background_send_recv_running) { - _logger->warn("Joint command timestamp ({:.4f}s) is not 0 but in the past (current timestamp: {:.4f}s). New " - "joint command " - "is ignored.", - new_cmd.timestamp, get_timestamp()); + _logger->warn("send_recv task is already running in background. send_recv_once is ignored."); return; } - _input_joint_cmd = new_cmd; - _interp_start_joint_cmd = _intermediate_joint_cmd; - _interp_start_joint_cmd.torque = VecDoF::Zero(_robot_config.joint_dof); -} - -std::tuple Arx5JointController::get_joint_cmd() -{ - std::lock_guard guard(_cmd_mutex); - return std::make_tuple(_input_joint_cmd, _output_joint_cmd); -} - -Gain Arx5JointController::get_gain() -{ - return _gain; -} - -void Arx5JointController::set_gain(Gain new_gain) -{ - - // Make sure the robot doesn't jump when setting kp to non-zero - if (_gain.kp.isZero() && !new_gain.kp.isZero()) - { - double max_pos_error = (_joint_state.pos - _output_joint_cmd.pos).cwiseAbs().maxCoeff(); - double pos_error_threshold = 0.2; - double kp_threshold = 1; - if (max_pos_error > pos_error_threshold && new_gain.kp.maxCoeff() > kp_threshold) - { - _logger->error("Cannot set kp too large when the joint pos cmd is far from current pos."); - _logger->error( - "Target max kp: {}, kp threshold: {}. Current pos: {}, cmd pos: {}, position error threshold: {}", - new_gain.kp.maxCoeff(), kp_threshold, vec2str(_joint_state.pos), vec2str(_output_joint_cmd.pos), - pos_error_threshold); - _background_send_recv_running = false; - throw std::runtime_error("Cannot set kp to non-zero when the joint pos cmd is far from current pos."); - } - } - _gain = new_gain; -} - -void Arx5JointController::reset_to_home() -{ - JointState cmd{_robot_config.joint_dof}; - Gain gain{_robot_config.joint_dof}; - JointState init_state = get_state(); - Gain init_gain = get_gain(); - double init_gripper_kp = _gain.gripper_kp; - double init_gripper_kd = _gain.gripper_kd; - Gain target_gain{_robot_config.joint_dof}; - if (init_gain.kp.isZero()) - { - _logger->info("Current kp is zero. Setting to default kp kd"); - target_gain = Gain(_controller_config.default_kp, _controller_config.default_kd, - _controller_config.default_gripper_kp, _controller_config.default_gripper_kd); - } - else - { - target_gain = init_gain; - } - - JointState target_state{_robot_config.joint_dof}; - _logger->debug("init_state.pos: {}, target_state.pos: {}", vec2str(init_state.pos), vec2str(target_state.pos)); - if (init_state.pos == VecDoF::Zero(_robot_config.joint_dof)) - { - _logger->error("Motor positions are not initialized. Please check the connection."); - _background_send_recv_running = false; - throw std::runtime_error("Motor positions are not initialized. Please check the connection."); - } - // calculate the maximum joint position error - double max_pos_error = (init_state.pos - VecDoF::Zero(_robot_config.joint_dof)).cwiseAbs().maxCoeff(); - max_pos_error = std::max(max_pos_error, init_state.gripper_pos * 2 / _robot_config.gripper_width); - // interpolate from current kp kd to default kp kd in max(max_pos_error, 0.5)s - // and keep the target for max(max_pos_error, 0.5)s - double step_num = std::max(max_pos_error, 0.5) / _controller_config.controller_dt; - _logger->info("Start reset to home in {:.3f}s, max_pos_error: {:.3f}", std::max(max_pos_error, double(0.5)) + 0.5, - max_pos_error); - - bool prev_running = _background_send_recv_running; - _background_send_recv_running = true; - for (int i = 0; i <= step_num; ++i) - { - double alpha = double(i) / step_num; - gain = init_gain * (1 - alpha) + target_gain * alpha; - cmd = init_state * (1 - alpha) + target_state * alpha; - cmd.vel = VecDoF::Zero(_robot_config.joint_dof); - cmd.torque = VecDoF::Zero(_robot_config.joint_dof); - cmd.timestamp = 0; - set_joint_cmd(cmd); - sleep_ms(5); - set_gain(gain); - } - // Hardcode 0.5 s - sleep_ms(500); - _logger->info("Finish reset to home"); - _background_send_recv_running = prev_running; -} - -void Arx5JointController::set_to_damping() -{ - JointState cmd{_robot_config.joint_dof}; - JointState state{_robot_config.joint_dof}; - Gain gain{_robot_config.joint_dof}; - JointState init_state = get_state(); - Gain init_gain = get_gain(); - Gain target_gain{_robot_config.joint_dof}; - target_gain.kd = _controller_config.default_kd; - _logger->info("Start set to damping"); - // interpolate from current kp kd to default kp kd in 0.5s - bool prev_running = _background_send_recv_running; - _background_send_recv_running = true; - int step_num = 20; // 0.1s in total - for (int i = 0; i <= step_num; ++i) - { - state = get_state(); - cmd.pos = state.pos; - cmd.gripper_pos = state.gripper_pos; - cmd.torque = VecDoF::Zero(_robot_config.joint_dof); - cmd.vel = VecDoF::Zero(_robot_config.joint_dof); - double alpha = double(i) / double(step_num); - gain = init_gain * (1.0 - alpha) + target_gain * alpha; - set_gain(gain); - set_joint_cmd(cmd); - sleep_ms(5); - } - sleep_ms(500); - _logger->info("Finish set to damping"); - _background_send_recv_running = prev_running; + _check_joint_state_sanity(); + _over_current_protection(); + _send_recv(); } void Arx5JointController::calibrate_gripper() @@ -731,36 +165,3 @@ void Arx5JointController::calibrate_joint(int joint_id) _background_send_recv_running = true; } } - -void Arx5JointController::set_log_level(spdlog::level::level_enum log_level) -{ - _logger->set_level(log_level); -} - -void Arx5JointController::enable_gravity_compensation(std::string urdf_path) -{ - _logger->info("Enable gravity compensation"); - _logger->info("Loading urdf from {}", urdf_path); - _solver = std::make_shared(urdf_path, _robot_config.joint_dof, _robot_config.base_link_name, - _robot_config.eef_link_name, _robot_config.gravity_vector); - _enable_gravity_compensation = true; -} - -void Arx5JointController::disable_gravity_compensation() -{ - _logger->info("Disable gravity compensation"); - _enable_gravity_compensation = false; - _solver.reset(); -} - -void Arx5JointController::enable_background_send_recv() -{ - _logger->info("Enable background send_recv"); - _background_send_recv_running = true; -} - -void Arx5JointController::disable_background_send_recv() -{ - _logger->info("Disable background send_recv"); - _background_send_recv_running = false; -} diff --git a/src/utils.cpp b/src/utils.cpp index 6ea538a..b0bcffc 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -46,6 +46,407 @@ Eigen::VectorXd MovingAverageXd::filter(Eigen::VectorXd new_data) // str += "]"; // return str; // } + +JointStateInterpolator::JointStateInterpolator(int dof, std::string method) +{ + if (method != "linear" && method != "cubic") + { + throw std::invalid_argument("Invalid interpolation method: " + method + + ". Currently available: 'linear' or 'cubic'"); + } + _dof = dof; + _method = method; + _initialized = false; + _traj = std::vector(); +} + +void JointStateInterpolator::init(JointState start_state, JointState end_state) +{ + if (end_state.timestamp < start_state.timestamp) + { + throw std::invalid_argument("End time must be no less than start time"); + } + else if (end_state.timestamp == start_state.timestamp) + { + throw std::invalid_argument("Start and end time are the same, plsease use init_fixed() instead"); + } + if (start_state.pos.size() != _dof || end_state.pos.size() != _dof) + { + throw std::invalid_argument("Joint state dimension mismatch"); + } + _traj.clear(); + _traj.push_back(start_state); + _traj.push_back(end_state); + _initialized = true; +} + +void JointStateInterpolator::init_fixed(JointState start_state) +{ + if (start_state.pos.size() != _dof) + { + throw std::invalid_argument("Joint state dimension mismatch"); + } + _traj.clear(); + _traj.push_back(start_state); + _initialized = true; +} + +void JointStateInterpolator::append_waypoint(double current_time, JointState end_state) +{ + if (!_initialized) + { + throw std::runtime_error("Interpolator not initialized"); + } + + if (end_state.pos.size() != _dof) + { + throw std::invalid_argument("Joint state dimension mismatch"); + } + + if (end_state.timestamp <= current_time) + { + throw std::invalid_argument("End time must be no less than current time"); + } + + JointState current_state{_dof}; + + if (current_time < _traj[0].timestamp) + { + throw std::runtime_error("Current time must be no less than start time"); + } + else + { + current_state = interpolate(current_time); + } + + std::vector prev_traj = _traj; + _traj.clear(); + _traj.push_back(current_state); + for (int i = 0; i < prev_traj.size(); i++) + { + if (prev_traj[i].timestamp > current_time) + { + if (prev_traj[i].timestamp > end_state.timestamp) + { + _traj.push_back(end_state); + break; + } + else + { + _traj.push_back(prev_traj[i]); + } + } + if (i == prev_traj.size() - 1) + { + _traj.push_back(end_state); + } + } +} + +void JointStateInterpolator::override_waypoint(double current_time, JointState end_state) +{ + if (!_initialized) + { + throw std::runtime_error("Interpolator not initialized"); + } + + if (end_state.pos.size() != _dof) + { + throw std::invalid_argument("Joint state dimension mismatch"); + } + + if (end_state.timestamp <= current_time) + { + throw std::invalid_argument("End time must be no less than current time"); + } + + JointState current_state{_dof}; + + if (current_time < _traj[0].timestamp) + { + throw std::runtime_error("Current time must be no less than start time"); + } + else + { + current_state = interpolate(current_time); + } + + std::vector prev_traj = _traj; + _traj.clear(); + _traj.push_back(current_state); + _traj.push_back(end_state); +} + +void JointStateInterpolator::append_traj(double current_time, std::vector traj) +{ + if (!_initialized) + { + throw std::runtime_error("Interpolator not initialized"); + } + + // remove all the new traj points that are before current time + while (traj.size() > 0 && traj[0].timestamp < current_time) + { + traj.erase(traj.begin()); + } + + if (traj.size() == 0) + { + printf("JointStateInterpolator::append_traj: Empty trajectory\n"); + return; + } + + for (int i = 0; i < traj.size() - 1; i++) + { + if (traj[i].timestamp > traj[i + 1].timestamp) + { + throw std::invalid_argument("Trajectory timestamps must be in strictly ascending order"); + } + if (traj[i].pos.size() != _dof || traj[i + 1].pos.size() != _dof) + { + throw std::invalid_argument("Joint state dimension mismatch"); + } + } + + JointState current_state{_dof}; + if (current_time < _traj[0].timestamp) + { + throw std::runtime_error("Current time must be no less than start time"); + } + else + { + current_state = interpolate(current_time); + } + + std::vector prev_traj = _traj; + _traj.clear(); + _traj.push_back(current_state); + double new_traj_start_time = traj[0].timestamp; + // Merge prev_traj before new_traj + + while (prev_traj.size() > 0 && prev_traj[0].timestamp < new_traj_start_time) + { + if (prev_traj[0].timestamp > current_time) + { + _traj.push_back(prev_traj[0]); + } + prev_traj.erase(prev_traj.begin()); + } + while (traj.size() > 0) + { + _traj.push_back(traj[0]); + traj.erase(traj.begin()); + } +} + +void JointStateInterpolator::override_traj(double current_time, std::vector traj) +{ + + if (!_initialized) + { + throw std::runtime_error("Interpolator not initialized"); + } + + // remove all the new traj points that are before current time + while (traj.size() > 0 && traj[0].timestamp < current_time) + { + traj.erase(traj.begin()); + } + + if (traj.size() == 0) + { + printf("JointStateInterpolator::append_traj: Empty trajectory\n"); + return; + } + + for (int i = 0; i < traj.size() - 1; i++) + { + if (traj[i].timestamp > traj[i + 1].timestamp) + { + throw std::invalid_argument("Trajectory timestamps must be in strictly ascending order"); + } + if (traj[i].pos.size() != _dof || traj[i + 1].pos.size() != _dof) + { + throw std::invalid_argument("Joint state dimension mismatch"); + } + } + + JointState current_state{_dof}; + if (current_time < _traj[0].timestamp) + { + throw std::runtime_error("Current time must be no less than start time"); + } + else + { + current_state = interpolate(current_time); + } + + std::vector prev_traj = _traj; + _traj.clear(); + _traj.push_back(current_state); + + while (traj.size() > 0) + { + _traj.push_back(traj[0]); + traj.erase(traj.begin()); + } +} + +JointState JointStateInterpolator::interpolate(double time) +{ + + if (!_initialized) + { + throw std::runtime_error("Interpolator not initialized"); + } + if (time <= 0) + { + throw std::invalid_argument("Interpolate time must be greater than 0"); + } + + if (_traj.size() == 0) + { + throw std::runtime_error("Empty trajectory"); + } + if (_traj.size() == 1) + { + JointState interp_state = _traj[0]; + interp_state.timestamp = time; + return interp_state; + } + + if (time <= _traj[0].timestamp) + { + JointState interp_state = _traj[0]; + interp_state.timestamp = time; + return interp_state; + } + else if (time >= _traj.back().timestamp) + { + JointState interp_state = _traj.back(); + interp_state.timestamp = time; + return interp_state; + } + + for (int i = 0; i <= _traj.size() - 2; i++) + { + JointState start_state = _traj[i]; + JointState end_state = _traj[i + 1]; + if (time >= start_state.timestamp && time <= end_state.timestamp) + { + if (_method == "linear") + { + JointState interp_result = start_state + (end_state - start_state) * (time - start_state.timestamp) / + (end_state.timestamp - start_state.timestamp); + interp_result.timestamp = time; + return interp_result; + } + else if (_method == "cubic") + { + // Torque and gripper pos will still be linearly interpolated + JointState interp_result = start_state + (end_state - start_state) * (time - start_state.timestamp) / + (end_state.timestamp - start_state.timestamp); + interp_result.timestamp = time; + + // Cubic interpolation for pos and vel + double t = (time - start_state.timestamp) / (end_state.timestamp - start_state.timestamp); + double t2 = t * t; + double t3 = t2 * t; + double pos_a = 2 * t3 - 3 * t2 + 1; + double pos_b = t3 - 2 * t2 + t; + double pos_c = -2 * t3 + 3 * t2; + double pos_d = t3 - t2; + interp_result.pos = + pos_a * start_state.pos + pos_b * start_state.vel + pos_c * end_state.pos + pos_d * end_state.vel; + + double vel_a = 6 * t2 - 6 * t; + double vel_b = 3 * t2 - 4 * t + 1; + double vel_c = -6 * t2 + 6 * t; + double vel_d = 3 * t2 - 2 * t; + interp_result.vel = + vel_a * start_state.pos + vel_b * start_state.vel + vel_c * end_state.pos + vel_d * end_state.vel; + return interp_result; + } + } + if (i == _traj.size() - 2) + { + throw std::runtime_error("Interpolation failed"); + } + } +} + +std::string JointStateInterpolator::to_string() +{ + std::string str = "JointStateInterpolator DOF: " + std::to_string(_dof) + " Method: " + _method + + " Length: " + std::to_string(_traj.size()) + "\n"; + for (int i = 0; i < _traj.size(); i++) + { + str += state2str(_traj[i]); + } + + return str; +} + +void calc_joint_vel(std::vector &traj, double avg_window_s) +{ + if (traj.size() < 2) + { + return; + } + int idx_0 = 0; + int idx_1 = 0; + int idx_2 = 0; + int idx_3 = 0; + for (int i = 0; i < traj.size(); i++) + { + while (idx_0 < traj.size() - 2 && traj[idx_0 + 1].timestamp < traj[i].timestamp - avg_window_s) + { + idx_0++; + } + while (idx_1 < traj.size() - 2 && traj[idx_1 + 1].timestamp < traj[i].timestamp - avg_window_s / 2) + { + idx_1++; + } + while (idx_2 < traj.size() - 1 && traj[idx_2].timestamp < traj[i].timestamp + avg_window_s / 2) + { + idx_2++; + } + while (idx_3 < traj.size() - 1 && traj[idx_3].timestamp < traj[i].timestamp + avg_window_s) + { + idx_3++; + } + assert(idx_0 <= idx_1 && idx_1 < idx_2 && idx_2 <= idx_3); + traj[i].vel = (traj[idx_3].pos - traj[idx_0].pos) / (traj[idx_3].timestamp - traj[idx_0].timestamp) / 2 + + (traj[idx_2].pos - traj[idx_1].pos) / (traj[idx_2].timestamp - traj[idx_1].timestamp) / 2; + } +} + +std::string joint_traj2str(const std::vector &traj, int precision) +{ + std::string str = ""; + for (int i = 0; i < traj.size(); i++) + { + str += state2str(traj[i], precision); + } + return str; +} + +std::string state2str(const JointState &state, int precision) +{ + std::string str = ""; + str += "pos:" + vec2str(state.pos, precision); + str += " vel:" + vec2str(state.vel, precision); + str += " torque:" + vec2str(state.torque, precision); + str += " gripper_pos:" + std::to_string(state.gripper_pos); + str += " timestamp:" + std::to_string(state.timestamp); + str += "\n"; + return str; +} + +bool JointStateInterpolator::is_initialized() +{ + return _initialized; +} } // namespace arx std::string vec2str(const Eigen::VectorXd &vec, int precision) @@ -63,4 +464,4 @@ std::string vec2str(const Eigen::VectorXd &vec, int precision) } str += "]"; return str; -} \ No newline at end of file +}