Real-Time Mode Precautions and Common Issues

1 Usage Precautions

1.1 Command Planning Requirements

Motion commands issued by users need to meet recommended conditions and necessary conditions. Recommended conditions are for smoother robot motion and optimal performance. Necessary conditions must be met; if not, the robot will stop. If recommended conditions are not met, jitter or motor noise may occur during motion.

1.1.1 Joint Space Motion

1. Necessary Conditions

Joint space trajectory is smooth, at least ensuring velocity is continuously differentiable:

$$\begin{aligned}q_{\min} &< q_c < q_{\max} \\-\dot{q}_{\max} &< \dot{q}_c < \dot{q}_{\max} \\-\ddot{q}_{\max} &< \ddot{q}_c < \ddot{q}_{\max} \\-\dddot{q}_{\max} &< \dddot{q}_c < \dddot{q}_{\max}\\\end{aligned}$$

2. Recommended Conditions

Torque commands do not exceed limits:

$$\begin{aligned}-\tau_{j\text{max}} &< \tau_{jc} < \tau_{j\text{max}} \\-\dot{\tau}_{j\text{max}} &< \dot{\tau}_{jc} < \dot{\tau}_{j\text{max}}\\\end{aligned}$$

At motion start:

$$\begin{aligned}q &= q_c \\\dot{q}_c &= 0 \\\ddot{q}_c &= 0\\\end{aligned}$$

At motion end:

$$\begin{aligned}\dot{q}_{c} &= 0 \\\ddot{q}_{c} &= 0\\\end{aligned}$$

1.2 Cartesian Space Motion

1. Necessary Conditions

Not in singular position and within workspace:

$$\begin{aligned}-\dot{p}_{\max } & <\dot{p}_{c}<\dot{p}_{\max } \\-\ddot{p}_{\max } & <\ddot{p}_{c}<\ddot{p}_{\max } \\-\dddot{p}_{\max } & <\dddot{p}_{c}<\dddot{p}_{\max }\\\end{aligned}$$

2. Recommended Conditions

Torque commands do not exceed limits:

$$\begin{aligned}\\-\tau_{j\max} &< \tau_{jc} < \tau_{j\max} \\-\dot{\tau}_{j\max} &< \dot{\tau}_{jc} < \dot{\tau}_{j\max}\end{aligned}$$

At motion start:

$$\begin{aligned}\\T &= T_c \\\dot{p}_c &= 0 \\\ddot{p}_c &= 0\\\end{aligned}$$

At motion end:

$$\begin{aligned}\dot{\mathbf{p}}_{\mathrm{c}} &= 0 \\\ddot{\mathbf{p}}_{\mathrm{c}} &= 0\\\end{aligned}$$

1.1.3 Direct Torque Control

1. Necessary Conditions

Torque commands do not exceed limits and are continuous:

$$\begin{aligned}-\dot{\tau}_{j \max } & <\dot{\tau}_{j c}<\dot{\tau}_{j \max } \\-\tau_{j \max } & <\tau_{j c}<\tau_{j \max }\\\end{aligned}$$

1.1.4 Motion Limit Conditions

xMateER3 Pro, xMateER7 Pro, xMateER3, xMateER7 Cartesian space motion limit conditions:

Parameter	Translation	Rotation
Velocity	$1.0 m/s$	$2.5 rad/s$
Acceleration	$10.0 m/s^2$	$10.0 rad/s2$
Jerk	$5000.0 m/s^3$	$5000.0 rad/s^3$

xMateER3 Pro and xMateER7 Pro joint space motion limit conditions:

Parameter	Axis 1	Axis 2	Axis 3	Axis 4	Axis 5	Axis 6	Axis 7	Unit
Limit Upper	170	120	170	120	170	120	360	°
Limit Lower	-170	-120	-170	-120	-170	-120	-360	°
Velocity Upper	2.175	2.175	2.175	2.175	2.610	2.610	2.610	$rad/s$
Acceleration Upper	15	7.5	10	10	15	15	20	$rad/s^2$
Jerk Upper	5000	3500	5000	5000	7500	7500	7500	$rad/s^3$

xMate3 and xMate7 joint space motion limit conditions:

Parameter	Axis 1	Axis 2	Axis 3	Axis 4	Axis 5	Axis 6	Unit
Limit Upper	170	120	120	170	120	360	°
Limit Lower	-170	-120	-120	-170	-120	-360	°
Velocity Upper	2.175	2.175	2.175	2.610	2.610	2.610	$rad/s$
Acceleration Upper	15	7.5	10	15	15	20	$rad/s^2$
Jerk Upper	5000	3500	5000	7500	7500	7500	$rad/s^3$

xMateER3 Pro and xMateER7 Pro direct torque control limit conditions

Parameter	Axis 1	Axis 2	Axis 3	Axis 4	Axis 5	Axis 6	Axis 7	Unit
Torque Upper	85	85	85	85	36	36	36	$Nm$
Torque Derivative Upper	1500	1500	1500	1500	1000	1000	1000	$Nm/s$

xMate3 and xMate7 direct torque control limit conditions

Parameter	Axis 1	Axis 2	Axis 3	Axis 4	Axis 5	Axis 6	Unit
Torque Upper	85	85	85	36	36	36	$Nm$
Torque Derivative Upper	1500	1500	1500	1000	1000	1000	$Nm/s$

1.2 DH Parameters

xMateER3 Pro DH parameter table:

Joint	A(mm)	Alpha(rad)	D(mm)	Theta(rad)
1	0	-pi/2	341.5	0
2	0	pi/2	0	0
3	0	-pi/2	394.0	0
4	0	pi/2	0	0
5	0	-pi/2	366	0
6	0	pi/2	0	0
7	0	0	250.3	0

xMateER7 Pro DH parameter table:

Joint	A(mm)	Alpha(rad)	D(mm)	Theta(rad)
1	0	-pi/2	404.0	0
2	0	pi/2	0	0
3	0	-pi/2	437.5	0
4	0	pi/2	0	0
5	0	-pi/2	412.5	0
6	0	pi/2	0	0
7	0	0	275.5	0

xMate3 DH parameter table:

Joint	A(mm)	Alpha(rad)	D(mm)	Theta(rad)
1	0	-pi/2	341.5	0
2	394	0	0	0
3	0	pi/2	0	0
4	0	-pi/2	366	0
5	0	pi/2	0	0
6	0	0	250.3	0

xMate7 DH parameter table:

Joint	A(mm)	Alpha(rad)	D(mm)	Theta(rad)
1	0	-pi/2	404	0
2	437.5	0	0	0
3	0	pi/2	0	0
4	0	-pi/2	412.5	0
5	0	pi/2	0	0
6	0	0	275.5	0

1.3 RCI Client Compatibility

For original RCI client users, xCoreSDK can be used directly after upgrading the controller to v1.7 and later versions.

1.3.1 First Use

1. Ensure RobotAssist-Communication-RCI setting is turned off, or confirm via the robot status in the middle of the status bar;

2. Call setMotionControlMode (RtCommand) interface to enable RCI function. If successful, robot status changes to RCI, and the status bar icon also shows RCI:

3. After that, RCI function can be turned on/off through the above interface or RobotAssist. Note: If configuration erase operation is performed, it is also considered first use, and RCI needs to be enabled again through the SDK interface.

1.3.2 Switching to RCI Client

After using SDK, RCI client cannot be used simultaneously. To switch back, call useRciClient(true). Before calling, ensure RCI is turned off as per function description. Then turn RCI function on/off through RobotAssist.

2 Common Issues

2.1 Network Connection Issues

Real-time motion commands and status information are sent via UDP protocol unicast. When first used, UDP network related errors may occur.

2.1.1 UDP Socket Open Failed

Exception showing "An invalid parameter was provided" or "The requested address is invalid" is basically because local address was not filled in or is incorrect.

1. How to View Local Address

Ubuntu system can use ifconfig command to view. Find the network device connected to the robot. IP address should be in the same subnet as the robot.

Windows can be found in Settings-Network & Internet.

Can also be viewed using ipconfig command.

2. How to Correctly Fill in Local Address

int main() {
    // Assuming robot address is 10.0.2.160, SDK program runs on my Windows PC, local address is 10.0.2.121
    rokae::xMateRobot robot("10.0.2.160", "10.0.2.121");

    // Or fill in when connecting to robot
    rokae::xMateRobot robot;
    robot.connectToRobot("10.0.2.160", "10.0.2.121");
}

2.1.2 No Robot Status Feedback Before Timeout

If "No robot status feedback before timeout" real-time status exception occurs, it means IP settings are correct, but UDP messages are being blocked.

1. Windows System

For Windows system, please check firewall settings and whether UDP inbound rules are allowed. When running SDK program for the first time, a firewall popup may appear. Be sure to click Allow.

Additionally, you can check inbound rules for running programs in Advanced Security Windows Defender Firewall.

2. Linux System

This section is for reference only. If commands for specific system versions are not compatible, please search for alternatives.

Linux rarely has firewall blocking issues. First test whether messages sent by robot can be received using the following method: Open terminal, enter command nc -vul -p 1337, then run any real-time mode control example program, and check if there is a port connection from the robot address and if data is received. If host 1337 port (real-time status data is 1338 port) never receives data, you can investigate Linux firewall restriction issues.

Common Linux system firewalls include: UFW, firewall, iptables. Among them, UFW is the default firewall for Debian series, firewall is the firewall for Red Hat series 7 and above (e.g., CentOS7.x), iptables is the firewall for Red Hat series 6 and below (e.g., CentOS6.x).

2.1 UFW Firewall

1. Determine firewall status. Execute command in terminal: sudo ufw status If output shows "Status: active", firewall is enabled; if shows "Status: inactive", firewall is not enabled (if ufw firewall is not enabled, go directly to check other firewall issues).
2. Check if UDP ports 1337 and 1338 are allowed for communication. If UFW firewall is enabled, after executing the above status view command, configured rules will be listed. Check if there are rules allowing UDP ports 1337 and 1338 in the rule list, similar to:
3. Open UDP ports 1337 and 1338. If there are no rules allowing these two ports in the rule list, you can open them using the following commands:

sudo ufw allow 1337/udp
sudo ufw allow 1338/udp

Finally execute sudo ufw status again to confirm rules have been added. 4. Disable firewall. If above operations are ineffective, you can directly disable firewall with command: sudo ufw disable

2.2 firewalld Firewall

1. Determine firewall status. Can use commands to view firewall status: sudo systemctl status firewalld or sudo firewall-cmd --state If output is "running", firewall is running; if output is "not running" or similar, firewall is not running.
2. Check if UDP ports 1337 and 1338 are allowed for communication. Execute following command to view current allowed port list: sudo firewall-cmd --list-ports Check if output contains 1337/udp and 1338/udp.
3. Open UDP ports 1337 and 1338. Use following commands to temporarily open ports (rules expire after restart):

sudo firewall-cmd --add-port=1337/udp
sudo firewall-cmd --add-port=1338/udp

To permanently open ports, need to add --permanent parameter and reload firewall rules:

sudo firewall-cmd --permanent --add-port=1337/udp
sudo firewall-cmd --permanent --add-port=1338/udp
sudo firewall-cmd --reload

4. Disable firewall. If above operations are ineffective, you can directly disable firewall with commands. Temporary disable: systemctl stop firewalld; Permanent disable: systemctl disable firewalld

2.3 iptables Firewall

1. Determine firewall status. Execute command to view iptables rules: sudo iptables -L If output has many rules, firewall is active; if output is empty or only has few default rules, firewall may not have additional filtering configuration.

2. Check if UDP ports 1337 and 1338 are allowed for communication. In above command output, look for rules allowing UDP ports 1337 and 1338, similar to:

3. Open UDP ports 1337 and 1338. Use following commands to add allow rules:

sudo iptables -A INPUT -p udp --dport 1337 -j ACCEPT
sudo iptables -A INPUT -p udp --dport 1338 -j ACCEPT

To make rules persist after restart, need to save rules. Methods to save rules may differ in different distributions. For example, in CentOS system you can use: sudo service iptables save

In Ubuntu and other systems, you can use "iptables-persistent" tool to save rules. 4. Disable firewall. If above operations are ineffective, you can directly disable firewall with commands (CentOS/RHEL): Temporarily disable firewall: sudo systemctl stop iptables Disable firewall from starting on boot: sudo systemctl disable iptables Or clear firewall rules (Ubuntu/Debian): sudo iptables -F

2.2 Real-Time Mode Motion Errors

If errors occur during real-time mode motion, controller will power off. This section describes how to get error reports and handle errors.

2.2.1 How to Capture Motion Errors

void funRTControl() {
    try {
        auto rtCon = robot.getRtMotionController().lock();
        rtCon->startMove(RtControllerMode::jointPosition);
        // Method 1: Blocking run, errors during motion will throw exceptions
        rtCon->startLoop(true);
        // Method 2: Non-blocking run, if error occurs during motion, exception will be thrown when stopLoop
        rtCon->startLoop(false);
        rtCon->stopLoop(); 

    } catch(const std::exception &e) {
        // error handler
        std::cerr << e.what();
    }
}

2.2.2 Error Causes and Handling

Real-time mode has 20 error types

2.2.2.1 kInstabilityDetection Error

If kInstabilityDetection occurs frequently, you can increase network threshold. Call setRtNetworkTolerance when real-time mode is off, set threshold to above 60%.

2.2.2.2 Velocity/Acceleration Exceeds Threshold

• If it occurs at the start of a motion, meaning it stops right after startMove, then most likely the starting position given is not the current position. How to correctly set starting position see next section.
• If it occurs during motion and running SDK example programs, it may be related to host real-time performance and network fluctuations. Best to adjust host program running environment.
• If it's your own trajectory planning, then there likely is indeed excessive position command difference that needs improvement.
• If network connection is complex, using switches, etc., there will be packet loss. Need to change to direct connection.

2.2.2.3 How to Correctly Set Starting Position

For example, MoveJ(start_pos, target_pos, speed), how to give start_pos? There are three methods:

1. Most recommended: Read with posture()/jointPos()

std::array<double, 16> init_position {};
Utils::postureToTransArray(robot.posture(rokae::CoordinateType::flangeInBase, ec), init_position);

2. Loop calling updateRobotState until latest data is updated. Calling once updates one frame of data, so calling only once likely reads old data.

// Status data receive queue does not automatically overwrite old data, can clear old data by loop reading method
while (robot.updateRobotState(chrono::steady_clock::duration::zero()));

3. Update starting position when callback runs for the first time.

std::function<CartesianPosition()> callback = [&, rtCon]() {
    if(init) {
        // Read starting position
        robot.getStateData(RtSupportedFields::tcpPose_m, init_pos);
        init = false;
    }
};
// Because callback uses getStateData(), parameter useStateDataInLoop=true
rtCon->setControlLoop(callback, 0, true);

2.2.2.4 Noise Occurs During Position Control

• Occurs when running SDK example programs
  • Set filtering, smooth commands: setFilterFrequency, setFilterLimit. Lower frequency gives better smoothing effect
  • Host real-time performance is not good, common on Windows. Generally Windows runs need filtering.
  • Network conditions also have impact. Recommend using direct connection to reduce network fluctuations
  • Also related to host device network card. Generally Realtek network cards can basically ensure synchronization, Intel network cards often timeout
• Occurs when doing your own trajectory planning
  • Check all 4 items above
  • Callback code timeout, one cycle takes more than 1ms
  • If none solve it, planning needs adjustment. Noise is because position difference between adjacent commands is too large. For example, differentiate position command to velocity and check if velocity curve is smooth.

2.2.2.5 Do Motion and Safety Settings on Teach Pendant Take Effect in Real-Time Mode

Most do not take effect, including acceleration and other settings. Can be understood as robot becoming a pure actuator - if command is ok it executes, if not ok it powers off and reports error, with no additional constraints. Those that take effect:

• Collision detection: Powers off and stops after collision. No rollback, compliant stop, etc.
• Joint limits
• Base coordinate system
• Force control parameters, effective under impedance control

2.2.2.6 Callback Real-Time Issues

Operations executed within callback per cycle: Receive one controller sent data feedback -> Send one command. Controller sends status data in real-time. But host side does not require execution in real-time thread, so may not be strictly 1ms. Execution cycle is indirectly controlled by controller send data cycle.

2.2.2.7 Sagging Issue in Real-Time Mode Direct Torque Control

Torque control is related to robot arm hardware state. Can verify with following two methods, choose one:

1. Open drag mode, check if robot arm can maintain stationary state when holding drag button, without floating up or sagging.
2. Run RL force control related commands, check if they work normally. If above two verifications are not normal, then there is force control model error.

Solutions:

1. If force control model error exists, perform following operations to correct error:
   1. Set accurate load mass and center of mass, cannot be roughly estimated
   2. After setting load, perform force sensor calibration
2. If force control itself is fine, then sent torque values are inappropriate. Recommend sending all 0 for first torque control use. Under normal conditions robot arm should remain stationary.

2.2.2.8 Point Following Example Cannot Compile

Real-time mode example programs follow_joint_position and torque_control both use xMate model library. Model library is not supported on every system and architecture, so macro definitions are set. Need to set corresponding CMake option to ON. Take Visual Studio as example:

Using command line (other cmake command parameters omitted):

cmake -DXCORE_USE_XMATE_MODEL=ON

If not CMake project, add macro definition XMATEMODEL_LIB_SUPPORTED in project configuration.

2.2.2.9 Real-Time Mode Motion is Slow

• Issue 1: When using SDK to issue open-loop control commands (1ms call), found about 20ms lag between issued commands and actual execution.

• Issue 2: Following is slow.

Solution:

Real-time mode receive status data default wait timeout is 1ms. According to actual network conditions on site, can add configuration file xcoresdk_config.json in same directory as executable to set timeout, to adapt to actual conditions and optimize real-time effect. If device network conditions are good and after control for a while, sent motion commands have delayed execution phenomenon, can appropriately increase timeout. Setting range is 1~4ms. Specific json file format:

{
 "rt":{
  "_timeout_": 2
 }
}

2.2.2.10 Packet Loss Causes Power Off

Customer site provided logs often show packet loss causing power off.

1. Generally network card type Intel I219-LM is prone to packet loss. Recommend changing host to Realtek.
2. Set network threshold: Generally set 20, 30. Over 50 needs to investigate if it's a network issue.

 robot.setRtNetworkTolerance(20, ec);

3. Customers often re-encapsulate Rokae SDK interface. During communication may use same port 1337, causing network congestion. Host data packets cannot reach control system, forming packet loss.

2.2.2.11 SDK Interface Reports Communication Protocol Error xCore Minimum Version Required 2.3.0.0

Generally caused by Intel model network card. This network card's cache forwarding mechanism causes real-time mode to not receive data packets. Phenomenon is packet loss. Recommend customer change to Realtek model network card.

2.2.2.12 Motion Timeout, UDP Open Failed

Can be resolved using latest 1124 version or latest trunk version 3.1.3.

Through logs can see motion timeout when starting startMove:

Host error:

2.2.9 Network Connection Error Reported

Error Type: realtime: Failed to create RT controller.network: network connection

Please check set tool workpiece coordinate system, and startMove, startLoop interface call order.

2.3 AR Arm Usage

2.3.1 Prerequisites

• AR models mainly include two types, cross-wrist and SR type, divided into left and right arms (L: left arm, R: right arm). Generally customers choose side-mount installation.

• Version requirements

Control version: 3.0.1.6.AR1124

SDK version: librokae-v0.5.1.ar_9 and xcoresdk_python-v0.5.1.ar_9

• Installation method

  Left and right arms generally choose side-mount installation (left arm base and world coordinate do not coincide). Note: Left and right arm side-mount parameters are different. Following is left arm:

  

2.3.2 DEMO Usage

Currently most customers using AR models are for teleoperation, dexterous hands, grasping business scenarios. Customers have high requirements for robot real-time control scenarios, generally based on Cartesian position and joint control. Trajectory planning is particularly important. Therefore, during trajectory planning process, need to focus on motion smoothness and real-time performance. Recommend using spline interpolation or trapezoidal velocity curve for path optimization. At the same time, combine with AR model inverse kinematics solving characteristics to ensure each target point is within workspace, avoiding singular configurations. For teleoperation delay issues, set packet loss network threshold. For slow robot arm response, set timeout method.

2.3.2.1 Real-Time Mode - Cartesian Position Control

Refer to demo: cartesian_impedance_control.cpp implementation.

Notes

• Delete following code

// Set force control coordinate system to tool coordinate system, end relative to flange coordinate system
std::array<double, 16> toolToFlange = {0, 0, 1, 0, 0, 1, 0, 0, -1, 0, 0, 0, 0, 0, 0, 1};
rtCon->setFcCoor(toolToFlange, FrameType::tool, ec);
// Set Cartesian impedance coefficients
rtCon->setCartesianImpedance({1200, 1200, 0, 100, 100, 0}, ec);
// Set 3N desired force in X and Z directions
rtCon->setCartesianImpedanceDesiredTorque({3, 0, 3, 0, 0, 0}, ec);

• Provided Demo initialization class is 6-axis, so code won't run. AR is 7-axis, need to define following class. First parameter is robot IP, second parameter is host IP.

rokae::xMateErProRobot robot(ip, "192.168.2.190");

Callback interface updates Cartesian position in real-time. Customer can pass from external. Note callback interface should not have any blocking tasks, time-consuming tasks, such as socket communication, printing logs, writing files. Only single assignment, brief calculation is needed, ensuring tasks that can complete within 1ms.

std::function<CartesianPosition(void)> callback =[&,rtCon]()->CartesianPosition{
    time+=0.001;       
    output.pos = init_position;
    output.pos[7]+=delta_z;
    std::array<double, 6> xyzabc;       
    print(std::cout,"Position:",xyzabc)
        if(time > 0.01){
            std::cout<<"Motion End:"<<deltaz<<std::endl;
            output.setFinished();
            stopManually.store(false);//loop is non-blocking, synchronize stop status with main thread
            return output;
        } 
}

Code Flow

• Construct class

rokae::xMateErProRobot

• Set parameters, auto mode, real-time mode, power on. First switch to auto mode, then power on.

robot.setOperateMode(rokae::OperateMode::automatic,ec);
robot.setMotionControlMode(MotionControlMode::RtCommand, ec);
robot.setPowerState(true, ec);

• If user has tool workpiece installed at end, also need to set tool workpiece coordinates.

Refer to specific interface

//Set handheld tool coordinate system, Rv rotate 90
Toolsettoolset1:
toolset1.end.rpvl1l= MPt.2:
robot.setToolset(toolset1.ec) 

• Move to drag pose (customer may not need). AR drag pose joint angles need modification.

std::array<double,7> q_drag_xm7p = {0,M_PI/6,0,M_PI/3,0,0,0};
rtCon->MoveJ(0.5, robot.jointPos(ec), q_drag_xm7p);

• If customer experiences much packet loss during use, need to set network threshold. Generally recommend 20-30.

robot.setRtNetworkTolerance(50, ec);

• Callback implementation: Ensure callback content implementation is singular, don't add delay tasks, blocking tasks, time-consuming tasks, otherwise affects controller scheduling, causing robot arm scheduling to be slow, noise, motion delay and other unpredictable errors.

Given Cartesian point is 4*4 matrix, based on base coordinate system. For conversion please refer to interfaces provided in utility.h file.

std::function<CartesianPosition(void)> callback = [&, rtCon]()->CartesianPosition{
    time += 0.001;
    constexpr double kRadius = 0.1;
    double angle = M_PI / 4 * (1 - std::cos(M_PI / 2 * time));
    double delta_z = kRadius * (std::cos(angle) - 1);
    CartesianPosition output{};
    output.pos = init_position;
    output.pos[7] += delta_z; 
    if(time > 0.01){
        std::cout << "Motion End:"<<delta_z <<std::endl;
        output.setFinished();
        stopManually.store(false); // loop is non-blocking, synchronize stop status with main thread
    }
    return output;
};

• Start real-time mode

rtCon->setControlLoop(callback, 0, true);
//Before starting motion, set to Cartesian space position control
rtCon->startMove(RtControllerMode::cartesianPosition);
rtCon->startLoop(true);

2.3.2.2 Real-Time Mode - Joint Control

Refer to demo: joint_position_control.cpp

Notes

• Refer to point 2 in "Notes" section above.

Code Flow

• First 5 points refer to first 5 points in 2.3.2.1.2 above.
• Callback function, assignment is joint angles, unit is radians. Host planning mostly plans Cartesian points in world coordinate system, so use conversion interfaces provided in utility.h for conversion.

std::function<JointPosition()> callback = [&, rtCon]() {
    time += 0.001;
    double     delta_angle = M_PI / 20.0 * (1 - std::cos(M_PI / 2.5 * time));
    JointPosition cmd     = {{jntPos[0] + delta_angle, jntPos[1] + delta_angle, jntPos[2] - delta_angle,
                              jntPos[3] + delta_angle, jntPos[4] - delta_angle, jntPos[5] + delta_angle,
                              jntPos[6] - delta_angle}};
    if (time > 60)
    {
        cmd.setFinished(); // End after 60 seconds
    }
    return cmd;
};

• Start real-time mode: Joint control

// Set callback function
rtCon->setControlLoop(callback);
// Update starting angle to current angle
jntPos = robot.jointPos(ec);
// Start joint space position control
rtCon->startMove(RtControllerMode::jointPosition);
// Blocking loop, start motion
rtCon->startLoop(true);

2.3.2.3 Forward/Inverse Kinematics

Process and code description, steps and code description. To prevent singularities from causing forward/inverse kinematics calculation failure, verification process should first teach points on HMI. If can normally reach via MOVL or movj on HMI, SDK interface call will be fine.

• Connect to robot

Notes:

• Don't get IP addresses reversed or wrong, otherwise connection fails.
• Initialization class is xMateErProRobot, this is 7-axis robot class.

xMateErProRobot robot("192.168.2.160", "192.168.2.110"); // Use xMateErProRobot to match 7-axis

• Set auto mode and power on

robot.setOperateMode(OperateMode::automatic, ec);
if (ec) {
    cout << "Set auto mode failed: " << ec.message() << endl;
    return -1;
}
robot.setPowerState(true, ec);
if (ec) {
    cout << "Power on failed: " << ec.message() << endl;
    return -1;
} 

• Set tool workpiece coordinates

If necessary, need to set tool workpiece coordinates.

Toolset toolset1;
toolset1 = robot.toolset(ec);
print(std::cout, "Tool workpiece coordinate system read from controller:", toolset1);

• Get joint angles

Used for forward kinematics calculation.

// Get current robot joint angles
auto  current_joints = robot.jointPos(ec);
if (ec) {
    cout << "Get joint angles failed: " << ec.message() << endl;
    return -1;
} 

• Get end pose

Used for inverse kinematics calculation. Pay special attention to screenshot parameters. Generally flange will have tool installed. If not installed, this value is flange pose.

// Get current end pose reported by controller
auto current_cartesian_from_robot = robot.cartPosture(CoordinateType::endInRef, ec);
if (ec) {
    cout << "Get end pose failed: " << ec.message() << endl;
    return -1;
} 

• Set not to use axis configuration parameters for inverse kinematics calculation

This value doesn't need to be set by default, default is off. Some customers directly use SDK interface without HMI operation, so better to set.

//Set not to use axis configuration parameters for inverse kinematics calculation
robot.setDefaultConfOpt(false, ec); 

• Calculate forward kinematics

Use value from step 3 to get joint angles.

auto fk_cartesian = robot.model().calcFk(current_joints, toolset1, ec);
if (ec) {
    cout << "Forward kinematics calculation failed: " << ec.message() << endl;
} 

Check if printed data is correct and compare with HMI interface data. If consistent, it's correct.

• Calculate inverse kinematics

Use data obtained in step 5. If calculation fails, can use pose calculated from forward kinematics for calculation.

If still fails, controller has problem. Find corresponding technical personnel to solve.

auto ik_joints_m1 = robot.model().calcIk(endPos, toolset1, ec); 

Compare with joint angles displayed on HMI.

• New interface parameter description: Add arm angle precision value

Search arm angle precision. If arm angle or arm angle search precision is not set, calculated value will be inaccurate with larger difference.

auto ik_joints_m1 =robot.model().calcIk_SearchElbow(endPos,toolset1,5,ec) 

2.3.2.4 Forward/Inverse Kinematics Based on Model Library

Using this method can speed up inverse kinematics. Currently released controller version does not support all models. Currently for AR model control version supports AR models. For other models to use, please select version 3.1.2 and above.

• Connect to robot

rokae::xMateErProRobot robot("192.168.0.160");

• Initialize joint angles

std::array<double, 7> zeros = {0, 0, 0, 0, 0, 0, 0},
//Initialize joint angles
jointPos_in = Utils::degToRad(array<double,7>({5, 46, -10, 91, 1, -105, 11})),
jointVel_in = {0.3, 0.2, 0.5, 0.4, 0.1, 0.1, 0.1},
jointAcc_in = {1.3, 3.1, 4.1, 1.5, 1.6, 4.1, 8.1},
jointInit = Utils::degToRad(array<double,7>({6, 45, -9, 92, 0, -103, 10})),
array1 {}, array2 {}, array3 {}, array4 {};

• Calculate forward kinematics

/**
* @brief Get Cartesian space position
* @param[in] jntPos Joint angles to calculate Cartesian pose
* @param[in] nr Specify coordinate system, default is flange
* @return Vectorized 4x4 pose matrix, row-major.
*/
std::array<double, 16> getCartPose(const std::array<double, DoF> &jntPos, SegmentFrame nr = SegmentFrame::flange);
auto pos = model.getCartPose(jointPos_in);

• Calculate inverse kinematics

Get joint angles according to input arm angle search precision.

/**
  * @brief Traverse from [-PI,PI] for target comparison, get closest inverse solution
  * @param[in] cartPos Flange Cartesian space pose
  * @param[in] psi_accuracy Arm angle solving precision, unit degrees (e.g., 1, 10, 20), higher precision, longer solving time
  * @param[in] elbow Arm angle, original interface extension, temporarily unused
  * @param[in] jntInit Initial joint angles
  * @param[out] jntPos Joint space position
  * @return Inverse kinematics calculation result -
  *   1) -1, -2, -3: No solution, reason is cartPos exceeds robot workspace or no solution
  *   2) -4, -5: jntPos differs greatly from jntInit. Generally jntInit represents robot current position, difference between jntPos and jntInit can be equivalent to motor speed.
  *  If exceeds robot axis rated speed, returns -4 or -5;
  *   3) -6, -7: jntPos exceeds soft limits;
  *   4) -8: Robot singularity;
  */
int getJointPosSearchPsi(const std::array<double, 16> &cartPos, const double& psi_accuracy, double elbow,const std::array<double, DoF> &q_init, std::array<double, DoF> &q);

Added arm angle traversal input.

double psi = Utils::degToRad(-7.177);
double psi2 = Utils::degToRad(0);
array<int, 8> confData = {0, 0, 0, 0, 0, 0, 0,0};
gettimeofday(&start, NULL);
auto ret = model.getJointPos(pos, psi, jointInit, array1);
auto ret = model.getJointPosSearchPsi(pos,1, psi2, jointInit, array1);

2.3.3 New Interface - Arm Angle Traversal

Added arm angle search precision setting in inverse kinematics interface. Value greater than 0 is acceptable.

• General

Adding this inverse kinematics interface generally requires more than 42ms to get results.

See section 2.3.2.4 demo implementation.

/**
  * @brief Calculate inverse kinematics from pose. Automatically traverse arm angle, search range -PI-PI
  * @param[in] posture Robot end pose, relative to external reference coordinate system. Reference coordinate system is set via setToolset()
  * @param[in] elb_accuracy Arm angle precision, greater than 0 is acceptable. Smaller value, higher search precision, longer calculation time. Recommended value greater than 10.
  * @param[out] ec Error code
  * @return Axis angles, unit: radians
  */
std::array<double, DoF> calcIk_SearchElbow(CartesianPosition posture, double elb_accuracy, error_code &ec) noexcept;

/**
  * @brief Calculate inverse kinematics under given tool workpiece coordinate system from pose. Automatically traverse arm angle, search range -180-180°
  * @param[in] posture Robot end pose, relative to external reference coordinate system
  * @param[in] tool_set Tool workpiece coordinate system
  * @param[in] elb_accuracy Arm angle precision, greater than 0 is acceptable. Smaller value, higher search precision, longer calculation time.
  * @param[out] ec Error code
  * @return Axis angles, unit: radians
  */
std::array<double, DoF> calcIk_SearchElbow(CartesianPosition posture,const Toolset &tool_set, double elb_accuracy, error_code& ec) noexcept;

• Model library inverse kinematics

Added arm angle traversal to meet inverse kinematics speed requirements.

/**
 * @brief Traverse from [-PI,PI] for target comparison, get closest inverse solution
 * @param[in] cartPos Flange Cartesian space pose
 * @param[in] psi_accuracy Arm angle solving precision, unit degrees (e.g., 1, 10, 20). Smaller value, higher precision, longer solving time.
 * @param[in] elbow Arm angle, interface extension use, temporarily unused.
 * @param[in] jntInit Initial joint angles
 * @param[out] jntPos Joint space position
 * @return Inverse kinematics calculation result -
 *   1) -1, -2, -3: No solution, reason is cartPos exceeds robot workspace or no solution
 *   2) -4, -5: jntPos differs greatly from jntInit. Generally jntInit represents robot current position, difference between jntPos and jntInit can be equivalent to motor speed.
 *  If exceeds robot axis rated speed, returns -4 or -5;
 *   3) -6, -7: jntPos exceeds soft limits;
 *   4) -8: Robot singularity;
  */
int getJointPosSearchPsi(const std::array<double, 16> &cartPos, const double& psi_accuracy, double elbow, const std::array<double, DoF> &q_init, std::array<double, DoF> &q);

2.4 Real-Time Mode AR Arm Common Issues

2.4.1 Left and Right Arm Motion Inconsistent, One Fast One Slow

If same version, same configuration, but two arms have inconsistent motion, check if following switch is enabled on HMI.

2.4.2 Forward/Inverse Kinematics Calculation Failed

• Confirm on HMI if following parameter is turned off.

  

• Call interface to turn off conf.

​ Set not to use axis configuration parameters for inverse kinematics calculation.

robot.setDefaultConfOpt(false, ec);