Prerequisites
Scene Description
- Each scene example specifies the tested robot model. If using a different model, please refer to the "xCore Control System Manual" to ensure parameters are appropriate before running the example.
- This document describes C++ SDK scene usage examples
Two Control Modes of the Robot
Real-time Mode
Real-time Mode Concept
Concept
The real-time mode control interface allows users to establish a fast, direct low-level bidirectional connection with the robot through external devices. It provides the robot's current state and enables direct control via an externally connected workstation PC over Ethernet. Control commands are sent at a frequency of 1 kHz using the control interface.
User-side algorithm: Target position = (X+Δx, Y+Δy) User program executes this algorithm every millisecond Robot does not perform interpolation
Why Real-time Mode
- From a practical application perspective, customers have precision requirements for the robot's end trajectory and the robot's position in space, such as obstacle avoidance in complex environments. Conventional commands like MoveL/J cannot fully follow the desired trajectory
- The robot's target position cannot be predicted in advance
- Real-time planning is needed for research purposes, such as university projects and algorithm verification
Real-time Mode - Cartesian Impedance Control
For the AR5 7-axis robot, if you need to modify it, simply change rokae::xMateErProRobot to the corresponding model
This scene requires actual robot testing, impedance control cannot be tested virtually
#include <iostream>
#include <cmath>
#include <thread>
#include <atomic>
#include "rokae/robot.h"
#include "rokae/utility.h"
using namespace rokae;
/**
* @brief Main program - Robot Cartesian impedance control example
* This program demonstrates how to use the rokae robot SDK to implement Cartesian space impedance control,
* allowing the robot to move along a specific trajectory while maintaining a certain contact force
*/
int main() {
using namespace std;
try {
// ==================== Robot Initialization Configuration ====================
std::string ip = "192.168.21.10";
std::error_code ec;
// Create xMate 7-axis robot instance
// Parameter 1: Robot IP address, Parameter 2: Local IP
rokae::xMateErProRobot robot(ip, "192.168.21.150");
// Set real-time network tolerance to 50ms to ensure stability of real-time control
robot.setRtNetworkTolerance(50, ec);
// Set robot operation mode to automatic mode
robot.setOperateMode(rokae::OperateMode::automatic, ec);
// Set motion control mode to real-time command mode, allowing real-time control commands
robot.setMotionControlMode(MotionControlMode::RtCommand, ec);
// Power on the robot
robot.setPowerState(true, ec);
// Get real-time motion controller
auto rtCon = robot.getRtMotionController().lock();
// ==================== Initial Position Movement ====================
// Define target joint angles (radians), this needs to be set according to the current model's joint angles
std::array<double,7> q_drag_xm7p = {0, M_PI/6, 0, M_PI/3, 0, M_PI/2, 0};
// Joint space movement to initial position, speed ratio 0.5
rtCon->MoveJ(0.5, robot.jointPos(ec), q_drag_xm7p);
// ==================== Force Control Parameter Setting ====================
// Set force control coordinate system to tool coordinate system
// Transformation matrix from tool coordinate system to flange coordinate system
// This matrix indicates: Z-axis points to the outside of the flange, Y-axis points to the top of the flange
std::array<double, 16> toolToFlange = {
0, 0, 1, 0, // X-axis direction in base coordinate system
0, 1, 0, 0, // Y-axis direction in base coordinate system
-1, 0, 0, 0, // Z-axis direction in base coordinate system
0, 0, 0, 1 // Position coordinates (no offset)
};
rtCon->setFcCoor(toolToFlange, FrameType::tool, ec);
// Set Cartesian impedance coefficients
// Parameter order: {X-direction stiffness, Y-direction stiffness, Z-direction stiffness, Rx-direction stiffness, Ry-direction stiffness, Rz-direction stiffness}
// Units: stiffness (N/m), rotational stiffness (Nm/rad)
// Here, X and Y directions are set to high stiffness (1200 N/m), Z direction is set to 0 for force control
rtCon->setCartesianImpedance({1200, 1200, 0, 100, 100, 0}, ec);
// Set desired contact force/torque
// Parameter order: {Fx, Fy, Fz, Mx, My, Mz}
// Here, 3N force in X direction, 3N force in Z direction, 0 in other directions
rtCon->setCartesianImpedanceDesiredTorque({3, 0, 3, 0, 0, 0}, ec);
// ==================== Get Initial Pose ====================
// Get current pose of robot end in base coordinate system
std::array<double, 16> init_position {};
Utils::postureToTransArray(robot.posture(rokae::CoordinateType::flangeInBase, ec), init_position);
// ==================== Start Impedance Control ====================
// Start Cartesian impedance control mode
rtCon->startMove(RtControllerMode::cartesianImpedance);
// ==================== Real-time Control Loop Setting ====================
double time = 0; // Time counter
std::atomic<bool> stopManually {true}; // Thread-safe stop flag
/**
* @brief Real-time control callback function
* This function is called every 1ms to generate the robot's target trajectory
*/
std::function<CartesianPosition(void)> callback = [&, rtCon]()->CartesianPosition{
time += 0.001; // Increase by 1ms each call
// Trajectory parameters
constexpr double kRadius = 0.2; // Motion radius
double angle = M_PI / 4 * (1 - std::cos(M_PI / 2 * time)); // Smooth angle change
double delta_z = kRadius * (std::cos(angle) - 1); // Z-axis position change
// Create output position command
CartesianPosition output{};
output.pos = init_position; // Maintain initial position and posture
output.pos[7] += delta_z; // Only modify Z-axis position (8th element in transformation matrix)
// Motion end condition: stop after 40 seconds
if(time > 40){
std::cout << "Motion ended" << std::endl;
output.setFinished(); // Mark motion as completed
stopManually.store(false); // Notify main thread to stop
}
return output;
};
// ==================== Start Control Loop ====================
// Set control callback function
rtCon->setControlLoop(callback);
// Start real-time control loop, false indicates non-blocking mode
rtCon->startLoop(false);
// ==================== Wait for Motion Completion ====================
// Main thread waits until control loop sets stop flag
while(stopManually.load());
// Stop control loop
rtCon->stopLoop();
// Wait 2 seconds to ensure complete stop
std::this_thread::sleep_for(std::chrono::seconds(2));
} catch (const std::exception &e) {
// Exception handling
std::cerr << "Program exception: " << e.what() << std::endl;
}
return 0;
}
Real-time Mode - Cartesian Space S Planning
Program applicable model: xMateER7 Pro, AR5 model, requires actual robot testing
#include <iostream>
#include <cmath>
#include <thread>
#include "rokae/robot.h"
#include "../print_helper.hpp"
using namespace rokae;
/**
* @brief Robot Cartesian space linear motion example program
* This program demonstrates how to use the rokae SDK to control the robot for Cartesian space linear interpolation motion
* Main functions: Move from current position to drag posture, then move TCP tool along a straight line
*/
int main() {
using namespace std;
// Create robot object - xMateErProRobot is the model of ROKAE robot
rokae::xMateErProRobot robot;
try {
// Connect to robot
// Parameter 1: Robot IP address (192.168.0.160)
// Parameter 2: Local IP address (192.168.0.100)
// Note: Both devices need to be in the same network segment, and network connection should be normal
robot.connectToRobot("192.168.0.160", "192.168.0.100");
} catch(const rokae::Exception &e) {
std::cerr << "Connection failed " << e.what();
return 0; // Connection failed, exit program
}
std::error_code ec; // Used to receive error codes
// Set robot operation mode to automatic mode
// Automatic mode: Robot can execute programs and accept external control commands
robot.setOperateMode(rokae::OperateMode::automatic, ec);
// Set motion control mode to real-time command mode
// RtCommand mode: Allows control of robot motion through real-time callback functions
robot.setMotionControlMode(MotionControlMode::RtCommand, ec);
// Turn on robot power (servo on)
// After power on, the robot can move; after power off, the robot is in brake state
robot.setPowerState(true, ec);
try {
// Get real-time motion controller
// lock() converts weak_ptr to shared_ptr to ensure the controller object exists
auto rtCon = robot.getRtMotionController().lock();
// Start receiving robot state data
// Parameter 1: Update period (1ms) - Update robot state every 1 millisecond
// Parameter 2: Data fields to receive - Joint position and TCP pose
robot.startReceiveRobotState(std::chrono::milliseconds(1),
{RtSupportedFields::jointPos_m, RtSupportedFields::tcpPose_m});
// Define variables for storing position data
std::array<double, 16> init_pos{}, end_pos{}; // Initial and end poses (4x4 homogeneous transformation matrix)
std::array<double,7> jntPos{}; // 7 joint angle positions (radians)
Eigen::Quaterniond rot_cur; // Current posture as quaternion
Eigen::Matrix3d mat_cur; // Current posture as rotation matrix
double delta_s; // Path interpolation parameter, range [0,1]
// Update robot state data (get current state)
robot.updateRobotState(std::chrono::milliseconds(1));
// Get current joint position
robot.getStateData(RtSupportedFields::jointPos_m, jntPos);
// Define target drag posture (7 joint angles)
// This is a preset joint space posture, the robot will first move to this position
std::array<double,7> q_drag_xm7p = {0, M_PI/6, 0, M_PI/3, 0, M_PI/2, 0};
// Execute joint space motion: move from current position to drag posture
// Parameter 1: Speed ratio 0.5
// Parameter 2: Start joint position
// Parameter 3: Target joint position
rtCon->MoveJ(0.5, jntPos, q_drag_xm7p);
// Initialize flag and time variables
static bool init = true; // Flag to indicate if it's the first time entering the callback function
double time = 0; // Motion time accumulation
/**
* @brief Real-time control callback function
* This function will be called in real-time loop (approximately 1ms cycle) to generate Cartesian space motion trajectory
* Return value: CartesianPosition containing target pose control command
*
* Motion principle: All joints move according to sine wave law, but in different directions
* Joints 0,1,3,5: Forward motion (+delta_angle)
* Joints 2,4: Reverse motion (-delta_angle)
*/
std::function<CartesianPosition()> callback = [&, rtCon]() {
time += 0.001; // Time accumulation, increment by 1ms per cycle
// Initialization phase: Get start and end poses
if(init) {
// Read current TCP tool pose (4x4 homogeneous transformation matrix)
robot.getStateData(RtSupportedFields::tcpPose_m, init_pos);
// Set target pose: Move 0.2 meters down in Z-axis direction
end_pos = init_pos;
end_pos[11] -= 0.2; // The 11th element in the homogeneous matrix is Z-axis position
init = false; // Initialization completed
}
std::array<double, 16> pose_start = init_pos;
// Extract position vector from homogeneous transformation matrix
// Homogeneous matrix format: [R11, R12, R13, Tx, R21, R22, R23, Ty, R31, R32, R33, Tz, 0, 0, 0, 1]
Eigen::Vector3d pos_1(pose_start[3], pose_start[7], pose_start[11]); // Start position (Tx, Ty, Tz)
Eigen::Vector3d pos_2(end_pos[3], end_pos[7], end_pos[11]); // End position
// Calculate displacement vector and total distance
Eigen::Vector3d pos_delta = pos_2 - pos_1; // Displacement vector
double s = pos_delta.norm(); // Total distance
Eigen::Vector3d pos_delta_vector = pos_delta.normalized(); // Unit direction vector
// Create Cartesian motion planner
// Parameter 1: Maximum speed (0.05 m/s)
// Parameter 2: Total distance
CartMotionGenerator cart_s(0.05, s);
cart_s.calculateSynchronizedValues(0); // Calculate synchronized motion parameters
// Extract rotation matrices from homogeneous matrices
Eigen::Matrix3d mat_start, mat_end;
mat_start << pose_start[0], pose_start[1], pose_start[2], // First row: R11, R12, R13
pose_start[4], pose_start[5], pose_start[6], // Second row: R21, R22, R23
pose_start[8], pose_start[9], pose_start[10]; // Third row: R31, R32, R33
mat_end << end_pos[0], end_pos[1], end_pos[2],
end_pos[4], end_pos[5], end_pos[6],
end_pos[8], end_pos[9], end_pos[10];
// Convert rotation matrices to quaternions (for interpolation calculation)
Eigen::Quaterniond rot_start(mat_start); // Start posture quaternion
Eigen::Quaterniond rot_end(mat_end); // End posture quaternion
Eigen::Vector3d pos_cur; // Current target position
CartesianPosition cmd; // Control command
// Calculate desired motion trajectory
// calculateDesiredValues returns false when motion is not completed, continue calculating trajectory
if (!cart_s.calculateDesiredValues(time, &delta_s)) {
// Linear interpolation to calculate current position
// delta_s/s is the normalized progress parameter [0,1]
pos_cur = pos_1 + pos_delta * delta_s / s;
// Spherical linear interpolation (SLERP) to calculate current posture
// slerp provides smooth posture transition
Eigen::Quaterniond rot_cur = rot_start.slerp(delta_s / s, rot_end);
mat_cur = rot_cur.normalized().toRotationMatrix(); // Convert back to rotation matrix
// Build new homogeneous transformation matrix
std::array<double, 16> new_pose = {
{mat_cur(0, 0), mat_cur(0, 1), mat_cur(0, 2), pos_cur(0), // First row: R11,R12,R13,Tx
mat_cur(1, 0), mat_cur(1, 1), mat_cur(1, 2), pos_cur(1), // Second row: R21,R22,R23,Ty
mat_cur(2, 0), mat_cur(2, 1), mat_cur(2, 2), pos_cur(2), // Third row: R31,R32,R33,Tz
0, 0, 0, 1}}; // Fourth row: 0,0,0,1
cmd.pos = new_pose; // Set target pose of control command
} else {
// Motion completed, set completion flag
cmd.setFinished();
}
return cmd; // Return control command
};
// Set real-time control loop
// Parameter 1: Control callback function
// Parameter 2: Reserved parameter, usually set to 0
// Parameter 3: useStateDataInLoop=true indicates using robot state data in the loop
rtCon->setControlLoop(callback, 0, true);
// Set control mode to Cartesian space position control
// In this mode, the robot moves according to Cartesian space position commands
rtCon->startMove(RtControllerMode::cartesianPosition);
// Start real-time control loop
// Parameter: true indicates blocking execution until motion is completed
rtCon->startLoop(true);
print(std::cout, "Control ended"); // Motion completed, output prompt message
} catch (const std::exception &e) {
// Exception handling: Catch and print exception information
print(std::cerr, e.what());
}
return 0;
}
Real-time Mode - Point Following Function
This function requires the use of xMateModel library, please set the compilation option XCORE_USE_XMATE_MODEL=ON
#include <thread>
#include <atomic>
#include "rokae/robot.h"
#include "../print_helper.hpp"
// Global variable definitions
std::atomic_bool running = true; ///< Program running status flag, using atomic operation to ensure thread safety
std::ostream &os = std::cout; ///< Output stream reference, used for printing to console
/// Drag posture definitions for different robot models (joint space positions, unit: radians)
std::array<double, 7u> q_drag_xm7p = { 0, M_PI/6, 0, M_PI/3, 0, M_PI/2, 0 }; ///< xMateER Pro 7-axis robot drag posture
std::array<double, 6u> q_drag_er3 = { 0, M_PI/6, M_PI/3, 0, M_PI/2, 0 }; ///< xMateER 6-axis robot drag posture
std::array<double, 6u> q_drag_cr7 = { 0, M_PI/6, -M_PI_2, 0, -M_PI/3, 0 }; ///< CR series robot drag posture
std::array<double, 6u> q_drag_sr3 = { 0, M_PI/6, -M_PI_2, 0, -M_PI/3, 0 }; ///< SR series robot drag posture
// Forward declarations: Path point functions for different robot models
std::vector<std::array<double, 6>> points_xMateSR3();
std::vector<std::array<double, 6>> points_xMateER3();
std::vector<std::array<double, 6>> points_xMateCR();
std::vector<std::array<double, 7>> points_xMateERPro();
/**
* @brief 6-axis robot follow position control example
* @param robot Robot object reference
* @param start_jnt Start joint position
* @param points_list List of joint position points to follow
*
* Function: Control 6-axis robot to follow a predefined joint position sequence
* Motion mode: Joint space position following
*/
void example_followPosition6(rokae::xMateRobot& robot,
const std::array<double, 6>& start_jnt,
const std::vector<std::array<double, 6>>& points_list) {
// Get real-time motion controller
auto rtCon = robot.getRtMotionController().lock();
std::thread updater; // Update thread for periodically sending position commands
error_code ec; // Error code object
try {
// Step 1: Move the robot to the start position
// MoveJ: Joint space point-to-point motion, maximum speed 30%, from current position to start_jnt
rtCon->MoveJ(0.3, robot.jointPos(ec), start_jnt);
std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait 1 second to ensure motion completion
// Step 2: Initialize follow position controller
auto model = robot.model(); // Get robot model
rokae::FollowPosition follow_pose(robot, model); // Create follow position control object
print(os, "Start following");
// Step 3: Set desired end tool coordinate system transformation
// bMe_desire: Base to end desired transformation matrix
Eigen::Transform<double, 3, Eigen::Isometry> bMe_desire = Eigen::Transform<double, 3, Eigen::Isometry>::Identity(); // Set rotation: Quaternion(0.0, 0.0, 1.0, 0.0) corresponds to Euler angles A-180°, B-0°, C-180°
// Quaternion format: (x, y, z, w), here represents 180-degree rotation around Z-axis
bMe_desire.rotate(Eigen::Quaterniond(0.0, 0.0, 1.0, 0.0));
// Set translation: X=0.464m, Y=0.136m, Z=0.364m
bMe_desire.pretranslate(Eigen::Vector3d(0.464, 0.136, 0.364));
// Start follow control
follow_pose.start(bMe_desire);
// Step 4: Create update thread to periodically send position commands
updater = std::thread([&]() {
std::this_thread::sleep_for(std::chrono::seconds(2)); // Wait 2 seconds before starting
follow_pose.setScale(2); // Set motion scale factor to 2 (may be speed or acceleration scaling)
auto it = points_list.begin(); // Iterator pointing to the start of the path point list
while (running) { // Main loop until running flag is false
// Forward motion: from first point to last point
while (running) {
follow_pose.update(*it++); // Send current point position, move iterator forward
std::this_thread::sleep_for(std::chrono::milliseconds(600)); // Update every 600ms
if (it == points_list.end()) { // If reaching the end point
it--; // Move back to the last point
break;
}
}
// Reverse motion: from last point back to first point
while (running) {
follow_pose.update(*it--); // Send current point position, move iterator backward
std::this_thread::sleep_for(std::chrono::milliseconds(600));
if (it == points_list.begin()) { // If back to the starting point
break;
}
}
}
});
// Step 5: Create console input thread to listen for user input
std::thread consoleInput([] {
while (getchar() != 'q'); // Wait for user to press 'q' key
running = false; // Set running flag to false, stop all motion
});
consoleInput.detach(); // Detach thread to run independently
// Step 6: Main thread waits until running becomes false
while (running);
// Step 7: Clean up resources
follow_pose.stop(); // Stop follow control
updater.join(); // Wait for update thread to end
}
catch (const std::exception& e) {
// Exception handling
print(std::cerr, e.what());
if (updater.joinable()) {
running = false;
updater.join();
}
}
}
/**
* @brief 7-axis robot follow position control example
* @param robot 7-axis robot object reference
* @param start_jnt Start joint position (7 joints)
* @param points_list List of joint position points to follow (7 joints)
*
* Function: Similar to the 6-axis version, but designed for 7-axis robots
*/
void example_followPosition7(rokae::xMateErProRobot &robot,
const std::array<double, 7> &start_jnt,
const std::vector<std::array<double, 7>> &points_list) {
auto rtCon = robot.getRtMotionController().lock(); // Get motion controller
std::thread updater; // Update thread
error_code ec; // Error code
try {
// Move to start position
rtCon->MoveJ(0.3, robot.jointPos(ec), start_jnt);
std::this_thread::sleep_for(std::chrono::seconds(1));
// Initialize follow controller
auto model = robot.model();
rokae::FollowPosition follow_pose(robot, model);
print(os, "Start following");
// Set desired end pose
Eigen::Transform<double, 3, Eigen::Isometry> bMe_desire = Eigen::Transform<double, 3, Eigen::Isometry>::Identity();
bMe_desire.rotate(Eigen::Quaterniond(0.0, 0.0, 1.0, 0.0)); // Rotation
bMe_desire.pretranslate(Eigen::Vector3d(0.464, 0.136, 0.364)); // Translation
follow_pose.start(bMe_desire);
// Position update thread (same logic as 6-axis version)
updater = std::thread([&]() {
std::this_thread::sleep_for(std::chrono::seconds(2));
follow_pose.setScale(2);
auto it = points_list.begin();
while(running) {
// Forward motion
while (running) {
follow_pose.update(*it++);
std::this_thread::sleep_for(std::chrono::milliseconds (600));
if (it == points_list.end()) {
it--;
break;
}
}
// Reverse motion
while (running) {
follow_pose.update(*it--);
std::this_thread::sleep_for(std::chrono::milliseconds (600));
if (it == points_list.begin()) {
break;
}
}
}
});
// Console input monitoring
std::thread consoleInput([]{
while(getchar() != 'q'); // Press 'q' to stop
running = false;
});
consoleInput.detach();
// Wait for stop signal
while(running);
follow_pose.stop();
updater.join();
} catch (const std::exception &e) {
// Exception handling
print(std::cerr, e.what());
if(updater.joinable()) {
running = false;
updater.join();
}
}
}
/**
* @brief Main program
* Function: Automatically select the corresponding control program based on the connected robot model
*/
int main() {
using namespace rokae;
using namespace std;
using namespace rokae::RtSupportedFields;
// Network configuration
string remoteIP = "192.168.0.160"; // Robot IP address
string localIP = "192.168.0.100"; // Local IP address
error_code ec;
std::thread updater;
// Robot object declarations (support 6-axis and 7-axis)
xMateRobot robot; // 6-axis robot
xMateErProRobot robot_pro; // 7-axis robot
try {
// Connect to robot
robot.connectToRobot(remoteIP, localIP);
} catch (const std::exception &e) {
std::cerr << e.what();
return 0;
}
// Get robot information and set control parameters
std::string robot_name = robot.robotInfo(ec).type; // Robot model name
robot.setRtNetworkTolerance(20, ec); // Set network tolerance time to 20ms
robot.setMotionControlMode(rokae::MotionControlMode::RtCommand, ec); // Real-time command mode
robot.setOperateMode(rokae::OperateMode::automatic, ec); // Automatic operation mode
robot.setPowerState(true, ec); // Power on
try {
// Initialize real-time control
auto rtCon = robot.getRtMotionController().lock();
// Start receiving robot state, 1ms cycle, only receive joint position data
robot.startReceiveRobotState(std::chrono::milliseconds(1), {jointPos_m});
} catch (const std::exception &e) {
std::cerr << e.what();
return 0;
}
/**
* Select the corresponding control program according to the robot model
* Note: Different model robots have different motion ranges and structures, requiring adapted point data
*/
if(robot_name.find("CR7") != std::string::npos || robot_name.find("CR12") != std::string::npos ) {
// CR series robots (6-axis)
print(std::cout, "Run example for", robot_name);
example_followPosition6(robot, q_drag_cr7, points_xMateCR());
}
else if(robot_name.find("SR3") != std::string::npos || robot_name.find("SR4") != std::string::npos) {
// SR series robots (6-axis)
print(std::cout, "Run example for", robot_name);
example_followPosition6(robot, q_drag_sr3, points_xMateSR3());
}
else if(robot_name.find("xMate3") != std::string::npos ){
// xMateER3/7 series robots (6-axis)
print(std::cout, "Run example for", robot_name);
example_followPosition6(robot, q_drag_er3, points_xMateER3());
}
else if (robot_name.find("Pro")!=std::string::npos){
// xMateER Pro series robots (7-axis)
print(std::cout,"Run example for",robot_name);
example_followPosition7(robot_pro,q_drag_xm7p,points_xMateERPro());
print(std::cout, "Follow ended");
// 7-axis robot special handling: Set to idle mode and power off
robot_pro.setMotionControlMode(MotionControlMode::Idle, ec);
robot_pro.setPowerState(false, ec);
}
else {
// Unsupported model
print(std::cerr, "The points in the example program have not been used on this model");
}
return 0;
}
// Path point data for various robot models below
// These point data are obtained through teaching or calculation to ensure the robot moves within a safe range
/**
* @brief Joint angle path points for xMateCR7/12
* Each point contains 6 joint angles (radians)
*/
std::vector<std::array<double, 6>> points_xMateCR() {
return {
{0.0,0.52359877559829882,-1.5707963267948966,0.0,-1.0471975511965976,0.0},
{0.035487243764309395,0.51814607597827012,-1.5806637828906964,2.2607167760800269e-06,-1.0427832278773319,0.035483868678714996},
// ... more path points
{-7.7890503523341657e-07,0.51711158261127554,-1.5612157759668301,2.5489758866954378e-07,-1.0632642491857673,-9.773599037238323e-07}
};
}
/**
* @brief Joint angle path points for SR3 & SR4
*/
std::vector<std::array<double, 6>> points_xMateSR3() {
return {
{-0.017938136876709584,0.49435159910311133,-1.6278070272570087,0.10223539561771972,-1.0233324615672779,-0.071357800901066984},
{-0.032594083928095562,0.46634275144052351,-1.6842348590894223,0.20686587914942534,-1.0065436700890904,-0.14486251700897126},
// ... more path points
{-0.037080277744188789,0.3130583447962762,-1.9938135626505962,0.99639141933330022,-1.1477775002697621,-0.62289965112758638}
};
}
/**
* @brief Joint angle path points for xMateER3/7 Pro (7-axis)
*/
std::vector<std::array<double, (unsigned short)7>> points_xMateERPro() {
return {
{1.1984224905356572e-06,0.52361854956939269, 0, 1.0472093756318377,8.9881686790174287e-07,1.5708220928784431,4.4940843395087144e-06},
{0.053236563559073066,0.52595866921924528, 0, 1.043601404881831,4.7936899621426287e-06,1.5720861290003356,0.053240518353291834},
// ... more path points
{2.6365294791784457e-06,0.52364731170916556, 0, 1.0472302281831731,1.4980281131695715e-06,1.5708358747370843,1.2883041773258315e-05}
};
}
/**
* @brief Joint angle path points for xMateER3/7 (6-axis)
*/
std::vector<std::array<double, 6>> points_xMateER3() {
return {
{1.1984224905356572e-06,0.52361854956939269,1.0472093756318377,8.9881686790174287e-07,1.5708220928784431,4.4940843395087144e-06},
{0.053236563559073066,0.52595866921924528,1.043601404881831,4.7936899621426287e-06,1.5720861290003356,0.053240518353291834},
// ... more path points
{2.6365294791784457e-06,0.52364731170916556,1.0472302281831731,1.4980281131695715e-06,1.5708358747370843,1.2883041773258315e-05}
};
}
Real-time Mode - Joint Space Impedance Control
Program applicable model: xMateER7 Pro, implements joint space impedance control for the robot, making the robot compliant in joint space and able to respond compliantly to external torques.
#include <iostream>
#include <cmath>
#include <thread>
#include "rokae/robot.h"
#include "../print_helper.hpp"
using namespace rokae;
/**
* @brief Main program - Robot joint space impedance control example
* This program demonstrates how to use the rokae robot SDK to implement joint space impedance control,
* enabling compliant motion of the robot in joint space
*/
int main() {
using namespace std;
try {
// ==================== Robot Initialization Configuration ====================
std::string ip = "192.168.0.160"; // Robot IP address
std::error_code ec;
// Create xMateErProRobot robot instance
// Parameter 1: Robot IP address, Parameter 2: Local IP address
rokae::xMateErProRobot robot(ip, "192.168.0.100");
// Set robot operation mode to automatic mode
robot.setOperateMode(rokae::OperateMode::automatic, ec);
// Set motion control mode to real-time command mode
// This mode allows real-time control commands, suitable for advanced control strategies like impedance control
robot.setMotionControlMode(MotionControlMode::RtCommand, ec);
// Power on the robot
robot.setPowerState(true, ec);
// Get real-time motion controller
// lock() ensures safe access to the controller in a multi-threaded environment
auto rtCon = robot.getRtMotionController().lock();
// ==================== Robot State Data Reception Setup ====================
// Start robot state data reception, update every 1 millisecond
// Only receive joint position data to reduce network bandwidth usage
robot.startReceiveRobotState(std::chrono::milliseconds(1), {
RtSupportedFields::jointPos_m // Joint position information (unit: radians)
});
double time = 0; // Time counter for trajectory generation
// ==================== Get Current State and Plan Initial Motion ====================
std::array<double, 7> jntPos{}; // Array to store joint positions (7 joints)
// Get current joint position data
robot.getStateData(RtSupportedFields::jointPos_m, jntPos);
// Define target drag posture (joint angles, unit: radians)
// This is a preset joint angle configuration, each joint moves to a different angle
std::array<double,7> q_drag_xm7p = {
0, // Joint 1: 0 radians
M_PI/6, // Joint 2: 30 degrees
0, // Joint 3: 0 radians
M_PI/3, // Joint 4: 60 degrees
0, // Joint 5: 0 radians
M_PI/2, // Joint 6: 90 degrees
0 // Joint 7: 0 radians
};
// Joint space movement from current position to drag posture
// Parameter explanation:
// 0.5 - Speed ratio
// jntPos - Start joint position
// q_drag_xm7p - Target joint position
rtCon->MoveJ(0.5, jntPos, q_drag_xm7p);
// ==================== Real-time Control Callback Function Definition ====================
/**
* @brief Joint space impedance control callback function
* This Lambda function is called every 1ms to generate joint position commands
* Capture list explanation:
* [&jntPos] - Capture initial joint position by reference
* [rtCon] - Capture motion controller by value
* [&time] - Capture time variable by reference
*/
std::function<JointPosition(void)> callback = [&jntPos, rtCon, &time] {
time += 0.001; // Increase by 1ms each call
// Calculate joint angle increment
// Use cosine function to generate smooth angle change:
// - M_PI/20.0: Maximum angle change amplitude (9 degrees)
// - M_PI/4 * time: Angular frequency, controls change speed
double delta_angle = M_PI / 20.0 * (1 - std::cos(M_PI/4 * time));
// Create joint position command object, 7 joints
JointPosition cmd(7);
// Calculate target position for each joint
// All joints move with the same angle increment synchronously
for(unsigned i = 0; i < cmd.joints.size(); ++i) {
cmd.joints[i] = jntPos[i] + delta_angle;
}
// Motion end condition: stop after 60 seconds
if(time > 60) {
cmd.setFinished(); // Set completion flag, stop control loop
}
return cmd;
};
// ==================== Impedance Control Parameter Setting ====================
// Set joint space impedance coefficients (unit: Nm/rad)
// Larger impedance coefficients mean higher joint stiffness and stronger anti-interference ability
// Smaller impedance coefficients mean better joint compliance and easier to be affected by external forces
std::array<double, 7> impedance_gains = {
500, // Joint 1 stiffness: 500 Nm/rad
500, // Joint 2 stiffness: 500 Nm/rad
500, // Joint 3 stiffness: 500 Nm/rad
500, // Joint 4 stiffness: 500 Nm/rad
50, // Joint 5 stiffness: 50 Nm/rad (lower stiffness, more compliant)
50, // Joint 6 stiffness: 50 Nm/rad
50 // Joint 7 stiffness: 50 Nm/rad
};
rtCon->setJointImpedance(impedance_gains, ec);
// ==================== Start Real-time Control Loop ====================
// Set control callback function
rtCon->setControlLoop(callback);
// Update start position to current position (ensure starting impedance control from the correct position)
jntPos = robot.jointPos(ec);
// Start joint space impedance motion mode
rtCon->startMove(RtControllerMode::jointImpedance);
// Start blocking control loop
// Parameter true indicates blocking mode, program will wait here until control loop ends
rtCon->startLoop(true);
// Execute after control loop ends
print(std::cout, "Control ended");
} catch (const std::exception &e) {
// Exception handling: Output error information
std::cout << "Program exception: " << e.what();
}
return 0;
}
Real-time Mode - Joint Angle Control
Program applicable model: xMateER3
#include <iostream>
#include <cmath>
#include <thread>
#include "rokae/robot.h"
#include "../print_helper.hpp"
using namespace rokae;
using namespace std;
/**
* @brief Robot joint space sine wave motion example program
* Function: Control the robot to perform sine wave trajectory motion in joint space, all joints move synchronously
* Features: Real-time control, smooth trajectory, safe stop
*/
int main() {
// Create robot object - xMateRobot represents ROKAE 6-axis robot
rokae::xMateRobot robot;
std::error_code ec; // Error code object, used to receive function call results
// Connect to robot
try {
// Parameter 1: Robot IP address (192.168.0.160)
// Parameter 2: Local IP address (192.168.0.100)
// Note: Both devices need to be in the same network segment, network connection normal
robot.connectToRobot("192.168.0.160", "192.168.0.100");
} catch (const std::exception &e) {
// Connection failure handling
print(std::cerr, e.what());
return 0;
}
// Set robot working state (three steps)
robot.setOperateMode(rokae::OperateMode::automatic, ec); // Set to automatic mode, can execute programs
robot.setMotionControlMode(MotionControlMode::RtCommand, ec); // Set to real-time command mode, allowing high-frequency control
robot.setPowerState(true, ec); // Power on (turn on servo), robot can move
try {
// Get real-time motion controller
// lock() converts weak_ptr to shared_ptr to ensure the controller object exists
auto rtCon = robot.getRtMotionController().lock();
// Optional: Set to receive robot state data
// Parameter 1: Update period 1ms - Receive state every 1 millisecond
// Parameter 2: Received data fields - Only receive joint position information
robot.startReceiveRobotState(std::chrono::milliseconds(1), {RtSupportedFields::jointPos_m});
// Define and initialize motion control variables
double time = 0; // Motion time accumulation, unit: seconds
std::array<double, 6> jntPos{}; // Current joint position array, 6 elements corresponding to 6 joints
// Drag posture - a safe starting position
std::array<double, 6> q_drag_xm3 = {0, M_PI/6, M_PI/3, 0, M_PI/2, 0};
/**
* @brief Real-time control callback function
* This function will be called in real-time loop (approximately 1ms cycle) to generate joint space motion trajectory
* Return value: JointPosition containing 6 joint target positions control command
*
* Motion principle: All joints move according to sine wave law, but in different directions
* Joints 0,1,3,5: Forward motion (+delta_angle)
* Joints 2,4: Reverse motion (-delta_angle)
*/
std::function<JointPosition()> callback = [&, rtCon](){
time += 0.001; // Time accumulation, increase by 1ms each time (assuming 1ms call cycle)
// Calculate joint angle increment - Use cosine function to generate smooth S-curve trajectory
// Formula: Δθ = (π/20) * [1 - cos(π/2.5 * t)]
// - π/20: Amplitude coefficient, limiting maximum angle change
// - 1-cos(): Generate motion curve that starts smoothly accelerating
// - π/2.5: Frequency coefficient, controlling motion speed
double delta_angle = M_PI / 20.0 * (1 - std::cos(M_PI / 2.5 * time));
// Build control command - Apply angle increment to each joint
JointPosition cmd = {{
jntPos[0] + delta_angle, // Joint 0: Forward motion
jntPos[1] + delta_angle, // Joint 1: Forward motion
jntPos[2] - delta_angle, // Joint 2: Reverse motion
jntPos[3] + delta_angle, // Joint 3: Forward motion
jntPos[4] - delta_angle, // Joint 4: Reverse motion
jntPos[5] + delta_angle // Joint 5: Forward motion
}};
// Motion stop condition: End motion after 60 seconds
if(time > 60) {
cmd.setFinished(); // Set completion flag, stop real-time control loop
}
return cmd; // Return control command to robot
};
// Step 1: Move from current position to safe drag posture
// MoveJ: Joint space point-to-point motion
// Parameter 1: Speed ratio 0.3
// Parameter 2: Start position - Current joint position
// Parameter 3: Target position - Preset drag posture
rtCon->MoveJ(0.3, robot.jointPos(ec), q_drag_xm3);
// Step 2: Set real-time control loop
// Register callback function, robot will call this function at approximately 1ms cycle
rtCon->setControlLoop(callback);
// Step 3: Get current joint position as motion starting point
// Note: Must be obtained after setting callback to ensure using the latest position data
jntPos = robot.jointPos(ec);
// Step 4: Start joint space position control
// Set control mode to joint position control, in this mode the robot tracks joint angle commands
rtCon->startMove(RtControllerMode::jointPosition);
// Step 5: Start real-time control loop
// Parameter true indicates blocking execution, program will stop here until motion completes (60 seconds or exception)
rtCon->startLoop(true);
// Motion completion prompt
print(std::cout, "Control ended");
// Step 6: Safe exit - Restore robot to safe state
robot.setMotionControlMode(MotionControlMode::Idle, ec); // Set to idle mode
robot.setPowerState(false, ec); // Power off (turn off servo), robot enters brake state
} catch (const std::exception &e) {
// Exception handling: Ensure robot can be safely stopped in any exception case
print(std::cerr, e.what()); // Print exception information
// Safe recovery operations
robot.setMotionControlMode(MotionControlMode::Idle, ec);
robot.setPowerState(false, ec);
}
return 0;
}
Real-time Mode - Joint Space S Planning
Program applicable model: xMateER7 Pro
#include <iostream>
#include <cmath>
#include <thread>
#include "rokae/robot.h"
#include "../print_helper.hpp"
using namespace rokae;
using namespace std;
/**
* @brief Robot joint space sine wave motion example program
* Function: Control the robot to perform sine wave trajectory motion in joint space, all joints move synchronously
* Features: Real-time control, smooth trajectory, safe stop
*/
int main() {
// Create robot object - xMateRobot represents ROKAE 6-axis robot
rokae::xMateRobot robot;
std::error_code ec; // Error code object, used to receive function call results
// Connect to robot
try {
// Parameter 1: Robot IP address (192.168.0.160)
// Parameter 2: Local IP address (192.168.0.100)
// Note: Both devices need to be in the same network segment, network connection normal
robot.connectToRobot("192.168.0.160", "192.168.0.100");
} catch (const std::exception &e) {
// Connection failure handling
print(std::cerr, e.what());
return 0;
}
// Set robot working state (three steps)
robot.setOperateMode(rokae::OperateMode::automatic, ec); // Set to automatic mode, can execute programs
robot.setMotionControlMode(MotionControlMode::RtCommand, ec); // Set to real-time command mode, allowing high-frequency control
robot.setPowerState(true, ec); // Power on (turn on servo), robot can move
try {
// Get real-time motion controller
// lock() converts weak_ptr to shared_ptr to ensure the controller object exists
auto rtCon = robot.getRtMotionController().lock();
// Optional: Set to receive robot state data
// Parameter 1: Update period 1ms - Receive state every 1 millisecond
// Parameter 2: Received data fields - Only receive joint position information
robot.startReceiveRobotState(std::chrono::milliseconds(1), {RtSupportedFields::jointPos_m});
// Define and initialize motion control variables
double time = 0; // Motion time accumulation, unit: seconds
std::array<double, 6> jntPos{}; // Current joint position array, 6 elements corresponding to 6 joints
// Drag posture - a safe starting position
std::array<double, 6> q_drag_xm3 = {0, M_PI/6, M_PI/3, 0, M_PI/2, 0};
/**
* @brief Real-time control callback function
* This function will be called in real-time loop (approximately 1ms cycle) to generate joint space motion trajectory
* Return value: JointPosition containing 6 joint target positions control command
*
* Motion principle: All joints move according to sine wave law, but in different directions
* Joints 0,1,3,5: Forward motion (+delta_angle)
* Joints 2,4: Reverse motion (-delta_angle)
*/
std::function<JointPosition()> callback = [&, rtCon](){
time += 0.001; // Time accumulation, increase by 1ms each time (assuming 1ms call cycle)
// Calculate joint angle increment - Use cosine function to generate smooth S-curve trajectory
// Formula: Δθ = (π/20) * [1 - cos(π/2.5 * t)]
// - π/20: Amplitude coefficient, limiting maximum angle change
// - 1-cos(): Generate motion curve that starts smoothly accelerating
// - π/2.5: Frequency coefficient, controlling motion speed
double delta_angle = M_PI / 20.0 * (1 - std::cos(M_PI / 2.5 * time));
// Build control command - Apply angle increment to each joint
JointPosition cmd = {{
jntPos[0] + delta_angle, // Joint 0: Forward motion
jntPos[1] + delta_angle, // Joint 1: Forward motion
jntPos[2] - delta_angle, // Joint 2: Reverse motion
jntPos[3] + delta_angle, // Joint 3: Forward motion
jntPos[4] - delta_angle, // Joint 4: Reverse motion
jntPos[5] + delta_angle // Joint 5: Forward motion
}};
// Motion stop condition: End motion after 60 seconds
if(time > 60) {
cmd.setFinished(); // Set completion flag, stop real-time control loop
}
return cmd; // Return control command to robot
};
// Step 1: Move from current position to safe drag posture
// MoveJ: Joint space point-to-point motion
// Parameter 1: Speed ratio 0.3
// Parameter 2: Start position - Current joint position
// Parameter 3: Target position - Preset drag posture
rtCon->MoveJ(0.3, robot.jointPos(ec), q_drag_xm3);
// Step 2: Set real-time control loop
// Register callback function, robot will call this function at approximately 1ms cycle
rtCon->setControlLoop(callback);
// Step 3: Get current joint position as motion starting point
// Note: Must be obtained after setting callback to ensure using the latest position data
jntPos = robot.jointPos(ec);
// Step 4: Start joint space position control
// Set control mode to joint position control, in this mode the robot tracks joint angle commands
rtCon->startMove(RtControllerMode::jointPosition);
// Step 5: Start real-time control loop
// Parameter true indicates blocking execution, program will stop here until motion completes (60 seconds or exception)
rtCon->startLoop(true);
// Motion completion prompt
print(std::cout, "Control ended");
// Step 6: Safe exit - Restore robot to safe state
robot.setMotionControlMode(MotionControlMode::Idle, ec); // Set to idle mode
robot.setPowerState(false, ec); // Power off (turn off servo), robot enters brake state
} catch (const std::exception &e) {
// Exception handling: Ensure robot can be safely stopped in any exception case
print(std::cerr, e.what()); // Print exception information
// Safe recovery operations
robot.setMotionControlMode(MotionControlMode::Idle, ec);
robot.setPowerState(false, ec);
}
return 0;
}
Real-time Mode - Motion Planning
Suitable for MoveJ & MoveL & MoveC
int main() {
using namespace std;
try {
// Network configuration
std::string ip = "192.168.21.10"; // Robot controller IP address
std::error_code ec; // Error code object, used to receive function call results
// Create 7-axis robot object and connect
// Parameter 1: Robot IP (192.168.21.10)
// Parameter 2: Local IP (192.168.21.1) - Need to be in the same network segment as the robot
rokae::xMateErProRobot robot(ip, "192.168.21.10"); // xMate 7-axis robot
// Set robot operation mode to automatic mode
// Automatic mode: Robot can execute programs and accept external control commands
robot.setOperateMode(rokae::OperateMode::automatic, ec);
// Set real-time network tolerance time to 20ms
// Important: If the controller is already in real-time mode when the program runs, you need to switch to non-real-time mode first before setting this parameter
// Network tolerance time refers to the allowed network delay, exceeding this time will trigger safety protection
robot.setRtNetworkTolerance(20, ec);
// Set motion control mode to real-time command mode
// RtCommand mode: Allows high-frequency real-time control, suitable for applications requiring precise trajectories
robot.setMotionControlMode(MotionControlMode::RtCommand, ec);
// Turn on robot power (servo on)
// After power on, the robot can move; after power off, the robot is in brake state
robot.setPowerState(true, ec);
// Get real-time motion controller
// lock() converts weak_ptr to shared_ptr to ensure the controller object exists
auto rtCon = robot.getRtMotionController().lock();
// Example program uses model: xMateER7 Pro (7-axis robot)
/******************************************************************
* 1. Joint space motion: MoveJ from current position to drag position
******************************************************************/
// Define drag posture - 7 joint target angles (unit: radians)
// This is a preset safe position, convenient for subsequent Cartesian space motion
std::array<double, 7> q_drag_xm7p = {
0, // Joint 1: 0 radians
M_PI / 6, // Joint 2: 30 degrees
0, // Joint 3: 0 radians
M_PI / 3, // Joint 4: 60 degrees
0, // Joint 5: 0 radians
M_PI / 2, // Joint 6: 90 degrees
0 // Joint 7: 0 radians
};
// Execute joint space point-to-point motion
// Parameter 1: Speed ratio 0.5
// Parameter 2: Start position - Current joint position
// Parameter 3: Target position - Drag posture
rtCon->MoveJ(0.5, robot.jointPos(ec), q_drag_xm7p);
/******************************************************************
* 2. Arc motion (on X-Y plane)
******************************************************************/
// Define three key points for arc motion: start, auxiliary, target
CartesianPosition start, aux, target;
// Get current flange pose in base coordinate system and convert to homogeneous transformation matrix
// Flange coordinate system: The coordinate system at the end of the robot, where the tool is installed
Utils::postureToTransArray(robot.posture(rokae::CoordinateType::flangeInBase, ec), start.pos);
// Define variables to store rotation matrix and translation vector
Eigen::Matrix3d rot_start; // Start point rotation matrix
Eigen::Vector3d trans_start; // Start point translation vector [x, y, z]
Eigen::Vector3d trans_aux; // Auxiliary point translation vector
Eigen::Vector3d trans_end; // End point translation vector
// Decompose homogeneous transformation matrix into rotation matrix and translation vector
Utils::arrayToTransMatrix(start.pos, rot_start, trans_start);
// Initialize auxiliary point and end point coordinates (based on start point)
trans_end = trans_start;
trans_aux = trans_start;
// Set auxiliary point coordinates: X direction -0.28m, Y direction -0.05m
// The auxiliary point together with the start and end points determines the curvature of the arc
trans_aux[0] -= 0.28; // X coordinate decreases by 0.28 meters
trans_aux[1] -= 0.05; // Y coordinate decreases by 0.05 meters
// Set end point coordinates: Y direction -0.15m
trans_end[1] -= 0.15; // Y coordinate decreases by 0.15 meters
// Convert auxiliary point and end point coordinates back to homogeneous transformation matrix format
// Note: The rotation matrix remains unchanged, only the position is changed
Utils::transMatrixToArray(rot_start, trans_aux, aux.pos);
Utils::transMatrixToArray(rot_start, trans_end, target.pos);
// Execute arc interpolation motion
// Parameter 1: Speed ratio 0.2
// Parameter 2: Start point pose
// Parameter 3: Auxiliary point pose
// Parameter 4: End point pose
// Motion trajectory: Starting from start, passing through aux point, reaching target point, forming an arc
rtCon->MoveC(0.2, start, aux, target);
/******************************************************************
* 3. Linear motion
******************************************************************/
// Re-acquire current position as the start point of linear motion
Utils::postureToTransArray(robot.posture(rokae::CoordinateType::flangeInBase, ec), start.pos);
Utils::arrayToTransMatrix(start.pos, rot_start, trans_start);
// Set linear motion end point coordinates
trans_end = trans_start;
// Move 0.1 meters in negative X-axis direction, 0.3 meters in negative Y-axis direction, 0.25 meters in negative Z-axis direction
trans_end[0] -= 0.1; // X: -0.1m
trans_end[1] -= 0.3; // Y: -0.3m
trans_end[2] -= 0.25; // Z: -0.25m
// Convert end point coordinates back to homogeneous transformation matrix
Utils::transMatrixToArray(rot_start, trans_end, target.pos);
// Print motion information for debugging
print(std::cout, "MoveL start position:", start.pos, "Target:", target.pos);
// Execute linear interpolation motion
// Parameter 1: Speed ratio 0.3
// Parameter 2: Start point pose
// Parameter 3: End point pose
// Motion trajectory: Straight line path from start to target, tool posture remains unchanged
rtCon->MoveL(0.3, start, target);
/******************************************************************
* 4. Safe shutdown: Restore robot to safe state
******************************************************************/
// Set motion control mode to non-real-time command mode
// NrtCommand mode: Suitable for non-real-time, low-speed control scenarios
robot.setMotionControlMode(rokae::MotionControlMode::NrtCommand, ec);
// Set operation mode to manual mode
// Manual mode: Usually used for teaching, debugging, limiting automatic operation
robot.setOperateMode(rokae::OperateMode::manual, ec);
robot.setPowerState(false, ec); // Power off (turn off servo), robot enters brake state
}
catch (const std::exception& e) {
// Exception handling: Catch and print all exception information
std::cerr << e.what();
}
return 0;
}
Real-time Mode - Industrial 6-axis Model Position Control Support
#include <cmath>
#include <thread>
#include <iterator>
#include "../print_helper.hpp"
#include "Eigen/Geometry"
#include "rokae/robot.h"
#include "rokae/utility.h"
using namespace rokae;
std::ostream &os = std::cout; ///< Output stream reference, used for printing to console
/**
* @brief Robot multiple motion mode demonstration program
* Function: Demonstrates joint motion, arc motion, linear motion and tool coordinate system setting of standard robot
* Features: Includes safety area setting, tool coordinate system transformation and other advanced functions
*/
int main() {
using namespace std;
try {
// Network configuration and robot initialization
std::string ip = "192.168.0.160"; // Robot controller IP
std::error_code ec; // Error code object
rokae::StandardRobot robot(ip, "192.168.0.180"); // Create standard robot object and connect
// Set robot working mode
robot.setOperateMode(rokae::OperateMode::automatic, ec); // Automatic mode
robot.setRtNetworkTolerance(20, ec); // Set real-time network tolerance to 20ms
robot.setMotionControlMode(MotionControlMode::RtCommand, ec); // Real-time command mode
robot.setPowerState(true, ec); // Power on
// Get real-time motion controller
auto rtCon = robot.getRtMotionController().lock();
/******************************************************************
* Tool coordinate system setting: Reset end coordinate system to coincide with flange
******************************************************************/
std::array<double, 16> _end{}; // 4x4 homogeneous transformation matrix
// Convert pose {0,0,0,0,0,0} to homogeneous matrix, indicating no offset and rotation
// Parameters: {x, y, z, rx, ry, rz} - Position and Euler angles
Utils::postureToTransArray({0,0,0,0,0,0}, _end);
// Set end tool coordinate system to completely coincide with flange coordinate system
rtCon->setEndEffectorFrame(_end, ec);
// Example program uses model: XB7h-R707
/******************************************************************
* 1. Joint space motion: MoveJ from current position to shipping position
******************************************************************/
// Use MoveJ for joint space point-to-point motion
// Parameter 1: Speed ratio 0.4
// Parameter 2: Start position - Current joint position
// Parameter 3: Target position - Joint angles after converting degrees to radians
// Target pose: {0°, -15°, 60°, 0°, 45°, 0°}
rtCon->MoveJ(0.4, robot.jointPos(ec), Utils::degToRad(std::array<double, 6>({0, -15, 60, 0, 45, 0})));
/******************************************************************
* 2. Arc motion (on X-Y plane)
******************************************************************/
CartesianPosition start, aux, target; // Define three key points for arc motion
// Get current end pose in reference coordinate system as arc start point
Utils::postureToTransArray(robot.posture(rokae::CoordinateType::endInRef, ec), start.pos);
Eigen::Matrix3d rot_start; // Start point rotation matrix
Eigen::Vector3d trans_start; // Start point translation vector [x, y, z]
Eigen::Vector3d trans_aux; // Auxiliary point translation vector
Eigen::Vector3d trans_end; // End point translation vector
// Decompose homogeneous transformation matrix into rotation matrix and translation vector
Utils::arrayToTransMatrix(start.pos, rot_start, trans_start);
// Initialize end point and auxiliary point coordinates
trans_end = trans_start;
trans_aux = trans_start;
// Set auxiliary point coordinates: X direction +0.28m, Y direction -0.05m
trans_aux[0] += 0.28; // X coordinate increases by 0.28 meters
trans_aux[1] -= 0.05; // Y coordinate decreases by 0.05 meters
// Set end point coordinates: Y direction -0.15m
trans_end[1] -= 0.15; // Y coordinate decreases by 0.15 meters
// Convert coordinates back to homogeneous transformation matrix format (maintain original rotation)
Utils::transMatrixToArray(rot_start, trans_aux, aux.pos);
Utils::transMatrixToArray(rot_start, trans_end, target.pos);
// Execute arc interpolation motion
// Motion trajectory: Starting from start, passing through aux point, reaching target point, forming an arc on X-Y plane
rtCon->MoveC(0.2, start, aux, target);
/******************************************************************
* 3. Linear motion (using Euler angles to represent posture)
******************************************************************/
// Get current end pose in reference coordinate system (Euler angle representation)
auto _pose_start = robot.posture(rokae::CoordinateType::endInRef, ec);
auto _pose_target = _pose_start; // Copy start pose
// Set target pose: Move +0.2m along Z-axis, rotate +60° around Y-axis
_pose_target[2] += 0.2; // Z coordinate increases by 0.2 meters
_pose_target[4] += Utils::degToRad(60); // Rotate 60 degrees around Y-axis
// Convert Euler angle pose to homogeneous transformation matrix
Utils::postureToTransArray(_pose_start, start.pos);
Utils::postureToTransArray(_pose_target, target.pos);
// Print motion information for debugging
print(os, "MoveL start position:", start.pos, "Target:", target.pos);
// Execute linear interpolation motion
// Motion trajectory: Straight line path from start to target, tool posture smoothly transitions
rtCon->MoveL(0.3, start, target);
/******************************************************************
* 4. Set real-time mode end coordinate system and execute motion
******************************************************************/
// Set new tool coordinate system: Offset 0.1m in X direction, rotate 90° around Y-axis
// New tool coordinate system relative to flange transformation: {0.1, 0, 0, 0, π/2, 0}
Utils::postureToTransArray(std::array<double, 6>({0.1, 0, 0, 0, M_PI_2, 0}), _end);
rtCon->setEndEffectorFrame(_end, ec);
// Execute joint motion using new tool coordinate system
rtCon->MoveJ(0.4, robot.jointPos(ec), Utils::degToRad(std::array<double, 6>({0, -15, 60, 0, 45, 0})));
// Important note:
// Real-time mode tool coordinate system setting is independent, cannot use robot.posture(CoordinateType::endInRef) interface to get pose
// Because this interface returns pose based on non-real-time mode tool coordinate system
// Method 1: Directly give start pose after setting end coordinate system
// Pose format: {x, y, z, rx, ry, rz}
Utils::postureToTransArray({0.1036, 0, 0.415, 0.042, -M_PI_2, -0.0424}, start.pos);
// Method 2: Receive real-time status data to get TCP pose
//robot.getStateData(RtSupportedFields::tcpPose_m, start.pos);
target.pos = start.pos;
target.pos[3] += 0.35; // X coordinate increases by 0.35 meters
print(os, "MoveL start position:", start.pos, "Target:", target.pos);
// Execute linear motion using new tool coordinate system
rtCon->MoveL(0.3, start, target);
/******************************************************************
* 5. Safe shutdown: Restore robot to safe state
******************************************************************/
robot.setMotionControlMode(rokae::MotionControlMode::NrtCommand, ec); // Switch to non-real-time mode
robot.setOperateMode(rokae::OperateMode::manual, ec); // Set to manual mode
} catch (const std::exception &e) {
// Exception handling
print(std::cerr, e.what());
}
return 0;
}
Real-time Mode - Direct Torque Control
Conditions: 1. This example requires the use of xMateModel library, please set the compilation option XCORE_USE_XMATE_MODEL=ON
- Requires actual robot testing
#include <iostream>
#include <cmath>
#include <thread>
#include "rokae/robot.h"
#include "Eigen/Geometry"
#include "../print_helper.hpp"
#include "rokae/utility.h"
using namespace rokae;
/**
* @brief Torque control example program
* Important notes:
* 1) Torque values should not exceed the model's limit conditions (see manual);
* 2) Please hold the emergency stop button when running for the first time to avoid unexpected motion causing collision
* 3) Torque control is a low-level control mode requiring accurate dynamic model
*/
void torqueControl(xMateErProRobot &robot) {
using namespace RtSupportedFields;
// Get real-time motion controller
auto rtCon = robot.getRtMotionController().lock();
auto model = robot.model(); // Get robot dynamic model
error_code ec;
// Define drag posture - Safe start position for 7-axis robot
std::array<double,7> q_drag = {0, M_PI/6, 0, M_PI/3, 0, M_PI/2, 0 };
// Reconfigure robot state reception
// Stop previous reception and restart to get required state data
robot.stopReceiveRobotState();
// Start receiving robot state, 1ms cycle, receive joint position, velocity, acceleration and TCP pose
robot.startReceiveRobotState(std::chrono::milliseconds(1),
{jointPos_m, jointVel_m, jointAcc_c, tcpPose_m});
// Step 1: Move to drag position (safe start point)
rtCon->MoveJ(0.2, robot.jointPos(ec), q_drag);
// Step 2: Set control mode to torque control
// In torque control mode, directly send torque commands to joints instead of position or velocity commands
rtCon->startMove(RtControllerMode::torque);
/******************************************************************
* Impedance control parameter setting
* Impedance control simulates spring-damping system, making the robot compliant
******************************************************************/
// Stiffness parameters (spring coefficient)
const double translational_stiffness{200.0}; // Translational stiffness N/m
const double rotational_stiffness{5.0}; // Rotational stiffness N·m/rad
// Define stiffness and damping matrices (6x6, corresponding to 6 DOF: 3 translation + 3 rotation)
Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
stiffness.setZero();
// Set translational and rotational stiffness diagonal matrices
stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3);
stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3);
damping.setZero();
// Critical damping coefficient calculation: damping = 2 * sqrt(stiffness)
// Here set to 0 means no damping, robot will continue to oscillate
damping.topLeftCorner(3, 3) << 0.0 * sqrt(translational_stiffness) * Eigen::MatrixXd::Identity(3, 3);
damping.bottomRightCorner(3, 3) << 0.0 * sqrt(rotational_stiffness) * Eigen::MatrixXd::Identity(3, 3);
// Store initial position (as desired position)
std::array<double, 16> init_position {};
Eigen::Matrix<double, 6, 7> jacobian; // Jacobian matrix: 6x7 (task space DOF x joint number)
// Get current flange pose in base coordinate system as initial position
Utils::postureToTransArray(robot.posture(rokae::CoordinateType::flangeInBase, ec), init_position);
/******************************************************************
* Real-time torque control callback function
* This function will be called at 1ms cycle to calculate and return 7 joint torque commands
******************************************************************/
std::function<Torque(void)> callback = [&]{
using namespace RtSupportedFields;
static double time=0; // Time accumulation for controlling motion duration
time += 0.001; // Increase by 1ms each time
// Convert initial position to Eigen transformation matrix
Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(init_position.data()).transpose());
Eigen::Vector3d position_d(initial_transform.translation()); // Desired position
Eigen::Quaterniond orientation_d(initial_transform.linear()); // Desired posture (quaternion)
// Define state variables
std::array<double, 7> q{}, dq_m{}, ddq_c{}; // Joint position, velocity, acceleration
std::array<double, 16> pos_m {}; // TCP pose
// Read real-time data from robot state
robot.getStateData(jointPos_m, q); // Joint position
robot.getStateData(jointVel_m, dq_m); // Joint velocity
robot.getStateData(jointAcc_c, ddq_c); // Joint acceleration
/******************************************************************
* Dynamics calculation part
******************************************************************/
// Calculate Jacobian matrix - Describes mapping from joint velocity to end velocity
std::array<double, 42> jacobian_array = model.jacobian(q);
// Calculate gravity compensation torque
std::array<double, 7> gravity_array = model.getTorque(q, dq_m, ddq_c, TorqueType::gravity);
// Calculate friction compensation torque
std::array<double, 7> friction_array = model.getTorque(q, dq_m, ddq_c, TorqueType::friction);
// Convert array data to Eigen matrix for mathematical operations
Eigen::Map<const Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
Eigen::Map<const Eigen::Matrix<double, 7, 1>> friction(friction_array.data());
Eigen::Map<const Eigen::Matrix<double, 7, 6>> jacobian_(jacobian_array.data());
jacobian = jacobian_.transpose(); // Transpose to 6x7 matrix
Eigen::Map<const Eigen::Matrix<double, 7, 1>> q_mat(q.data());
Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq_mat(dq_m.data());
// Get current TCP pose
robot.getStateData(tcpPose_m, pos_m);
Eigen::Affine3d transform(Eigen::Matrix4d::Map(pos_m.data()).transpose());
Eigen::Vector3d position(transform.translation()); // Current position
Eigen::Quaterniond orientation(transform.linear()); // Current posture
/******************************************************************
* Error calculation part
******************************************************************/
Eigen::Matrix<double, 6, 1> error; // 6-dimensional error vector [position error(3), posture error(3)]
// Position error: Current position - Desired position
error.head(3) << position - position_d;
// Posture error calculation (using quaternion)
// Ensure quaternions are in the same hemisphere (avoid double cover problem)
if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
orientation.coeffs() << -orientation.coeffs();
}
// Calculate posture error quaternion
Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
// Convert quaternion error to rotation vector in axis-angle representation
error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
// Convert to base coordinate system
error.tail(3) << -transform.linear() * error.tail(3);
/******************************************************************
* Control law calculation part - Impedance control
* τ = J^T * [ -K * e - D * (J * dq) ]
* Where:
* τ: Joint torque
* J: Jacobian matrix
* K: Stiffness matrix
* D: Damping matrix
* e: Pose error
* dq: Joint velocity
******************************************************************/
Eigen::VectorXd tau_d(7); // 7-dimensional joint torque vector
// Cartesian space impedance calculation and mapping to joint space
tau_d << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq_mat));
// Note: Gravity compensation and friction compensation are not added here
// In actual applications usually need: tau_total = tau_impedance + gravity + friction
// Build control command
Torque cmd(7);
Eigen::VectorXd::Map(cmd.tau.data(), 7) = tau_d;
// Stop condition: End after 30 seconds
if(time > 30){
cmd.setFinished();
}
return cmd;
};
// Set real-time control loop
// Parameter useStateDataInLoop = true indicates reading state data in callback function
// Sending period is consistent with the 1ms period set by startReceiveRobotState()
rtCon->setControlLoop(callback, 0, true);
// Start real-time control loop (blocking execution)
rtCon->startLoop(true);
}
/**
* @brief Send zero torque test function
* Function: If the force control model is accurate, the robot arm should remain stationary
* This is a simple torque control test to verify the accuracy of the dynamic model
*/
template <unsigned short DoF>
void zeroTorque(Cobot<DoF> &robot) {
error_code ec;
std::array<double,7> q_drag = {0, M_PI/6, 0, M_PI/3, 0, M_PI/2, 0 };
auto rtCon = robot.getRtMotionController().lock();
// Move to drag position
rtCon->MoveJ(0.2, robot.jointPos(ec), q_drag);
// Control mode is torque control
rtCon->startMove(RtControllerMode::torque);
// Create zero torque command
Torque cmd {};
cmd.tau.resize(DoF); // Adjust torque vector size according to DOF number
// Simple callback function, always returns zero torque
std::function<Torque(void)> callback = [&]() {
static double time=0;
time += 0.001;
if(time > 30){
cmd.setFinished(); // Stop after 30 seconds
}
return cmd; // Return zero torque
};
rtCon->setControlLoop(callback);
rtCon->startLoop();
print(std::cout, "Torque control ended");
}
/**
* @brief Main program
* Function: Initialize robot and execute torque control demonstration
*/
int main() {
try {
std::string ip = "192.168.0.160";
std::error_code ec;
// Create 7-axis robot object and connect
rokae::xMateErProRobot robot(ip, "192.168.0.100");
// Set robot working mode
robot.setOperateMode(rokae::OperateMode::automatic, ec);
robot.setMotionControlMode(MotionControlMode::RtCommand, ec);
robot.setPowerState(true, ec);
try {
// Execute torque control demonstration
torqueControl(robot);
} catch (const rokae::RealtimeMotionException &e) {
// Real-time motion exception handling
print(std::cerr, e.what());
// When error occurs, switch back to non-real-time mode to ensure safety
robot.setMotionControlMode(rokae::MotionControlMode::NrtCommand, ec);
}
// Normal exit: Restore robot to safe state
robot.setMotionControlMode(rokae::MotionControlMode::NrtCommand, ec);
robot.setOperateMode(rokae::OperateMode::manual, ec);
} catch (const std::exception &e) {
// Global exception handling
print(std::cerr, e.what());
}
return 0;
}
Non-real-time Mode
Overview
Same effect as RL program implementation
User-side algorithm:
-
Get end point coordinates (X,Y,Z,a,b,c), methods: photography, teaching, etc.
-
Decide movement method: Linear
-
Send target point
-
Wait for robot to reach target point
Non-real-time Motion Commands
Prerequisites
Depending on the robot model and coordinate system, the points in each example may not be reachable, for reference only for interface usage methods
#include <iostream>
#include <thread>
#include <cmath>
#include "rokae/robot.h"
#include "rokae/utility.h"
#include "print_helper.hpp"
using namespace std;
using namespace rokae;
std::ostream &os = std::cout; ///< Output to console
namespace Predefines {
// ****** Predefined drag postures ******
const std::vector<double> ErDragPosture = {0, M_PI/6, M_PI/3, 0, M_PI_2, 0}; ///< xMateEr3, xMateEr7 drag posture
const std::vector<double> ErProDragPosture = {0, M_PI/6, 0, M_PI/3, 0, M_PI_2, 0}; ///< xMateEr3 Pro, xMateEr7 Pro drag posture
const std::vector<double> CrDragPosture {0, M_PI/6, -M_PI_2, 0, -M_PI/3, 0}; ///< xMateCR drag posture
const std::vector<double> Cr5DragPostre = { 0, M_PI / 6, -M_PI_2, -M_PI / 3, 0}; ///< CR5-axis configuration drag posture
Toolset defaultToolset; ///< Default tool workpiece coordinate system
}
/**
* @brief Print motion execution information
* @param info Motion event information
*/
void printInfo(const rokae::EventInfo &info) {
using namespace rokae::EventInfoKey::MoveExecution;
// Extract and print motion execution information
print(std::cout, "[Motion Execution Info] ID:", std::any_cast<std::string>(info.at(ID)),
"Index:", std::any_cast<int>(info.at(WaypointIndex)),
"Completed: ", std::any_cast<bool>(info.at(ReachTarget)) ? "YES": "NO",
std::any_cast<error_code>(info.at(Error)),
std::any_cast<std::string>(info.at(Remark)));
// If custom information is set, print this information
if(info.count(CustomInfo)) {
auto custom_info = std::any_cast<std::string>(info.at(CustomInfo));
if(!custom_info.empty()) print(std::cout, "Custom Info: ",custom_info);
}
}
/**
* @brief Main program
* Demonstrates how to connect to robot and execute various motion examples
*/
int main() {
try {
using namespace rokae;
// *** 1. Connect to robot ***
std::string ip = "192.168.21.10";
std::error_code ec;
rokae::xMateRobot robot(ip); // Create xMate 6-axis robot instance
// *** 2. Switch to automatic mode and power on ***
robot.setOperateMode(OperateMode::automatic, ec);
robot.setPowerState(true, ec);
// *** 3. Set default motion parameters ***
robot.setMotionControlMode(MotionControlMode::NrtCommand, ec);
robot.setDefaultZone(50, ec); // Set default blending zone 50mm
robot.setDefaultSpeed(200, ec); // Set default speed 200mm/s
// Optional: Set motion event callback function
robot.setEventWatcher(Event::moveExecution, printInfo, ec);
// *** 4. Safe shutdown ***
robot.setPowerState(false, ec);
robot.disconnectFromRobot(ec);
} catch (const std::exception &e) {
print(std::cerr, e.what());
}
return 0;
}
Query Robot Current Running Status
Query robot status through operationState interface in a loop, below is a simple code implementation
- By querying whether path ID and waypoint index are completed
- By querying whether the robot arm is in motion
/**
* @brief Wait for motion to end - By querying whether path ID and waypoint index are completed
* @param robot Robot object
* @param traj_id Trajectory ID
* @param index Waypoint index
*/
void waitForFinish(BaseRobot &robot, const std::string &traj_id, int index){
using namespace rokae::EventInfoKey::MoveExecution;
error_code ec;
while(true) {
// Query motion execution event information
auto info = robot.queryEventInfo(Event::moveExecution, ec);
auto _id = std::any_cast<std::string>(info.at(ID));
auto _index = std::any_cast<int>(info.at(WaypointIndex));
// Check if there is an error
if(auto _ec = std::any_cast<error_code>(info.at(Error))) {
print(std::cout, "Path", _id, ":", _index, "Error:", _ec.message());
return;
}
// Check if specified path and waypoint are completed
if(_id == traj_id && _index == index) {
if(std::any_cast<bool>(info.at(ReachTarget))) {
print(std::cout, "Path", traj_id, ":", index, "Completed");
}
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
}
/**
* @brief Wait for motion to end - By querying whether the robot arm is in motion
* @param robot Robot object
* @param running Running status flag
*/
void waitRobot(BaseRobot &robot, bool &running) {
running = true;
while (running) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
error_code ec;
auto st = robot.operationState(ec);
// If robot is in idle or unknown state, consider motion ended
if(st == OperationState::idle || st == OperationState::unknown){
running = false;
}
}
}
Errors During Motion
If errors occur during motion, such as exceeding limits, singular points, etc., they can be received in the following way:
void printInfo(const rokae::EventInfo &info) {
using namespace rokae::EventInfoKey::MoveExecution;
print(std::cout, "[Motion Execution Info] ID:", std::any_cast<std::string>(info.at(ID)), "Index:", std::any_cast<int>(info.at(WaypointIndex)),
"Completed: ", std::any_cast<bool>(info.at(ReachTarget)) ? "YES": "NO", std::any_cast<error_code>(info.at(Error)),
std::any_cast<std::string>(info.at(Remark)));
}
// Set listener for motion execution information
robot.setEventWatcher(Event::moveExecution, printInfo, ec);
confdata Usage
conf data only applies to taught points. Points planned by upper-level applications do not have this data unless first moving to point A, then obtaining point A's Cartesian coordinates through the interface, then this data will be available
- For taught points, cartPosture() function returns results containing confdata, and subsequent motion commands must also include it.
- For non-taught points, confdata cannot be known, so the controller needs to be told not to use confdata for inverse kinematics calculation:
// Disable using conf
robot.setDefaultConfOpt(false, ec);
// Enable conf data
CartesianPosition cartesian_position1({0.786, 0, 0.431, M_PI, 0.98, M_PI});
robot.setDefaultConfOpt(true, ec);
// The following data must be from taught points, configure conf data
cartesian_position1.confData = {-1,1,-1,0,1,0,0,2};
/**
* @brief Linear motion strictly following axis configuration data (conf data). Points applicable to model xMateER3
* Demonstrates how to use axis configuration data to solve inverse kinematics multi-solution problem
* @param robot Robot object
*/
void moveWithForcedConf(xMateRobot &robot) {
Toolset default_toolset;
error_code ec;
bool running;
std::string id;
// Set tool workpiece coordinate system
robot.setToolset(default_toolset, ec);
robot.setDefaultSpeed(200,ec);
robot.setDefaultZone(5, ec);
// Move to drag posture
print(std::cout, "Moving to drag posture");
robot.executeCommand({MoveAbsJCommand(Predefines::ErDragPosture)}, ec);
waitRobot(robot, running);
// Define two Cartesian positions
CartesianPosition cartesian_position0({0.786, 0, 0.431, M_PI, 0.6, M_PI});
CartesianPosition cartesian_position1({0.786, 0, 0.431, M_PI, 0.98, M_PI});
// Create motion commands
MoveJCommand j0({cartesian_position0}), j1({cartesian_position1});
MoveLCommand l0({cartesian_position0}), l1({cartesian_position1});
// Phase 1: Not strictly following axis configuration data, MoveL & MoveJ inverse kinematics selects nearest solution to current joint angles
print(std::cout, "Starting MoveJ - Not following axis configuration");
robot.moveAppend({j0, j1}, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 1);
// Phase 2: Following axis configuration data, use conf to calculate inverse kinematics
robot.setDefaultConfOpt(true, ec); // Enable axis configuration option
print(std::cerr, ec);
// Set specific axis configuration data
cartesian_position1.confData = {-1,1,-1,0,1,0,0,2};
l1.target = cartesian_position1;
j1.target = cartesian_position1;
print(std::cout, "Starting MoveJ - Following axis configuration");
robot.moveAppend({j0, j1}, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 1);
// Phase 3: Test MoveL command
print(std::cout, "Moving to drag posture");
robot.executeCommand({MoveAbsJCommand(Predefines::ErDragPosture)}, ec);
waitRobot(robot, running);
print(std::cout, "Starting MoveL - Following axis configuration");
robot.moveAppend({l0, l1}, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 1);
// Restore default settings
robot.setDefaultConfOpt(false, ec);
}
You can see conf data when creating new Cartesian points in HMI
Configure axis data inverse kinematics
Note: Points must be taught points
/**
* @brief Example - Setting axis configuration data (confData) to handle multi-inverse solution problem, points applicable to model xMateCR7
* Demonstrates how to use joint configuration data to obtain desired inverse kinematics solution
* @param robot Robot object
*/
void multiplePosture(xMateRobot &robot) {
error_code ec;
std::string id;
// Use default tool workpiece coordinate system
Toolset defaultToolset;
robot.setToolset(defaultToolset, ec);
// Set to use confdata for inverse kinematics calculation
robot.setDefaultConfOpt(true, ec);
// Create joint motion command
MoveJCommand moveJ({0.2434, -0.314, 0.591, 1.5456, 0.314, 2.173});
// Same end pose, different confData will produce different joint angle solutions
moveJ.target.confData = {-67, 100, 88, -79, 90, -120, 0, 0};
moveJ.jointSpeed = 0.1; // Set joint speed percentage to 10%
robot.moveAppend({moveJ}, id, ec);
moveJ.target.confData = {-76, 8, -133, -106, 103, 108, 0, 0};
robot.moveAppend({moveJ}, id, ec);
moveJ.target.confData = {-70, 8, -88, 90, -105, -25, 0, 0};
robot.moveAppend({moveJ}, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 0);
// Restore default settings
robot.setDefaultConfOpt(false, ec);
}
Tool Workpiece Coordinate System Setting
Usually tools are installed at the robot flange end, thus tool coordinates, and external reference coordinate systems (workpiece coordinate systems) are also set
/**
* @brief Example - Using tool workpiece coordinate system, points applicable to model xMateCR7
* Demonstrates how to move in different tool workpiece coordinate systems
* @param robot Robot object
*/
void moveInToolsetCoordinate(BaseRobot &robot) {
error_code ec;
std::string id;
// Use default tool workpiece coordinate system
robot.setToolset(Predefines::defaultToolset, ec);
// Move to initial position
MoveAbsJCommand moveAbs({0, M_PI/6, -M_PI_2, 0, -M_PI/3, 0});
robot.moveAppend({moveAbs}, id, ec);
// Move in default coordinate system
MoveLCommand movel1({0.563, 0, 0.432, M_PI, 0, M_PI}, 1000, 100);
MoveLCommand movel2({0.33467, -0.095, 0.51, M_PI, 0, M_PI}, 1000, 100);
robot.moveAppend({movel1, movel2}, id, ec);
robot.moveStart(ec);
bool moving = true;
waitRobot(robot, moving);
// Switch to custom tool workpiece coordinate system
Toolset toolset1;
toolset1.ref = {{0.1, 0.1, 0}, {0, 0, 0}}; // External reference coordinate system offset: X+0.1m, Y+0.1m
toolset1.end = {{ 0, 0, 0.01}, {0, M_PI/6, 0}}; // End coordinate system offset: Z+0.01m, Ry+30°
#if 0
toolset1.load.mass = 2; // Set load 2kg
#endif
robot.setToolset(toolset1, ec);
#if 0
// Method 2: Use created tool workpiece
robot.setToolset("tool1", "wobj1", ec);
#endif
// Move in new coordinate system
MoveLCommand movel3({0.5, 0, 0.4, M_PI, 0, M_PI}, 1000, 100);
robot.moveAppend({movel3}, id, ec);
robot.moveStart(ec);
waitRobot(robot, moving);
}
Singularity Avoidance
Only effective for non-real-time mode, invalid for real-time mode
Turn on/off singularity avoidance function. Only applicable to some models
- Four-axis lock: Supports industrial 6-axis, xMateCR and xMateSR 6-axis models;
- Sacrifice posture: Supports all 6-axis models; Allowed posture error, range (0, PI*2], unit radians
- Joint space interpolation: Supports industrial 6-axis models Avoidance radius, range[0.005, 10], unit meters
/**
* @brief Lock 4-axis singularity avoidance method. Example applicable to model xMateCR7
*/
void avoidSingularityMove_Lock4(rokae::xMateRobot &robot) {
error_code ec;
std::string id;
bool running;
robot.setToolset(Predefines::defaultToolset, ec);
// First move to start posture
robot.executeCommand({MoveAbsJCommand({0.453,0.539,-1.581,0.0,0.026,0})}, ec);
waitRobot(robot, running);
std::vector<rokae::MoveLCommand> cmds = {
MoveLCommand({0.66675437164302165, -0.23850040314585069, 0.85182031,-3.1415926535897931, 1.0471975511965979, 3.01151}),
MoveLCommand({0.66675437164302154, 0.15775146321850292, 0.464946,-3.1415926535897931, 1.0471975511965979, -2.6885547129789127})
};
// Without turning on singularity avoidance mode, will report error exceeding motion range
robot.setAvoidSingularity(AvoidSingularityMethod::lockAxis4, false, 0, ec);
robot.moveAppend(cmds, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, (int)cmds.size() - 1);
// Turn on singularity avoidance mode, points reachable
// Note, all singularity avoidance functions will be turned off when motion reset
robot.moveReset(ec);
robot.setAvoidSingularity(AvoidSingularityMethod::lockAxis4, true, 0, ec);
std::cerr << ec;
print(std::cout, "Four-axis lock singularity avoidance function", robot.getAvoidSingularity(AvoidSingularityMethod::lockAxis4, ec) ? "On" : "Off");
robot.moveAppend(cmds, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, (int)cmds.size() - 1);
robot.setAvoidSingularity(AvoidSingularityMethod::lockAxis4, false, 0, ec);
}
Cartesian Point Offset Setting & Pause and Resume During Motion
Points applicable to model xMateEr7 Pro, demonstrates how to set position offset and control robot during motion
void cartesianPointWithOffset(BaseRobot &robot) {
error_code ec;
// Define base position
std::array<double, 6> pos = {0.631, 0, 0.38, M_PI, 0, M_PI};
std::array<double,6> offset_z = {0, 0, 0.2, 0, 0, 0}; // Z-axis offset 0.2m
// Create linear motion command
MoveLCommand moveL1(pos, 500, 5), moveL2(pos, 800, 0);
// Set space rotation speed to 100°/s (converted to radians)
moveL1.rotSpeed = 100 / 180.0 * M_PI;
// Relative to workpiece coordinate system Z+ offset 0.2m
moveL2.offset = { CartesianPosition::Offset::offs, offset_z};
// Create joint motion command
MoveJCommand moveJ1(pos, 200, 0), moveJ2(pos, 1000, 80);
// Relative to tool coordinate system Z+ offset 0.2m
moveJ2.offset = {CartesianPosition::Offset::relTool, offset_z};
// Execute motion sequence
robot.executeCommand({MoveAbsJCommand(Predefines::ErProDragPosture)}, ec); // First go to drag posture
robot.executeCommand({moveL1, moveL2}, ec); // Execute linear motion
robot.executeCommand({moveJ1, moveJ2}, ec); // Execute joint motion
// Create keyboard input thread for controlling during motion
std::thread input([&]{
int c{};
print(os, "[p]Pause [c]Continue [q]Quit");
while(c != 'q') {
c = getchar();
switch(c) {
case 'p':
robot.stop(ec); // Pause motion
print(std::cerr, ec); break;
case 'c':
robot.moveStart(ec); // Continue motion
print(std::cerr, ec); break;
default: break;
}
}
});
input.join();
robot.moveReset(ec); // Motion reset
}
Collision Recovery
No special recovery method needed, wait for diagnostic data to be saved then power on and continue running
Simulate collision and wait 20 seconds after collision to power on and continue running
/**
* @brief Event handling - Simulate waiting 20 seconds after collision to power on and continue running
* After collision occurs, the robot controller will immediately record diagnostic data, taking about 10 seconds, motion can only start after recording is complete.
* @param robot Robot object
* @param info Event information
*/
void recoverFromCollision(BaseRobot &robot, const rokae::EventInfo &info) {
using namespace rokae::EventInfoKey;
bool isCollided = std::any_cast<bool>(info.at(Safety::Collided));
print(std::cout, "Collided:", isCollided);
if(isCollided) {
// Wait 20 seconds for controller to record diagnostic data
std::this_thread::sleep_for(std::chrono::seconds(20));
error_code ec;
// Re-power on and continue motion
robot.setPowerState(true, ec);
robot.moveStart(ec);
print(std::cout, "Recovered from collision");
}
}
Seven-axis Redundant Motion & Resume Motion After Collision Detection
/**
* @brief Example - Seven-axis redundant motion & resume motion after collision detection, points applicable to model xMateER3 Pro
* Demonstrates seven-axis robot redundant feature usage and collision recovery
* @param robot Robot object
*/
void redundantMove(xMateErProRobot &robot) {
error_code ec;
std::string id;
// Set tool workpiece and motion parameters
Toolset defaultToolset;
robot.setToolset(defaultToolset, ec);
robot.setDefaultSpeed(500, ec);
robot.setDefaultZone(0, ec); // fine mode
// Set collision detection event callback function
robot.setEventWatcher(Event::safety, [&](const EventInfo &info){
recoverFromCollision(robot, info);
}, ec);
// ** 1) Variable arm angle motion **
MoveAbsJCommand moveAbsj({0, M_PI/6, 0, M_PI/3, 0, M_PI_2, 0});
MoveLCommand moveL1({0.562, 0, 0.432, M_PI, 0, -M_PI});
moveL1.target.elbow = 1.45; // Set arm angle
robot.moveAppend({moveAbsj}, id, ec);
robot.moveAppend({moveL1}, id, ec);
moveL1.target.elbow = -1.51; // Change arm angle
robot.moveAppend({moveL1}, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 0);
// ** 2) 60° arm angle arc **
CartesianPosition circle_p1({0.472, 0, 0.342, M_PI, 0, -M_PI}),
circle_p2({0.602, 0, 0.342, M_PI, 0, -M_PI}),
circle_a1({0.537, 0.065, 0.342, M_PI, 0, -M_PI}),
circle_a2({0.537, -0.065, 0.342, M_PI, 0, -M_PI});
// All points set to same 60° arm angle
circle_p1.elbow = M_PI/3;
circle_p2.elbow = M_PI/3;
circle_a1.elbow = M_PI/3;
circle_a2.elbow = M_PI/3;
MoveLCommand moveL2(circle_p1);
robot.moveAppend({moveL2}, id, ec);
MoveCCommand moveC1(circle_p2, circle_a1), moveC2(circle_p1, circle_a2);
std::vector<MoveCCommand> movec_cmds = {moveC1, moveC2};
robot.moveAppend(movec_cmds, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, (int)movec_cmds.size() - 1);
}
Motion Speed Adjustment
/**
* @brief Example - Adjust motion speed during movement
* Demonstrates how to adjust the robot's motion speed online
* @param robot Robot object
*/
void adjustSpeed(BaseRobot &robot) {
error_code ec;
std::string id;
double scale = 0.5;
// Set initial speed ratio to 50%
robot.adjustSpeedOnline(scale, ec);
// Create reciprocating motion sequence
rokae::MoveAbsJCommand cmd1({0, 0, 0, 0, 0, 0}),
cmd2({1.5, 1.5,1.5,1.5,1.5,1.5});
robot.moveAppend({cmd1, cmd2,cmd1,cmd2,cmd1,cmd2,cmd1,cmd2}, id, ec);
robot.moveStart(ec);
bool running = true;
// Create keyboard input thread for speed adjustment
std::thread readInput([&]{
while(running) {
auto ch = std::getchar();
if(ch == 'a') {
if(scale < 0.1) {
print(std::cerr, "Already at 1%");
continue;
}
scale -= 1e-1; // Decrease by 10%
} else if(ch == 'd'){
if(scale > 1) {
print(std::cerr, "Already at 100%");
continue;
}
scale += 1e-1; // Increase by 10%
} else {
continue;
}
// Apply speed adjustment
robot.adjustSpeedOnline(scale, ec);
print(os, "Adjusted to", scale);
}
});
print(os, "Robot starts moving, press [a] to decrease speed [d] to increase speed, step is 10%");
// Wait for motion to end
waitRobot(robot, running);
readInput.join();
}
Spiral Motion MoveSPCommand
/**
* @brief Spiral motion, applicable model: xMate3
* Demonstrates spiral trajectory generation and motion
* @param robot Robot object
*/
void spiralMove(rokae::xMateRobot &robot) {
error_code ec;
std::string id;
rokae::Toolset default_toolset = {};
robot.setToolset(default_toolset, ec);
// Define spiral end pose (only rpy used, xyz values arbitrary)
rokae::CartesianPosition cart_target({0, 0, 0, 2.967, -0.2, 3.1415}),
cart_target1({0, 0, 0, -2.787577,0.1639,-2.9});
// Define initial joint position
rokae::MoveAbsJCommand absjcmd({0.0,0.22150561307150393,1.4779577696969546,0.0,1.2675963456219013,0.0});
// Spiral 1: Initial radius 0.01m, radius change step 0.0005m/rad, counterclockwise rotation 720°, speed v500
rokae::MoveSPCommand spcmd1({cart_target, 0.01, 0.0005, M_PI * 4, false, 500}),
// Spiral 2: Initial radius 0.05m, radius change step 0.001m/rad, clockwise rotation 360°, speed v100
spcmd2({cart_target1, 0.05, 0.001, M_PI * 2, true, 100});
std::vector<rokae::MoveSPCommand> spcmds = {spcmd1, spcmd2};
// Execute motion sequence
robot.moveAppend({absjcmd}, id, ec); // Move to initial position first
robot.moveAppend(spcmds, id, ec); // Execute spiral motion
robot.moveStart(ec);
waitForFinish(robot, id, (int)spcmds.size() - 1);
}
Full Circle Motion MoveCFCommand
/**
* @brief Example - Full circle motion, points applicable to model XMC20
* Demonstrates three different types of full circle motion
* @param robot Robot object
*/
void fullCircleMove(xMateRobot &robot) {
error_code ec;
// Set default tool workpiece
Toolset defaultToolset;
robot.setToolset(defaultToolset, ec);
// Start joint angles
std::array<double, 6> start_angle = {0, 0.557737,-1.5184888, 0,-1.3036738, 0};
auto robot_model = robot.model();
// Calculate Cartesian pose corresponding to start angles
auto cart_pose = robot_model.calcFk(start_angle, ec);
// Joint absolute motion to start position
MoveAbsJCommand abs_j({start_angle[0], start_angle[1], start_angle[2],
start_angle[3], start_angle[4], start_angle[5]}, 1000, 0);
// Full circle command, execute 360 degree circular motion
MoveCFCommand move_cf1(cart_pose, cart_pose, M_PI * 2, 100, 10);
// Set auxiliary point offset
move_cf1.auxOffset.type = CartesianPosition::Offset::offs;
move_cf1.auxOffset.frame.trans[1] = 0.01; // Y+10mm
move_cf1.targetOffset.type = CartesianPosition::Offset::offs;
move_cf1.targetOffset.frame.trans[0] = 0.005; // X+5mm
move_cf1.targetOffset.frame.trans[1] = -0.01; // Y-10mm
// Create three different rotation types of full circle motion
MoveCFCommand move_cf2 = move_cf1, move_cf3 = move_cf1;
move_cf1.rotType = MoveCFCommand::constPose; // Keep posture unchanged
move_cf2.rotType = MoveCFCommand::rotAxis; // Rotate around rotation axis
move_cf3.rotType = MoveCFCommand::fixedAxis; // Rotate around fixed axis
std::string id;
// Execute three full circle motions separately
robot.moveAppend({abs_j}, id, ec);
robot.moveAppend({ move_cf1 }, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 0);
robot.moveAppend({abs_j}, id, ec);
robot.moveAppend({ move_cf2 }, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 0);
robot.moveAppend({abs_j}, id, ec);
robot.moveAppend({ move_cf3 }, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 0);
}
Motion Wait Command MoveWaitCommand
Can be inserted between two motion commands, after the previous motion reaches the position, wait for a period of time before executing the next one. This command does not have information feedback after execution, stay duration, minimum effective duration 1ms
void spiralMove(rokae::xMateRobot &robot) {
error_code ec;
std::string id;
rokae::Toolset default_toolset = {};
robot.setToolset(default_toolset, ec);
// Spiral end pose, only rpy used, xyz values arbitrary
rokae::CartesianPosition cart_target({0, 0, 0, 2.967, -0.2, 3.1415}),
cart_target1({0, 0, 0, -2.787577,0.1639,-2.9});
rokae::MoveAbsJCommand absjcmd({0.0,0.22150561307150393,1.4779577696969546,0.0,1.2675963456219013,0.0});
// Spiral 1: Initial radius 0.01m, radius change step 0.0005m/rad, counterclockwise rotation 720°, speed v500
rokae::MoveSPCommand spcmd1({cart_target, 0.01, 0.0005, M_PI * 4, false, 500}),
// Spiral 2: Initial radius 0.05m, radius change step 0.001m/rad, clockwise rotation 360°, speed v100
spcmd2({cart_target1, 0.05, 0.001, M_PI * 2, true, 100});
auto duration1 = 100ms; // 100 milliseconds
MoveWaitCommand waittime(duration1);
std::vector<rokae::MoveSPCommand> spcmds = {spcmd1, spcmd2};
robot.moveAppend({absjcmd}, id, ec);
robot.moveAppend({ waittime }, id, ec);// Motion command waits 100ms before executing next command
robot.moveAppend(spcmds, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, (int)spcmds.size() - 1);
}
External Axis/Rail Control
/**
* @brief Example - Motion with rail. Points applicable to model xMateSR4
* Demonstrates how to control robot system with rail
* @param robot Robot object pointer
*/
template <WorkType wt, unsigned short dof>
void moveWithRail(Robot_T<wt, dof> *robot) {
error_code ec;
bool is_rail_enabled;
// Check if rail function is enabled
robot->getRailParameter("enable", is_rail_enabled, ec);
if(!is_rail_enabled) {
print(os, "Rail not enabled");
return;
}
// Set rail parameters (requires controller restart to take effect)
robot->setRailParameter("enable", true, ec); // Enable rail function
robot->setRailParameter("maxSpeed", 1, ec); // Set maximum speed 1m/s
robot->setRailParameter("softLimit", std::vector<double>({-0.8, 0.8}), ec); // Set soft limit to +-0.8m
robot->setRailParameter("reductionRatio", 1.0, ec); // Set reduction ratio
auto curr = robot->BaseRobot::jointPos(ec);
print(os, "Current axis angles", robot->BaseRobot::jointPos(ec));
// *** Jog rail example ***
// Switch to manual mode and power on
robot->setOperateMode(OperateMode::manual, ec);
robot->setPowerState(true, ec);
std::vector<double> soft_limit;
robot->getRailParameter("softLimit", soft_limit, ec);
// Jog within soft limit
double step = (curr.back() - soft_limit[0] > 0.1 ? 0.1 : (curr.back() - soft_limit[0])) * 1000.0;
// Determine rail axis index (for 6-axis robot, external axis index is 6)
int ex_jnt_index = robot->robotInfo(ec).joint_num;
// Rail joint space negative motion 100mm
robot->startJog(JogOpt::jointSpace, 0.6, step, ex_jnt_index, false, ec);
// Wait for jog to end
while(true) {
std::this_thread::sleep_for(std::chrono::milliseconds(50));
if(robot->operationState(ec) != OperationState::jogging) break;
}
robot->stop(ec);
// *** Motion command example with rail ***
CartesianPosition p0({0.56, 0.136, 0.416, M_PI, 0, M_PI}),
p1({0.56, 0.136, 0.3, M_PI, 0, M_PI});
p0.external = { 0.02 }; // Rail position 0.02m
p1.external = { -0.04 }; // Rail position -0.04m
MoveAbsJCommand abs_j_command({0, M_PI/6, -M_PI_2,0, -M_PI/3, 0 });
abs_j_command.target.external = { 0.1 }; // Rail moves to 0.1m
MoveJCommand j_command(p0);
MoveLCommand l_command(p1);
MoveCCommand c_command(p1, p0);
// Add custom information, will be returned in motion information feedback
l_command.customInfo = "hello";
std::string id;
robot->moveAppend(abs_j_command, id, ec);
robot->moveAppend(j_command, id, ec);
robot->moveAppend(l_command, id, ec);
robot->moveAppend(abs_j_command, id, ec);
robot->moveAppend(c_command, id, ec);
robot->moveStart(ec);
waitForFinish(*robot, id, 0);
}
Force Control Command Operations
#include <thread>
#include "rokae/robot.h"
#include "print_helper.hpp"
using namespace rokae;
/**
* @brief Wait for robot motion to complete
* @param robot Robot pointer
*/
void waitRobot(BaseRobot *robot) {
bool running = true;
while (running) {
std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Check every 100ms
error_code ec;
auto st = robot->operationState(ec); // Get robot operation state
// When robot is in idle or unknown state, consider motion completed
if(st == OperationState::idle || st == OperationState::unknown){
running = false;
}
}
}
/**
* @brief Example - Cartesian force control. Applicable model: xMateCR
* Function: Perform force control in Cartesian space, suitable for scenarios requiring precise force control
* Example - cartesian space force control
*/
void fcCartesianControl(xMateRobot &robot) {
error_code ec;
// Set handheld tool coordinate system, rotate 90° around Y-axis
// Tool coordinate system defines the position and posture of tool end relative to flange
Toolset toolset1;
toolset1.end.rpy[1] = M_PI_2; // Rotate 90 degrees around Y-axis
robot.setToolset(toolset1, ec);
// Get force control interface
auto fc = robot.forceControl();
// Force control initialization, using tool coordinate system
// FrameType::tool means force control in tool coordinate system
fc.fcInit(FrameType::tool, ec);
// Set control type to Cartesian control mode (1=Cartesian control, 0=joint control)
fc.setControlType(1, ec);
// Set Cartesian stiffness. This example uses tool coordinate system, so tool coordinate system x direction has 0 impedance, other directions have larger impedance
// Parameter format: {x, y, z, rx, ry, rz} - corresponding to 6 DOF stiffness
// x direction stiffness=0: Completely compliant in tool x direction, can move freely
// Other directions high stiffness: Maintain posture stability
fc.setCartesianStiffness({0, 1000, 1000, 500, 500, 500}, ec);
// Start force control
print(std::cout , "Start Cartesian mode force control");
fc.fcStart(ec);
#if 0
// Set load, please set according to actual situation to ensure safety
// Commented load setting code, need to set according to tool weight when actually using
Load load;
load.mass = 1; // Load mass 1kg
fc.setLoad(load, ec);
#endif
// Set desired force - Apply 1N force in tool coordinate system Z-axis direction
// Parameter format: {fx, fy, fz, tx, ty, tz} - force and torque
fc.setCartesianDesiredForce({0, 0, 1, 0, 0, 0}, ec);
// Press enter to end force control
// Wait for user to press enter key to stop force control
while(getchar() != '\n');
fc.fcStop(ec);
}
/**
* @brief Example - Joint mode force control. Applicable model: xMateCR
* Function: Perform force control in joint space, directly control joint torque
*/
template <unsigned short DoF>
void fcJointControl(ForceControl_T<DoF> &fc) {
error_code ec;
// Force control initialization, using base coordinate system
fc.fcInit(FrameType::base, ec);
// Set control type to joint control mode
fc.setControlType(0, ec);
// Set each axis stiffness. 2nd and 4th axes have small impedance, others have larger impedance
// Joints with small stiffness are easier to be pushed by external force, joints with large stiffness maintain position
fc.setJointStiffness({1000, 10, 1000, 5, 50, 50}, ec);
print(std::cout, "Start joint mode force control");
fc.fcStart(ec);
// Set desired torque - Apply different desired torques to each joint
// Parameter: 6 joint desired torque values (Nm)
fc.setJointDesiredTorque({1, 1, 3, 0, 0, 0}, ec);
// Press enter to end force control
while(getchar() != '\n');
fc.fcStop(ec);
}
/**
* @brief Example - Search motion & force control monitoring. Test model: xMateER3
* Function: Superimpose search motion on force control basis, used for finding specific positions or contour tracking
*/
void fcOverlay(xMateRobot &robot){
error_code ec;
// Set handheld tool coordinate system, rotate 90° around Y-axis
Toolset toolset1;
toolset1.end.rpy[1] = M_PI_2;
robot.setToolset(toolset1, ec);
auto fc = robot.forceControl();
/******************************************************************
* Force control monitoring parameter setting - Safety protection
* Set various limit values, exceeding these values will trigger protection
******************************************************************/
// Set maximum joint speed (rad/s)
fc.setJointMaxVel({3.0, 3.0, 3.5, 3.5, 3.5, 4.0}, ec);
// Set maximum joint momentum (kg·m²/s)
fc.setJointMaxMomentum({0.1, 0.1, 0.1, 0.055, 0.055, 0.055}, ec);
// Set maximum joint kinetic energy (J)
fc.setJointMaxEnergy({250, 250, 250, 150, 150, 100}, ec);
// Set Cartesian space maximum speed (m/s, rad/s)
fc.setCartesianMaxVel({1.0, 1.0, 1.0, 2.5, 2.5, 2.5}, ec);
// Start monitoring
fc.fcMonitor(true, ec);
/******************************************************************
* Search motion setting
******************************************************************/
// Force control initialization
fc.fcInit(FrameType::tool, ec);
// Search motion must be Cartesian impedance control
fc.setControlType(1, ec);
// Set sine search motion around Z-axis (since force control coordinate system is tool coordinate system, this is tool Z-axis)
// Parameters: Motion axis(2=Z-axis), frequency(6Hz), amplitude type(1), amplitude(π), phase(1)
fc.setSineOverlay(2, 6, 1, M_PI, 1, ec);
// Start force control
fc.fcStart(ec);
// Superimpose XZ plane Lissajous search motion - generate complex trajectory
// Lissajous curve: synthesis of two perpendicular sinusoidal motions
fc.setLissajousOverlay(1, 5, 1, 10, 5, 0, ec);
// Start search motion
print(std::cout, "Start search motion");
fc.startOverlay(ec);
#if 0
// Pause and restart search motion (example code, not actually executed)
fc.pauseOverlay(ec);
fc.restartOverlay(ec);
#endif
// Press enter to end force control
while(getchar() != '\n');
fc.stopOverlay(ec);
// Monitoring parameters restore to default values
fc.fcMonitor(false, ec);
// Stop force control
fc.fcStop(ec);
}
/**
* @brief Example - Set force control termination conditions. Test model: xMateER3
* Function: Set force or position termination conditions, automatically stop when conditions are met
*/
void fcCondition(xMateRobot &robot) {
auto fc = robot.forceControl();
error_code ec;
// Set tool coordinate system, offset 0.1m in Z direction
Toolset toolset;
toolset.ref.trans[2] = 0.1;
robot.setToolset(toolset, ec);
// Force control initialization, using world coordinate system
fc.fcInit(FrameType::world, ec);
fc.setControlType(1, ec);
fc.fcStart(ec);
// Set force limit conditions
// Parameters: {fx_min, fx_max, fy_min, fy_max, fz_min, fz_max}, enable, timeout
// Trigger termination when measured force exceeds set range
fc.setForceCondition({-20, 20, -15, 15, -15, 15}, true, 20, ec);
// Set cuboid area limit conditions
// isInside=false means terminate waiting when inside this area
// Coordinate system where the cuboid is located, will overlay external workpiece coordinate system
Frame supvFrame;
supvFrame.trans[2] = -0.1; // Coordinate system offset -0.1m in Z direction
// Cuboid range: {x_min, x_max, y_min, y_max, z_min, z_max}
fc.setPoseBoxCondition(supvFrame, {-0.6, 0.6, -0.6, 0.6, 0.2, 0.3}, false, 20, ec);
// Block waiting for termination conditions to be met
print(std::cout, "Start waiting");
fc.waitCondition(ec);
print(std::cout, "Waiting ended, stop force control");
fc.fcStop(ec);
}
/**
* @brief Read end torque information
* Function: Read and display current torque sensor data
*/
template <unsigned short DoF>
void readTorqueInfo(ForceControl_T<DoF> &fc) {
error_code ec;
std::array<double, DoF> joint_torque{}, external_torque{};
std::array<double, 3> cart_force{}, cart_torque{};
// Read current torque information
// Read torque data from flange coordinate system
fc.getEndTorque(FrameType::flange, joint_torque, external_torque, cart_torque, cart_force, ec);
// Print torque information
print(std::cout, "End torque");
print(std::cout, "Joint measured force -", joint_torque); // Joint measured torque
print(std::cout, "Joint external force -", external_torque); // External force torque
print(std::cout, "Cartesian torque -", cart_torque); // Cartesian space torque
print(std::cout, "Cartesian force -", cart_force); // Cartesian space force
}
/**
* @brief Example - Torque sensor calibration
* Function: Calibrate the robot's torque sensor to improve force control accuracy
*/
void example_CalibrateForceSensor(xMateRobot &robot) {
error_code ec;
// Calibrate all axes - Calibrate torque sensors for all joints
robot.calibrateForceSensor(true, 0, ec);
// Single axis (4th axis) calibration - Calibrate only specified joint
robot.calibrateForceSensor(false, 3, ec); // 3 represents the 4th joint (starting from 0)
}
/**
* @brief Main program
* Function: Demonstrates various force control functions of the robot
*/
int main() {
error_code ec;
xMateRobot robot;
// Connect to robot
try {
robot.connectToRobot("192.168.0.160");
} catch(const Exception &e) {
std::cerr << e.what();
return 1;
}
// Get force control interface and read torque information
auto fc = robot.forceControl();
readTorqueInfo(fc);
// Robot power on and mode setting
robot.setOperateMode(rokae::OperateMode::automatic, ec);
robot.setPowerState(true, ec);
// First move to drag posture, note to select correct model
// Different models have different drag postures, need to choose according to actual model
std::vector<double> drag_cr = {0, M_PI/6, -M_PI_2, 0, -M_PI/3, 0}, // CR series drag posture
drag_er = {0, M_PI/6, M_PI/3, 0, M_PI/2, 0}; // ER series drag posture
// Execute absolute joint motion to drag posture
robot.executeCommand({MoveAbsJCommand(drag_cr)}, ec);
// Wait for robot motion to complete
waitRobot(&robot);
// Run example programs
fcCartesianControl(robot); // Cartesian force control
fcJointControl(fc); // Joint force control
// Safe shutdown
robot.setPowerState(false, ec);
robot.setOperateMode(OperateMode::manual, ec);
return 0;
}
Reading End Button Status
#include <iostream>
#include <cmath>
#include <thread>
#include "rokae/robot.h"
#include "print_helper.hpp"
using namespace rokae;
using namespace std;
/**
* @brief Robot end button status reading example program
* Function: Demonstrates how to read the status of robot end buttons in real-time
* Application scenario: Used for robot teaching, manual control, status monitoring, etc.
*/
int main() {
try {
// Network configuration
std::string ip = "192.168.0.160"; // Robot controller IP address
std::error_code ec; // Error code object for receiving function call results
// Create robot object and connect
// Parameter 1: Robot IP address (192.168.0.160)
// Parameter 2: Local IP address (192.168.0.100) - must be on the same network segment as the robot
// Note: Since this example uses real-time status data, local address must be set
rokae::xMateRobot robot(ip, "192.168.0.100");
/******************************************************************
* 1. Read button status - non-real-time method
* Use getKeypadState interface to read current button status at once
******************************************************************/
KeyPadState state = robot.getKeypadState(ec);
// Print current status of all end buttons
// Usually the robot end has multiple buttons for teaching, mode switching, etc.
std::cout << "Current end button status, "
<< "key1: " << state.key1_state
<< ", key2: " << state.key2_state
<< ", key3: " << state.key3_state
<< ", key4: " << state.key4_state
<< ", key5: " << state.key5_state
<< ", key6: " << state.key6_state
<< ", key7: " << state.key7_state
<< std::endl;
/******************************************************************
* 2. Configure real-time status reception
* Set robot to send status data at 1ms cycle, specifically receive button status
******************************************************************/
// startReceiveRobotState: Start receiving robot status data
// Parameter 1: Send cycle - 1 millisecond, robot sends status data every 1ms
// Parameter 2: Received data fields - only receive button status data to reduce network load
robot.startReceiveRobotState(std::chrono::milliseconds(1),
{ RtSupportedFields::keypads });
/******************************************************************
* 3. Initialize button status array
* Create array for storing real-time status of 7 buttons
******************************************************************/
std::array<bool, 7> keypad{}; // 7 boolean values, corresponding to 7 end buttons
// First read button status to array
robot.getStateData(RtSupportedFields::keypads, keypad);
/******************************************************************
* 4. Create real-time reading thread
* Continuously read button status in a new thread to avoid blocking main thread
******************************************************************/
int count = 50; // Read 50 times in loop
// Create thread for real-time reading button status
std::thread readKeyPad([&] {
// Loop 50 times, read button status once each time
while (count--) {
/******************************************************************
* Real-time status update process:
* 1. updateRobotState: Update robot status cache
* 2. getStateData: Read specific data from cache
******************************************************************/
// Step 1: Update robot status
// Receive latest status data from robot and update internal cache
// Parameter: Timeout 1ms, will return error if no data received within 1ms
robot.updateRobotState(std::chrono::milliseconds(1));
// Step 2: Read button data from status cache
// Read button status data into keypad array
robot.getStateData(RtSupportedFields::keypads, keypad);
// Step 3: Print current status of all buttons
std::cout << "Current end button status, "
<< "key1: " << keypad[0]
<< ", key2: " << keypad[1]
<< ", key3: " << keypad[2]
<< ", key4: " << keypad[3]
<< ", key5: " << keypad[4]
<< ", key6: " << keypad[5]
<< ", key7: " << keypad[6]
<< std::endl;
// Note: No delay here, will read as fast as possible
// In actual application, appropriate delay may be needed
}
});
/******************************************************************
* 5. Wait for thread to end
* Main thread waits for reading thread to complete 50 reads before continuing execution
******************************************************************/
readKeyPad.join(); // Block main thread until readKeyPad thread completes
}
catch (const std::exception& e) {
// Exception handling: Catch and print all exceptions
std::cout << "Program execution error: " << e.what() << std::endl;
}
return 0;
}
Pass-through Protocol for Controlling End Tools
Different manufacturers' end tools send different data, please refer to the demo to change the data structure,
#include <iostream>
#include <cmath>
#include <thread>
#include <chrono>
#include "rokae/robot.h"
#include "print_helper.hpp"
using namespace rokae;
/**
* @brief DH gripper parameter namespace
* Contains all parameters and data structures related to DH gripper communication
*/
namespace DhGripParams {
// Modbus RTU communication related parameters
// Original Modbus data frame for initializing gripper
std::vector<uint8_t> send_data = { 0x01,0x06,0x01,0x00,0x00,0xA5,0x48,0x4D };
// Buffer for receiving gripper return data
std::vector<uint8_t> rev_data = { 0,0,0,0,0,0,0,0 };
// Gripper initialization parameters
std::vector<int> init_set = { 0xA5 };
// Parameters for getting initialization status
std::vector<int> init_get = { 100 };
// Parameters for getting gripping status
std::vector<int> grip_status_get = { 0 };
// Torque setting parameters (range: 20-100)
std::vector<int> trq_set = { 100 };
// Speed setting parameters (range: 1-100)
std::vector<int> vel_set = { 2 };
// Position setting parameters (range: 0-1000)
std::vector<int> pos_set = { 100 };
// Torque reading buffer
std::vector<int> trq_get = { 0 };
// Speed reading buffer
std::vector<int> vel_get = { 0 };
// Position reading buffer
std::vector<int> pos_get = { 0 };
// Real-time position reading buffer
std::vector<int> pos_now_get = { 0 };
}
/**
* @brief DH gripper initialization function
* Function: Initialize DH gripper through Modbus RTU protocol
* Principle: Send specific initialization command sequence to gripper controller
* @param robot Robot object reference
*/
void DHGripInit(xMateRobot &robot) {
error_code ec;
// Method 1: Initialize using original data frame (commented code)
// std::vector<uint8_t> send_data = { 0x01,0x06,0x01,0x00,0x00,0xA5,0x48,0x4D };
// robot.XPRS485SendData(send_data.size(), rev_data.size(), send_data, DhGripParams::rev_data, ec);
// Method 2: Initialize using Modbus RTU register write operation
// Parameter description:
// 0x01 - Slave address (gripper device address)
// 0x06 - Function code (write single register)
// 0x0100 - Register address (initialization command register)
// "int16" - Data type (16-bit signed integer)
// 1 - Data length
// init_set - Initialization data {0xA5}
// false - Non-blocking mode
std::vector<int> init_set = { 0xA5 };
robot.XPRWModbusRTUReg(1, 0x06, 0x0100, "int16", 1, init_set, false, ec);
std::cout << "Query DH initialization execution result: " << ec << std::endl;
}
/**
* @brief DH gripper motion control function
* Function: Set gripper torque, speed and target position, and execute motion
* @param robot Robot object reference
* @param trq_set Torque setting value (20-100)
* @param vel_set Speed setting value (1-100)
* @param pos_set Position setting value (0-1000)
*/
void DHGripMove(xMateRobot &robot, int& trq_set, int& vel_set, int& pos_set) {
error_code ec;
// Set torque parameter
std::vector<int> trq_set_vec;
trq_set_vec.push_back(trq_set);
// Register address 0x0101: Torque setting register
robot.XPRWModbusRTUReg(0x01, 0x06, 0x0101, "int16", 0x01, trq_set_vec, false, ec);
std::cout << "Set force execution result: " << ec << std::endl;
// Set speed parameter
std::vector<int> vel_set_vec;
vel_set_vec.push_back(vel_set);
// Register address 0x0104: Speed setting register
robot.XPRWModbusRTUReg(0x01, 0x06, 0x0104, "int16", 0x01, vel_set_vec, false, ec);
std::cout << "Set speed execution result: " << ec << std::endl;
// Set position parameter
std::vector<int> pos_set_vec;
pos_set_vec.push_back(pos_set);
// Register address 0x0103: Position setting register
robot.XPRWModbusRTUReg(0x01, 0x06, 0x0103, "int16", 0x01, pos_set_vec, false, ec);
std::cout << "Set position execution result: " << ec << std::endl;
}
/**
* @brief Get DH gripper initialization status
* Function: Read gripper initialization status to confirm if initialization is complete
* @param robot Robot object reference
* @param init_get Initialization status output parameter
* 0: Initialization completed
* 1: Initializing
* 2: Initialization failed
*/
void DHGripGetInitStatus(xMateRobot& robot, int& init_get) {
error_code ec;
std::vector<int> init_get_vec = { -1 }; // Initialize to invalid value
// Register address 0x0200: Initialization status register
robot.XPRWModbusRTUReg(0x01, 0x03, 0x0200, "int16", 0x01, init_get_vec, false, ec);
init_get = init_get_vec[0];
std::cout << "Get initialization status execution result: " << ec << std::endl;
}
/**
* @brief Get DH gripper gripping status
* Function: Read current gripping status of gripper
* @param robot Robot object reference
* @param grip_status_get Gripping status output parameter
* 0-3: Different gripping states
*/
void DHGripGetStatus(xMateRobot& robot, int& grip_status_get) {
error_code ec;
std::vector<int> grip_status_get_vec = { -1 }; // Initialize to invalid value
// Register address 0x0201: Gripping status register
robot.XPRWModbusRTUReg(0x01, 0x03, 0x0201, "int16", 0x01, grip_status_get_vec, false, ec);
grip_status_get = grip_status_get_vec[0];
std::cout << "Get gripping status execution result: " << ec << std::endl;
}
/**
* @brief Get DH gripper operation information
* Function: Read current torque, speed and position information of gripper
* @param robot Robot object reference
* @param trq_get Torque reading output parameter
* @param vel_get Speed reading output parameter
* @param pos_get Position reading output parameter
*/
void DHGripGetInfo(xMateRobot& robot, int& trq_get, int& vel_get, int& pos_get) {
error_code ec;
// Get torque information
std::vector<int> trq_get_vec = { 0 };
robot.XPRWModbusRTUReg(0x01, 0x03, 0x0101, "int16", 0x01, trq_get_vec, false, ec);
trq_get = trq_get_vec[0];
std::cout << "Get force execution result: " << ec << std::endl;
// Get speed information
std::vector<int> vel_get_vec = { 0 };
robot.XPRWModbusRTUReg(0x01, 0x03, 0x0104, "int16", 0x01, vel_get_vec, false, ec);
vel_get = vel_get_vec[0];
std::cout << "Get speed execution result: " << ec << std::endl;
// Get position information
std::vector<int> pos_get_vec = { 0 };
robot.XPRWModbusRTUReg(0x01, 0x03, 0x0103, "int16", 0x01, pos_get_vec, false, ec);
pos_get = pos_get_vec[0];
std::cout << "Get position execution result: " << ec << std::endl;
}
/**
* @brief Get DH gripper real-time position
* Function: Read real-time position information of gripper
* @param robot Robot object reference
* @param pos_now_get Real-time position output parameter
*/
void DHGripGetNewPos(xMateRobot& robot, int& pos_now_get) {
error_code ec;
std::vector<int> pos_now_get_vec = { 0 };
// Register address 0x0202: Real-time position register
robot.XPRWModbusRTUReg(1, 0x03, 0x0202, "int16", 1, pos_now_get_vec, false, ec);
pos_now_get = pos_now_get_vec[0];
std::cout << "Get real-time position execution status: " << ec << std::endl;
}
/**
* @brief Coil test interface (for suction cups and other devices)
* Function: Test Modbus RTU coil read/write function
* Coils are used to control digital devices such as solenoid valves, relays, etc.
*/
void CoilTest(xMateRobot& robot) {
error_code ec;
// Test data definition
std::vector<bool> bool_data_len1 = { 0 };
std::vector<bool> bool_data_len4 = { 0,0,0,1 };
std::vector<bool> bool_data_len10 = { 1,1,1,1,1,1,1,1,1,1 };
// Function code 0x01: Read coil
robot.XPRWModbusRTUCoil(0x01, 0x01, 0x0001, 1, bool_data_len1, false, ec);
std::cout << "coiltest 0x01 code:" << ec << std::endl;
// Function code 0x02: Read discrete input
robot.XPRWModbusRTUCoil(0x01, 0x02, 0x0001, 1, bool_data_len1, false, ec);
std::cout << "coiltest 0x02 code:" << ec << std::endl;
// Function code 0x05: Write single coil
robot.XPRWModbusRTUCoil(0x01, 0x05, 0x0001, 1, bool_data_len4, false, ec);
std::cout << "coiltest 0x05 code:" << ec << std::endl;
// Function code 0x0F: Write multiple coils
robot.XPRWModbusRTUCoil(0x01, 0x0F, 0x0001, 4, bool_data_len4, false, ec);
std::cout << "coiltest 0x10 code:" << ec << std::endl;
robot.XPRWModbusRTUCoil(0x01, 0x0F, 0x0001, 10, bool_data_len10, false, ec);
std::cout << "coiltest 0x10 code:" << ec << std::endl;
}
/**
* @brief Register function code test interface
* Function: Test Modbus RTU register read/write function
*/
void RegFuncCodeTest(xMateRobot& robot) {
error_code ec;
std::vector<int> int_data_len1 = { 0 };
std::vector<int> int_data_len3 = { 0,0,1 };
std::vector<int> int_data_len4 = { 0,0,0,1 };
// Function code 0x03: Read holding register
robot.XPRWModbusRTUReg(0x01, 0x03, 0x0001, "int16", 1, int_data_len1, false, ec);
std::cout << "regtest 0x03 code:" << ec << std::endl;
// Function code 0x04: Read input register
robot.XPRWModbusRTUReg(0x01, 0x04, 0x0001, "int16", 1, int_data_len1, false, ec);
std::cout << "regtest 0x04 code:" << ec << std::endl;
// Function code 0x06: Write single register
robot.XPRWModbusRTUReg(0x01, 0x06, 0x0001, "int16", 1, int_data_len3, false, ec);
std::cout << "regtest 0x06 code:" << ec << std::endl;
// Function code 0x10: Write multiple registers
robot.XPRWModbusRTUReg(0x01, 0x10, 0x0001, "int16", 1, int_data_len3, false, ec);
std::cout << "regtest 0x10 code:" << ec << std::endl;
robot.XPRWModbusRTUReg(0x01, 0x10, 0x0001, "int32", 1, int_data_len1, false, ec);
std::cout << "regtest 0x10 code:" << ec << std::endl;
}
/**
* @brief Register data type test interface (for RM gripper)
* Function: Test register read/write with different data types
*/
void RegDataTypeTest(xMateRobot& robot) {
error_code ec;
std::vector<int> int16_data_len1 = { 0 };
std::vector<int> int16_data_len2 = { 0 };
std::vector<int> int32_data_len1_1 = { 65535 };
std::vector<int> int32_data_len1_2 = { 255 };
// int16 data type test
robot.XPRWModbusRTUReg(1, 0x03, 0x0200, "int16", 1, int16_data_len1, false, ec);
std::cout << "regdate type test int16:" << ec << std::endl;
// uint16 data type test
robot.XPRWModbusRTUReg(1, 0x03, 0x0200, "uint16", 1, int16_data_len1, false, ec);
std::cout << "regdate type test uint16:" << ec << std::endl;
// int32 data type test
robot.XPRWModbusRTUReg(1, 0x10, 0x0200, "int32", 1, int32_data_len1_1, false, ec);
std::cout << "regdate type test int32:" << ec << std::endl;
robot.XPRWModbusRTUReg(1, 0x10, 0x0200, "int32", 1, int32_data_len1_2, false, ec);
std::cout << "regdate type test int32:" << ec << std::endl;
// uint32 data type test
robot.XPRWModbusRTUReg(1, 0x10, 0x0200, "uint32", 1, int32_data_len1_1, false, ec);
std::cout << "regdate type test uint32:" << ec << std::endl;
robot.XPRWModbusRTUReg(1, 0x10, 0x0200, "uint32", 1, int32_data_len1_2, false, ec);
std::cout << "regdate type test uint32:" << ec << std::endl;
}
/**
* @brief Raw data transmission test interface
* Function: Test direct transmission of raw Modbus data frames
*/
void RWData_Test(xMateRobot& robot) {
error_code ec;
// Initialize DH gripper using raw data frame
std::vector<uint8_t> send_data = { 0x01,0x06,0x01,0x00,0x00,0xA5,0x48,0x4D };
std::vector<uint8_t> rev_data = {};
robot.XPRS485SendData((int)send_data.size(), (int)rev_data.size(), send_data, DhGripParams::rev_data, ec);
std::cout << "DH gripper initialization execution result by raw transmission: " << ec << std::endl;
}
/**
* @brief Main program
* Function: Demonstrates complete control flow of DH gripper
*/
int main() {
try {
// Network configuration
std::string ip = "192.168.21.10";
std::error_code ec;
// Create robot object and connect
rokae::xMateRobot robot(ip, "192.168.21.10");
std::cout << "Establish connection with Robot" << std::endl;
// Test functions (commented code, can be enabled as needed)
// CoilTest(robot); // Coil interface test
// RegFuncCodeTest(robot); // Register function code test
// RegDataTypeTest(robot); // Register data type test
// -------------------------- DH Gripper Control Main Flow --------------------------
// Step 1: Open end xPanel, provide 24V power and RS485 communication
std::cout << "Setting xpanel..." << std::endl;
// Parameter description:
// supply24v - Provide 24V power
// true - Enable RS485 communication
robot.setxPanelRS485(xPanelOpt::Vout::supply24v, true, ec);
std::cout << "Result of setting xpanel: " << ec << std::endl;
// Step 2: DH gripper initialization
DHGripInit(robot);
// Step 3: Wait for initialization to complete (DH gripper initialization takes about 7 seconds)
int i = 14; // 14 times * 0.5 seconds = 7 seconds
while (i--) {
int init_status = -1;
DHGripGetInitStatus(robot, init_status);
std::cout << "Get initialization status: "<< init_status << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
// Step 4: Set gripper parameters and execute motion
int trq_set = 100; // Torque setting (20-100)
int vel_set = 2; // Speed setting (1-100)
int pos_set = 0; // Position setting (0-1000)
DHGripMove(robot, trq_set, vel_set, pos_set);
// Step 5: Read gripper operation information
int trq_get = 0;
int vel_get = 0;
int pos_get = 0;
DHGripGetInfo(robot, trq_get, vel_get, pos_get);
std::cout << "Obtained force: " << trq_get << std::endl;
std::cout << "Obtained speed: " << vel_get << std::endl;
std::cout << "Obtained position: " << pos_get << std::endl;
// Step 6: Continuously monitor gripper status
while (true) {
int grip_status_get = -1;
DHGripGetStatus(robot, grip_status_get);
std::cout << "Get gripper gripping status: "<< grip_status_get << std::endl;
int pos_now_get = -1;
DHGripGetNewPos(robot, pos_now_get);
std::cout << "Get real-time position: " << pos_now_get << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
}
catch (const std::exception& e) {
std::cout << "Program exception: " << e.what() << std::endl;
}
return 0;
}
Path Recording and Playback
Suitable for collaborative robots, not used for industrial robots
Note: The paths saved by HMI and SDK drag teaching are different and cannot be mixed
#include <iostream>
#include <thread>
#include <unordered_map>
#include "rokae/robot.h"
#include "print_helper.hpp"
using namespace std;
using namespace rokae;
char parseInput(std::string &str);
void WaitRobot(BaseRobot *robot);
void printHelp();
/**
* @brief Print motion execution information
* This callback function will be automatically called when the robot motion state changes
* @param info Structure containing detailed motion execution information
*/
void printInfo(const rokae::EventInfo &info) {
using namespace rokae::EventInfoKey::MoveExecution;
// Extract and print key motion execution information
print(std::cout, "[Motion Execution Info] ID:", std::any_cast<std::string>(info.at(ID)),
"Index:", std::any_cast<int>(info.at(WaypointIndex)),
"Completed: ", std::any_cast<bool>(info.at(ReachTarget)) ? "YES": "NO",
std::any_cast<error_code>(info.at(Error)),
std::any_cast<std::string>(info.at(Remark)));
}
/**
* @brief Main program - Robot drag teaching and path playback system
* This program implements a complete robot drag teaching function, including:
* 1. Manual drag recording path
* 2. Path saving and management
* 3. Path playback execution
* 4. Robot status monitoring
*/
int main() {
try {
// ==================== Robot Initialization and Connection ====================
std::string ip = "192.168.21.10"; // Robot controller IP address
error_code ec; // Error code object for receiving function call results
std::vector<std::string> paths; // List to store saved path names
// Create xMate 6-axis robot instance and connect
xMateRobot robot(ip);
// Set motion control mode to non-real-time command mode (NrtCommand)
// This mode is suitable for scenarios with high non-real-time requirements such as teaching and path playback
robot.setMotionControlMode(MotionControlMode::NrtCommand, ec);
// Set motion event listener, automatically call printInfo function when motion state changes
robot.setEventWatcher(Event::moveExecution, printInfo, ec);
// Print usage instructions
printHelp();
// ==================== Main Command Loop ====================
char cmd = ' '; // Command character
while(cmd != 'q') { // 'q' command exits the loop
std::string str; // Store the complete string entered by the user
// Read user input from console
getline(std::cin, str);
// Parse input, extract command character and parameters
cmd = parseInput(str);
// Execute corresponding operations based on command character
switch(cmd) {
case 'p': // Power control command
if(str == "on") {
robot.setPowerState(true, ec);
std::cout << "* Robot powered on\n";
} else {
robot.setPowerState(false, ec);
std::cout << "* Robot powered off\n";
}
if(ec) break; // If there is an error, jump out of switch to execute error handling
continue; // Continue waiting for next command
case 'm': // Mode switching command
if(str == "manual") {
robot.setOperateMode(OperateMode::manual, ec);
std::cout << "* Manual mode\n";
} else {
robot.setOperateMode(OperateMode::automatic, ec);
std::cout << "* Automatic mode\n";
}
if(ec) break;
continue;
case 'd': // Drag function control
// Preconditions for enabling drag: manual mode and robot powered off state
if(str == "on") {
// Enable Cartesian space free drag mode
robot.enableDrag(DragParameter::cartesianSpace, DragParameter::freely, ec);
cout << "* Drag enabled\n";
} else {
robot.disableDrag(ec);
std::cout << "* Drag disabled\n";
}
if(ec) break;
continue;
case 'a': // Start recording path
// Start recording path, parameter 30 means maximum recording duration 30 seconds
robot.startRecordPath(30, ec);
std::cout << "* Start recording path\n";
if(ec) break;
continue;
case 'b': // Stop recording path
robot.stopRecordPath(ec);
std::cout << "* Stop recording path\n";
if(ec) break;
continue;
case 's': // Save recorded path
// str contains the path name specified by the user
robot.saveRecordPath(str, ec);
std::cout << "* Save path as: " << str << endl;
if(ec) break;
continue;
case 'c': // Cancel recording (not save)
robot.cancelRecordPath(ec);
cout << "* Cancel recording\n";
if(ec) break;
continue;
case 'u': // Query all saved paths
paths = robot.queryPathLists(ec);
if(paths.empty())
cout << "* No saved paths\n";
else {
cout << "* Saved paths: ";
for(auto p : paths)
cout << p << ", ";
cout << endl;
}
if(ec) break;
continue;
case 'v': // Delete specified path
cout << "* Delete path\"" << str << "\"\n";
robot.removePath(str, ec);
if(ec) break;
continue;
case 'r': { // Replay specified path
// Replay path, 1.0 means 100% speed playback
robot.replayPath(str, 1.0, ec);
if (ec) break;
// Start executing playback motion
robot.moveStart(ec);
if (ec) break;
cout << "* Start replaying path\"" << str << "\", speed 100%\n";
// Wait for playback motion to complete
WaitRobot(&robot);
cout << "* Replay ended\n";
continue;
}
case 'z': // Reset motion cache
robot.moveReset(ec);
cout << "* Reset motion cache\n";
if(ec) break;
continue;
case 'h': // Display help information
printHelp();
continue;
case 'q': // Exit program
std::cout << " --- Quit --- \n";
continue;
default: // Invalid command
std::cerr << "Invalid input\n";
continue;
}
// Error handling: If an error occurs when executing the command, print error information
cerr << "! Error message: " << ec.message() << endl;
}
// ==================== Program Exit Cleanup ====================
// Disconnect from the robot
robot.disconnectFromRobot(ec);
} catch (const rokae::Exception &e) {
// Catch and print all exceptions thrown by the rokae library
std::cerr << "Program exception: " << e.what();
}
return 0;
}
// ==================== Global Variables and Function Definitions ====================
/**
* @brief Command mapping table
* Map user input command strings to single characters for easy switch statement processing
*/
static const std::unordered_map<std::string, char> ConsoleInput = {
{"quit", 'q'}, // Exit program
{"power", 'p'}, // Power control
{"drag", 'd'}, // Drag control
{"mode", 'm'}, // Mode switching
{"start", 'a'}, // Start recording
{"stop", 'b'}, // Stop recording
{"save", 's'}, // Save path
{"cancel", 'c'}, // Cancel recording
{"query", 'u'}, // Query paths
{"remove", 'v'}, // Delete path
{"reset", 'z'}, // Reset motion
{"replay", 'r'}, // Replay path
{"help", 'h'} // Display help
};
/**
* @brief Print usage instructions
* Display all available commands and their usage
*/
void printHelp() {
cout << " --- Drag and Path Playback Usage Example --- " << endl
<< "Format <command>[:parameter] e.g. save:track0" << endl << endl
<< "Command | Parameter" << endl
<< "power Robot power | on|off" << endl
<< "mode Manual/Auto mode| manual|auto" << endl
<< "drag Enable/disable drag | on|off" << endl
<< "start Start recording |" << endl
<< "stop Stop recording |" << endl
<< "save Save path | Path name" << endl
<< "cancel Cancel recording|" << endl
<< "query Query saved paths|" << endl
<< "remove Delete path | Path name" << endl
<< "reset Reset motion cache|" << endl
<< "replay Path replay | Path name" << endl
<< "quit Exit\n" << endl
<< "Usage examples:" << endl
<< " power:on - Power on robot" << endl
<< " mode:manual - Switch to manual mode" << endl
<< " drag:on - Enable drag function" << endl
<< " start - Start recording path" << endl
<< " stop - Stop recording" << endl
<< " save:my_path - Save path as my_path" << endl
<< " replay:my_path - Replay path my_path" << endl;
}
/**
* @brief Parse user input
* Parse input string into command character and parameters
* @param str User input string (will be modified, parameter part remains in str)
* @return Command character, returns ' ' if invalid
*/
char parseInput(std::string &str) {
size_t delimiter;
std::string cmd(str); // Default entire string is command
// Find delimiter ':'
if((delimiter = str.find(':')) != std::string::npos) {
// Separate command and parameters
cmd = str.substr(0, delimiter); // Command part
str = str.substr(delimiter + 1); // Parameter part (modify original string)
}
// Find corresponding command character in command mapping table
if(ConsoleInput.count(cmd))
return ConsoleInput.at(cmd);
else
return ' '; // Invalid command
}
/**
* @brief Wait for robot motion to end
* Detect whether motion is complete by polling robot operation status
* @param robot Robot object pointer
*/
void WaitRobot(BaseRobot *robot) {
bool running = true; // Motion status flag
while (running) {
// Check status every 100 milliseconds to avoid excessive CPU usage
std::this_thread::sleep_for(std::chrono::milliseconds(100));
error_code ec;
auto st = robot->operationState(ec);
// If robot is in idle or unknown state, consider motion ended
if(st == OperationState::idle || st == OperationState::unknown){
running = false;
}
}
}
Reading Robot Status Data
Difference between jointPos and posture
//Robot current axis angles, robot body + external axis, unit: radians, external axis guide, unit meters
std::vector<double> rokae::BaseRobot::jointPos (error_code & ec);
//Get the current pose of the robot flange or end, return: double array, [X , Y , Z , Rx , Ry , Rz ], where translation unit is meters and rotation unit is radians
std::array< double,6>rokae::BaseRobot::posture(CoordinateType ct, error_code & ec);
#include <thread>
#include <atomic>
#include <fstream>
#include "rokae/utility.h"
#include "rokae/robot.h"
#include "print_helper.hpp"
using namespace std;
using namespace rokae;
/**
* @brief Wait for robot motion to complete
* @param robot Robot pointer
* Function: Poll robot operation status until robot enters idle state
*/
void WaitRobot(BaseRobot *robot) {
bool checking = true;
while (checking) {
std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Check every 100ms
error_code ec;
auto st = robot->operationState(ec); // Get robot operation status
// When robot is in idle or unknown state, consider motion complete
if(st == OperationState::idle || st == OperationState::unknown){
checking = false;
}
}
}
/**
* @brief Robot status data recording example program
* Function: Record end pose and joint angle data to CSV file in real-time during robot motion
* Application scenario: Motion trajectory analysis, performance testing, data collection, etc.
*/
int main() {
try {
using namespace RtSupportedFields;
/******************************************************************
* 1. Robot initialization and connection
******************************************************************/
// Create robot object and establish connection
// Parameter 1: Robot IP address (192.168.0.160)
// Parameter 2: Local IP address (192.168.0.100) - must be on the same network segment as the robot
xMateRobot robot("192.168.0.160", "192.168.0.100");
error_code ec;
std::ostream &os = std::cout;
// Set motion control mode to non-real-time command mode
// NrtCommand mode is suitable for non-real-time, low-speed control scenarios
robot.setMotionControlMode(rokae::MotionControlMode::NrtCommand, ec);
/******************************************************************
* 2. Configure status data reception
******************************************************************/
// Start receiving robot status data, set send interval to 1 second
// Received data fields:
// - tcpPoseAbc_m: End pose (ABC Euler angle representation)
// - tau_m: Joint torque
// - jointPos_m: Joint angles
//jointVel_m: joint velocity
robot.startReceiveRobotState(chrono::seconds(1), {tcpPoseAbc_m, tau_m, jointPos_m,jointVel_m});
// Define data storage arrays
std::array<double, 6> tcpPose{}; // Store end pose [x, y, z, A, B, C]
std::array<double, 6> arr6{}; // Store joint angles [j1, j2, j3, j4, j5, j6]
// Atomic boolean variable for inter-thread synchronization
std::atomic_bool running{true};
/******************************************************************
* 3. Clear status data queue
******************************************************************/
// The queue for receiving status data does not automatically overwrite old data
// Clear old data in buffer by loop reading to ensure starting from latest data
while (robot.updateRobotState(chrono::steady_clock::duration::zero()));
/******************************************************************
* 4. Create data recording file
******************************************************************/
// Create CSV file, filename includes timestamp to ensure uniqueness
std::ofstream ofs;
string filename = "read_" +
std::to_string(std::chrono::duration_cast<std::chrono::seconds>(
std::chrono::steady_clock::now().time_since_epoch()).count()) +
".csv";
ofs.open(filename, std::ios::out);
// Write CSV file header (optional)
ofs << "TCP_X,TCP_Y,TCP_Z,TCP_A,TCP_B,TCP_C,,Joint1,Joint2,Joint3,Joint4,Joint5,Joint6" << std::endl;
/******************************************************************
* 5. Create status data reading thread
******************************************************************/
std::thread readState([&] {
print(os, "Start recording robot status data...");
while (running) {
/******************************************************************
* Status data reading process:
* 1. updateRobotState: Update robot status cache (blocking wait for new data)
* 2. getStateData: Read specific type of data from cache
* 3. Write data to file
******************************************************************/
// Periodically get current status data, timeout set to 1 second (consistent with data send interval)
// If no new data is received within 1 second, the function will return timeout error
robot.updateRobotState(chrono::seconds(1));
// Read end pose data (TCP Pose in ABC Euler angles)
// Format: [x, y, z, A, B, C]
// x,y,z: Position coordinates (meters)
// A,B,C: Euler angles (radians)
robot.getStateData(tcpPoseAbc_m, tcpPose);
// Read joint angle data (Joint Positions)
// Format: [j1, j2, j3, j4, j5, j6] (radians)
robot.getStateData(jointPos_m, arr6);
/******************************************************************
* Write data to CSV file
* Format: TCP_X,TCP_Y,TCP_Z,TCP_A,TCP_B,TCP_C,,Joint1,Joint2,Joint3,Joint4,Joint5,Joint6
******************************************************************/
ofs << tcpPose[0] << "," << tcpPose[1] << "," << tcpPose[2] << ","
<< tcpPose[3] << "," << tcpPose[4] << "," << tcpPose[5] << ",,"
<< arr6[0] << "," << arr6[1] << "," << arr6[2] << ","
<< arr6[3] << "," << arr6[4] << "," << arr6[5] << std::endl;
}
print(os, "Stop recording robot status data");
});
/******************************************************************
* 6. Create robot motion thread
******************************************************************/
std::thread moveThread([&]{
print(os, "Start robot motion...");
// Set robot operation mode to automatic mode
robot.setOperateMode(rokae::OperateMode::automatic, ec);
// Turn on robot power (enable servo)
robot.setPowerState(true, ec);
/******************************************************************
* Define motion waypoints
* Use absolute joint motion command MoveAbsJCommand
******************************************************************/
// Point 1: All joints zero position
MoveAbsJCommand p1({0, 0, 0, 0, 0, 0});
// Point 2: Preset target position
// Joint angles: [0, 30°, 60°, 0, 90°, 0]
MoveAbsJCommand p2({0, M_PI/6, M_PI/3, 0, M_PI_2, 0});
// Execute motion sequence
std::string id; // Motion ID for tracking motion status
robot.moveAppend({p1, p2}, id, ec); // Add motion commands to queue
robot.moveStart(ec); // Start executing motion
// Wait for motion to complete
WaitRobot(&robot);
print(os, "Robot motion completed");
});
/******************************************************************
* 7. Thread synchronization and resource cleanup
******************************************************************/
// Wait for motion thread to complete
moveThread.join();
// Set running flag to false, stop data reading thread
running = false;
// Wait for data reading thread to complete
readState.join();
// Stop receiving robot status data
robot.stopReceiveRobotState();
// File stream will be automatically closed in destructor
print(os, "Data recording completed, file saved as: " + filename);
} catch(const std::exception &e) {
// Exception handling
print(std::cerr, "Program execution error: ", e.what());
}
return 0;
}
Loading and Running RL Project
Execution sequence: Query project information (info) -> Load project (load) -> Power on robot (on) -> Program pointer to main (main) -> Start running project (start) -> Pause running (pause)
#include <iostream>
#include <unordered_map>
#include "rokae/robot.h"
using namespace std;
using namespace rokae;
void printHelp();
/**
* @brief Command mapping table
* Map user input command strings to single characters for easy switch statement processing
*/
static const std::unordered_map<std::string, char> ConsoleInput = {
{"on", '0'}, // Power on robot
{"off", 'x'}, // Power off robot
{"quit", 'q'}, // Exit program
{"info", 'i'}, // Query project information
{"load", 'l'}, // Load project
{"main", 'm'}, // Program pointer to main
{"start", 's'}, // Start running project
{"pause", 'p'}, // Pause running
{"tool", 't'}, // Query tool information
{"wobj", 'w'}, // Query workpiece information
{"opt", 'o'}, // Set running parameters
{"help", 'h'} // Display help
};
/**
* @brief RL program execution callback function
* When RL program executes, this function will be automatically called to display program execution line number information
* @param info Structure containing RL program execution information
*/
void rlExecutionCb(const EventInfo &info) {
using namespace EventInfoKey::RlExecution;
// Extract RL program execution details from event information
std::string task_name = std::any_cast<std::string>(info.at(TaskName)); // Task name
std::string lookahead_file = std::any_cast<std::string>(info.at(LookaheadFile)); // Lookahead file
int lookahead_line = std::any_cast<int>(info.at(LookaheadLine)); // Lookahead line number
std::string execute_file = std::any_cast<std::string>(info.at(ExecuteFile)); // Execute file
int execute_line = std::any_cast<int>(info.at(ExecuteLine)); // Execute line number
// Print program execution status information
std::cout << "Task Name: " << task_name
<< " , Lookahead: " << lookahead_file << ", " << lookahead_line
<< ", Executing: " << execute_file << ", " << execute_line
<< std::endl;
}
/**
* @brief Main program - RL project operation manager
* This program implements the management and control of robot RL (Rokae Language) projects, including:
* 1. Project loading and operation control
* 2. Tool and workpiece information query
* 3. Program execution status monitoring
* 4. Operation parameter configuration
*/
int main() {
try {
// ==================== Robot Initialization and Connection ====================
std::string ip = "192.168.0.160"; // Robot controller IP address
std::error_code ec; // Error code object for receiving function call results
// Create xMate 6-axis robot instance and connect
rokae::xMateRobot robot(ip);
// ==================== Set RL Task Control Mode ====================
// Set to non-real-time RL task control mode, suitable for running pre-written RL programs
robot.setMotionControlMode(MotionControlMode::NrtRLTask, ec);
// Set RL program execution event listener, automatically call rlExecutionCb function when program executes
// This allows real-time monitoring of program execution progress
robot.setEventWatcher(rokae::Event::rlExecution, rlExecutionCb, ec);
// ==================== Set Operation Mode ====================
// Set to automatic mode, allowing RL project programs to run
robot.setOperateMode(OperateMode::automatic, ec);
// ==================== Main Command Loop ====================
printHelp(); // Display usage instructions
char cmd = ' '; // Command character
while(cmd != 'q') { // 'q' command exits the loop
std::string str; // Store the string entered by the user
// Read user input from console
getline(cin, str);
// Find corresponding command character in command mapping table
if(ConsoleInput.count(str)){
cmd = ConsoleInput.at(str);
} else {
cmd = ' '; // Invalid command
}
// Execute corresponding operations based on command character
switch(cmd) {
case '0': // Power on robot
robot.setPowerState(true, ec);
cout << "* Robot powered on\n";
if(ec) break;
continue;
case 'x': // Power off robot
robot.setPowerState(false, ec);
cout << "* Robot powered off\n";
if(ec) break;
continue;
case 'i': { // Query project information
cout << "* Query project information:\n";
// Get all project information in the controller
auto infos = robot.projectsInfo(ec);
if(infos.empty()) {
cout << "No projects\n";
} else {
// Traverse and display all project information
for(auto &info : infos) {
cout << "Name: " << info.name << " Tasks: ";
// Display all tasks included in the project
for(auto &t: info.taskList) {
cout << t << " ";
}
cout << endl;
}
}
if(ec) break;
} continue;
case 'l': { // Load project
cout << "* Load project, please enter the project name: ";
std::string name, line, task;
vector<string> tasks;
// Read project name
getline(cin, name);
cout << "Please enter the tasks to run, separated by spaces: ";
// Read task list to run
getline(cin, line);
istringstream iss(line);
while (iss >> task)
tasks.push_back(task);
// Load specified project and tasks
robot.loadProject(name, tasks, ec);
if(ec) break;
continue;
}
case 'm': // Program pointer to main
robot.ppToMain(ec);
cout << "* Program pointer to main\n";
if(ec) break;
continue;
case 's': // Start running project
robot.runProject(ec);
cout << "* Start running project\n";
if(ec) break;
continue;
case 'p': // Pause running
robot.pauseProject(ec);
cout << "* Pause running\n";
if(ec) break;
continue;
case 't': { // Query tool information
cout << "* Query tool information\n";
// Get all tool information defined in the controller
auto tools = robot.toolsInfo(ec);
if(tools.empty())
cout << "No tools\n";
else {
// Display tool details
for(auto &tool : tools) {
cout << "Tool: " << tool.name
<< ", Mass: " << tool.load.mass
<< "kg" << endl;
}
}
if(ec) break;
continue;
}
case 'w': { // Query workpiece information
cout << "* Query workpiece information\n";
// Get all workpiece information defined in the controller
auto wobjs = robot.wobjsInfo(ec);
if(wobjs.empty())
cout << "No workpieces\n";
else {
// Display workpiece details
for(auto &wobj : wobjs) {
cout << "Workpiece: " << wobj.name
<< ", Robot held: " << boolalpha << wobj.robotHeld
<< endl;
}
}
if(ec) break;
continue;
}
case 'o': { // Set operation parameters
cout << "* Set operation parameters, please enter run rate and whether to loop ([0] single/[1] loop), separated by spaces: ";
double rate;
bool isLoop;
string line;
// Read operation parameters
getline(cin, line);
istringstream iss(line);
iss >> rate >> isLoop;
// Set project operation parameters
// rate: Operation rate (0.0-1.0, 1.0 means 100% speed)
// isLoop: Whether to run in loop
robot.setProjectRunningOpt(rate, isLoop, ec);
if(ec) break;
continue;
}
case 'h': // Display help
printHelp();
continue;
case 'q': // Exit program
std::cout << " --- Quit --- \n";
continue;
default: // Invalid command
std::cerr << "Invalid input\n";
continue;
}
// Error handling: If an error occurs when executing the command, print error information
cerr << "! Error message: " << ec.message() << endl;
}
} catch (const rokae::Exception &e) {
// Catch and print all exceptions thrown by the rokae library
std::cout << "Program exception: " << e.what();
}
return 0;
}
/**
* @brief Print usage instructions
* Display all available commands and their function descriptions
*/
void printHelp() {
cout << " --- Running RL Project Example --- \n\n"
<< "RL (Rokae Language) is a programming language dedicated to robots, similar to scripting languages of other robots\n\n"
<< " Command Description\n"
<< " --- --------\n"
<< "on Power on robot\n"
<< "off Power off robot\n"
<< "info Query all project lists in the controller\n"
<< "load Load specified project and tasks\n"
<< "main Set program pointer to main function (reset program)\n"
<< "start Start running loaded project\n"
<< "pause Pause current running program\n"
<< "opt Set operation parameters (speed and loop mode)\n"
<< "tool Query tool information defined in the controller\n"
<< "wobj Query workpiece information defined in the controller\n"
<< "help View all command descriptions\n"
<< "quit Exit program\n\n"
<< "Usage flow example:\n"
<< "1. info - View available projects\n"
<< "2. load - Load project (enter project name and tasks as prompted)\n"
<< "3. on - Power on robot\n"
<< "4. main - Reset program pointer\n"
<< "5. start - Start running program\n"
<< "6. pause/start - Pause/resume running\n"
<< "7. off - Power off after running completed\n";
}
Tool/Workpiece/Base Coordinate System Calibration
Using N-point method to calibrate robot tool or workpiece coordinate system
/**
* @brief Tool/workpiece/base coordinate system calibration class
* Used to implement robot coordinate system calibration functionality, supporting N-point calibration method
*/
template<WorkType Wt, unsigned short DoF>
class CalibrateFrame {
public:
/**
* @brief Constructor
* @param robot Created robot class
* @param type Calibration coordinate system type (tool, workpiece or base coordinate system)
* @param point_num Number of calibration points, corresponding to N-point calibration method
* @param is_held Whether robot held (true for tool calibration, depends on actual situation for workpiece calibration)
* @param base_aux Base coordinate system calibration auxiliary point (only needed for base coordinate system calibration)
*/
CalibrateFrame(Robot_T<Wt,DoF> &robot, FrameType type, int point_num, bool is_held, const std::array<double, 3> &base_aux = {})
: robot_(&robot), type_(type), point_list_(point_num), is_held_(is_held), base_aux_(base_aux) {}
/**
* @brief Set calibration point
* Record current robot joint position as calibration point
* @param point_index Calibration point index (starting from 0)
*/
void setPoint(unsigned point_index) {
if(point_index >= point_list_.size()) {
// Add exception handling yourself
print(std::cerr, "Calibration point index out of range");
return;
}
error_code ec;
// Please ensure the robot object is valid before calling
point_list_[point_index] = robot_->jointPos(ec);
}
/**
* @brief All calibration positions confirmed, get calibration result
* Call this function for calculation when all calibration points are set
* @param ec Calibration result error code
* @return Calibration result, including coordinate system transformation matrix and error information
*/
FrameCalibrationResult confirm(error_code &ec) {
return robot_->calibrateFrame(type_, point_list_, is_held_, ec, base_aux_);
}
private:
Robot_T<Wt, DoF> *robot_; ///< Robot object pointer
FrameType type_; ///< Calibration coordinate system type
std::vector<std::array<double, DoF>> point_list_; ///< Calibration point joint position list
bool is_held_; ///< Whether robot held
std::array<double, 3> base_aux_; ///< Base coordinate system calibration auxiliary point
};
/**
* @brief Example - Calibrate tool/workpiece coordinate system
* Use N-point method to calibrate robot tool or workpiece coordinate system
* @param robot Robot object pointer
*/
template<WorkType Wt, unsigned short DoF>
void example_calibrateFrame(Robot_T<Wt, DoF> *robot) {
int point_count = 4; // 4-point calibration method
// Create tool coordinate system calibration object (true means tool is held by robot)
Workflow::CalibrateFrame calibrate_frame(*robot, FrameType::tool, point_count, true);
// Set each calibration point in sequence
for(int i = 0; i < point_count; i++) {
print(std::cout, "Jog the robot to calibration point", i+1, ", press Enter to confirm");
while(getchar() != '\n'); // Wait for user confirmation
calibrate_frame.setPoint(i); // Record current joint position
}
// Calculate calibration result
error_code ec;
FrameCalibrationResult calibrate_result = calibrate_frame.confirm(ec);
if(ec) {
print(std::cerr, "Calibration failed:", ec);
} else {
print(std::cout, "Calibration successful, result -", calibrate_result.frame, "\nDeviation:", calibrate_result.errors);
}
}
Jog Back to Limit After Robot Arm Exceeds Soft Limit
/**
* @brief Jog back to limit after robot arm exceeds soft limit
* Safety recovery function, automatically move robot joint back to safe range when it exceeds soft limit
* @param robot Robot object pointer
*/
template<WorkType Wt, unsigned short DoF>
void recoveryFromOverJointRange(Robot_T<Wt, DoF> *robot) {
error_code ec;
auto curr_joint = robot->jointPos(ec); // Get current joint position
std::array<double[2], DoF> soft_limits {}; // Store soft limits for each joint [lower limit, upper limit]
// Read current soft limit settings
robot->getSoftLimit(soft_limits, ec);
std::array<double, DoF> jog_steps {}; // Jog angles (radians) needed for each joint
bool outofRange = false; // Mark whether any joint exceeds limit
// Jog the axes that exceed limits to within the soft limit ±0.08rad (about 5 degrees) safe boundary
double margin = 0.08; // Safe boundary
for(unsigned i = 0; i < DoF; ++i) {
if(curr_joint[i] > soft_limits[i][1]) { // Exceed upper limit
jog_steps[i] = soft_limits[i][1] - curr_joint[i] - margin;
outofRange = true;
}
if(curr_joint[i] < soft_limits[i][0]) { // Exceed lower limit
jog_steps[i] = soft_limits[i][0] - curr_joint[i] + margin;
outofRange = true;
}
}
if(!outofRange) {
print(std::cout, "Current axis angles are within soft limits, no recovery needed");
return;
}
// Safety recovery process
// 1. After power off, turn off soft limit (avoid being blocked by soft limit during Jog)
robot->setPowerState(false, ec);
robot->setOperateMode(OperateMode::manual, ec); // Switch to manual mode
robot->setSoftLimit(false, ec); // Temporarily turn off soft limit
robot->setPowerState(true, ec); // Power on again
// 2. Jog each joint that exceeds limit in sequence
double rate = 0.2; // Jog rate (20%)
for(unsigned i = 0; i < DoF; ++i) {
if(jog_steps[i] != 0) {
// Start Jog motion: joint space, specified rate, angle (converted to degrees), joint index, direction
robot->startJog(JogOpt::Space::jointSpace, rate, Utils::radToDeg(abs(jog_steps[i])), i,
jog_steps[i] > 0, ec);
// Wait for Jog to complete
bool running = true;
while (running) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
auto st = robot->operationState(ec);
print(std::cout, st);
if(st == OperationState::jog){
running = false;
}
}
}
}
// 3. Restore safety settings
robot->stop(ec); // Stop Jog
robot->setPowerState(false, ec); // Power off
// Re-enable soft limit protection
robot->setSoftLimit(true, ec);
}
}
Basic Information Query
- Query robot basic information, control version, SDK version, model information
- Query controller logs: queryControllerLog, maximum 10 entries
/**
* @brief Example - Basic information query
* Demonstrate how to query robot basic status information
*/
template <WorkType wt, unsigned short dof>
void example_basicOperation(Robot_T<wt, dof> *robot){
error_code ec;
// *** Query robot basic information ***
auto robotinfo = robot->robotInfo(ec);
print(os, "Controller version:", robotinfo.version, "Model:", robotinfo.type);
print(os, "xCore-SDK version:", robot->sdkVersion());
// *** Get robot current status ***
auto joint_pos = robot->jointPos(ec); // Axis angles [rad]
auto joint_vel = robot->jointVel(ec); // Axis velocities [rad/s]
auto joint_torque = robot->jointTorque(ec); // Axis torques [Nm]
auto tcp_xyzabc = robot->posture(CoordinateType::endInRef, ec); // End pose
auto flan_cart = robot->cartPosture(CoordinateType::flangeInBase, ec); // Flange pose
robot->baseFrame(ec); // Base coordinate system
print(os, "End relative to external reference coordinate system pose", tcp_xyzabc);
print(os, "Flange relative to base coordinate system -", flan_cart);
// Query recent 5 error level controller logs
print(os, "Query controller log content");
auto controller_logs = robot->queryControllerLog(5, {LogInfo::error}, ec);
for(const auto &log: controller_logs) {
print(os, log.content);
}
}
- Main differences between queryControllerLog and setEventWatcher
queryControllerLog is actively queried by the upper computer and queries log information according to log levels, setEventWatcher is events actively reported by the controller, including collision status, non-real-time motion command execution information, RL execution status
Enable/Disable Drag Function
/**
* @brief Example - Enable/disable drag function
* Demonstrate robot drag teaching function
*/
void example_drag(BaseCobot *robot) {
error_code ec;
// Drag function preconditions: manual mode power off state
robot->setOperateMode(rokae::OperateMode::manual, ec);
robot->setPowerState(false, ec);
// Enable Cartesian space free drag
robot->enableDrag(DragParameter::cartesianSpace, DragParameter::freely, ec);
print(os, "Drag enabled", ec, "Press Enter to continue");
std::this_thread::sleep_for(std::chrono::seconds(2)); // Wait for control mode switching to complete
while(getchar() != '\n');
// Disable drag function
robot->disableDrag(ec);
std::this_thread::sleep_for(std::chrono::seconds(2)); // Wait for control mode switching to complete
}
Read/Write IO and Registers
/**
* @brief Example - Read/write IO and registers
* Demonstrate digital input/output and register operations
*/
void example_io_register(BaseRobot *robot) {
error_code ec;
// *** Digital input/output operations ***
print(os, "DO1_0 current signal value:", robot->getDO(1,0,ec));
// Set simulation mode to simulate DI signals (DI can only be set in simulation mode)
robot->setSimulationMode(true, ec);
robot->setDI(0, 2, true, ec); // Set DI0_2 to true
print(os, "DI0_2 current signal value:", robot->getDI(0, 2, ec));
robot->setSimulationMode(false, ec); // Turn off simulation mode
// Read single float type register
float val_f;
std::vector<float> val_af;
// Read first register (index 0)
robot->readRegister("register0", 0, val_f, ec);
// Read tenth register (index 9)
robot->readRegister("register0", 9, val_f, ec);
// Read entire register array
robot->readRegister("register0", 9, val_af, ec);
// Read int type register array
std::vector<int> val_ai;
robot->readRegister("register1", 1, val_ai, ec);
// Write bool type register
robot->writeRegister("register0", 0, true, ec);
// Write bool type register array
std::vector<bool> val_bool_array = { false,true,false,true,false,true,false };
robot->writeRegister("register2", 0, val_bool_array, ec);
}
Jog Robot
Jog preconditions: non-real-time command mode + manual mode
/**
* @brief Example - Jog robot
* Demonstrate manual jog control of robot
*/
void example_jog(BaseRobot *robot) {
error_code ec;
robot->setMotionControlMode(rokae::MotionControlMode::NrtCommand, ec);
robot->setOperateMode(rokae::OperateMode::manual, ec); print(os, "Prepare to jog robot, need manual mode power on, please confirm power on and press Enter");
// For cases with external enable switch, need to hold the switch to manually power on
robot->setPowerState(true, ec);
// Move 50mm along Z+ direction in world coordinate system
print(os, "-- Start jogging robot-- \nIn world coordinate system, move 50mm along Z+ direction, rate 50%, wait for robot to stop moving and press Enter to continue");
robot->startJog(JogOpt::world, 0.5, 50, 2, true, ec);
while(getchar() != '\n');
// Continuous rotation of 6th axis in negative direction in joint space
print(os, "Joint space, 6th axis negative continuous rotation, rate 5%, press Enter to stop jog");
robot->startJog(JogOpt::jointSpace, 0.05, 5000, 5, false, ec);
while(getchar() != '\n'); // Press Enter to stop
robot->stop(ec); // Must call stop() to stop after jog ends
}
Singularity Avoidance Jog
Applicable to xMateSR, xMateCR series models
/**
* @brief Example - Singularity avoidance jog,
* Automatically avoid singularities during jog
*/
void example_avoidSingularityJog(xMateRobot &robot) {
error_code ec;
robot.setOperateMode(rokae::OperateMode::manual, ec);
print(os, "Prepare to jog robot, need manual mode power on, please confirm power on and press Enter");
robot.setPowerState(true, ec);
while(getchar() != '\n');
// Use singularity avoidance mode for jog
print(os, "-- Start jogging robot-- \nSingularity avoidance mode, move 50mm along Y+ direction, rate 20%, wait for robot to stop moving and press Enter to continue");
robot.startJog(JogOpt::singularityAvoidMode, 0.2, 50, 1, true, ec);
while(getchar() != '\n'); // Press Enter to stop
robot.stop(ec); // Must call stop() to stop after jog ends
}
Enable and Disable Collision Detection
/**
* @brief Example - Enable and disable collision detection
* Set robot collision detection parameters
*/
template <unsigned short dof>
void example_setCollisionDetection(Cobot<dof> *robot) {
error_code ec;
// Set collision detection sensitivity for each axis, range 0.01 ~ 2.0 (equivalent to 1% ~ 200%)
// Parameters: axis sensitivity array, stop level, retreat distance
std::array<double, dof> sensitivities;
if constexpr (dof == 6) {
sensitivities = {1.0, 1.0, 1.0, 2.0, 1.0, 1.0}; // 6-axis robot
} else if constexpr (dof == 7) {
sensitivities = {1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 1.0}; // 7-axis robot
}
robot->enableCollisionDetection(sensitivities, StopLevel::stop1, 0.01, ec);
std::this_thread::sleep_for(std::chrono::seconds(2));
// Disable collision detection
robot->disableCollisionDetection(ec);
}
Set collision detection event callback function
void recoverFromCollision(BaseRobot &robot, const rokae::EventInfo &info) {
using namespace rokae::EventInfoKey;
bool isCollided = std::any_cast<bool>(info.at(Safety::Collided));
print(std::cout, "Collided:", isCollided);
if(isCollided) {
std::this_thread::sleep_for(std::chrono::seconds(20));
error_code ec;
robot.setPowerState(true, ec);
robot.moveStart(ec);
print(std::cout, "Recovered from collision");
}
}
robot.setEventWatcher(Event::safety, [&](const EventInfo &info){
recoverFromCollision(robot, info);
}, ec);
Emergency Stop Reset
When the user presses the emergency stop button and the emergency stop button is turned on, emergency stop recovery can be used
/**
* @brief Emergency stop reset
* Restore robot state after emergency stop event
*/
void example_emergencyStopReset(BaseRobot *robot) {
error_code ec;
print(os, "Emergency stop reset");
robot->recoverState(1, ec); // Parameter 1 indicates recovery level
if (ec) {
print(os, "Reset failed:", ec);
} else {
print(os, "Reset successful");
}
}
Common Forward and Inverse Kinematics Calculation
Notes:
- Trajectories planned by upper-layer applications do not use conf data for forward and inverse calculations, usually need to call the interface to turn off Conf robot.setDefaultConfOpt(false, ec);
- Robot flange end usually mounts tools, when calculating forward and inverse solutions, need to set tool workpiece coordinates
Calculation interfaces:
- Forward kinematics: calcFk
- Inverse kinematics: calcIk
/**
* @brief Example - Calculate forward and inverse kinematics
* Demonstrate robot kinematics calculation function
*/
template <WorkType wt, unsigned short dof>
void example_coordinateCalculation(Robot_T<wt, dof> *robot){
error_code ec;
// Get current end pose in reference coordinate system
auto tcp_xyzabc = robot->posture(CoordinateType::endInRef, ec);
// Get current tool coordinate system settings
Toolset toolset1;
toolset1 = robot->toolset(ec);
print(os, "Tool workpiece coordinate system read from controller:", toolset1);
auto model = robot->model(); // Get robot model interface
// Calculate inverse kinematics under currently set tool workpiece coordinate system
model.calcIk(tcp_xyzabc, ec);
// Calculate inverse kinematics under specified tool workpiece coordinate system
auto ik = model.calcIk(tcp_xyzabc, toolset1, ec);
// Calculate forward kinematics under currently set tool workpiece coordinate system
model.calcFk(ik, ec);
// Calculate forward kinematics under specified tool workpiece coordinate system
auto fk_ret = model.calcFk(ik, toolset1, ec);
print(os, "Current kinematics inverse solution:", ik);
print(os, "Kinematics forward solution:", fk_ret);
//*** Coordinate system transformation: end relative to external reference & flange relative to base ***
// Query base coordinate setting
auto base_in_world = robot->baseFrame(ec);
// Coordinate system transformation calculation
auto flan_in_base = Utils::EndInRefToFlanInBase(base_in_world, toolset1, tcp_xyzabc);
auto flan_pos = robot->posture(CoordinateType::flangeInBase, ec);
auto end_in_ref = Utils::FlanInBaseToEndInRef(base_in_world, toolset1, flan_pos);
print(os, "Input end relative to external reference coordinate system pose", tcp_xyzabc);
print(os, "Calculated end relative to external reference coordinate system pose", end_in_ref);
print(os, "Input flange relative to base coordinate system pose", flan_pos);
print(os, "Calculated flange relative to base coordinate system pose", flan_in_base);
}
Model Library Forward and Inverse Kinematics
Notes:
SDK provides xMate kinematics and dynamics calculation library for users, convenient for calculating robot forward and inverse solutions and Jacobian matrix, currently supported models:
- ER: All xMateER series models,
- CR: XMC7/12,
- SR: XMS3/4/5 (XMS5 is a new model, need to upgrade special version of model files to use model library)
- AR: Need to upgrade AR model-specific controller version
Main interfaces include
- Forward kinematics interface: getCartPose
- Inverse kinematics interface
The forward and inverse kinematics interfaces of the model library are faster than CalcIK and CalcFK calculations, usually returning calculation results within 1ms
#include "rokae/robot.h"
#include "print_helper.hpp"
#include "rokae/utility.h"
using namespace std;
using namespace rokae;
ostream &os = std::cout; ///< Output stream reference, used for printing to console
/**
* @brief xMateER7 Pro 7-axis robot model function demonstration
* Function: Display 7-axis robot kinematics, dynamics model calculation functions
* Including: Jacobian matrix, forward and inverse kinematics, velocity acceleration calculation, torque calculation, etc.
*/
void xMateErPro7_model(xMateModel<7> &model) {
try {
// Define test data - 7-axis robot
std::array<double, 7> zeros = {0, 0, 0, 0, 0, 0, 0}, // Zero joint angles
// Input joint positions (angles converted to radians)
jointPos_in = Utils::degToRad(array<double,7>({5, 46, -10, 91, 1, -105, 11})),
// Input joint velocities (rad/s)
jointVel_in = {0.3, 0.2, 0.5, 0.4, 0.1, 0.1, 0.1},
// Input joint accelerations (rad/s²)
jointAcc_in = {1.3, 3.1, 4.1, 1.5, 1.6, 4.1, 8.1},
// Inverse kinematics initial guess position
jointInit = Utils::degToRad(array<double,7>({6, 45, -9, 92, 0, -103, 10})),
// Output data buffers
array1 {}, array2 {}, array3 {}, array4 {};
// Define coordinate system transformation matrix (4x4 homogeneous transformation matrix)
// F_TO_EE: Transformation from flange to end effector
// EE_TO_K: Transformation from end effector to tool
std::array<double, 16> F_TO_EE = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1},
EE_TO_K = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
/******************************************************************
* 1. Jacobian matrix calculation
* Jacobian matrix describes the mapping relationship from joint velocities to end velocities
******************************************************************/
print(os, "Jacobian -", model.jacobian(zeros));
print(os, "Jacobian given EE -", model.jacobian(zeros, F_TO_EE, EE_TO_K, SegmentFrame::endEffector));
/******************************************************************
* 2. Torque calculation (without friction)
* Decompose and calculate torque components of inertial force, Coriolis force, and gravity
******************************************************************/
model.getTorqueNoFriction(zeros, zeros, zeros, array1, array2, array3, array4);
print(os, "TorqueNoFriction full -", array1); // Total torque
print(os, "TorqueNoFriction inertia -", array2); // Inertial torque
print(os, "TorqueNoFriction coriolis -", array3); // Coriolis torque
print(os, "TorqueNoFriction gravity -", array4); // Gravity torque
/******************************************************************
* 3. Forward kinematics calculation (Forward Kinematics)
* Calculate robot end pose based on joint angles
******************************************************************/
auto pos = model.getCartPose(jointPos_in);
print(os, "Flange posture -", pos); // Flange pose
/******************************************************************
* 4. Set tool coordinate system and calculate end pose
******************************************************************/
model.setTcpCoor(F_TO_EE, EE_TO_K); // Set end effector coordinate system
auto pos_e = model.getCartPose(jointPos_in, SegmentFrame::endEffector);
print(os, "EE posture -", pos_e); // End effector pose
/******************************************************************
* 5. Velocity and acceleration calculation
* Mutual conversion between joint space and Cartesian space
******************************************************************/
// Joint space to Cartesian space velocity/acceleration conversion
auto cartVel_out = model.getCartVel(jointPos_in, jointVel_in);
auto cartAcc_out = model.getCartAcc(jointPos_in, jointVel_in, jointAcc_in);
print(os, "Cartesian velocity -", cartVel_out);
print(os, "Cartesian acceleration -", cartAcc_out);
// Cartesian space to joint space velocity/acceleration conversion
print(os, "Joint velocities -", model.getJointVel(cartVel_out, jointPos_in));
print(os, "Joint accelerations -", model.getJointAcc(cartAcc_out, jointPos_in, jointVel_in));
/******************************************************************
* 6. Inverse kinematics calculation (Inverse Kinematics)
* Calculate joint angles based on end pose
******************************************************************/
double psi = Utils::degToRad(-7.543); // Redundant degree of freedom parameter (unique to 7-axis robots)
array<int, 8> confData = {0, 0, -1, 1, 0, -2, 0, 1}; // Configuration selection parameter
auto ret = model.getJointPos(pos, psi, jointInit, array1);
print(os, "IK calculation -", array1, "| ret:", ret); // Inverse kinematics calculation result
/******************************************************************
* 7. Verify inverse kinematics result
* Recalculate forward kinematics using joint angles calculated by inverse kinematics to verify accuracy
******************************************************************/
pos = model.getCartPose(array1);
print(os, "FK calculation -", pos);
/******************************************************************
* 8. Set load parameters
* Configure end load for accurate dynamics calculation
******************************************************************/
double load_mass = 1.0; // Load mass (kg)
std::array<double, 3> load_centre = {0.1, 0.1, 0.1}, // Load center of mass position (m)
load_inertia = {3.0, 2.0, 5.0}; // Load moment of inertia (kg·m²)
model.setLoad(load_mass, load_centre, load_inertia);
} catch (const std::exception &e) {
print(cerr, e.what());
}
}
/**
* @brief xMateER3 6-axis robot model function demonstration
* Function: Display 6-axis robot kinematics, dynamics model calculation functions
*/
void xMateEr3_model(xMateModel<6> &model) {
try {
// Define test data - 6-axis robot
std::array<double, 6> zeros {},
jointPos_in = Utils::degToRad(array<double,6>({-20, 37, 70, 0, 71, -19})),
jointVel_in = {0.3, 0.2, 0.5, 0.4, 0.1, 0.1},
jointAcc_in = {1.3, 3.1, 4.1, 1.5, 1.6, 4.1},
jointInit = Utils::degToRad(array<double,6>({-21, 38, 71, 0, 70, -20})),
array1 {}, array2 {}, array3 {}, array4 {};
std::array<double, 16> F_TO_EE = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1},
EE_TO_K = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
/******************************************************************
* Same function demonstration as 7-axis version, but for 6-axis robot
******************************************************************/
print(os, "Jacobian -", model.jacobian(zeros));
print(os, "Jacobian given EE -", model.jacobian(zeros, F_TO_EE, EE_TO_K, SegmentFrame::endEffector));
model.getTorqueNoFriction(zeros, zeros, zeros, array1, array2, array3, array4);
print(os, "TorqueNoFriction full -", array1);
print(os, "TorqueNoFriction inertia -", array2);
print(os, "TorqueNoFriction coriolis -", array3);
print(os, "TorqueNoFriction gravity -", array4);
print(os, "Torque inertia -", model.getTorque(zeros, zeros, zeros, TorqueType::inertia));
auto pos = model.getCartPose(jointPos_in);
print(os, "Flange posture -", pos);
model.setTcpCoor(F_TO_EE, EE_TO_K);
auto pos_e = model.getCartPose(jointPos_in, SegmentFrame::endEffector);
print(os, "EE posture -", pos_e);
auto cartVel_out = model.getCartVel(jointPos_in, jointVel_in);
auto cartAcc_out = model.getCartAcc(jointPos_in, jointVel_in, jointAcc_in);
print(os, "Cartesian velocity -", cartVel_out);
print(os, "Cartesian acceleration -", cartAcc_out);
print(os, "Joint velocities -", model.getJointVel(cartVel_out, jointPos_in));
print(os, "Joint accelerations -", model.getJointAcc(cartAcc_out, jointPos_in, jointVel_in));
double psi = Utils::degToRad(-7.543);
array<int, 8> confData = {0, 0, -1, 1, 0, -2, 0, 1};
auto ret = model.getJointPos(pos, psi, jointInit, array1);
print(os, "IK calculation -", array1, "| ret:", ret);
pos = model.getCartPose(array1);
print(os, "FK calculation -", pos);
// Set load (6-axis version uses larger load)
double load_mass = 2.0;
std::array<double, 3> load_centre = {0.1, 0.1, 0.1}, load_inertia = {3.0, 2.0, 5.0};
model.setLoad(load_mass, load_centre, load_inertia);
} catch (const std::exception &e) {
print(cerr, e.what());
}
}
/**
* @brief Main program
* Function: Select test 6-axis or 7-axis robot model according to configuration
*/
int main() {
bool test_Er3 = false; // Test mode selection: false-7-axis, true-6-axis
try {
std::string ip = "192.168.21.10";
if(test_Er3) {
// Test 6-axis robot xMateER3
rokae::xMateRobot robot(ip); // Create 6-axis robot object
auto model = robot.model(); // Get robot model (xMateModel<6>)
xMateEr3_model(model); // Execute 6-axis model function demonstration
} else {
// Test 7-axis robot xMateER7 Pro
rokae::xMateErProRobot robot(ip); // Create 7-axis robot object
auto model = robot.model(); // Get robot model (xMateModel<7>)
xMateErPro7_model(model); // Execute 7-axis model function demonstration
}
} catch (const std::exception &e) {
print(cerr, e.what());
}
}
3. How to use SDK library on VS2022
1. Create new solution
2. Confirm SDK library location
Note that the library path and solution are in the same directory
3. Add header files and libraries
4. Run to generate exe, successfully run the program
Copy the corresponding dll to the generated exe directory
4. Using SDK library in Qt environment
Prerequisites
Currently only Qt6 version supports Cmakefile.txt files, the current project is created based on the following environment,
Create new project
Create a normal project according to qtcreator's new project
Add libraries and paths to Cmakefile.txt
Note the current debug or release version being built, if mixed up, there will be library linking issues
set(Robot_Include_DIR "${CMAKE_SOURCE_DIR}/rokae/include/rokae")
set(Robot_Lib_DIR "${CMAKE_SOURCE_DIR}/rokae/lib/Windows/Release/64bit")
set(Eigen_DIR "${CMAKE_SOURCE_DIR}/rokae/external")
# 1. Add header file search path
include_directories(
${Robot_Include_DIR}/
${Eigen_DIR}/
${Eigen_DIR}/Eigen
)
# 2. Add library file search path
link_directories(
${Robot_Lib_DIR}/
)
# 3. Set library file name (according to platform)
find_library(robotsdk NAMES xCoreSDK PATHS ${CMAKE_SOURCE_DIR}/rokae/lib/Windows/Release/64bit)
target_link_libraries(RobotTest
PRIVATE Qt${QT_VERSION_MAJOR}::Widgets
# PRIVATE ${robotsdk}
PRIVATE ${CMAKE_SOURCE_DIR}/rokae/lib/Windows/Release/64bit/xCoreSDK_static.lib
PRIVATE ${CMAKE_SOURCE_DIR}/rokae/lib/Windows/Release/64bit/xMateModel.lib
)
Build version
