Loading [MathJax]/extensions/TeX/AMSmath.js
Flexiv RDK APIs  1.6.0
All Classes Files Functions Variables Typedefs Enumerations Enumerator Pages
intermediate3_realtime_joint_torque_control.cpp
1 
13 #include <flexiv/rdk/robot.hpp>
14 #include <flexiv/rdk/scheduler.hpp>
15 #include <flexiv/rdk/utility.hpp>
16 #include <spdlog/spdlog.h>
17 
18 #include <iostream>
19 #include <string>
20 #include <cmath>
21 #include <thread>
22 #include <atomic>
23 
24 using namespace flexiv;
25 
26 namespace {
28 constexpr double kLoopPeriod = 0.001;
29 
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};
33 
35 constexpr double kSineAmp = 0.035;
36 constexpr double kSineFreq = 0.3;
37 
39 std::atomic<bool> g_stop_sched = {false};
40 }
41 
43 void PrintHelp()
44 {
45  // clang-format off
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;
51  // clang-format on
52 }
53 
55 void PeriodicTask(
56  rdk::Robot& robot, const std::string& motion_type, const std::vector<double>& init_pos)
57 {
58  // Local periodic loop counter
59  static unsigned int loop_counter = 0;
60 
61  try {
62  // Monitor fault on the connected robot
63  if (robot.fault()) {
64  throw std::runtime_error(
65  "PeriodicTask: Fault occurred on the connected robot, exiting ...");
66  }
67 
68  // Target joint positions
69  std::vector<double> target_pos(robot.info().DoF);
70 
71  // Set target position based on motion type
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);
78  }
79  } else {
80  throw std::invalid_argument(
81  "PeriodicTask: Unknown motion type. Accepted motion types: hold, sine-sweep");
82  }
83 
84  // Run impedance control on all joints
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])
88  - kImpedanceKd[i] * robot.states().dtheta[i];
89  }
90 
91  // Send target joint torque to RDK server
92  robot.StreamJointTorque(target_torque, true);
93 
94  loop_counter++;
95 
96  } catch (const std::exception& e) {
97  spdlog::error(e.what());
98  g_stop_sched = true;
99  }
100 }
101 
102 int main(int argc, char* argv[])
103 {
104  // Program Setup
105  // =============================================================================================
106  // Parse parameters
107  if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
108  PrintHelp();
109  return 1;
110  }
111  // Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456
112  std::string robot_sn = argv[1];
113 
114  // Print description
115  spdlog::info(
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");
120 
121  // Type of motion specified by user
122  std::string motion_type = "";
123  if (rdk::utility::ProgramArgsExist(argc, argv, "--hold")) {
124  spdlog::info("Robot holding current pose");
125  motion_type = "hold";
126  } else {
127  spdlog::info("Robot running joint sine-sweep");
128  motion_type = "sine-sweep";
129  }
130 
131  try {
132  // RDK Initialization
133  // =========================================================================================
134  // Instantiate robot interface
135  rdk::Robot robot(robot_sn);
136 
137  // Clear fault on the connected robot if any
138  if (robot.fault()) {
139  spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
140  // Try to clear the fault
141  if (!robot.ClearFault()) {
142  spdlog::error("Fault cannot be cleared, exiting ...");
143  return 1;
144  }
145  spdlog::info("Fault on the connected robot is cleared");
146  }
147 
148  // Enable the robot, make sure the E-stop is released before enabling
149  spdlog::info("Enabling robot ...");
150  robot.Enable();
151 
152  // Wait for the robot to become operational
153  while (!robot.operational()) {
154  std::this_thread::sleep_for(std::chrono::seconds(1));
155  }
156  spdlog::info("Robot is now operational");
157 
158  // Move robot to home pose
159  spdlog::info("Moving to home pose");
160  robot.SwitchMode(rdk::Mode::NRT_PLAN_EXECUTION);
161  robot.ExecutePlan("PLAN-Home");
162  // Wait for the plan to finish
163  while (robot.busy()) {
164  std::this_thread::sleep_for(std::chrono::seconds(1));
165  }
166 
167  // Real-time Joint Torque Control
168  // =========================================================================================
169  // Switch to real-time joint torque control mode
170  robot.SwitchMode(rdk::Mode::RT_JOINT_TORQUE);
171 
172  // Set initial joint positions
173  auto init_pos = robot.states().q;
174  spdlog::info("Initial joint positions set to: {}", rdk::utility::Vec2Str(init_pos));
175 
176  // Create real-time scheduler to run periodic tasks
177  rdk::Scheduler scheduler;
178  // Add periodic task with 1ms interval and highest applicable priority
179  scheduler.AddTask(
180  std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type), std::ref(init_pos)),
181  "HP periodic", 1, scheduler.max_priority());
182  // Start all added tasks
183  scheduler.Start();
184 
185  // Block and wait for signal to stop scheduler tasks
186  while (!g_stop_sched) {
187  std::this_thread::sleep_for(std::chrono::milliseconds(1));
188  }
189  // Received signal to stop scheduler tasks
190  scheduler.Stop();
191 
192  } catch (const std::exception& e) {
193  spdlog::error(e.what());
194  return 1;
195  }
196 
197  return 0;
198 }
Main interface with the robot, containing several function categories and background services.
Definition: robot.hpp:25
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...
Definition: scheduler.hpp:22
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
Definition: data.hpp:158
std::vector< double > q
Definition: data.hpp:136