11 #include <spdlog/spdlog.h>
21 constexpr
double kLoopPeriod = 0.001;
24 constexpr
double kSineAmp = 0.035;
25 constexpr
double kSineFreq = 0.3;
28 std::atomic<bool> g_stop_sched = {
false};
35 std::cout <<
"Required arguments: [robot SN]" << std::endl;
36 std::cout <<
" robot SN: Serial number of the robot to connect to. "
37 "Remove any space, for example: Rizon4s-123456" << std::endl;
38 std::cout <<
"Optional arguments: [--hold]" << std::endl;
39 std::cout <<
" --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl;
40 std::cout << std::endl;
46 flexiv::rdk::Robot& robot,
const std::string& motion_type,
const std::vector<double>& init_pos)
49 static unsigned int loop_counter = 0;
54 throw std::runtime_error(
55 "PeriodicTask: Fault occurred on the connected robot, exiting ...");
59 std::vector<double> target_pos(robot.
info().
DoF);
60 std::vector<double> target_vel(robot.
info().
DoF);
61 std::vector<double> target_acc(robot.
info().
DoF);
64 if (motion_type ==
"hold") {
65 target_pos = init_pos;
66 }
else if (motion_type ==
"sine-sweep") {
67 for (
size_t i = 0; i < target_pos.size(); ++i) {
68 target_pos[i] = init_pos[i]
69 + kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod);
72 throw std::invalid_argument(
73 "PeriodicTask: unknown motion type. Accepted motion types: hold, sine-sweep");
77 if (loop_counter == 5000) {
79 for (
auto& v : new_Kq) {
84 "PeriodicTask: joint stiffness set to [{}]", flexiv::rdk::utility::Vec2Str(new_Kq));
88 if (loop_counter == 10000) {
90 spdlog::info(
"PeriodicTask: joint stiffness reset to [{}]",
91 flexiv::rdk::utility::Vec2Str(robot.
info().
K_q_nom));
100 }
catch (
const std::exception& e) {
101 spdlog::error(e.what());
106 int main(
int argc,
char* argv[])
111 if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {
"-h",
"--help"})) {
116 std::string robot_sn = argv[1];
120 ">>> Tutorial description <<<\nThis tutorial runs real-time joint impedance control to "
121 "hold or sine-sweep all robot joints.");
124 std::string motion_type =
"";
125 if (flexiv::rdk::utility::ProgramArgsExist(argc, argv,
"--hold")) {
126 spdlog::info(
"Robot holding current pose");
127 motion_type =
"hold";
129 spdlog::info(
"Robot running joint sine-sweep");
130 motion_type =
"sine-sweep";
141 spdlog::warn(
"Fault occurred on the connected robot, trying to clear ...");
144 spdlog::error(
"Fault cannot be cleared, exiting ...");
147 spdlog::info(
"Fault on the connected robot is cleared");
151 spdlog::info(
"Enabling robot ...");
156 std::this_thread::sleep_for(std::chrono::seconds(1));
158 spdlog::info(
"Robot is now operational");
161 spdlog::info(
"Moving to home pose");
162 robot.
SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION);
166 while (robot.
busy()) {
167 std::this_thread::sleep_for(std::chrono::seconds(1));
173 robot.
SwitchMode(flexiv::rdk::Mode::RT_JOINT_IMPEDANCE);
176 auto init_pos = robot.
states().
q;
177 spdlog::info(
"Initial joint positions set to: {}", flexiv::rdk::utility::Vec2Str(init_pos));
183 std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type), std::ref(init_pos)),
189 while (!g_stop_sched) {
190 std::this_thread::sleep_for(std::chrono::milliseconds(1));
195 }
catch (
const std::exception& e) {
196 spdlog::error(e.what());
Main interface with the robot, containing several function categories and background services.
const RobotStates states() const
[Non-blocking] Access the current robot states.
void SetJointImpedance(const std::vector< double > &K_q, const std::vector< double > &Z_q={0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7})
[Non-blocking] Set impedance properties of the robot's joint motion controller used in the joint impe...
const RobotInfo info() const
[Non-blocking] Access general information of the robot.
bool operational(bool verbose=true) const
[Non-blocking] Whether the robot is normally operational, which requires the following conditions to ...
void ExecutePrimitive(const std::string &pt_cmd)
[Blocking] Execute a primitive by specifying its name and parameters, which can be found in the Flexi...
void SwitchMode(Mode mode)
[Blocking] Switch to a new control mode and wait until mode transition is finished.
void ResetJointImpedance()
[Non-blocking] Reset impedance properties of the robot's joint motion controller to nominal values.
void StreamJointPosition(const std::vector< double > &positions, const std::vector< double > &velocities, const std::vector< double > &accelerations)
[Non-blocking] Continuously stream joint position, velocity, and acceleration command to the robot....
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 > K_q_nom