Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Yihuai #15

Open
wants to merge 30 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
30 commits
Select commit Hold shift + click to select a range
d5ba368
Enable initializing the arm in the air
yihuai-gao Oct 11, 2024
777bae9
Fix functionality for gravity compensation
yihuai-gao Oct 11, 2024
19dbe5b
Add `shutdown_to_passive` option
yihuai-gao Oct 11, 2024
aab6bd3
Init `controller_base`
yihuai-gao Oct 12, 2024
db9a8f2
Add interpolator
yihuai-gao Oct 12, 2024
bb7ef7d
Implement `reset_to_home`
yihuai-gao Oct 12, 2024
de06495
Successful compiled
yihuai-gao Oct 13, 2024
13fb672
Update parameters
yihuai-gao Oct 13, 2024
7139c20
Fix gripper control
yihuai-gao Oct 13, 2024
21c92b4
Include `multi_trial_IK`
yihuai-gao Oct 14, 2024
eef818b
Add `default_preview_time`
yihuai-gao Oct 14, 2024
2e5a6fe
Segfault version
yihuai-gao Oct 15, 2024
8933b98
Fix segfault
yihuai-gao Oct 15, 2024
c076ab7
Reorganize joint commands
yihuai-gao Oct 16, 2024
926d377
Update `joint_controller` as a child of `controller_base`
yihuai-gao Oct 16, 2024
22978e6
Fix interpolator initialization bug
yihuai-gao Oct 16, 2024
6a4948c
Fix ik failure bugs
yihuai-gao Oct 16, 2024
943eff8
Fix bugs in reset_to_home
yihuai-gao Oct 16, 2024
7e41322
Export `get_home_pose` and `get_eef_state` to python
yihuai-gao Oct 18, 2024
e9457b8
Fix interpolator bug
yihuai-gao Oct 20, 2024
6787bde
Update interpolator with trajectory
yihuai-gao Oct 23, 2024
dc96338
Add `update_traj` to `JointStateInterpolator`
yihuai-gao Oct 23, 2024
9cce93c
Using trajectory as interface
yihuai-gao Oct 24, 2024
39ed62b
Fixed bugs in set_eef_traj
yihuai-gao Oct 25, 2024
5ba399c
Add multi_trial_ik to the cartesian controller
yihuai-gao Oct 30, 2024
1c5628a
Move `urdf_path` into `config.h`
yihuai-gao Oct 31, 2024
24b9a53
Change joint vel and pos clipping order
yihuai-gao Nov 3, 2024
83a892e
Enable trajectory input for joint controller
yihuai-gao Nov 4, 2024
a9184bf
Update urdf
yihuai-gao Nov 6, 2024
d046707
Update lib for aarch64
yihuai-gao Nov 11, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion .gitignore
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
build
__pycache__
*cpython*.so
python/data
python/data
build*
6 changes: 5 additions & 1 deletion .vscode/c_cpp_properties.json
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
6 changes: 4 additions & 2 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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
Expand All @@ -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
Expand Down
6 changes: 3 additions & 3 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -3,18 +3,18 @@
## 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.

## 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.).
Expand Down
12 changes: 8 additions & 4 deletions examples/test_cartesian_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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)
{
Expand All @@ -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<bool, VecDoF> result = solver.inverse_kinematics(eef_state.pose_6d, joint_state.pos);
std::tuple<int, VecDoF> result = solver.inverse_kinematics(eef_state.pose_6d, joint_state.pos);
if (std::get<0>(result))
{
VecDoF ik_joint_pos = std::get<1>(result);
Expand Down
11 changes: 5 additions & 6 deletions examples/test_joint_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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();
Expand Down
73 changes: 14 additions & 59 deletions include/app/cartesian_controller.h
Original file line number Diff line number Diff line change
@@ -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 <chrono>
#include <memory>
#include <mutex>
#include <spdlog/sinks/stdout_color_sinks.h>
#include <spdlog/spdlog.h>
#include <stdlib.h>
#include <thread>
#include <unistd.h>

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<EEFState, EEFState> get_eef_cmd();
std::tuple<JointState, JointState> 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<EEFState> 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<spdlog::logger> _logger;
std::thread _background_send_recv_thread;
std::shared_ptr<Arx5Solver> _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<int, Eigen::VectorXd> multi_trial_ik(Eigen::Matrix<double, 6, 1> target_pose_6d,
Eigen::VectorXd current_joint_pos, int additional_trial_num = 5);
};

} // namespace arx
#endif

#endif // CARTESIAN_CONTROLLER
14 changes: 12 additions & 2 deletions include/app/common.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,23 +30,32 @@ 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))
{
}
JointState(VecDoF pos, VecDoF vel, VecDoF torque, double gripper_pos)
: 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()
{
Expand All @@ -60,6 +69,7 @@ struct JointState
{
return torque;
}

};

struct Gain
Expand Down
Loading