16 #include <spdlog/spdlog.h>
24 using namespace flexiv;
28 constexpr
double kLoopPeriod = 0.001;
31 const std::vector<double> kImpedanceKp = {3000.0, 3000.0, 800.0, 800.0, 200.0, 200.0, 200.0};
32 const std::vector<double> kImpedanceKd = {80.0, 80.0, 40.0, 40.0, 8.0, 8.0, 8.0};
35 constexpr
double kSineAmp = 0.035;
36 constexpr
double kSineFreq = 0.3;
39 std::atomic<bool> g_stop_sched = {
false};
46 std::cout <<
"Required arguments: [robot_sn]" << std::endl;
47 std::cout <<
" robot_sn: Serial number of the robot to connect. Remove any space, e.g. Rizon4s-123456" << std::endl;
48 std::cout <<
"Optional arguments: [--hold]" << std::endl;
49 std::cout <<
" --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl;
50 std::cout << std::endl;
56 rdk::Robot& robot,
const std::string& motion_type,
const std::vector<double>& init_pos)
59 static unsigned int loop_counter = 0;
64 throw std::runtime_error(
65 "PeriodicTask: Fault occurred on the connected robot, exiting ...");
69 std::vector<double> target_pos(robot.
info().
DoF);
72 if (motion_type ==
"hold") {
73 target_pos = init_pos;
74 }
else if (motion_type ==
"sine-sweep") {
75 for (
size_t i = 0; i < target_pos.size(); ++i) {
76 target_pos[i] = init_pos[i]
77 + kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod);
80 throw std::invalid_argument(
81 "PeriodicTask: Unknown motion type. Accepted motion types: hold, sine-sweep");
85 std::vector<double> target_torque(robot.
info().
DoF);
86 for (
size_t i = 0; i < target_torque.size(); ++i) {
87 target_torque[i] = kImpedanceKp[i] * (target_pos[i] - robot.
states().
q[i])
96 }
catch (
const std::exception& e) {
97 spdlog::error(e.what());
102 int main(
int argc,
char* argv[])
107 if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {
"-h",
"--help"})) {
112 std::string robot_sn = argv[1];
116 ">>> Tutorial description <<<\nThis tutorial runs real-time joint torque control to hold "
117 "or sine-sweep all robot joints. An outer position loop is used to generate joint torque "
118 "commands. This outer position loop + inner torque loop together is also known as an "
119 "impedance controller.\n");
122 std::string motion_type =
"";
123 if (rdk::utility::ProgramArgsExist(argc, argv,
"--hold")) {
124 spdlog::info(
"Robot holding current pose");
125 motion_type =
"hold";
127 spdlog::info(
"Robot running joint sine-sweep");
128 motion_type =
"sine-sweep";
139 spdlog::warn(
"Fault occurred on the connected robot, trying to clear ...");
142 spdlog::error(
"Fault cannot be cleared, exiting ...");
145 spdlog::info(
"Fault on the connected robot is cleared");
149 spdlog::info(
"Enabling robot ...");
154 std::this_thread::sleep_for(std::chrono::seconds(1));
156 spdlog::info(
"Robot is now operational");
159 spdlog::info(
"Moving to home pose");
160 robot.
SwitchMode(rdk::Mode::NRT_PLAN_EXECUTION);
163 while (robot.
busy()) {
164 std::this_thread::sleep_for(std::chrono::seconds(1));
173 auto init_pos = robot.
states().
q;
174 spdlog::info(
"Initial joint positions set to: {}", rdk::utility::Vec2Str(init_pos));
180 std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type), std::ref(init_pos)),
186 while (!g_stop_sched) {
187 std::this_thread::sleep_for(std::chrono::milliseconds(1));
192 }
catch (
const std::exception& e) {
193 spdlog::error(e.what());
Main interface with the robot, containing several function categories and background services.
void ExecutePlan(unsigned int index, bool continue_exec=false, bool block_until_started=true)
[Blocking] Execute a plan by specifying its index.
const RobotStates states() const
[Non-blocking] Current states data of the robot.
const RobotInfo info() const
[Non-blocking] General information about the connected robot.
bool operational(bool verbose=true) const
[Non-blocking] Whether the robot is ready to be operated, which requires the following conditions to ...
void StreamJointTorque(const std::vector< double > &torques, bool enable_gravity_comp=true, bool enable_soft_limits=true)
[Non-blocking] Continuously stream joint torque command to the robot.
void SwitchMode(Mode mode)
[Blocking] Switch to a new control mode and wait until mode transition is finished.
void Enable()
[Blocking] Enable the robot, if E-stop is released and there's no fault, the robot will release brake...
bool fault() const
[Non-blocking] Whether the robot is in fault state.
bool ClearFault(unsigned int timeout_sec=30)
[Blocking] Try to clear minor or critical fault of the robot without a power cycle.
bool busy() const
[Non-blocking] Whether the robot is currently executing a task. This includes any user commanded oper...
Real-time scheduler that can simultaneously run multiple periodic tasks. Parameters for each task are...
int max_priority() const
[Non-blocking] Get maximum available priority for user tasks.
void AddTask(std::function< void(void)> &&callback, const std::string &task_name, int interval, int priority, int cpu_affinity=-1)
[Non-blocking] Add a new periodic task to the scheduler's task pool. Each task in the pool is assigne...
void Stop()
[Blocking] Stop all added tasks. The periodic execution will stop and all task threads will be closed...
void Start()
[Blocking] Start all added tasks. A dedicated thread will be created for each added task and the peri...
std::vector< double > dtheta