Flexiv RDK APIs  1.4
basics5_zero_force_torque_sensors.cpp

This tutorial zeros the robot's force and torque sensors, which is a recommended (but not mandatory) step before any operations that require accurate force/torque measurement.

Author
Flexiv
#include <spdlog/spdlog.h>
#include <iostream>
#include <thread>
void PrintHelp()
{
// clang-format off
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;
// clang-format on
}
int main(int argc, char* argv[])
{
// Program Setup
// =============================================================================================
// Parse parameters
if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
PrintHelp();
return 1;
}
// Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456
std::string robot_sn = argv[1];
// Print description
spdlog::info(
">>> Tutorial description <<<\nThis tutorial zeros the robot's force and torque sensors, "
"which is a recommended (but not mandatory) step before any operations that require "
"accurate force/torque measurement.");
try {
// RDK Initialization
// =========================================================================================
// Instantiate robot interface
flexiv::rdk::Robot robot(robot_sn);
// Clear fault on the connected robot if any
if (robot.fault()) {
spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
// Try to clear the fault
if (!robot.ClearFault()) {
spdlog::error("Fault cannot be cleared, exiting ...");
return 1;
}
spdlog::info("Fault on the connected robot is cleared");
}
// Enable the robot, make sure the E-stop is released before enabling
spdlog::info("Enabling robot ...");
robot.Enable();
// Wait for the robot to become operational
while (!robot.operational()) {
std::this_thread::sleep_for(std::chrono::seconds(1));
}
spdlog::info("Robot is now operational");
// Zero Sensors
// =========================================================================================
// Get and print the current TCP force/moment readings
spdlog::info("TCP force and moment reading in base frame BEFORE sensor zeroing: "
+ flexiv::rdk::utility::Arr2Str(robot.states().ext_wrench_in_world)
+ "[N][Nm]");
// Run the "ZeroFTSensor" primitive to automatically zero force and torque sensors
robot.SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION);
robot.ExecutePrimitive("ZeroFTSensor()");
// WARNING: during the process, the robot must not contact anything, otherwise the result
// will be inaccurate and affect following operations
spdlog::warn(
"Zeroing force/torque sensors, make sure nothing is in contact with the robot");
// Wait for primitive completion
while (robot.busy()) {
std::this_thread::sleep_for(std::chrono::seconds(1));
}
spdlog::info("Sensor zeroing complete");
// Get and print the current TCP force/moment readings
spdlog::info("TCP force and moment reading in base frame AFTER sensor zeroing: "
+ flexiv::rdk::utility::Arr2Str(robot.states().ext_wrench_in_world)
+ "[N][Nm]");
} 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.
Definition: robot.hpp:24