Initial import of ORISYS arm workstation
This commit is contained in:
cheng
+59
@@ -0,0 +1,59 @@
|
||||
cmake_minimum_required(VERSION 3.22.1)
|
||||
project(flexiv_rdk-examples)
|
||||
|
||||
# CMake >= 3.30 warns for FindBoost behavior unless CMP0167 is explicitly set.
|
||||
if(POLICY CMP0167)
|
||||
cmake_policy(SET CMP0167 OLD)
|
||||
endif()
|
||||
|
||||
# Show verbose build info
|
||||
SET(CMAKE_VERBOSE_MAKEFILE ON)
|
||||
|
||||
message("OS: ${CMAKE_SYSTEM_NAME}")
|
||||
message("Processor: ${CMAKE_SYSTEM_PROCESSOR}")
|
||||
|
||||
# Configure build type
|
||||
if(NOT CMAKE_BUILD_TYPE)
|
||||
set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE)
|
||||
endif()
|
||||
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Release" "Debug" "RelWithDebInfo" "MinSizeRel")
|
||||
|
||||
# Minimum example list for all OS
|
||||
set(EXAMPLE_LIST
|
||||
basics1_display_robot_states
|
||||
basics2_clear_fault
|
||||
basics3_primitive_execution
|
||||
basics4_plan_execution
|
||||
basics5_zero_force_torque_sensors
|
||||
basics6_gripper_control
|
||||
basics7_update_robot_tool
|
||||
basics8_global_variables
|
||||
basics9_logging_behavior
|
||||
intermediate7_robot_dynamics
|
||||
)
|
||||
|
||||
# Additional examples for Linux and Mac
|
||||
if(CMAKE_HOST_UNIX)
|
||||
list(APPEND EXAMPLE_LIST
|
||||
intermediate1_realtime_joint_position_control
|
||||
intermediate2_realtime_joint_impedance_control
|
||||
intermediate3_realtime_joint_torque_control
|
||||
intermediate4_realtime_joint_floating
|
||||
intermediate5_realtime_cartesian_pure_motion_control
|
||||
intermediate6_realtime_cartesian_motion_force_control
|
||||
)
|
||||
endif()
|
||||
|
||||
# Find flexiv_rdk INTERFACE library
|
||||
find_package(flexiv_rdk REQUIRED)
|
||||
|
||||
# Build all selected examples
|
||||
foreach(example ${EXAMPLE_LIST})
|
||||
add_executable(${example} ${example}.cpp)
|
||||
target_link_libraries(${example} flexiv::flexiv_rdk)
|
||||
|
||||
# C++20 required
|
||||
set_target_properties(${example} PROPERTIES
|
||||
CXX_STANDARD 20
|
||||
CXX_STANDARD_REQUIRED ON)
|
||||
endforeach()
|
||||
@@ -0,0 +1,120 @@
|
||||
/**
|
||||
* @example basics1_display_robot_states.cpp
|
||||
* This tutorial does the very first thing: check connection with the robot server and print
|
||||
* received robot states.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
|
||||
std::cout << "Optional arguments: None" << std::endl;
|
||||
std::cout << std::endl;
|
||||
// clang-format on
|
||||
}
|
||||
|
||||
/** @brief Print robot states data @ 1Hz */
|
||||
void PrintRobotStates(rdk::Robot& robot)
|
||||
{
|
||||
while (true) {
|
||||
// Print Available joint groups
|
||||
std::string joint_groups_str;
|
||||
for (const auto& [_, name] : robot.info().all_groups) {
|
||||
joint_groups_str += "[" + name + "] ";
|
||||
}
|
||||
spdlog::info("Available joint groups: {}", joint_groups_str);
|
||||
|
||||
// Print all robot states in JSON format using the built-in ostream operator overloading
|
||||
for (const auto& [group, states] : robot.states()) {
|
||||
spdlog::info("[{}] robot states:", rdk::kJointGroupNames.at(group));
|
||||
std::cout << states << std::endl;
|
||||
}
|
||||
|
||||
// Print all robot actions in JSON format using the built-in ostream operator overloading
|
||||
for (const auto& [group, actions] : robot.actions()) {
|
||||
spdlog::info("[{}] robot actions:", rdk::kJointGroupNames.at(group));
|
||||
std::cout << actions << std::endl;
|
||||
}
|
||||
|
||||
// Print digital inputs and outputs
|
||||
spdlog::info("Digital inputs:");
|
||||
std::cout << rdk::utility::Arr2Str(robot.digital_inputs()) << std::endl;
|
||||
spdlog::info("Digital outputs:");
|
||||
std::cout << rdk::utility::Arr2Str(robot.digital_outputs()) << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
// Program Setup
|
||||
// =============================================================================================
|
||||
// Parse parameters
|
||||
if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
|
||||
// Print description
|
||||
spdlog::info(
|
||||
">>> Tutorial description <<<\nThis tutorial does the very first thing: check connection "
|
||||
"with the robot server and print received robot states.\n");
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
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");
|
||||
}
|
||||
|
||||
// Servo on the robot, make sure the E-stop is released
|
||||
spdlog::info("Servo on the robot ...");
|
||||
robot.ServoOn();
|
||||
|
||||
// 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");
|
||||
|
||||
// Print States
|
||||
// =========================================================================================
|
||||
// Use std::thread to do scheduling so that this example can run on all OS, since not all OS
|
||||
// support rdk::Scheduler
|
||||
std::thread low_priority_thread(std::bind(PrintRobotStates, std::ref(robot)));
|
||||
|
||||
// Properly exit thread
|
||||
low_priority_thread.join();
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,72 @@
|
||||
/**
|
||||
* @example basics2_clear_fault.cpp
|
||||
* This tutorial clears minor or critical faults, if any, of the connected robot.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-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 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
|
||||
// Print description
|
||||
spdlog::info(
|
||||
">>> Tutorial description <<<\nThis tutorial clears minor or critical faults, if any, of "
|
||||
"the connected robot.\n");
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
rdk::Robot robot(robot_sn);
|
||||
|
||||
// Fault Clearing
|
||||
// =========================================================================================
|
||||
// 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");
|
||||
} else {
|
||||
spdlog::info("No fault on the connected robot");
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,207 @@
|
||||
/**
|
||||
* @example basics3_primitive_execution.cpp
|
||||
* This tutorial executes several basic robot primitives (unit skills). For detailed documentation
|
||||
* on all available primitives, please see [Flexiv Primitives](https://www.flexiv.com/primitives/).
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <thread>
|
||||
#include <algorithm>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-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 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
|
||||
// Print description
|
||||
spdlog::info(
|
||||
">>> Tutorial description <<<\nThis tutorial executes several basic robot primitives (unit "
|
||||
"skills). For detailed documentation on all available primitives, please see [Flexiv "
|
||||
"Primitives](https://www.flexiv.com/primitives/).\n");
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
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");
|
||||
}
|
||||
|
||||
// Servo on the robot, make sure the E-stop is released
|
||||
spdlog::info("Servo on the robot ...");
|
||||
robot.ServoOn();
|
||||
|
||||
// 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");
|
||||
|
||||
// Execute Primitives
|
||||
// =========================================================================================
|
||||
// Primitives can only be executed on single-arm joint groups
|
||||
const auto& single_arm_groups = robot.info().single_arm_groups;
|
||||
if (single_arm_groups.empty()) {
|
||||
throw std::runtime_error("No single-arm joint group found on the connected robot");
|
||||
}
|
||||
|
||||
// (1) Move robot to home pose
|
||||
// -----------------------------------------------------------------------------------------
|
||||
spdlog::info("Moving to home pose");
|
||||
robot.Home();
|
||||
|
||||
// Switch to primitive execution mode
|
||||
robot.SwitchMode(rdk::Mode::NRT_PRIMITIVE_EXECUTION);
|
||||
|
||||
// (2) Move robot joints to target positions
|
||||
// -----------------------------------------------------------------------------------------
|
||||
// Required parameters:
|
||||
// target: final joint positions, unit: degrees
|
||||
// {arm joint positions}, {external axis joint positions}
|
||||
// Optional parameters:
|
||||
// waypoints: waypoints to pass before reaching the target
|
||||
// (same format as above, but can be more than one)
|
||||
// vel: TCP linear velocity, unit: m/s
|
||||
spdlog::info("Executing primitive: MoveJ");
|
||||
|
||||
// Send command to robot
|
||||
std::map<rdk::JointGroup, rdk::PrimitiveArgs> pt_args;
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
pt_args[group] = rdk::PrimitiveArgs("MoveJ",
|
||||
{
|
||||
{"target", rdk::JPos({30, -45, 0, 90, 0, 40, 30}, {-50, 30})},
|
||||
{"waypoints",
|
||||
std::vector<rdk::JPos> {rdk::JPos({10, -30, 10, 30, 10, 15, 10}, {-15, 10}),
|
||||
rdk::JPos({20, -60, -10, 60, -10, 30, 20}, {-30, 20})}},
|
||||
});
|
||||
}
|
||||
robot.ExecutePrimitive(pt_args);
|
||||
|
||||
// Most primitives won't exit by themselves and require users to explicitly trigger
|
||||
// transitions based on specific primitive states. Here we check if the primitive state
|
||||
// [reachedTarget] becomes true and trigger the transition manually by sending a new
|
||||
// primitive command.
|
||||
while (true) {
|
||||
const auto primitive_states = robot.primitive_states();
|
||||
if (rdk::utility::PrimitiveStateTrueForGroups(
|
||||
primitive_states, "reachedTarget")) {
|
||||
break;
|
||||
}
|
||||
|
||||
// Print current primitive states
|
||||
spdlog::info("Current primitive states:");
|
||||
for (const auto& [group, pt_states] : primitive_states) {
|
||||
std::cout << rdk::kJointGroupNames.at(group) << ":" << std::endl;
|
||||
std::cout << "primitiveName: " << pt_states.pt_name << std::endl;
|
||||
for (const auto& [name, value] : pt_states.names_and_values) {
|
||||
std::cout << name << ": " << rdk::utility::FlexivTypes2Str(value);
|
||||
std::cout << std::endl;
|
||||
}
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
|
||||
// (3) Move robot TCP to a target pose in world (base) frame
|
||||
// -----------------------------------------------------------------------------------------
|
||||
// Required parameters:
|
||||
// target: final TCP pose, unit: m and degrees
|
||||
// {pos_x, pos_y, pos_z}, {rot_x, rot_y, rot_z}, {ref_frame, ref_point}
|
||||
// Optional parameters:
|
||||
// waypoints: waypoints to pass before reaching the target
|
||||
// (same format as above, but can be more than one)
|
||||
// vel: TCP linear velocity, unit: m/s
|
||||
// NOTE: The rotations use Euler ZYX convention, rot_x means Euler ZYX angle around X axis
|
||||
spdlog::info("Executing primitive: MoveL");
|
||||
|
||||
// Send command to robot
|
||||
pt_args.clear();
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
pt_args[group] = rdk::PrimitiveArgs("MoveL",
|
||||
{{"target",
|
||||
rdk::Coord({0.3, -0.1, 0.2}, {160, 20, 180}, {"WORLD", "WORLD_ORIGIN"})},
|
||||
{"waypoints",
|
||||
std::vector<rdk::Coord> {rdk::Coord({0.1, 0.1, 0.1}, {-160, -20, 180},
|
||||
{"WORLD", "WORLD_ORIGIN"}),
|
||||
rdk::Coord({0.3, 0.2, 0.1}, {180, 0, 180}, {"WORLD", "WORLD_ORIGIN"})}},
|
||||
|
||||
{"vel", 0.6}, {"zoneRadius", "Z50"}});
|
||||
}
|
||||
robot.ExecutePrimitive(pt_args);
|
||||
|
||||
// Wait for reached target
|
||||
while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "reachedTarget")) {
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
|
||||
// (4) Another MoveL that uses TCP frame
|
||||
// -----------------------------------------------------------------------------------------
|
||||
// In this example the reference frame is changed from WORLD::WORLD_ORIGIN to TRAJ::START,
|
||||
// which represents the current TCP frame
|
||||
spdlog::info("Executing primitive: MoveL");
|
||||
|
||||
// Example to convert target quaternion [w,x,y,z] to Euler ZYX using utility functions
|
||||
std::array<double, 4> targetQuat = {0.9185587, 0.1767767, 0.3061862, 0.1767767};
|
||||
// ZYX = [30, 30, 30] degrees
|
||||
auto targetEulerDeg = rdk::utility::Rad2Deg(rdk::utility::Quat2EulerZYX(targetQuat));
|
||||
|
||||
// Send command to robot. This motion will hold current TCP position and only do rotation
|
||||
pt_args.clear();
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
pt_args[group] = rdk::PrimitiveArgs("MoveL",
|
||||
{
|
||||
{"target", rdk::Coord({0.0, 0.0, 0.0}, targetEulerDeg, {"TRAJ", "START"})},
|
||||
{"vel", 0.2},
|
||||
});
|
||||
}
|
||||
robot.ExecutePrimitive(pt_args);
|
||||
// Wait for reached target
|
||||
while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "reachedTarget")) {
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
|
||||
// All done, stop robot and put into IDLE mode
|
||||
robot.Stop();
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,152 @@
|
||||
/**
|
||||
* @example basics4_plan_execution.cpp
|
||||
* This tutorial executes a plan selected by the user from a list of available plans. A plan is a
|
||||
* pre-written script to execute a series of robot primitives with pre-defined transition conditions
|
||||
* between 2 adjacent primitives. Users can use Flexiv Elements to compose their own plan and assign
|
||||
* to the robot, which will appear in the plan list.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-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 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
|
||||
// Print description
|
||||
spdlog::info(
|
||||
">>> Tutorial description <<<\nThis tutorial executes a plan selected by the user from a "
|
||||
"list of available plans. A plan is a pre-written script to execute a series of robot "
|
||||
"primitives with pre-defined transition conditions between 2 adjacent primitives. Users "
|
||||
"can use Flexiv Elements to compose their own plan and assign to the robot, which will "
|
||||
"appear in the plan list.\n");
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
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");
|
||||
}
|
||||
|
||||
// Servo on the robot, make sure the E-stop is released
|
||||
spdlog::info("Servo on the robot ...");
|
||||
robot.ServoOn();
|
||||
|
||||
// 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");
|
||||
|
||||
// Execute Plans
|
||||
// =========================================================================================
|
||||
// Switch to plan execution mode
|
||||
robot.SwitchMode(rdk::Mode::NRT_PLAN_EXECUTION);
|
||||
|
||||
while (true) {
|
||||
// Monitor fault on the connected robot
|
||||
if (robot.fault()) {
|
||||
throw std::runtime_error("Fault occurred on the connected robot, exiting ...");
|
||||
}
|
||||
|
||||
// Get user input
|
||||
spdlog::info("Choose an action:");
|
||||
std::cout << "[1] Show available plans" << std::endl;
|
||||
std::cout << "[2] Execute a plan by index" << std::endl;
|
||||
std::cout << "[3] Execute a plan by name" << std::endl;
|
||||
|
||||
std::string inputBuffer;
|
||||
std::cin >> inputBuffer;
|
||||
int userInput = std::stoi(inputBuffer);
|
||||
|
||||
switch (userInput) {
|
||||
// Get and show plan list
|
||||
case 1: {
|
||||
auto plan_list = robot.plan_list();
|
||||
for (size_t i = 0; i < plan_list.size(); i++) {
|
||||
std::cout << "[" << i << "] " << plan_list[i] << std::endl;
|
||||
}
|
||||
std::cout << std::endl;
|
||||
} break;
|
||||
// Execute plan by index
|
||||
case 2: {
|
||||
spdlog::info("Enter plan index to execute:");
|
||||
int index;
|
||||
std::cin >> index;
|
||||
// Allow the plan to continue its execution even if the RDK program is closed or
|
||||
// the connection is lost
|
||||
robot.ExecutePlan(index, true);
|
||||
|
||||
// Print plan info while the current plan is running
|
||||
while (robot.busy()) {
|
||||
spdlog::info("Current plan info:");
|
||||
std::cout << robot.plan_info() << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
} break;
|
||||
// Execute plan by name
|
||||
case 3: {
|
||||
spdlog::info("Enter plan name to execute:");
|
||||
std::string name;
|
||||
std::cin >> name;
|
||||
// Allow the plan to continue its execution even if the RDK program is closed or
|
||||
// the connection is lost
|
||||
robot.ExecutePlan(name, true);
|
||||
|
||||
// Print plan info while the current plan is running
|
||||
while (robot.busy()) {
|
||||
spdlog::info("Current plan info:");
|
||||
std::cout << robot.plan_info() << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
} break;
|
||||
default:
|
||||
spdlog::warn("Invalid input");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
/**
|
||||
* @example 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.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
#include <algorithm>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-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 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
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.\n");
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
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");
|
||||
}
|
||||
|
||||
// Servo on the robot, make sure the E-stop is released
|
||||
spdlog::info("Servo on the robot ...");
|
||||
robot.ServoOn();
|
||||
|
||||
// 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
|
||||
for (const auto& [group, states] : robot.states()) {
|
||||
spdlog::info(
|
||||
"[{}] TCP force and moment reading in world frame BEFORE sensor zeroing: {} N-Nm",
|
||||
rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(states.tcp_wrench));
|
||||
}
|
||||
|
||||
// Primitives can only be executed on single-arm joint groups
|
||||
const auto& single_arm_groups = robot.info().single_arm_groups;
|
||||
if (single_arm_groups.empty()) {
|
||||
throw std::runtime_error("No single-arm joint group found on the connected robot");
|
||||
}
|
||||
|
||||
// Run the "ZeroFTSensor" primitive to automatically zero force and torque sensors
|
||||
robot.SwitchMode(rdk::Mode::NRT_PRIMITIVE_EXECUTION);
|
||||
std::map<rdk::JointGroup, rdk::PrimitiveArgs> pt_args;
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
pt_args[group] = rdk::PrimitiveArgs("ZeroFTSensor", {});
|
||||
}
|
||||
robot.ExecutePrimitive(pt_args);
|
||||
|
||||
// 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 to finish
|
||||
while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "terminated")) {
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
spdlog::info("Sensor zeroing complete");
|
||||
|
||||
// Get and print the current TCP force/moment readings
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
spdlog::info(
|
||||
"[{}] TCP force and moment reading in world frame AFTER sensor zeroing: {} N-Nm",
|
||||
rdk::kJointGroupNames.at(group),
|
||||
rdk::utility::Arr2Str(robot.states().at(group).tcp_wrench));
|
||||
}
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,250 @@
|
||||
/**
|
||||
* @example basics6_gripper_control.cpp
|
||||
* This tutorial does position and force (if available) control of grippers supported by Flexiv.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/gripper.hpp>
|
||||
#include <flexiv/rdk/tool.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <limits>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
namespace {
|
||||
/** Global flag: whether the gripper control tasks are finished */
|
||||
std::atomic<bool> g_finished = {false};
|
||||
}
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn] [gripper_device_name] [gripper_tool_name]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect to. Remove any space, for example: Enlight-L-123456" << std::endl;
|
||||
std::cout << " gripper_device_name: Full name of the device representing the gripper, can be found in Flexiv Elements->Settings->Device" << std::endl;
|
||||
std::cout << " gripper_tool_name: Full name of the tool representing the gripper, can be found in Flexiv Elements->Settings->Tool" << std::endl;
|
||||
std::cout << "Optional arguments: None" << std::endl;
|
||||
std::cout << std::endl;
|
||||
// clang-format on
|
||||
}
|
||||
|
||||
/** @brief Print gripper states data @ 1Hz */
|
||||
void PrintGripperStates(rdk::Gripper& gripper)
|
||||
{
|
||||
while (!g_finished) {
|
||||
const auto gripper_states = gripper.states();
|
||||
spdlog::info("Current gripper states:");
|
||||
for (const auto& [group, states] : gripper_states) {
|
||||
std::cout << "[" << rdk::kJointGroupNames.at(group) << "]\n" << states << std::endl;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
// Program Setup
|
||||
// =============================================================================================
|
||||
// Parse parameters
|
||||
if (argc < 4 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
std::string gripper_device_name = argv[2];
|
||||
std::string gripper_tool_name = argv[3];
|
||||
|
||||
// Print description
|
||||
spdlog::info(
|
||||
">>> Tutorial description <<<\nThis tutorial does position and force (if available) "
|
||||
"control of grippers supported by Flexiv.\n");
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
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");
|
||||
}
|
||||
|
||||
// Servo on the robot, make sure the E-stop is released
|
||||
spdlog::info("Servo on the robot ...");
|
||||
robot.ServoOn();
|
||||
|
||||
// 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");
|
||||
|
||||
// Gripper Control
|
||||
// =========================================================================================
|
||||
// Instantiate gripper control interface
|
||||
rdk::Gripper gripper(robot);
|
||||
|
||||
// Instantiate tool interface. Gripper is categorized as both a device and a tool. The
|
||||
// device attribute allows a gripper to be interactively controlled by the user; whereas the
|
||||
// tool attribute tells the robot to account for its mass properties and TCP location.
|
||||
rdk::Tool tool(robot);
|
||||
|
||||
// Grippers can only be assigned to single-arm joint groups
|
||||
const auto& single_arm_groups = robot.info().single_arm_groups;
|
||||
if (single_arm_groups.empty()) {
|
||||
throw std::runtime_error("No single-arm joint group found on the connected robot");
|
||||
}
|
||||
|
||||
// Enable the specified gripper as a device. This is equivalent to enabling the specified
|
||||
// gripper in Flexiv Elements -> Settings -> Device
|
||||
spdlog::info("Enabling gripper device [{}] for all single-arm groups", gripper_device_name);
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
gripper.Enable(group, gripper_device_name);
|
||||
}
|
||||
|
||||
// Print parameters of the enabled gripper
|
||||
spdlog::info("Gripper params:");
|
||||
const auto gripper_params = gripper.params();
|
||||
for (const auto& [group, params] : gripper_params) {
|
||||
std::cout << "[" << rdk::kJointGroupNames.at(group) << "]\n"
|
||||
<< std::fixed << std::setprecision(2) << "{\n"
|
||||
<< "name: " << params.name << "\nmin_width: " << params.min_width
|
||||
<< "\nmax_width: " << params.max_width << "\nmin_force: " << params.min_force
|
||||
<< "\nmax_force: " << params.max_force << "\nmin_vel: " << params.min_vel
|
||||
<< "\nmax_vel: " << params.max_vel << "\n}" << std::endl;
|
||||
}
|
||||
|
||||
// Switch robot tool to gripper so the gravity compensation and TCP location is updated
|
||||
spdlog::info("Switching robot tool to [{}] for all single-arm groups", gripper_tool_name);
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
tool.Switch(group, gripper_tool_name);
|
||||
}
|
||||
|
||||
// User needs to determine if this gripper requires manual initialization
|
||||
int choice = 0;
|
||||
spdlog::info(
|
||||
"Manually trigger initialization for the gripper now? Choose Yes if it's a 48v Grav "
|
||||
"gripper");
|
||||
std::cout << "[1] No, it has already initialized automatically when power on" << std::endl;
|
||||
std::cout << "[2] Yes, it does not initialize itself when power on" << std::endl;
|
||||
std::cin >> choice;
|
||||
|
||||
// Trigger manual initialization based on choice
|
||||
if (choice == 1) {
|
||||
spdlog::info("Skipped manual initialization");
|
||||
} else if (choice == 2) {
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
gripper.Init(group);
|
||||
}
|
||||
// User determines if the manual initialization is finished
|
||||
spdlog::info(
|
||||
"Triggered manual initialization, press Enter when the initialization is finished "
|
||||
"to continue");
|
||||
std::cin.get();
|
||||
std::cin.get();
|
||||
} else {
|
||||
spdlog::error("Invalid choice");
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Start a separate thread to print gripper states. Use a scope guard so the thread is
|
||||
// always signaled and joined when leaving this scope, including via an exception thrown by
|
||||
// gripper.Move() etc. Otherwise the std::thread destructor would run while still joinable
|
||||
// and call std::terminate(), aborting before the exception reaches the catch block below.
|
||||
std::thread print_thread(PrintGripperStates, std::ref(gripper));
|
||||
auto join_print_thread = [&]() {
|
||||
g_finished = true;
|
||||
if (print_thread.joinable()) {
|
||||
print_thread.join();
|
||||
}
|
||||
};
|
||||
std::shared_ptr<void> print_thread_guard(nullptr, [&](void*) { join_print_thread(); });
|
||||
|
||||
// Position control
|
||||
spdlog::info("Closing gripper");
|
||||
for (const auto& [group, params] : gripper_params) {
|
||||
gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force);
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::seconds(2));
|
||||
spdlog::info("Opening gripper");
|
||||
for (const auto& [group, params] : gripper_params) {
|
||||
gripper.Move(group, params.max_width, params.max_vel, 0.25 * params.max_force);
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::seconds(2));
|
||||
|
||||
// Stop
|
||||
spdlog::info("Closing gripper");
|
||||
for (const auto& [group, params] : gripper_params) {
|
||||
gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force);
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
||||
spdlog::info("Stopping gripper");
|
||||
for (const auto& [group, _] : gripper_params) {
|
||||
gripper.Stop(group);
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::seconds(2));
|
||||
spdlog::info("Closing gripper");
|
||||
for (const auto& [group, params] : gripper_params) {
|
||||
gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force);
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::seconds(2));
|
||||
spdlog::info("Opening gripper");
|
||||
for (const auto& [group, params] : gripper_params) {
|
||||
gripper.Move(group, params.max_width, params.max_vel, 0.25 * params.max_force);
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
||||
spdlog::info("Stopping gripper");
|
||||
for (const auto& [group, _] : gripper_params) {
|
||||
gripper.Stop(group);
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::seconds(2));
|
||||
|
||||
// Force control, if available (sensed force is not zero)
|
||||
const auto gripper_states = gripper.states();
|
||||
bool has_force_control = false;
|
||||
for (const auto& [_, states] : gripper_states) {
|
||||
if (fabs(states.force) > std::numeric_limits<double>::epsilon()) {
|
||||
has_force_control = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (has_force_control) {
|
||||
spdlog::info("Gripper running zero force control");
|
||||
for (const auto& [group, _] : gripper_states) {
|
||||
gripper.Grasp(group, 0);
|
||||
}
|
||||
// Exit after 10 seconds
|
||||
std::this_thread::sleep_for(std::chrono::seconds(10));
|
||||
}
|
||||
|
||||
// Finished, exit all threads
|
||||
for (const auto& [group, _] : gripper_states) {
|
||||
gripper.Stop(group);
|
||||
}
|
||||
spdlog::info("Program finished");
|
||||
// The print thread is signaled and joined by the scope guard above.
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,168 @@
|
||||
/**
|
||||
* @example basics7_update_robot_tool.cpp
|
||||
* 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.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/tool.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-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 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
|
||||
// Print description
|
||||
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.\n");
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
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");
|
||||
}
|
||||
|
||||
// Servo on the robot, make sure the E-stop is released
|
||||
spdlog::info("Servo on the robot ...");
|
||||
robot.ServoOn();
|
||||
|
||||
// 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");
|
||||
|
||||
// Update Robot Tool
|
||||
// =========================================================================================
|
||||
// Make sure the robot is in IDLE mode
|
||||
robot.SwitchMode(rdk::Mode::IDLE);
|
||||
|
||||
// Instantiate tool interface
|
||||
rdk::Tool tool(robot);
|
||||
|
||||
// Tools can only be assigned to single-arm joint groups
|
||||
const auto& single_arm_groups = robot.info().single_arm_groups;
|
||||
if (single_arm_groups.empty()) {
|
||||
throw std::runtime_error("No single-arm joint group found on the connected robot");
|
||||
}
|
||||
|
||||
// Get and print a list of already configured tools currently in the robot's tools pool
|
||||
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;
|
||||
|
||||
// Get and print the current active tool
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
spdlog::info(
|
||||
"[{}] Current active tool: {}", rdk::kJointGroupNames.at(group), tool.name(group));
|
||||
}
|
||||
|
||||
// Set name and parameters for a new tool
|
||||
std::string new_tool_name = "ExampleTool1";
|
||||
rdk::ToolParams new_tool_params;
|
||||
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 there's already a tool with the same name in the robot's tools pool, then remove it
|
||||
// first, because duplicate tool names are not allowed
|
||||
if (tool.exist(new_tool_name)) {
|
||||
spdlog::warn(
|
||||
"Tool with the same name [{}] already exists, removing it now", new_tool_name);
|
||||
// Switch to other tool or no tool (Flange) before removing the current tool
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
tool.Switch(group, "Flange");
|
||||
}
|
||||
tool.Remove(new_tool_name);
|
||||
}
|
||||
|
||||
// Add the new tool
|
||||
spdlog::info("Adding new tool [{}] to the robot", new_tool_name);
|
||||
tool.Add(new_tool_name, new_tool_params);
|
||||
|
||||
// Get and print the tools list again, the new tool should appear at the end
|
||||
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;
|
||||
|
||||
// Switch to the newly added tool, i.e. set it as the active tool
|
||||
spdlog::info("Switching to tool [{}]", new_tool_name);
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
tool.Switch(group, new_tool_name);
|
||||
}
|
||||
|
||||
// Get and print the current active tool again, should be the new tool
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
spdlog::info(
|
||||
"[{}] Current active tool: {}", rdk::kJointGroupNames.at(group), tool.name(group));
|
||||
}
|
||||
|
||||
// Switch to other tool or no tool (Flange) before removing the current tool
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
tool.Switch(group, "Flange");
|
||||
}
|
||||
|
||||
// Clean up by removing the new tool
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,129 @@
|
||||
/**
|
||||
* @example basics8_global_variables.cpp
|
||||
* This tutorial shows how to get and set global variables.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-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 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
|
||||
// Print description
|
||||
spdlog::info(
|
||||
">>> Tutorial description <<<\nThis tutorial shows how to get and set global variables.\n");
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
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");
|
||||
}
|
||||
|
||||
// Servo on the robot, make sure the E-stop is released
|
||||
spdlog::info("Servo on the robot ...");
|
||||
robot.ServoOn();
|
||||
|
||||
// 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");
|
||||
|
||||
// Get existing global variables
|
||||
// =========================================================================================
|
||||
auto global_vars = robot.global_variables();
|
||||
if (global_vars.empty()) {
|
||||
spdlog::warn("No global variables available");
|
||||
return 1;
|
||||
} else {
|
||||
spdlog::info("Existing global variables and their original values:");
|
||||
for (const auto& var : global_vars) {
|
||||
std::cout << var.first << ": " << rdk::utility::FlexivTypes2Str(var.second)
|
||||
<< std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// Set global variables
|
||||
// =========================================================================================
|
||||
///@warning These specified global variables need to be created first using Flexiv Elements
|
||||
spdlog::info("Setting new values to existing global variables");
|
||||
robot.SetGlobalVariables({
|
||||
{"test_bool", 1},
|
||||
{"test_int", 100},
|
||||
{"test_double", 100.123},
|
||||
{"test_string", "Flexiv"},
|
||||
{"test_int_vec", std::vector<int> {1, 2, 3}},
|
||||
{"test_double_vec", std::vector<double> {1.1, 2.2, 3.3}},
|
||||
{"test_string_vec", std::vector<std::string> {"Go", "Flexiv", "Go!"}},
|
||||
{"test_pose", std::vector<double> {0.1, -0.2, 0.3, -90, -45, 120}},
|
||||
{"test_coord", rdk::Coord({0.1, -0.2, 0.3}, {-90, -45, 120}, {"WORLD", "WORLD_ORIGIN"},
|
||||
{1, 2, 3, 4, 5, 6, 7}, {10, 20, 0, 0, 0, 0})},
|
||||
{"test_coord_array",
|
||||
std::vector<rdk::Coord> {rdk::Coord({1, 2, 3}, {4, 5, 6}, {"WORK", "WorkCoord0"}),
|
||||
rdk::Coord({10, 20, 30}, {40, 50, 60}, {"WORLD", "WORLD_ORIGIN"},
|
||||
{1, 2, 3, 4, 5, 6, 7}, {10, 20, 0, 0, 0, 0}),
|
||||
rdk::Coord({3, 2, 1}, {180, 0, 180}, {"WORLD", "WORLD_ORIGIN"})}},
|
||||
});
|
||||
|
||||
// Get updated global variables
|
||||
// =========================================================================================
|
||||
global_vars = robot.global_variables();
|
||||
if (global_vars.empty()) {
|
||||
spdlog::warn("No global variables available");
|
||||
return 1;
|
||||
} else {
|
||||
spdlog::info("Updated global variables:");
|
||||
for (const auto& var : global_vars) {
|
||||
std::cout << var.first << ": " << rdk::utility::FlexivTypes2Str(var.second)
|
||||
<< std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
spdlog::info("Program finished");
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,97 @@
|
||||
/**
|
||||
* @example basics9_logging_behavior.cpp
|
||||
* This tutorial shows how to change the logging behaviors of RDK client.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
#include <spdlog/async.h>
|
||||
#include <spdlog/sinks/stdout_color_sinks.h>
|
||||
#include <spdlog/sinks/basic_file_sink.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
namespace {
|
||||
constexpr char kDefaultLogPattern[] = "[%Y-%m-%d %H:%M:%S.%e] [%^%l%$] %v";
|
||||
}
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-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 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
|
||||
// Print description
|
||||
spdlog::info(
|
||||
">>> Tutorial description <<<\nThis tutorial shows how to change the logging behaviors of "
|
||||
"RDK client.\n");
|
||||
|
||||
// Suppress log messages from RDK client
|
||||
// =========================================================================================
|
||||
// Change log level to suppress certain type of log messages from RDK client. The log level
|
||||
// setting will apply globally to all following spdlog messages
|
||||
spdlog::set_level(spdlog::level::warn); ///< Suppress info messages
|
||||
// messages spdlog::set_level(spdlog::level::off); ///< Suppress all messages
|
||||
|
||||
// Instantiate RDK client, all info messages are suppressed
|
||||
try {
|
||||
rdk::Robot robot(robot_sn);
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
}
|
||||
|
||||
// Output all log messages to a file
|
||||
// =========================================================================================
|
||||
// Create a logger that outputs the messages to both the console and a log file
|
||||
auto console_sink = std::make_shared<spdlog::sinks::stdout_color_sink_mt>();
|
||||
auto file_sink = std::make_shared<spdlog::sinks::basic_file_sink_mt>("example.log", true);
|
||||
std::vector<spdlog::sink_ptr> sinks {console_sink, file_sink};
|
||||
// Since writing to file takes some time, so we use async logger to avoid blocking the program
|
||||
spdlog::init_thread_pool(10240, 1);
|
||||
auto tp = spdlog::thread_pool();
|
||||
auto logger
|
||||
= std::make_shared<spdlog::async_logger>("global_logger", sinks.begin(), sinks.end(), tp);
|
||||
// Use default pattern for log messages
|
||||
logger->set_pattern(kDefaultLogPattern);
|
||||
|
||||
// Use the logger configured above as the default global logger
|
||||
spdlog::set_default_logger(logger);
|
||||
|
||||
// Allow all messages
|
||||
spdlog::set_level(spdlog::level::info);
|
||||
|
||||
// Instantiate RDK client again, all messages are printed to console and output to a log file
|
||||
try {
|
||||
rdk::Robot robot(robot_sn);
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
}
|
||||
|
||||
spdlog::warn("This message should also appear in the log file");
|
||||
spdlog::info("Program finished");
|
||||
|
||||
return 0;
|
||||
}
|
||||
+199
@@ -0,0 +1,199 @@
|
||||
/**
|
||||
* @example intermediate1_realtime_joint_position_control.cpp
|
||||
* This tutorial runs real-time joint position control to hold or sine-sweep all robot joints.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/scheduler.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
namespace {
|
||||
/** RT loop period [sec] */
|
||||
constexpr double kLoopPeriod = 0.001;
|
||||
|
||||
/** Sine-sweep trajectory amplitude and frequency */
|
||||
constexpr double kSineAmp = 0.035;
|
||||
constexpr double kSineFreq = 0.3;
|
||||
|
||||
/** Atomic signal to stop scheduler tasks */
|
||||
std::atomic<bool> g_stop_sched = {false};
|
||||
}
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
|
||||
std::cout << "Optional arguments: [--hold]" << std::endl;
|
||||
std::cout << " --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl;
|
||||
std::cout << std::endl;
|
||||
// clang-format on
|
||||
}
|
||||
|
||||
/** @brief Callback function for realtime periodic task */
|
||||
void PeriodicTask(rdk::Robot& robot, const std::string& motion_type,
|
||||
const std::map<rdk::JointGroup, std::vector<double>>& all_init_pos)
|
||||
{
|
||||
// Local periodic loop counter
|
||||
static unsigned int loop_counter = 0;
|
||||
|
||||
try {
|
||||
// Monitor fault on the connected robot
|
||||
if (robot.fault()) {
|
||||
throw std::runtime_error(
|
||||
"PeriodicTask: Fault occurred on the connected robot, exiting ...");
|
||||
}
|
||||
|
||||
std::map<rdk::JointGroup, rdk::RtJointPositionCmd> rt_cmds;
|
||||
|
||||
if ((motion_type != "hold") && (motion_type != "sine-sweep")) {
|
||||
throw std::invalid_argument(
|
||||
"PeriodicTask: Unknown motion type. Accepted motion types: hold, sine-sweep");
|
||||
}
|
||||
|
||||
for (const auto& [group, init_pos] : all_init_pos) {
|
||||
std::vector<double> target_pos(init_pos.size());
|
||||
std::vector<double> target_vel(init_pos.size());
|
||||
std::vector<double> target_acc(init_pos.size());
|
||||
|
||||
if (motion_type == "hold") {
|
||||
target_pos = init_pos;
|
||||
} else {
|
||||
for (size_t i = 0; i < target_pos.size(); ++i) {
|
||||
target_pos[i]
|
||||
= init_pos[i]
|
||||
+ kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod);
|
||||
}
|
||||
}
|
||||
rt_cmds[group] = rdk::RtJointPositionCmd(target_pos, target_vel, target_acc);
|
||||
}
|
||||
|
||||
// Send target joint position to RDK server
|
||||
robot.StreamJointPosition(rt_cmds);
|
||||
|
||||
loop_counter++;
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
g_stop_sched = true;
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
// Program Setup
|
||||
// =============================================================================================
|
||||
// Parse parameters
|
||||
if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
|
||||
// Print description
|
||||
spdlog::info(
|
||||
">>> Tutorial description <<<\nThis tutorial runs real-time joint position control to hold "
|
||||
"or sine-sweep all robot joints.\n");
|
||||
|
||||
// Type of motion specified by user
|
||||
std::string motion_type = "";
|
||||
if (rdk::utility::ProgramArgsExist(argc, argv, "--hold")) {
|
||||
spdlog::info("Robot holding current pose");
|
||||
motion_type = "hold";
|
||||
} else {
|
||||
spdlog::info("Robot running joint sine-sweep");
|
||||
motion_type = "sine-sweep";
|
||||
}
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
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");
|
||||
}
|
||||
|
||||
// Servo on the robot, make sure the E-stop is released
|
||||
spdlog::info("Servo on the robot ...");
|
||||
robot.ServoOn();
|
||||
|
||||
// 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");
|
||||
|
||||
// Move robot to home pose
|
||||
spdlog::info("Moving to home pose");
|
||||
robot.Home();
|
||||
|
||||
// Real-time Joint Position Control
|
||||
// =========================================================================================
|
||||
// Direct joint control can be executed by single-arm joint groups and the external axis
|
||||
auto exe_groups = robot.info().single_arm_groups;
|
||||
if (exe_groups.empty()) {
|
||||
throw std::runtime_error("No single-arm joint group found on the connected robot");
|
||||
}
|
||||
// The external axis joint group (if it exists) also supports direct joint control
|
||||
if (robot.info().all_groups.contains(rdk::JointGroup::EXT_AXIS)) {
|
||||
exe_groups.emplace(rdk::JointGroup::EXT_AXIS,
|
||||
robot.info().all_groups.at(rdk::JointGroup::EXT_AXIS));
|
||||
}
|
||||
|
||||
// Switch to real-time joint position control mode
|
||||
robot.SwitchMode(rdk::Mode::RT_JOINT_POSITION);
|
||||
|
||||
// Set initial joint positions
|
||||
std::map<rdk::JointGroup, std::vector<double>> all_init_pos;
|
||||
const auto robot_states = robot.states();
|
||||
for (const auto& [group, _] : exe_groups) {
|
||||
all_init_pos[group] = robot_states.at(group).q;
|
||||
spdlog::info("[{}] Initial joint positions: {}", rdk::kJointGroupNames.at(group),
|
||||
rdk::utility::Vec2Str(all_init_pos.at(group)));
|
||||
}
|
||||
|
||||
// Create real-time scheduler to run periodic tasks
|
||||
rdk::Scheduler scheduler;
|
||||
// Add periodic task with 1ms interval and highest applicable priority
|
||||
scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type),
|
||||
std::cref(all_init_pos)),
|
||||
"HP periodic", 1, scheduler.max_priority());
|
||||
// Start all added tasks
|
||||
scheduler.Start();
|
||||
|
||||
// Block and wait for signal to stop scheduler tasks
|
||||
while (!g_stop_sched) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
// Received signal to stop scheduler tasks
|
||||
scheduler.Stop();
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
+226
@@ -0,0 +1,226 @@
|
||||
/**
|
||||
* @example intermediate2_realtime_joint_impedance_control.cpp
|
||||
* This tutorial runs real-time joint impedance control to hold or sine-sweep all robot joints.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/scheduler.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
namespace {
|
||||
/** RT loop period [sec] */
|
||||
constexpr double kLoopPeriod = 0.001;
|
||||
|
||||
/** Sine-sweep trajectory amplitude and frequency */
|
||||
constexpr double kSineAmp = 0.035;
|
||||
constexpr double kSineFreq = 0.3;
|
||||
|
||||
/** Atomic signal to stop scheduler tasks */
|
||||
std::atomic<bool> g_stop_sched = {false};
|
||||
}
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
|
||||
std::cout << "Optional arguments: [--hold]" << std::endl;
|
||||
std::cout << " --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl;
|
||||
std::cout << std::endl;
|
||||
// clang-format on
|
||||
}
|
||||
|
||||
/** @brief Callback function for realtime periodic task */
|
||||
void PeriodicTask(rdk::Robot& robot, const std::string& motion_type,
|
||||
const std::map<rdk::JointGroup, std::string>& single_arm_groups,
|
||||
const std::map<rdk::JointGroup, std::vector<double>>& all_init_pos)
|
||||
{
|
||||
// Local periodic loop counter
|
||||
static unsigned int loop_counter = 0;
|
||||
|
||||
try {
|
||||
// Monitor fault on the connected robot
|
||||
if (robot.fault()) {
|
||||
throw std::runtime_error(
|
||||
"PeriodicTask: Fault occurred on the connected robot, exiting ...");
|
||||
}
|
||||
|
||||
std::map<rdk::JointGroup, rdk::RtJointPositionCmd> rt_cmds;
|
||||
|
||||
if ((motion_type != "hold") && (motion_type != "sine-sweep")) {
|
||||
throw std::invalid_argument(
|
||||
"PeriodicTask: Unknown motion type. Accepted motion types: hold, sine-sweep");
|
||||
}
|
||||
|
||||
for (const auto& [group, init_pos] : all_init_pos) {
|
||||
std::vector<double> target_pos(init_pos.size());
|
||||
std::vector<double> target_vel(init_pos.size());
|
||||
std::vector<double> target_acc(init_pos.size());
|
||||
|
||||
if (motion_type == "hold") {
|
||||
target_pos = init_pos;
|
||||
} else {
|
||||
for (size_t i = 0; i < target_pos.size(); ++i) {
|
||||
target_pos[i]
|
||||
= init_pos[i]
|
||||
+ kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod);
|
||||
}
|
||||
}
|
||||
|
||||
rt_cmds[group] = rdk::RtJointPositionCmd(target_pos, target_vel, target_acc);
|
||||
}
|
||||
|
||||
// Reduce stiffness to half of nominal values after 5 seconds
|
||||
if (loop_counter == 5000) {
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
auto new_Kq = robot.info().K_q_nom.at(group);
|
||||
for (auto& v : new_Kq) {
|
||||
v *= 0.5;
|
||||
}
|
||||
robot.SetJointImpedance(group, new_Kq);
|
||||
spdlog::info("[{}] Joint stiffness set to: [{}]", rdk::kJointGroupNames.at(group),
|
||||
rdk::utility::Vec2Str(new_Kq));
|
||||
}
|
||||
}
|
||||
|
||||
// Reset stiffness to nominal values after another 5 seconds
|
||||
if (loop_counter == 10000) {
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
const auto nominal_Kq = robot.info().K_q_nom.at(group);
|
||||
robot.SetJointImpedance(group, nominal_Kq);
|
||||
spdlog::info("[{}] Joint stiffness reset to nominal: [{}]",
|
||||
rdk::kJointGroupNames.at(group), rdk::utility::Vec2Str(nominal_Kq));
|
||||
}
|
||||
}
|
||||
|
||||
// Send commands
|
||||
robot.StreamJointPosition(rt_cmds);
|
||||
|
||||
// Increment loop counter
|
||||
loop_counter++;
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
g_stop_sched = true;
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
// Program Setup
|
||||
// =============================================================================================
|
||||
// Parse parameters
|
||||
if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
|
||||
// Print description
|
||||
spdlog::info(
|
||||
">>> Tutorial description <<<\nThis tutorial runs real-time joint impedance control to "
|
||||
"hold or sine-sweep all robot joints.\n");
|
||||
|
||||
// Type of motion specified by user
|
||||
std::string motion_type = "";
|
||||
if (rdk::utility::ProgramArgsExist(argc, argv, "--hold")) {
|
||||
spdlog::info("Robot holding current pose");
|
||||
motion_type = "hold";
|
||||
} else {
|
||||
spdlog::info("Robot running joint sine-sweep");
|
||||
motion_type = "sine-sweep";
|
||||
}
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
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");
|
||||
}
|
||||
|
||||
// Servo on the robot, make sure the E-stop is released
|
||||
spdlog::info("Servo on the robot ...");
|
||||
robot.ServoOn();
|
||||
|
||||
// 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");
|
||||
|
||||
// Move robot to home pose
|
||||
spdlog::info("Moving to home pose");
|
||||
robot.Home();
|
||||
|
||||
// Real-time Joint Impedance Control
|
||||
// =========================================================================================
|
||||
// Direct joint control can be executed by single-arm joint groups and the external axis
|
||||
const auto& single_arm_groups = robot.info().single_arm_groups;
|
||||
if (single_arm_groups.empty()) {
|
||||
throw std::runtime_error("No single-arm joint group found on the connected robot");
|
||||
}
|
||||
// The external axis joint group (if it exists) also supports direct joint control
|
||||
auto exe_groups = single_arm_groups;
|
||||
if (robot.info().all_groups.contains(rdk::JointGroup::EXT_AXIS)) {
|
||||
exe_groups.emplace(
|
||||
rdk::JointGroup::EXT_AXIS, robot.info().all_groups.at(rdk::JointGroup::EXT_AXIS));
|
||||
}
|
||||
|
||||
// Switch to real-time joint impedance control mode
|
||||
robot.SwitchMode(rdk::Mode::RT_JOINT_IMPEDANCE);
|
||||
|
||||
// Set initial joint positions
|
||||
std::map<rdk::JointGroup, std::vector<double>> all_init_pos;
|
||||
const auto robot_states = robot.states();
|
||||
for (const auto& [group, _] : exe_groups) {
|
||||
all_init_pos[group] = robot_states.at(group).q;
|
||||
spdlog::info("[{}] Initial joint positions: {}", rdk::kJointGroupNames.at(group),
|
||||
rdk::utility::Vec2Str(all_init_pos.at(group)));
|
||||
}
|
||||
|
||||
// Create real-time scheduler to run periodic tasks
|
||||
rdk::Scheduler scheduler;
|
||||
// Add periodic task with 1ms interval and highest applicable priority
|
||||
scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type),
|
||||
std::cref(single_arm_groups), std::cref(all_init_pos)),
|
||||
"HP periodic", 1, scheduler.max_priority());
|
||||
// Start all added tasks
|
||||
scheduler.Start();
|
||||
|
||||
// Block and wait for signal to stop scheduler tasks
|
||||
while (!g_stop_sched) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
// Received signal to stop scheduler tasks
|
||||
scheduler.Stop();
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
+220
@@ -0,0 +1,220 @@
|
||||
/**
|
||||
* @example intermediate3_realtime_joint_torque_control.cpp
|
||||
* This tutorial runs real-time joint torque control to hold or sine-sweep all robot joints. An
|
||||
* outer position loop is used to generate joint torque commands. This outer position loop + inner
|
||||
* torque loop together is also known as an impedance controller.
|
||||
* @warning The impedance controller in this example is for demo purpose only and has no performance
|
||||
* guarantee. To use the robot's built-in high-performance joint impedance controller, please refer
|
||||
* to intermediate2_realtime_joint_impedance_control.cpp.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/scheduler.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
namespace {
|
||||
/** RT loop period [sec] */
|
||||
constexpr double kLoopPeriod = 0.001;
|
||||
|
||||
/** Outer position loop (impedance) gains, values are only for demo purpose */
|
||||
const std::vector<double> kImpedanceKp = {3000.0, 3000.0, 800.0, 800.0, 200.0, 200.0, 200.0};
|
||||
const std::vector<double> kImpedanceKd = {80.0, 80.0, 40.0, 40.0, 8.0, 8.0, 8.0};
|
||||
|
||||
/** Sine-sweep trajectory amplitude and frequency */
|
||||
constexpr double kSineAmp = 0.035;
|
||||
constexpr double kSineFreq = 0.3;
|
||||
|
||||
/** Atomic signal to stop scheduler tasks */
|
||||
std::atomic<bool> g_stop_sched = {false};
|
||||
}
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
|
||||
std::cout << "Optional arguments: [--hold]" << std::endl;
|
||||
std::cout << " --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl;
|
||||
std::cout << std::endl;
|
||||
// clang-format on
|
||||
}
|
||||
|
||||
/** @brief Callback function for realtime periodic task */
|
||||
void PeriodicTask(rdk::Robot& robot, const std::string& motion_type,
|
||||
const std::map<rdk::JointGroup, std::vector<double>>& all_init_pos)
|
||||
{
|
||||
// Local periodic loop counter
|
||||
static unsigned int loop_counter = 0;
|
||||
|
||||
try {
|
||||
// Monitor fault on the connected robot
|
||||
if (robot.fault()) {
|
||||
throw std::runtime_error(
|
||||
"PeriodicTask: Fault occurred on the connected robot, exiting ...");
|
||||
}
|
||||
|
||||
if ((motion_type != "hold") && (motion_type != "sine-sweep")) {
|
||||
throw std::invalid_argument(
|
||||
"PeriodicTask: Unknown motion type. Accepted motion types: hold, sine-sweep");
|
||||
}
|
||||
|
||||
std::map<rdk::JointGroup, rdk::RtJointTorqueCmd> rt_cmds;
|
||||
const auto all_states = robot.states();
|
||||
for (const auto& [group, init_pos] : all_init_pos) {
|
||||
std::vector<double> target_pos(init_pos.size());
|
||||
if (motion_type == "hold") {
|
||||
target_pos = init_pos;
|
||||
} else {
|
||||
for (size_t i = 0; i < target_pos.size(); ++i) {
|
||||
target_pos[i]
|
||||
= init_pos[i]
|
||||
+ kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod);
|
||||
}
|
||||
}
|
||||
|
||||
// Compute target joint torque using outer position loop (impedance)
|
||||
std::vector<double> target_torque(target_pos.size());
|
||||
// Set target torque for external axis to zero, as the demo impedance gains are defined
|
||||
// for only the arms
|
||||
if (group != rdk::JointGroup::EXT_AXIS) {
|
||||
const auto& states = all_states.at(group);
|
||||
for (size_t i = 0; i < target_torque.size(); ++i) {
|
||||
target_torque[i] = kImpedanceKp[i] * (target_pos[i] - states.q[i])
|
||||
- kImpedanceKd[i] * states.dtheta[i];
|
||||
}
|
||||
}
|
||||
|
||||
rt_cmds[group] = rdk::RtJointTorqueCmd(target_torque, true, true);
|
||||
}
|
||||
|
||||
// Send target joint torque to RDK server
|
||||
robot.StreamJointTorque(rt_cmds);
|
||||
|
||||
loop_counter++;
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
g_stop_sched = true;
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
// Program Setup
|
||||
// =============================================================================================
|
||||
// Parse parameters
|
||||
if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
|
||||
// Print description
|
||||
spdlog::info(
|
||||
">>> Tutorial description <<<\nThis tutorial runs real-time joint torque control to hold "
|
||||
"or sine-sweep all robot joints. An outer position loop is used to generate joint torque "
|
||||
"commands. This outer position loop + inner torque loop together is also known as an "
|
||||
"impedance controller.\n");
|
||||
|
||||
// Type of motion specified by user
|
||||
std::string motion_type = "";
|
||||
if (rdk::utility::ProgramArgsExist(argc, argv, "--hold")) {
|
||||
spdlog::info("Robot holding current pose");
|
||||
motion_type = "hold";
|
||||
} else {
|
||||
spdlog::info("Robot running joint sine-sweep");
|
||||
motion_type = "sine-sweep";
|
||||
}
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
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");
|
||||
}
|
||||
|
||||
// Servo on the robot, make sure the E-stop is released
|
||||
spdlog::info("Servo on the robot ...");
|
||||
robot.ServoOn();
|
||||
|
||||
// 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");
|
||||
|
||||
// Move robot to home pose
|
||||
spdlog::info("Moving to home pose");
|
||||
robot.Home();
|
||||
|
||||
// Real-time Joint Torque Control
|
||||
// =========================================================================================
|
||||
// Direct joint control can be executed by single-arm joint groups and the external axis
|
||||
auto exe_groups = robot.info().single_arm_groups;
|
||||
if (exe_groups.empty()) {
|
||||
throw std::runtime_error("No single-arm joint group found on the connected robot");
|
||||
}
|
||||
// The external axis joint group (if it exists) also supports direct joint control
|
||||
if (robot.info().all_groups.contains(rdk::JointGroup::EXT_AXIS)) {
|
||||
exe_groups.emplace(
|
||||
rdk::JointGroup::EXT_AXIS, robot.info().all_groups.at(rdk::JointGroup::EXT_AXIS));
|
||||
}
|
||||
|
||||
// Switch to real-time joint torque control mode
|
||||
robot.SwitchMode(rdk::Mode::RT_JOINT_TORQUE);
|
||||
|
||||
// Set initial joint positions
|
||||
std::map<rdk::JointGroup, std::vector<double>> all_init_pos;
|
||||
const auto robot_states = robot.states();
|
||||
for (const auto& [group, _] : exe_groups) {
|
||||
all_init_pos[group] = robot_states.at(group).q;
|
||||
spdlog::info("[{}] Initial joint positions: {}", rdk::kJointGroupNames.at(group),
|
||||
rdk::utility::Vec2Str(all_init_pos.at(group)));
|
||||
}
|
||||
|
||||
// Create real-time scheduler to run periodic tasks
|
||||
rdk::Scheduler scheduler;
|
||||
// Add periodic task with 1ms interval and highest applicable priority
|
||||
scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type),
|
||||
std::cref(all_init_pos)),
|
||||
"HP periodic", 1, scheduler.max_priority());
|
||||
// Start all added tasks
|
||||
scheduler.Start();
|
||||
|
||||
// Block and wait for signal to stop scheduler tasks
|
||||
while (!g_stop_sched) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
// Received signal to stop scheduler tasks
|
||||
scheduler.Stop();
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,167 @@
|
||||
/**
|
||||
* @example intermediate4_realtime_joint_floating.cpp
|
||||
* This tutorial runs real-time joint floating with gentle velocity damping, gravity compensation,
|
||||
* and soft protection against position limits. This example is ideal for verifying the system's
|
||||
* whole-loop real-timeliness, accuracy of the robot dynamics model, and joint torque control
|
||||
* performance. If everything works well, all joints should float smoothly.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/scheduler.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
namespace {
|
||||
/** Joint velocity damping gains for floating */
|
||||
const std::vector<double> kFloatingDamping = {10.0, 10.0, 5.0, 5.0, 1.0, 1.0, 1.0};
|
||||
|
||||
/** Atomic signal to stop scheduler tasks */
|
||||
std::atomic<bool> g_stop_sched = {false};
|
||||
}
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
|
||||
std::cout << "Optional arguments: None" << std::endl;
|
||||
std::cout << std::endl;
|
||||
// clang-format on
|
||||
}
|
||||
|
||||
/** @brief Callback function for realtime periodic task */
|
||||
void PeriodicTask(rdk::Robot& robot, const std::map<rdk::JointGroup, std::string>& exe_groups)
|
||||
{
|
||||
try {
|
||||
// Monitor fault on the connected robot
|
||||
if (robot.fault()) {
|
||||
throw std::runtime_error(
|
||||
"PeriodicTask: Fault occurred on the connected robot, exiting ...");
|
||||
}
|
||||
|
||||
std::map<rdk::JointGroup, rdk::RtJointTorqueCmd> rt_cmds;
|
||||
const auto all_states = robot.states();
|
||||
for (const auto& [group, _] : exe_groups) {
|
||||
const auto& states = all_states.at(group);
|
||||
std::vector<double> target_torque(states.q.size());
|
||||
|
||||
// Don't add damping for external axis
|
||||
if (group != rdk::JointGroup::EXT_AXIS) {
|
||||
// Add some velocity damping
|
||||
for (size_t i = 0; i < target_torque.size(); ++i) {
|
||||
target_torque[i] += -kFloatingDamping[i] * states.dtheta[i];
|
||||
}
|
||||
}
|
||||
|
||||
rt_cmds[group] = rdk::RtJointTorqueCmd(target_torque, true, true);
|
||||
}
|
||||
|
||||
// Send target joint torque to RDK server, enable gravity compensation and joint limits soft
|
||||
// protection
|
||||
robot.StreamJointTorque(rt_cmds);
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
g_stop_sched = true;
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
// Program Setup
|
||||
// =============================================================================================
|
||||
// Parse parameters
|
||||
if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
|
||||
// Print description
|
||||
spdlog::info(
|
||||
">>> Tutorial description <<<\nThis tutorial runs real-time joint floating with gentle "
|
||||
"velocity damping, gravity compensation, and soft protection against position limits. This "
|
||||
"example is ideal for verifying the system's whole-loop real-timeliness, accuracy of the "
|
||||
"robot dynamics model, and joint torque control performance. If everything works well, all "
|
||||
"joints should float smoothly.\n");
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
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");
|
||||
}
|
||||
|
||||
// Servo on the robot, make sure the E-stop is released
|
||||
spdlog::info("Servo on the robot ...");
|
||||
robot.ServoOn();
|
||||
|
||||
// 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");
|
||||
|
||||
// Move robot to home pose
|
||||
spdlog::info("Moving to home pose");
|
||||
robot.Home();
|
||||
|
||||
// Real-time Joint Floating
|
||||
// =========================================================================================
|
||||
// Direct joint control can be executed by single-arm joint groups and the external axis
|
||||
auto exe_groups = robot.info().single_arm_groups;
|
||||
if (exe_groups.empty()) {
|
||||
throw std::runtime_error("No single-arm joint group found on the connected robot");
|
||||
}
|
||||
// The external axis joint group (if exists) also supports direct joint control
|
||||
if (robot.info().all_groups.contains(rdk::JointGroup::EXT_AXIS)) {
|
||||
exe_groups.emplace(
|
||||
rdk::JointGroup::EXT_AXIS, robot.info().all_groups.at(rdk::JointGroup::EXT_AXIS));
|
||||
}
|
||||
|
||||
// Switch to real-time joint torque control mode
|
||||
robot.SwitchMode(rdk::Mode::RT_JOINT_TORQUE);
|
||||
|
||||
// Create real-time scheduler to run periodic tasks
|
||||
rdk::Scheduler scheduler;
|
||||
// Add periodic task with 1ms interval and highest applicable priority
|
||||
scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::cref(exe_groups)),
|
||||
"HP periodic", 1, scheduler.max_priority());
|
||||
// Start all added tasks
|
||||
scheduler.Start();
|
||||
|
||||
// Block and wait for signal to stop scheduler tasks
|
||||
while (!g_stop_sched) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
// Received signal to stop scheduler tasks
|
||||
scheduler.Stop();
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
+346
@@ -0,0 +1,346 @@
|
||||
/**
|
||||
* @example intermediate5_realtime_cartesian_pure_motion_control.cpp
|
||||
* This tutorial runs real-time Cartesian-space pure motion control to hold or sine-sweep the robot
|
||||
* TCP. A simple collision detection is also included.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/scheduler.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <algorithm>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
namespace {
|
||||
/** RT loop frequency [Hz] */
|
||||
constexpr size_t kLoopFreq = 1000;
|
||||
|
||||
/** RT loop period [sec] */
|
||||
constexpr double kLoopPeriod = 0.001;
|
||||
|
||||
/** TCP sine-sweep amplitude [m] */
|
||||
constexpr double kSwingAmp = 0.1;
|
||||
|
||||
/** TCP sine-sweep frequency [Hz] */
|
||||
constexpr double kSwingFreq = 0.3;
|
||||
|
||||
/** External TCP force threshold for collision detection, value is only for demo purpose [N] */
|
||||
constexpr double kExtForceThreshold = 10.0;
|
||||
|
||||
/** External joint torque threshold for collision detection, value is only for demo purpose [Nm] */
|
||||
constexpr double kExtTorqueThreshold = 5.0;
|
||||
|
||||
/** Atomic signal to stop scheduler tasks */
|
||||
std::atomic<bool> g_stop_sched = {false};
|
||||
}
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
|
||||
std::cout << "Optional arguments: [--hold] [--collision]" << std::endl;
|
||||
std::cout << " --hold: robot holds current TCP pose, otherwise do a sine-sweep" << std::endl;
|
||||
std::cout << " --collision: enable collision detection, robot will stop upon collision" << std::endl;
|
||||
std::cout << std::endl;
|
||||
// clang-format on
|
||||
}
|
||||
|
||||
/** @brief Callback function for realtime periodic task */
|
||||
void PeriodicTask(rdk::Robot& robot,
|
||||
const std::map<rdk::JointGroup, std::string>& single_arm_groups,
|
||||
const std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>>& all_init_pose,
|
||||
const std::map<rdk::JointGroup, std::vector<double>>& all_init_q, bool enable_hold,
|
||||
bool enable_collision)
|
||||
{
|
||||
// Local periodic loop counter
|
||||
static uint64_t loop_counter = 0;
|
||||
|
||||
try {
|
||||
// Monitor fault on the connected robot
|
||||
if (robot.fault()) {
|
||||
throw std::runtime_error(
|
||||
"PeriodicTask: Fault occurred on the connected robot, exiting ...");
|
||||
}
|
||||
|
||||
std::map<rdk::JointGroup, rdk::RtCartesianCmd> rt_cmds;
|
||||
for (const auto& [group, init_pose] : all_init_pose) {
|
||||
auto target_pose = init_pose;
|
||||
|
||||
// Sine-sweep TCP along Y axis
|
||||
if (!enable_hold) {
|
||||
target_pose[1]
|
||||
= init_pose[1]
|
||||
+ kSwingAmp * sin(2 * M_PI * kSwingFreq * loop_counter * kLoopPeriod);
|
||||
}
|
||||
// Otherwise robot TCP will hold at initial pose
|
||||
|
||||
rt_cmds[group] = rdk::RtCartesianCmd(target_pose);
|
||||
}
|
||||
|
||||
// Send command. Calling this method with only target pose input results in pure motion
|
||||
// control
|
||||
robot.StreamCartesianMotionForce(rt_cmds);
|
||||
|
||||
// Do the following operations in sequence for every 20 seconds
|
||||
switch (loop_counter % (20 * kLoopFreq)) {
|
||||
// Online change reference joint positions at 3 seconds
|
||||
case (3 * kLoopFreq): {
|
||||
const std::vector<double> ref_q
|
||||
= {0.938, -1.108, -1.254, 1.464, 1.073, 0.278, -0.658};
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
robot.SetNullSpacePosture(group, ref_q);
|
||||
}
|
||||
spdlog::info("Reference joint positions updated for all groups");
|
||||
} break;
|
||||
// Online change stiffness to half of nominal at 6 seconds
|
||||
case (6 * kLoopFreq): {
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
auto new_K = robot.info().K_x_nom.at(group);
|
||||
for (auto& v : new_K) {
|
||||
v *= 0.5;
|
||||
}
|
||||
robot.SetCartesianImpedance(group, new_K);
|
||||
spdlog::info("[{}] Cartesian stiffness set to: [{}]",
|
||||
rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(new_K));
|
||||
}
|
||||
} break;
|
||||
// Online change to another reference joint positions at 9 seconds
|
||||
case (9 * kLoopFreq): {
|
||||
const std::vector<double> ref_q
|
||||
= {-0.938, -1.108, 1.254, 1.464, -1.073, 0.278, 0.658};
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
robot.SetNullSpacePosture(group, ref_q);
|
||||
}
|
||||
spdlog::info("Reference joint positions updated for all groups");
|
||||
} break;
|
||||
// Online reset stiffness to nominal at 12 seconds
|
||||
case (12 * kLoopFreq): {
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
const auto nominal_K = robot.info().K_x_nom.at(group);
|
||||
robot.SetCartesianImpedance(group, nominal_K);
|
||||
spdlog::info("[{}] Cartesian stiffness reset to nominal: [{}]",
|
||||
rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(nominal_K));
|
||||
}
|
||||
} break;
|
||||
// Online reset reference joint positions to nominal at 14 seconds
|
||||
case (14 * kLoopFreq): {
|
||||
for (const auto& [group, init_q] : all_init_q) {
|
||||
robot.SetNullSpacePosture(group, init_q);
|
||||
spdlog::info("[{}] Reference joint positions reset to initial: [{}]",
|
||||
rdk::kJointGroupNames.at(group), rdk::utility::Vec2Str(init_q));
|
||||
}
|
||||
} break;
|
||||
// Online enable max contact wrench regulation at 16 seconds
|
||||
case (16 * kLoopFreq): {
|
||||
std::array<double, rdk::kCartDoF> max_wrench = {10.0, 10.0, 10.0, 2.0, 2.0, 2.0};
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
robot.SetMaxContactWrench(group, max_wrench);
|
||||
spdlog::info("[{}] Max contact wrench set to: {}",
|
||||
rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(max_wrench));
|
||||
}
|
||||
} break;
|
||||
// Disable max contact wrench regulation at 19 seconds
|
||||
case (19 * kLoopFreq): {
|
||||
std::array<double, rdk::kCartDoF> inf;
|
||||
inf.fill(std::numeric_limits<double>::infinity());
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
robot.SetMaxContactWrench(group, inf);
|
||||
spdlog::info("[{}] Max contact wrench regulation is disabled",
|
||||
rdk::kJointGroupNames.at(group));
|
||||
}
|
||||
} break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// Simple collision detection: stop robot if collision is detected from either end-effector
|
||||
// or robot body
|
||||
if (enable_collision) {
|
||||
for (const auto& [group, states] : robot.states()) {
|
||||
bool collision_detected = false;
|
||||
Eigen::Vector3d ext_force
|
||||
= {states.tcp_wrench[0], states.tcp_wrench[1], states.tcp_wrench[2]};
|
||||
if (ext_force.norm() > kExtForceThreshold) {
|
||||
collision_detected = true;
|
||||
}
|
||||
for (const auto& v : states.tau_ext) {
|
||||
if (fabs(v) > kExtTorqueThreshold) {
|
||||
collision_detected = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (collision_detected) {
|
||||
robot.Stop();
|
||||
spdlog::warn("[{}] Collision detected, stopping robot and exit program ...",
|
||||
rdk::kJointGroupNames.at(group));
|
||||
g_stop_sched = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Increment loop counter
|
||||
loop_counter++;
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
g_stop_sched = true;
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
// Program Setup
|
||||
// =============================================================================================
|
||||
// Parse parameters
|
||||
if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
|
||||
// Print description
|
||||
spdlog::info(
|
||||
">>> Tutorial description <<<\nThis tutorial runs real-time Cartesian-space pure motion "
|
||||
"control to hold or sine-sweep the robot TCP. A simple collision detection is also "
|
||||
"included.\n");
|
||||
|
||||
// Type of motion specified by user
|
||||
bool enable_hold = false;
|
||||
if (rdk::utility::ProgramArgsExist(argc, argv, "--hold")) {
|
||||
spdlog::info("Robot holding current TCP pose");
|
||||
enable_hold = true;
|
||||
} else {
|
||||
spdlog::info("Robot running TCP sine-sweep");
|
||||
}
|
||||
|
||||
// Whether to enable collision detection
|
||||
bool enable_collision = false;
|
||||
if (rdk::utility::ProgramArgsExist(argc, argv, "--collision")) {
|
||||
spdlog::info("Collision detection enabled");
|
||||
enable_collision = true;
|
||||
} else {
|
||||
spdlog::info("Collision detection disabled");
|
||||
}
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
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");
|
||||
}
|
||||
|
||||
// Servo on the robot, make sure the E-stop is released
|
||||
spdlog::info("Servo on the robot ...");
|
||||
robot.ServoOn();
|
||||
|
||||
// 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");
|
||||
|
||||
// Move robot to home pose
|
||||
spdlog::info("Moving to home pose");
|
||||
robot.Home();
|
||||
|
||||
// Zero Force-torque Sensor
|
||||
// =========================================================================================
|
||||
// Direct Cartesian control can only be executed by single-arm joint groups
|
||||
const auto& single_arm_groups = robot.info().single_arm_groups;
|
||||
if (single_arm_groups.empty()) {
|
||||
throw std::runtime_error("No single-arm joint group found on the connected robot");
|
||||
}
|
||||
|
||||
robot.SwitchMode(rdk::Mode::NRT_PRIMITIVE_EXECUTION);
|
||||
// IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement
|
||||
std::map<rdk::JointGroup, rdk::PrimitiveArgs> pt_args;
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
pt_args[group] = rdk::PrimitiveArgs("ZeroFTSensor", {});
|
||||
}
|
||||
robot.ExecutePrimitive(pt_args);
|
||||
|
||||
// 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 to finish
|
||||
while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "terminated")) {
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
spdlog::info("Sensor zeroing complete");
|
||||
|
||||
// Configure Motion Control
|
||||
// =========================================================================================
|
||||
// The Cartesian motion force modes do pure motion control out of the box, thus nothing
|
||||
// needs to be explicitly configured
|
||||
|
||||
// NOTE: motion control always uses robot world frame, while force control can use
|
||||
// either world or TCP frame as reference frame
|
||||
|
||||
// Start Pure Motion Control
|
||||
// =========================================================================================
|
||||
// Switch to real-time mode for continuous motion control
|
||||
robot.SwitchMode(rdk::Mode::RT_CARTESIAN_MOTION_FORCE);
|
||||
|
||||
// Set all Cartesian axis(s) to motion control
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
robot.SetForceControlAxis(
|
||||
group, std::array<bool, rdk::kCartDoF> {false, false, false, false, false, false});
|
||||
}
|
||||
|
||||
// Save initial poses and joint positions
|
||||
std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> all_init_pose;
|
||||
std::map<rdk::JointGroup, std::vector<double>> all_init_q;
|
||||
const auto robot_states = robot.states();
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
all_init_pose[group] = robot_states.at(group).tcp_pose;
|
||||
all_init_q[group] = robot_states.at(group).q;
|
||||
}
|
||||
|
||||
// Create real-time scheduler to run periodic tasks
|
||||
rdk::Scheduler scheduler;
|
||||
// Add periodic task with 1ms interval and highest applicable priority
|
||||
scheduler.AddTask(
|
||||
std::bind(PeriodicTask, std::ref(robot), std::cref(single_arm_groups),
|
||||
std::cref(all_init_pose), std::cref(all_init_q), enable_hold, enable_collision),
|
||||
"HP periodic", 1, scheduler.max_priority());
|
||||
// Start all added tasks
|
||||
scheduler.Start();
|
||||
|
||||
// Block and wait for signal to stop scheduler tasks
|
||||
while (!g_stop_sched) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
// Received signal to stop scheduler tasks
|
||||
scheduler.Stop();
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
+335
@@ -0,0 +1,335 @@
|
||||
/**
|
||||
* @example intermediate6_realtime_cartesian_motion_force_control.cpp
|
||||
* This tutorial runs real-time Cartesian-space unified motion-force control. The Z axis of the
|
||||
* chosen reference frame will be activated for explicit force control, while the rest axes in the
|
||||
* same reference frame will stay motion controlled.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/scheduler.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <algorithm>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
namespace {
|
||||
/** RT loop period [sec] */
|
||||
constexpr double kLoopPeriod = 0.001;
|
||||
|
||||
/** TCP sine-sweep amplitude [m] */
|
||||
constexpr double kSwingAmp = 0.1;
|
||||
|
||||
/** TCP sine-sweep frequency [Hz] */
|
||||
constexpr double kSwingFreq = 0.3;
|
||||
|
||||
/** Pressing force to apply during the unified motion-force control [N] */
|
||||
constexpr double kPressingForce = 5.0;
|
||||
|
||||
/** Cartesian linear velocity used to search for contact [m/s] */
|
||||
constexpr double kSearchVelocity = 0.02;
|
||||
|
||||
/** Maximum distance to travel when searching for contact [m] */
|
||||
constexpr double kSearchDistance = 1.0;
|
||||
|
||||
/** Maximum contact wrench during contact search for soft contact */
|
||||
const std::array<double, rdk::kCartDoF> kMaxWrenchForContactSearch
|
||||
= {10.0, 10.0, 10.0, 3.0, 3.0, 3.0};
|
||||
|
||||
/** Atomic signal to stop scheduler tasks */
|
||||
std::atomic<bool> g_stop_sched = {false};
|
||||
}
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
|
||||
std::cout << "Optional arguments: [--TCP] [--polish]" << std::endl;
|
||||
std::cout << " --TCP: use TCP frame as reference frame for force control, otherwise use world frame" << std::endl;
|
||||
std::cout << " --polish: run a simple polish motion along XY plane in world frame, otherwise hold robot motion in non-force-control axes"
|
||||
<< std::endl
|
||||
<< std::endl;
|
||||
// clang-format on
|
||||
}
|
||||
|
||||
/** Callback function for realtime periodic task */
|
||||
void PeriodicTask(rdk::Robot& robot,
|
||||
const std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>>& all_init_pose,
|
||||
rdk::CoordType force_ctrl_frame, bool enable_polish)
|
||||
{
|
||||
// Local periodic loop counter
|
||||
static uint64_t loop_counter = 0;
|
||||
|
||||
try {
|
||||
// Monitor fault on the connected robot
|
||||
if (robot.fault()) {
|
||||
throw std::runtime_error(
|
||||
"PeriodicTask: Fault occurred on the connected robot, exiting ...");
|
||||
}
|
||||
|
||||
// Set Fz according to reference frame to achieve a "pressing down" behavior
|
||||
double Fz = 0.0;
|
||||
if (force_ctrl_frame == rdk::CoordType::WORLD) {
|
||||
Fz = kPressingForce;
|
||||
} else if (force_ctrl_frame == rdk::CoordType::TCP) {
|
||||
Fz = -kPressingForce;
|
||||
}
|
||||
std::array<double, rdk::kCartDoF> target_wrench = {0.0, 0.0, Fz, 0.0, 0.0, 0.0};
|
||||
|
||||
std::map<rdk::JointGroup, rdk::RtCartesianCmd> rt_cmds;
|
||||
for (const auto& [group, init_pose] : all_init_pose) {
|
||||
auto target_pose = init_pose;
|
||||
|
||||
// Apply constant force along Z axis of chosen reference frame, and do a simple polish
|
||||
// motion along XY plane in robot world frame
|
||||
if (enable_polish) {
|
||||
// Create motion command to sine-sweep along Y direction
|
||||
target_pose[1]
|
||||
= init_pose[1]
|
||||
+ kSwingAmp * sin(2 * M_PI * kSwingFreq * loop_counter * kLoopPeriod);
|
||||
}
|
||||
|
||||
rt_cmds[group] = rdk::RtCartesianCmd(target_pose, target_wrench);
|
||||
}
|
||||
|
||||
// Command target pose and target wrench
|
||||
robot.StreamCartesianMotionForce(rt_cmds);
|
||||
|
||||
// Increment loop counter
|
||||
loop_counter++;
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
g_stop_sched = true;
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
// Program Setup
|
||||
// =============================================================================================
|
||||
// Parse parameters
|
||||
if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
|
||||
// Print description
|
||||
spdlog::info(
|
||||
">>> Tutorial description <<<\nThis tutorial runs real-time Cartesian-space unified "
|
||||
"motion-force control. The Z axis of the chosen reference frame will be activated for "
|
||||
"explicit force control, while the rest axes in the same reference frame will stay motion "
|
||||
"controlled.\n");
|
||||
|
||||
// The reference frame used for force control, see Robot::SetForceControlFrame()
|
||||
auto force_ctrl_frame = rdk::CoordType::WORLD;
|
||||
if (rdk::utility::ProgramArgsExist(argc, argv, "--TCP")) {
|
||||
spdlog::info("Reference frame used for force control: robot TCP frame");
|
||||
force_ctrl_frame = rdk::CoordType::TCP;
|
||||
} else {
|
||||
spdlog::info("Reference frame used for force control: robot world frame");
|
||||
}
|
||||
|
||||
// Whether to enable polish motion
|
||||
bool enable_polish = false;
|
||||
if (rdk::utility::ProgramArgsExist(argc, argv, "--polish")) {
|
||||
spdlog::info("Robot will run a polish motion along XY plane in robot world frame");
|
||||
enable_polish = true;
|
||||
} else {
|
||||
spdlog::info("Robot will hold its motion in all non-force-controlled axes");
|
||||
}
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
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");
|
||||
}
|
||||
|
||||
// Servo on the robot, make sure the E-stop is released
|
||||
spdlog::info("Servo on the robot ...");
|
||||
robot.ServoOn();
|
||||
|
||||
// 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");
|
||||
|
||||
// Move robot to home pose
|
||||
spdlog::info("Moving to home pose");
|
||||
robot.Home();
|
||||
|
||||
// Zero Force-torque Sensor
|
||||
// =========================================================================================
|
||||
// Direct Cartesian control can only be executed by single-arm joint groups
|
||||
const auto& single_arm_groups = robot.info().single_arm_groups;
|
||||
if (single_arm_groups.empty()) {
|
||||
throw std::runtime_error("No single-arm joint group found on the connected robot");
|
||||
}
|
||||
|
||||
robot.SwitchMode(rdk::Mode::NRT_PRIMITIVE_EXECUTION);
|
||||
// IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement
|
||||
std::map<rdk::JointGroup, rdk::PrimitiveArgs> pt_args;
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
pt_args[group] = rdk::PrimitiveArgs("ZeroFTSensor", {});
|
||||
}
|
||||
robot.ExecutePrimitive(pt_args);
|
||||
|
||||
// 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 to finish
|
||||
while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "terminated")) {
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
spdlog::info("Sensor zeroing complete");
|
||||
|
||||
// Search for Contact
|
||||
// =========================================================================================
|
||||
// NOTE: there are several ways to do contact search, such as using primitives, or real-time
|
||||
// and non-real-time direct motion controls, etc. Here we use non-real-time direct Cartesian
|
||||
// control for example.
|
||||
spdlog::info("Searching for contact ...");
|
||||
|
||||
// Set initial poses to current TCP poses
|
||||
std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> all_init_pose;
|
||||
const auto robot_states = robot.states();
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
all_init_pose[group] = robot_states.at(group).tcp_pose;
|
||||
spdlog::info("[{}] Initial TCP pose [position 3x1, rotation (quaternion) 4x1]: {}",
|
||||
rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(all_init_pose.at(group)));
|
||||
}
|
||||
|
||||
// Use non-real-time mode to make the robot go to a set point with its own motion generator
|
||||
robot.SwitchMode(rdk::Mode::NRT_CARTESIAN_MOTION_FORCE);
|
||||
|
||||
// Search for contact with max contact wrench set to a small value for making soft contact
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
robot.SetMaxContactWrench(group, kMaxWrenchForContactSearch);
|
||||
}
|
||||
|
||||
// Set target point along -Z direction and expect contact to happen during the travel
|
||||
std::map<rdk::JointGroup, rdk::NrtCartesianCmd> nrt_cmds;
|
||||
for (const auto& [group, init_pose] : all_init_pose) {
|
||||
auto target_pose = init_pose;
|
||||
target_pose[2] -= kSearchDistance;
|
||||
nrt_cmds[group] = rdk::NrtCartesianCmd(target_pose, {}, {}, kSearchVelocity);
|
||||
}
|
||||
|
||||
// Send target point to robot to start searching for contact and limit the velocity. Keep
|
||||
// target wrench 0 at this stage since we are not doing force control yet
|
||||
robot.SendCartesianMotionForce(nrt_cmds);
|
||||
|
||||
// Use a while loop to poll robot states and check if a contact is made
|
||||
bool is_contacted = false;
|
||||
while (!is_contacted) {
|
||||
// Compute norm of sensed external force applied on robot TCP
|
||||
for (const auto& [group, states] : robot.states()) {
|
||||
Eigen::Vector3d ext_force
|
||||
= {states.tcp_wrench[0], states.tcp_wrench[1], states.tcp_wrench[2]};
|
||||
|
||||
// Contact is considered to be made if sensed TCP force exceeds the threshold
|
||||
if (ext_force.norm() > kPressingForce) {
|
||||
is_contacted = true;
|
||||
spdlog::info(
|
||||
"[{}] Contact detected at robot TCP", rdk::kJointGroupNames.at(group));
|
||||
break;
|
||||
}
|
||||
}
|
||||
// Check at 1ms interval
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
|
||||
// Configure Force Control
|
||||
// =========================================================================================
|
||||
// Switch to real-time mode for continuous motion force control
|
||||
robot.SwitchMode(rdk::Mode::RT_CARTESIAN_MOTION_FORCE);
|
||||
|
||||
// Set force control reference frame based on program argument. See function doc for more
|
||||
// details
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
robot.SetForceControlFrame(group, force_ctrl_frame);
|
||||
}
|
||||
|
||||
// Set which Cartesian axis(s) to activate for force control. See function doc for more
|
||||
// details. Here we only active Z axis
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
robot.SetForceControlAxis(
|
||||
group, std::array<bool, rdk::kCartDoF> {false, false, true, false, false, false});
|
||||
}
|
||||
|
||||
// Uncomment the following line to enable passive force control, otherwise active force
|
||||
// control is used by default. See function doc for more details
|
||||
/*
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
robot.SetPassiveForceControl(group, true);
|
||||
}
|
||||
*/
|
||||
|
||||
// NOTE: motion control always uses robot world frame, while force control can use
|
||||
// either world or TCP frame as reference frame
|
||||
|
||||
// Start Unified Motion Force Control
|
||||
// =========================================================================================
|
||||
// Disable max contact wrench regulation. Need to do this AFTER the force control in Z axis
|
||||
// is activated (i.e. motion control disabled in Z axis) and the motion force control mode
|
||||
// is entered, this way the contact force along Z axis is explicitly regulated and will not
|
||||
// spike after the max contact wrench regulation for motion control is disabled
|
||||
std::array<double, rdk::kCartDoF> inf;
|
||||
inf.fill(std::numeric_limits<double>::infinity());
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
robot.SetMaxContactWrench(group, inf);
|
||||
}
|
||||
|
||||
// Update initial poses to current TCP poses
|
||||
for (auto& [group, pose] : all_init_pose) {
|
||||
pose = robot.states().at(group).tcp_pose;
|
||||
}
|
||||
|
||||
// Create real-time scheduler to run periodic tasks
|
||||
rdk::Scheduler scheduler;
|
||||
// Add periodic task with 1ms interval and highest applicable priority
|
||||
scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::cref(all_init_pose),
|
||||
std::ref(force_ctrl_frame), enable_polish),
|
||||
"HP periodic", 1, scheduler.max_priority());
|
||||
// Start all added tasks
|
||||
scheduler.Start();
|
||||
|
||||
// Block and wait for signal to stop scheduler tasks
|
||||
while (!g_stop_sched) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
// Received signal to stop scheduler tasks
|
||||
scheduler.Stop();
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,158 @@
|
||||
/**
|
||||
* @example intermediate7_robot_dynamics.cpp
|
||||
* This tutorial runs the integrated dynamics engine to obtain robot Jacobian, mass matrix, and
|
||||
* gravity torques. Also checks reachability of a Cartesian pose.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#include <flexiv/rdk/robot.hpp>
|
||||
#include <flexiv/rdk/model.hpp>
|
||||
#include <flexiv/rdk/utility.hpp>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
#include <mutex>
|
||||
|
||||
using namespace flexiv;
|
||||
|
||||
/** @brief Print program usage help */
|
||||
void PrintHelp()
|
||||
{
|
||||
// clang-format off
|
||||
std::cout << "Required arguments: [robot_sn]" << std::endl;
|
||||
std::cout << " robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-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 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
|
||||
PrintHelp();
|
||||
return 1;
|
||||
}
|
||||
// Serial number of the robot to connect to
|
||||
std::string robot_sn = argv[1];
|
||||
|
||||
// Print description
|
||||
spdlog::info(
|
||||
">>> Tutorial description <<<\nThis tutorial runs the integrated dynamics engine to obtain "
|
||||
"robot Jacobian, mass matrix, and gravity torques. Also checks reachability of a Cartesian "
|
||||
"pose.\n");
|
||||
|
||||
try {
|
||||
// RDK Initialization
|
||||
// =========================================================================================
|
||||
// Instantiate robot interface
|
||||
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");
|
||||
}
|
||||
|
||||
// Servo on the robot, make sure the E-stop is released
|
||||
spdlog::info("Servo on the robot ...");
|
||||
robot.ServoOn();
|
||||
|
||||
// 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");
|
||||
|
||||
// Move robot to home pose
|
||||
spdlog::info("Moving to home pose");
|
||||
robot.Home();
|
||||
|
||||
// Robot Dynamics
|
||||
// =========================================================================================
|
||||
// Initialize dynamics engine
|
||||
rdk::Model model(robot);
|
||||
|
||||
// Step dynamics engine 5 times
|
||||
for (size_t i = 0; i < 5; i++) {
|
||||
// Mark timer start point
|
||||
auto tic = std::chrono::high_resolution_clock::now();
|
||||
|
||||
// Update robot model in dynamics engine
|
||||
const auto robot_states = robot.states();
|
||||
model.Update(robot_states.at(rdk::JointGroup::ALL).q,
|
||||
robot_states.at(rdk::JointGroup::ALL).dq);
|
||||
|
||||
// Compute gravity vector
|
||||
auto g = model.g();
|
||||
|
||||
// Compute mass matrix
|
||||
auto M = model.M();
|
||||
|
||||
// Compute Jacobian
|
||||
auto J = model.J("flange");
|
||||
|
||||
// Mark timer end point and get loop time
|
||||
auto toc = std::chrono::high_resolution_clock::now();
|
||||
auto computation_time
|
||||
= std::chrono::duration_cast<std::chrono::microseconds>(toc - tic).count();
|
||||
|
||||
// Print time used to compute g, M, J
|
||||
spdlog::info("Computation time = {} us", computation_time);
|
||||
// Print gravity
|
||||
std::cout << "g = \n"
|
||||
<< std::fixed << std::setprecision(5) << g.transpose() << std::endl;
|
||||
// Print mass matrix
|
||||
std::cout << "M = \n" << std::fixed << std::setprecision(5) << M << std::endl;
|
||||
// Print Jacobian
|
||||
std::cout << "J = \n" << std::fixed << std::setprecision(5) << J << std::endl;
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
// Check IK feasibility for a nearby Cartesian pose on all available single-arm joint groups
|
||||
const auto& single_arm_groups = robot.info().single_arm_groups;
|
||||
if (single_arm_groups.empty()) {
|
||||
throw std::runtime_error("No single-arm joint group found on the connected robot");
|
||||
}
|
||||
|
||||
const auto robot_states = robot.states();
|
||||
std::map<rdk::JointGroup, rdk::IKParams> ik_params_by_group;
|
||||
for (const auto& [group, _] : single_arm_groups) {
|
||||
auto pose_to_check = robot_states.at(group).tcp_pose;
|
||||
pose_to_check[0] += 0.1;
|
||||
spdlog::info("[{}] Checking IK feasibility of Cartesian pose [{}]",
|
||||
rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(pose_to_check));
|
||||
|
||||
rdk::IKParams ik_params;
|
||||
ik_params.cartesian_pose = pose_to_check;
|
||||
ik_params.seed_q = robot_states.at(group).q;
|
||||
ik_params.free_orientation = false;
|
||||
ik_params_by_group[group] = ik_params;
|
||||
}
|
||||
|
||||
// Print result
|
||||
auto result = model.SolveConstrainedIK(ik_params_by_group);
|
||||
spdlog::info("IK result success = {}", result.success);
|
||||
for (const auto& [group, q] : result.solved_q) {
|
||||
spdlog::info(
|
||||
"[{}] solved_q = [{}]", rdk::kJointGroupNames.at(group), rdk::utility::Vec2Str(q));
|
||||
}
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error(e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user