Closed acf986 closed 10 months ago
@acf986
std::atomic<bool> g_stop = {false};
/** @brief Callback function for realtime periodic task 1 */
void periodicTask1(flexiv::Robot& robot, flexiv::Scheduler& scheduler)
{
robot.streamCartesianMotionForce();
if (g_stop == true) {
robot.stop();
scheduler.stop();
}
}
/** @brief Callback function for realtime periodic task 2 */
void periodicTask2(flexiv::Robot& robot, flexiv::Scheduler& scheduler)
{
robot.streamJointTorque();
if (g_stop == true) {
robot.stop();
scheduler.stop();
}
}
int main(int argc, char* argv[])
{
flexiv::Robot robot("IP", "IP");
// Create some other thread that will set g_stop = true
// ...
{
robot.setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE_BASE);
// Create real-time scheduler to run periodicTask1
flexiv::Scheduler scheduler;
// Run periodicTask1 with blocking
scheduler.addTask(std::bind(periodicTask1, std::ref(robot), std::ref(scheduler)),
"HP periodic", 1, scheduler.maxPriority());
scheduler.start(true);
} // scheduler instance goes out of scope, destroyed
{
robot.setMode(flexiv::Mode::RT_JOINT_TORQUE);
// Create another real-time scheduler to run periodicTask2
flexiv::Scheduler scheduler;
// Run periodicTask1 with blocking
scheduler.addTask(std::bind(periodicTask2, std::ref(robot), std::ref(scheduler)),
"HP periodic", 1, scheduler.maxPriority());
scheduler.start(true);
} // scheduler instance goes out of scope, destroyed
return 0;
}
@pzhu-flexiv Thanks very much for your fast response and detailed example code. Following your example, may I ask:
@acf986 That's totally fine, you can manipulate the resources from wherever you want, just make sure to design proper protection for multi-thread resource access.
@acf986
You could also implement a state machine to perform the mode switching, the architecture design could be like this, but it depends on your use cases and you could add/delete more states.
void stateMachine(flexiv::Robot& robot)
{
switch (g_state) {
case SM_IDLE: {
// Move to another state depends on the condition
g_state = ...
break;
}
case SM_ENTER_RT_JOINT_POSITION: {
// Set operation mode to joint position control mode
robot->setMode(flexiv::Mode::RT_JOINT_POSITION);
g_state = SM_STREAM_JOINT_POSITION;
break;
}
case SM_STREAM_JOINT_POSITION: {
// execute joint trajectory
// exit this state if finish
g_state = SM_EXIT_RT_JOINT_POSITION;
break;
}
case SM_EXIT_RT_JOINT_POSITION: {
// Set operation mode to idle mode
robot->setMode(flexiv::Mode::IDLE);
g_state = SM_IDLE;
break;
}
case SM_ENTER_RT_CARTESIAN_MOTION_FORCE: {
// Set operation mode to Cartesian motion force control mode
robot->setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE_BASE);
g_state = SM_STREAM_CARTESIAN_MOTION_FORCE;
break;
}
case SM_STREAM_CARTESIAN_MOTION_FORCE: {
// execute Cartesian motion force
// exit this state if finish
g_state = SM_EXIT_RT_CARTESIAN_MOTION_FORCE;
break;
}
case SM_EXIT_RT_CARTESIAN_MOTION_FORCE: {
// Set operation mode to idle mode
robot->setMode(flexiv::Mode::IDLE);
g_state = SM_IDLE;
break;
}
}
}
@munseng-flexiv , Hi, I have tried to use your approach, however, by putting the controller switch inside the 1000Hz realtime loop, it somes times gives me the following warning:
[2023-10-10 15:25:52.650] [info] Stop from here.
[2023-10-10 15:25:52.650] [warning] [Timeliness Monitor] 20 missed messages in 5s, RDK connection is about to close
[2023-10-10 15:25:53.650] [info] [flexiv::Robot] Operation mode transited to [IDLE]
The switching code looks like this
// Stop the robot immediately from the RT loop
// if an IDLE state has been required
// if this does not prevent the RT monitor throw error, nothing user can do?
if (signal->payload_ == IDLE)
{
log_->info("Stop from here.");
robot_->stop();
}
Notice, the code has reached "Stop from here", then it gives the Timeliness monitor warning, then the stop has been finished. There is nothing I could do, because the stop function is blocking, and itself takes around 1s to finish.
The problem in this case is that the timeliness monitor would terminate the entire RDK connection and kill the program effectively.
@acf986 May I know in what mode you call the robot->stop()
? And I did not see any mode-switching API in your code.
@acf986 I did a quick test using the state machine to do mode switching between the two modes, and I did not encounter the timeliness monitor error. The example switches the RT control mode, and streams the same motion commands to hold the current robot positions. You can try running this example.
#include <flexiv/Robot.hpp>
#include <flexiv/Exception.hpp>
#include <flexiv/Log.hpp>
#include <flexiv/Scheduler.hpp>
#include <flexiv/Utility.hpp>
#include <iostream>
#include <string>
#include <thread>
namespace {
/** State machine for mode switching between Joint Position and Cartesian Motion Force */
enum StateMachine
{
SM_IDLE = 0,
SM_ENTER_RT_JOINT_POSITION,
SM_STREAM_JOINT_POSITION,
SM_EXIT_RT_JOINT_POSITION,
SM_ENTER_RT_CARTESIAN_MOTION_FORCE,
SM_STREAM_CARTESIAN_MOTION_FORCE,
SM_EXIT_RT_CARTESIAN_MOTION_FORCE
};
/** Commands */
enum CmdType
{
CMD_RUN_JOINTTRAJECTORY = 0,
CMD_RUN_CARTESIANMOTIONFORCE,
CMD_STOP
};
/** Global variables */
StateMachine g_state = SM_IDLE;
CmdType m_cmd = CMD_STOP;
}
/** @brief Print program usage help */
void printHelp()
{
// clang-format off
std::cout << "Required arguments: [robot IP] [local IP]" << std::endl;
std::cout << " robot IP: address of the robot server" << std::endl;
std::cout << " local IP: address of this PC" << std::endl;
std::cout << "Optional arguments: None" << std::endl;
std::cout << std::endl;
// clang-format on
}
void stateMachine(flexiv::Robot& robot, flexiv::RobotStates& robotStates, flexiv::Log& log)
{
switch (g_state) {
case SM_IDLE: {
// Move to another state depends on the commands
if (m_cmd == CMD_RUN_JOINTTRAJECTORY) {
log.info("Moving to SM_ENTER_RT_JOINT_POSITION state");
g_state = SM_ENTER_RT_JOINT_POSITION;
} else if (m_cmd == CMD_RUN_CARTESIANMOTIONFORCE) {
log.info("Moving to SM_ENTER_RT_CARTESIAN_MOTION_FORCE state");
g_state = SM_ENTER_RT_CARTESIAN_MOTION_FORCE;
}
break;
}
case SM_ENTER_RT_JOINT_POSITION: {
// Set operation mode to joint position control mode
robot.setMode(flexiv::Mode::RT_JOINT_POSITION);
log.info("Moving to SM_STREAM_JOINT_POSITION state");
g_state = SM_STREAM_JOINT_POSITION;
break;
}
case SM_STREAM_JOINT_POSITION: {
// Read robot states
robot.getRobotStates(robotStates);
// Get current position as initial position
auto initPos = robotStates.q;
// Robot degrees of freedom
size_t robotDOF = robotStates.q.size();
// Initialize target vectors to hold position
std::vector<double> targetPos(robotDOF, 0);
std::vector<double> targetVel(robotDOF, 0);
std::vector<double> targetAcc(robotDOF, 0);
targetPos = initPos;
// Stream smooth and continuous commands
// In this example, we set to stream the same commands to hold the initial position
robot.streamJointPosition(targetPos, targetVel, targetAcc);
// Exit this state if get stop command
// Or indicator condition that the joint position is reached
if (m_cmd == CMD_STOP) {
log.info("Moving to SM_EXIT_RT_JOINT_POSITION state");
g_state = SM_EXIT_RT_JOINT_POSITION;
}
break;
}
case SM_EXIT_RT_JOINT_POSITION: {
// Set operation mode to idle mode or stop the robot
// robot.setMode(flexiv::Mode::IDLE);
robot.stop();
log.info("Moving to SM_IDLE state");
g_state = SM_IDLE;
break;
}
case SM_ENTER_RT_CARTESIAN_MOTION_FORCE: {
// Set operation mode to Cartesian motion force control mode
robot.setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE_BASE);
log.info("Moving to SM_STREAM_CARTESIAN_MOTION_FORCE state");
g_state = SM_STREAM_CARTESIAN_MOTION_FORCE;
break;
}
case SM_STREAM_CARTESIAN_MOTION_FORCE: {
// Read robot states
robot.getRobotStates(robotStates);
// Get current position as initial position
auto initTcpPose = robotStates.tcpPose;
// Hold the TCP position
auto targetTcpPose = initTcpPose;
// Stream smooth and continuous commands
// In this example, we set to stream the same commands to hold the initial position
robot.streamCartesianMotionForce(targetTcpPose);
// exit this state if get stop command
// Or indicator condition that the joint position is reached
if (m_cmd == CMD_STOP) {
log.info("Moving to SM_EXIT_RT_CARTESIAN_MOTION_FORCE state");
g_state = SM_EXIT_RT_CARTESIAN_MOTION_FORCE;
}
break;
}
case SM_EXIT_RT_CARTESIAN_MOTION_FORCE: {
// Set operation mode to idle mode or stop the robot
// robot.setMode(flexiv::Mode::IDLE);
robot.stop();
log.info("Moving to SM_IDLE state");
g_state = SM_IDLE;
break;
}
}
}
int main(int argc, char* argv[])
{
// Program Setup
// =============================================================================================
// Logger for printing message with timestamp and coloring
flexiv::Log log;
// Parse parameters
if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) {
printHelp();
return 1;
}
// IP of the robot server
std::string robotIP = argv[1];
// IP of the workstation PC running this program
std::string localIP = argv[2];
try {
// RDK Initialization
// =========================================================================================
// Instantiate robot interface
flexiv::Robot robot(robotIP, localIP);
// Create data struct for storing robot states
flexiv::RobotStates robotStates;
// Clear fault on robot server if any
if (robot.isFault()) {
log.warn("Fault occurred on robot server, trying to clear ...");
// Try to clear the fault
robot.clearFault();
std::this_thread::sleep_for(std::chrono::seconds(2));
// Check again
if (robot.isFault()) {
log.error("Fault cannot be cleared, exiting ...");
return 1;
}
log.info("Fault on robot server is cleared");
}
// Enable the robot, make sure the E-stop is released before enabling
log.info("Enabling robot ...");
robot.enable();
// Wait for the robot to become operational
int secondsWaited = 0;
while (!robot.isOperational()) {
std::this_thread::sleep_for(std::chrono::seconds(1));
if (++secondsWaited == 10) {
log.warn(
"Still waiting for robot to become operational, please check that the robot 1) "
"has no fault, 2) is in [Auto (remote)] mode");
}
}
log.info("Robot is now operational");
// Move robot to home pose
log.info("Moving to home pose");
robot.setMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION);
robot.executePrimitive("Home()");
// Wait for the primitive to finish
while (robot.isBusy()) {
std::this_thread::sleep_for(std::chrono::seconds(1));
}
// Real-time Mode Switching
// =========================================================================================
// Create real-time scheduler to run periodic tasks
flexiv::Scheduler scheduler;
// Add periodic task with 1ms interval and highest applicable priority
scheduler.addTask(
std::bind(stateMachine, std::ref(robot), std::ref(robotStates), std::ref(log)),
"HP periodic", 1, scheduler.maxPriority());
// Start all added tasks, this is by default a blocking method
scheduler.start(false);
// TODO: Better to set the commands in a separate thread and left the main thread to start
// TODO: the scheduler only
// Wait for 2s
std::this_thread::sleep_for(std::chrono::seconds(2));
// Change to Joint Position Control Mode and
// Holds for 5s
m_cmd = CMD_RUN_JOINTTRAJECTORY;
std::this_thread::sleep_for(std::chrono::seconds(5));
// Stop the robot and wait for 1s
m_cmd = CMD_STOP;
std::this_thread::sleep_for(std::chrono::seconds(1));
// Change to CartesianMotionForce mode and
// Holds for 5s
m_cmd = CMD_RUN_CARTESIANMOTIONFORCE;
std::this_thread::sleep_for(std::chrono::seconds(5));
// Stop the robot command
m_cmd = CMD_STOP;
std::this_thread::sleep_for(std::chrono::seconds(1));
} catch (const flexiv::Exception& e) {
log.error(e.what());
return 1;
}
return 0;
}
Thanks very much, so it is fine for a cycle in the 1000hz realtime loop to exceed 1ms? Previously was told it will trigger an error.
Thanks very much, so it is fine for a cycle in the 1000hz realtime loop to exceed 1ms? Previously was told it will trigger an error.
setMode()
is fine, but not for the RT motion commands e.g. streamJointPosition()
. It will trigger the timeliness monitor error if the robot controller does not receive motion commands in 1ms in RT mode.
@munseng-flexiv , after some testing, by modifying the state switching part on the main thread to sth like:
for (int i=0; i< 20; i++)
{
m_cmd = CMD_RUN_JOINTTRAJECTORY;
std::this_thread::sleep_for(std::chrono::milliseconds(1));
// Stop the robot and wait for 1s
m_cmd = CMD_STOP;
std::this_thread::sleep_for(std::chrono::seconds(1));
}
could easily trigger this phenomenon:
[2023-10-10 18:52:36.583] [info] ================================================
[2023-10-10 18:52:36.583] [info] === Flexiv RDK 0.8 ===
[2023-10-10 18:52:36.583] [info] ================================================
[2023-10-10 18:52:36.583] [info] [flexiv::Robot] Network service started [RT:99]
[2023-10-10 18:52:37.83] [info] [flexiv::Robot] Connection established
[2023-10-10 18:52:37.86] [info] [flexiv::Robot] Server information:
[2023-10-10 18:52:37.86] [info] Serial number: Rizon 4s-062060
[2023-10-10 18:52:37.86] [info] Degrees of freedom: 7
[2023-10-10 18:52:37.86] [info] Software version: v2.10.5
[2023-10-10 18:52:37.89] [info] [flexiv::Robot] Initialization complete
[2023-10-10 18:52:37.89] [info] Enabling robot ...
[2023-10-10 18:52:37.114] [info] Robot is now operational
[2023-10-10 18:52:37.114] [info] Moving to home pose
[2023-10-10 18:52:38.114] [info] [flexiv::Robot] Operation mode transited to [NRT_PRIMITIVE_EXECUTION]
[2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Initialization complete
[2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Maximum available priority for the user task is [97]
[2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Added task [HP periodic]: interval [1 ms], priority [97], CPU affinity [-1]
[2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Starting all tasks
[2023-10-10 18:52:42.181] [info] Moving to SM_ENTER_RT_JOINT_POSITION state
[2023-10-10 18:52:44.182] [info] [flexiv::Robot] Operation mode transited to [RT_JOINT_POSITION]
[2023-10-10 18:52:44.182] [info] Moving to SM_STREAM_JOINT_POSITION state
[2023-10-10 18:52:44.182] [info] Moving to SM_EXIT_RT_JOINT_POSITION state
[2023-10-10 18:52:45.182] [info] [flexiv::Robot] Operation mode transited to [IDLE]
[2023-10-10 18:52:45.182] [info] Moving to SM_IDLE state
[2023-10-10 18:52:45.185] [info] Moving to SM_ENTER_RT_JOINT_POSITION state
[2023-10-10 18:52:46.186] [info] [flexiv::Robot] Operation mode transited to [RT_JOINT_POSITION]
[2023-10-10 18:52:46.186] [info] Moving to SM_STREAM_JOINT_POSITION state
[2023-10-10 18:52:46.187] [info] Moving to SM_EXIT_RT_JOINT_POSITION state
[2023-10-10 18:52:47.187] [info] [flexiv::Robot] Operation mode transited to [IDLE]
[2023-10-10 18:52:47.187] [info] Moving to SM_IDLE state
[2023-10-10 18:52:47.187] [info] Moving to SM_ENTER_RT_JOINT_POSITION state
//------------------------------For-the-ease-of-your-view-----------------------------------------------
[2023-10-10 18:52:47.260] [warning] [Timeliness Monitor] 10 missed messages in 5s
[2023-10-10 18:52:47.269] [warning] [Timeliness Monitor] 20 missed messages in 5s, RDK connection is about to close
//------------------------------For-the-ease-of-your-view-----------------------------------------------
[2023-10-10 18:52:48.187] [info] [flexiv::Robot] Operation mode transited to [RT_JOINT_POSITION]
[2023-10-10 18:52:48.187] [info] Moving to SM_STREAM_JOINT_POSITION state
[2023-10-10 18:52:48.187] [info] Moving to SM_EXIT_RT_JOINT_POSITION state
[2023-10-10 18:52:50.83] [info] [flexiv::Robot] Network service closed
^C[moveit_cpp_tutorial-2] killing on exit
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done
In my original program, because I set to switch the mode after a certain trajectory has been finished. Unfortunately, during testing, the trajectory I passed to it is just a single point (of current pose), so it always finishes instantly. Therefore, resulting in this issue. It seems as long as there are some delay, the issue will not appear.
However, it also suggest that the RDK probably should handle this formally through a logic synchronization? Relying on a user introduced delay probably isn't so reliable in the long term.
@acf986
in what use case do you have to send the motion command for only 1 ms and then stop the robot?
In a normal use case, you should stream continuous joint motion commands streamJointPosition()
until it reaches the joint target (check if the joint position is reached in the SM_STREAM_JOINT_POSITION
state) and then only stop the robot.
In RT mode (or SM_STREAM_JOINT_POSITION
in the state machine example), the joint motion commands / joint trajectory should be interpolated with your controller and sent continuously. If not, you could use the NRT sendJointPosition()
which uses the internal trajectory generator that will interpolate between two set points and make the motion smooth.
@pzhu-flexiv @munseng-flexiv, Hi, the cause for "send the motion command for only 1ms" is due to:
After this, we switch the mode, which is by putting the robot to stop first. These 3 steps together can be simulated by the code:
{
...
// Switch to joint trajectory mode, execute some trajectory
m_cmd = CMD_RUN_JOINTTRAJECTORY;
// The trajectory is finished instantly
std::this_thread::sleep_for(std::chrono::milliseconds(1));
// Stop the robot and prepare to switch mode
m_cmd = CMD_STOP;
...
}
We will add more check on the trajectory length and rules out those will be finished instantly. And if possible, could you kindly provide some guidelines on the minimum allowed execution time of the trajectory in this case.
@munseng-flexiv , after some testing, by modifying the state switching part on the main thread to sth like:
for (int i=0; i< 20; i++) { m_cmd = CMD_RUN_JOINTTRAJECTORY; std::this_thread::sleep_for(std::chrono::milliseconds(1)); // Stop the robot and wait for 1s m_cmd = CMD_STOP; std::this_thread::sleep_for(std::chrono::seconds(1)); }
could easily trigger this phenomenon:
[2023-10-10 18:52:36.583] [info] ================================================ [2023-10-10 18:52:36.583] [info] === Flexiv RDK 0.8 === [2023-10-10 18:52:36.583] [info] ================================================ [2023-10-10 18:52:36.583] [info] [flexiv::Robot] Network service started [RT:99] [2023-10-10 18:52:37.83] [info] [flexiv::Robot] Connection established [2023-10-10 18:52:37.86] [info] [flexiv::Robot] Server information: [2023-10-10 18:52:37.86] [info] Serial number: Rizon 4s-062060 [2023-10-10 18:52:37.86] [info] Degrees of freedom: 7 [2023-10-10 18:52:37.86] [info] Software version: v2.10.5 [2023-10-10 18:52:37.89] [info] [flexiv::Robot] Initialization complete [2023-10-10 18:52:37.89] [info] Enabling robot ... [2023-10-10 18:52:37.114] [info] Robot is now operational [2023-10-10 18:52:37.114] [info] Moving to home pose [2023-10-10 18:52:38.114] [info] [flexiv::Robot] Operation mode transited to [NRT_PRIMITIVE_EXECUTION] [2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Initialization complete [2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Maximum available priority for the user task is [97] [2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Added task [HP periodic]: interval [1 ms], priority [97], CPU affinity [-1] [2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Starting all tasks [2023-10-10 18:52:42.181] [info] Moving to SM_ENTER_RT_JOINT_POSITION state [2023-10-10 18:52:44.182] [info] [flexiv::Robot] Operation mode transited to [RT_JOINT_POSITION] [2023-10-10 18:52:44.182] [info] Moving to SM_STREAM_JOINT_POSITION state [2023-10-10 18:52:44.182] [info] Moving to SM_EXIT_RT_JOINT_POSITION state [2023-10-10 18:52:45.182] [info] [flexiv::Robot] Operation mode transited to [IDLE] [2023-10-10 18:52:45.182] [info] Moving to SM_IDLE state [2023-10-10 18:52:45.185] [info] Moving to SM_ENTER_RT_JOINT_POSITION state [2023-10-10 18:52:46.186] [info] [flexiv::Robot] Operation mode transited to [RT_JOINT_POSITION] [2023-10-10 18:52:46.186] [info] Moving to SM_STREAM_JOINT_POSITION state [2023-10-10 18:52:46.187] [info] Moving to SM_EXIT_RT_JOINT_POSITION state [2023-10-10 18:52:47.187] [info] [flexiv::Robot] Operation mode transited to [IDLE] [2023-10-10 18:52:47.187] [info] Moving to SM_IDLE state [2023-10-10 18:52:47.187] [info] Moving to SM_ENTER_RT_JOINT_POSITION state //------------------------------For-the-ease-of-your-view----------------------------------------------- [2023-10-10 18:52:47.260] [warning] [Timeliness Monitor] 10 missed messages in 5s [2023-10-10 18:52:47.269] [warning] [Timeliness Monitor] 20 missed messages in 5s, RDK connection is about to close //------------------------------For-the-ease-of-your-view----------------------------------------------- [2023-10-10 18:52:48.187] [info] [flexiv::Robot] Operation mode transited to [RT_JOINT_POSITION] [2023-10-10 18:52:48.187] [info] Moving to SM_STREAM_JOINT_POSITION state [2023-10-10 18:52:48.187] [info] Moving to SM_EXIT_RT_JOINT_POSITION state [2023-10-10 18:52:50.83] [info] [flexiv::Robot] Network service closed ^C[moveit_cpp_tutorial-2] killing on exit [rosout-1] killing on exit [master] killing on exit shutting down processing monitor... ... shutting down processing monitor complete done
In my original program, because I set to switch the mode after a certain trajectory has been finished. Unfortunately, during testing, the trajectory I passed to it is just a single point (of current pose), so it always finishes instantly. Therefore, resulting in this issue. It seems as long as there are some delay, the issue will not appear.
However, it also suggest that the RDK probably should handle this formally through a logic synchronization? Relying on a user introduced delay probably isn't so reliable in the long term.
@acf986 What do you mean by user-introduced delay? The only requirement for RT mode to work properly on the robot server side is that the commands arrive at 1kHz, not faster, not slower. flexiv::Scheduler is meant for this purpose.
After transited into an RT mode, the timeliness monitor is activated at the time point where you call the corresponding streamXXX() function for the first time, after which the timeliness monitor expect you to call streamXXX() at exactly 1kHz, the command cannot arrive sooner or later, which is why strict scheduling is required. Note that std::this_thread::sleep_for(std::chrono::milliseconds(1))
won't be able to do strict 1kHz scheduling as this will result in a loop time of (1ms + computation_time).
The timeliness monitor is deactivated when the robot server receives the signal to transit to IDLE or any NRT mode (calling Robot::stop()
sends a signal to the server to transit to IDLE mode). With a wired connection, the time taken for the server to receive the transition signal should be no longer than 1/10 of the time threshold required to trigger timeliness monitor violation, so you should be able to deactivate the timeliness monitor without violation by simply calling
robot.stop();
scheduler.stop();
Then you setMode() for the second RT mode, and re-initialize the scheduler to run another periodic task for that second RT mode. Just like mentioned in the first example I provided. The above mentioned RT mode transition is part of our unit test.
@acf986 I have fixed the memory leak issue https://github.com/flexivrobotics/flexiv_rdk/issues/37 in v0.9 so it'll be safe to use the first example I provided for switching RT modes.
Thanks very much for all your help. For the timeliness monitor related topic, I think I would stated it in a different thread.
@pzhu-flexiv From the example, a RT mode is always paired with a
periodicTask
that is scheduled by a scheduler. If the robot is atRT_CARTESIAN_MOTION_FORCE_BASE
mode, we need to callstreamCartesianMotionForce()
inside the periodicTask. If the robot is atRT_JOINT_POSITION
mode, we need to callstreamJointPosition()
inside theperiodicTask
. Could you kindly show the suggested way to organize the scheduler and periodicTask, when switching from one RT mode to a different RT mode (sayRT_CARTESIAN_MOTION_FORCE_BASE
toRT_JOINT_POSITION
).RT_CARTESIAN_MOTION_FORCE_BASE
and the other pair forRT_JOINT_POSITION
?stream
function?