This tutorial shows how to online update and interact with the robot tools. All changes made to the robot tool system will take effect immediately without needing to reboot. However, the robot must be put into IDLE mode when making these changes.
#include <spdlog/spdlog.h>
#include <iostream>
#include <string>
#include <thread>
void PrintHelp()
{
std::cout << "Required arguments: [robot SN]" << std::endl;
std::cout << " robot SN: Serial number of the robot to connect to. "
"Remove any space, for example: Rizon4s-123456" << std::endl;
std::cout << "Optional arguments: None" << std::endl;
std::cout << std::endl;
}
int main(int argc, char* argv[])
{
if (argc < 2 ||
flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {
"-h",
"--help"})) {
PrintHelp();
return 1;
}
std::string robot_sn = argv[1];
spdlog::info(
">>> Tutorial description <<<\nThis tutorial shows how to online update and interact with "
"the robot tools. All changes made to the robot tool system will take effect immediately "
"without needing to reboot. However, the robot must be put into IDLE mode when making "
"these changes.");
try {
if (robot.fault()) {
spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
if (!robot.ClearFault()) {
spdlog::error("Fault cannot be cleared, exiting ...");
return 1;
}
spdlog::info("Fault on the connected robot is cleared");
}
spdlog::info("Enabling robot ...");
robot.Enable();
while (!robot.operational()) {
std::this_thread::sleep_for(std::chrono::seconds(1));
}
spdlog::info("Robot is now operational");
robot.SwitchMode(
flexiv::rdk::Mode::IDLE);
spdlog::info("All configured tools:");
auto tool_list = tool.list();
for (size_t i = 0; i < tool_list.size(); i++) {
std::cout << "[" << i << "] " << tool_list[i] << std::endl;
}
std::cout << std::endl;
spdlog::info("Current active tool: {}", tool.name());
std::string new_tool_name = "ExampleTool1";
new_tool_params.
mass = 0.9;
new_tool_params.
CoM = {0.0, 0.0, 0.057};
new_tool_params.
inertia = {2.768e-03, 3.149e-03, 5.64e-04, 0.0, 0.0, 0.0};
new_tool_params.
tcp_location = {0.0, -0.207, 0.09, 0.7071068, 0.7071068, 0.0, 0.0};
if (tool.exist(new_tool_name)) {
spdlog::warn(
"Tool with the same name [{}] already exists, removing it now", new_tool_name);
tool.Switch("Flange");
tool.Remove(new_tool_name);
}
spdlog::info("Adding new tool [{}] to the robot", new_tool_name);
tool.Add(new_tool_name, new_tool_params);
spdlog::info("All configured tools:");
tool_list = tool.list();
for (size_t i = 0; i < tool_list.size(); i++) {
std::cout << "[" << i << "] " << tool_list[i] << std::endl;
}
std::cout << std::endl;
spdlog::info("Switching to tool [{}]", new_tool_name);
tool.Switch(new_tool_name);
spdlog::info("Current active tool: {}", tool.name());
tool.Switch("Flange");
std::this_thread::sleep_for(std::chrono::seconds(2));
spdlog::info("Removing tool [{}]", new_tool_name);
tool.Remove(new_tool_name);
spdlog::info("Program finished");
} catch (const std::exception& e) {
spdlog::error(e.what());
return 1;
}
return 0;
}
Main interface with the robot, containing several function categories and background services.