From 29ea12d53750ec629b4e4b1f9bcbb49dd95a6333 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Tue, 15 Jul 2025 11:44:51 -0400 Subject: [PATCH 001/101] Mark version as `pre` --- src/MotoROS.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/MotoROS.h b/src/MotoROS.h index 85f4d18f..6a4c8c61 100644 --- a/src/MotoROS.h +++ b/src/MotoROS.h @@ -9,7 +9,7 @@ #define MOTOROS2_MOTOROS_H #define APPLICATION_NAME "MotoROS2" -#define APPLICATION_VERSION "0.2.1" +#define APPLICATION_VERSION "0.2.2-pre" #include "motoPlus.h" From 1d457dc69983fe0f65bc3cc2dc14b95c70edc04f Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Tue, 15 Jul 2025 11:45:19 -0400 Subject: [PATCH 002/101] Add service `start_rt_mode` --- src/CommunicationExecutor.c | 5 ++ src/CommunicationExecutor.h | 3 +- src/ErrorHandling.h | 2 + src/MotoROS.h | 1 + src/MotoROS2_AllControllers.vcxproj | 2 + src/MotoROS2_AllControllers.vcxproj.filters | 6 ++ src/RosApiNameConstants.h | 1 + src/ServiceStartRtMode.c | 88 +++++++++++++++++++++ src/ServiceStartRtMode.h | 27 +++++++ src/main.c | 2 + 10 files changed, 136 insertions(+), 1 deletion(-) create mode 100644 src/ServiceStartRtMode.c create mode 100644 src/ServiceStartRtMode.h diff --git a/src/CommunicationExecutor.c b/src/CommunicationExecutor.c index 25a358d5..7b63d22b 100644 --- a/src/CommunicationExecutor.c +++ b/src/CommunicationExecutor.c @@ -381,6 +381,11 @@ void Ros_Communication_StartExecutors(SEM_ID semCommunicationExecutorStatus) g_messages_QueueTrajPoint.response, Ros_ServiceQueueTrajPoint_Trigger); motoRos_RCLAssertOK_withMsg(rc, SUBCODE_FAIL_ADD_SERVICE_QUEUE_POINT, "Failed adding service (%d)", (int)rc); + rc = rclc_executor_add_service( + &executor_motion_control, &g_serviceStartRtMode, &g_messages_StartRtMode.request, + &g_messages_StartRtMode.response, Ros_ServiceStartRtMode_Trigger); + motoRos_RCLAssertOK_withMsg(rc, SUBCODE_FAIL_ADD_SERVICE_START_RT_MODE, "Failed adding service (%d)", (int)rc); + rc = rclc_executor_add_service( &executor_motion_control, &g_serviceSelectMotionTool, &g_messages_SelectMotionTool.request, &g_messages_SelectMotionTool.response, Ros_ServiceSelectMotionTool_Trigger); diff --git a/src/CommunicationExecutor.h b/src/CommunicationExecutor.h index f4112fa0..bb05b763 100644 --- a/src/CommunicationExecutor.h +++ b/src/CommunicationExecutor.h @@ -18,10 +18,11 @@ // service reset 1 // service start_traj_mode 1 // service start_point_queue_mode 1 +// service start_rt_mode 1 // service stop_traj_mode 1 // service queue_traj_point 1 // service select_tool 1 -#define QUANTITY_OF_HANDLES_FOR_MOTION_EXECUTOR (9) +#define QUANTITY_OF_HANDLES_FOR_MOTION_EXECUTOR (10) // total number of handles = // timers + 1 diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h index e36a44f2..ac1d93c7 100644 --- a/src/ErrorHandling.h +++ b/src/ErrorHandling.h @@ -176,6 +176,8 @@ typedef enum SUBCODE_FAIL_INVALID_BASE_TRACK_MOTION_TYPE, SUBCODE_DEBUG_INIT_FAIL_MP_NICDATA, SUBCODE_CONFIGURATION_FILE_YAML_PARSING_ERROR, + SUBCODE_FAIL_INIT_SERVICE_START_RT_MODE, + SUBCODE_FAIL_ADD_SERVICE_START_RT_MODE, } ALARM_ASSERTION_FAIL_SUBCODE; //8011 diff --git a/src/MotoROS.h b/src/MotoROS.h index 6a4c8c61..46bffeab 100644 --- a/src/MotoROS.h +++ b/src/MotoROS.h @@ -100,6 +100,7 @@ #include "ServiceResetError.h" #include "ServiceStartTrajMode.h" #include "ServiceStartPointQueueMode.h" +#include "ServiceStartRtMode.h" #include "ServiceStopTrajMode.h" #include "ServiceSelectMotionTool.h" #include "MotionControl.h" diff --git a/src/MotoROS2_AllControllers.vcxproj b/src/MotoROS2_AllControllers.vcxproj index e16072b6..d17e4883 100644 --- a/src/MotoROS2_AllControllers.vcxproj +++ b/src/MotoROS2_AllControllers.vcxproj @@ -437,6 +437,7 @@ + @@ -472,6 +473,7 @@ + diff --git a/src/MotoROS2_AllControllers.vcxproj.filters b/src/MotoROS2_AllControllers.vcxproj.filters index ba01b99c..7fb86b6e 100644 --- a/src/MotoROS2_AllControllers.vcxproj.filters +++ b/src/MotoROS2_AllControllers.vcxproj.filters @@ -369,6 +369,9 @@ Source Files\Tests + + Source Files\Services + @@ -485,5 +488,8 @@ Header Files\Tests + + Header Files\Services + \ No newline at end of file diff --git a/src/RosApiNameConstants.h b/src/RosApiNameConstants.h index 4523d31a..3dd167e7 100644 --- a/src/RosApiNameConstants.h +++ b/src/RosApiNameConstants.h @@ -25,6 +25,7 @@ #define SERVICE_NAME_RESET_ERROR "reset_error" #define SERVICE_NAME_START_TRAJ_MODE "start_traj_mode" #define SERVICE_NAME_START_POINT_QUEUE_MODE "start_point_queue_mode" +#define SERVICE_NAME_START_RT_MODE "start_rt_mode" #define SERVICE_NAME_STOP_TRAJ_MODE "stop_traj_mode" #define SERVICE_NAME_QUEUE_TRAJ_POINT "queue_traj_point" #define SERVICE_NAME_SELECT_MOTION_TOOL "select_motion_tool" diff --git a/src/ServiceStartRtMode.c b/src/ServiceStartRtMode.c new file mode 100644 index 00000000..b91de351 --- /dev/null +++ b/src/ServiceStartRtMode.c @@ -0,0 +1,88 @@ +//ServiceStartRtMode.c + +// SPDX-FileCopyrightText: 2025, Yaskawa America, Inc. +// SPDX-FileCopyrightText: 2025, Delft University of Technology +// +// SPDX-License-Identifier: Apache-2.0 + +#include "MotoROS.h" + +rcl_service_t g_serviceStartRtMode; + +ServiceStartRtMode_Messages g_messages_StartRtMode; + +// shorten the typename a little, locally +typedef std_srvs__srv__Trigger_Response StartRtMode_Response; + +void Ros_ServiceStartRtMode_Initialize() +{ + MOTOROS2_MEM_TRACE_START(svc_start_rt_mode_init); + + rcl_ret_t ret = rclc_service_init_default(&g_serviceStartRtMode, &g_microRosNodeInfo.node, + ROSIDL_GET_SRV_TYPE_SUPPORT(std_srvs, srv, Trigger), + SERVICE_NAME_START_RT_MODE); + motoRos_RCLAssertOK_withMsg(ret, SUBCODE_FAIL_INIT_SERVICE_START_RT_MODE, "Failed to init service (%d)", (int)ret); + + rosidl_runtime_c__String__init(&g_messages_StartRtMode.response.message); + + MOTOROS2_MEM_TRACE_REPORT(svc_start_rt_mode_init); +} + +void Ros_ServiceStartRtMode_Cleanup() +{ + MOTOROS2_MEM_TRACE_START(svc_start_rt_mode_fini); + + rcl_ret_t ret; + + Ros_Debug_BroadcastMsg("Cleanup service " SERVICE_NAME_START_RT_MODE); + ret = rcl_service_fini(&g_serviceStartRtMode, &g_microRosNodeInfo.node); + if (ret != RCL_RET_OK) + Ros_Debug_BroadcastMsg( + "Failed cleaning up " SERVICE_NAME_START_RT_MODE " service: %d", ret); + rosidl_runtime_c__String__fini(&g_messages_StartRtMode.response.message); + + MOTOROS2_MEM_TRACE_REPORT(svc_start_rt_mode_fini); +} + +void Ros_ServiceStartRtMode_Trigger(const void* request_msg, void* response_msg) +{ + RCL_UNUSED(request_msg); + StartRtMode_Response* response = (StartRtMode_Response*)response_msg; + + // trust .. + //response->result_code.value = MOTION_READY; + response->success = TRUE; + rosidl_runtime_c__String__assign(&response->message, ""); + + Ros_Debug_BroadcastMsg("Creating new task: IncMoveTask"); + + g_Ros_Controller.tidIncMoveThread = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE, + (FUNCPTR)Ros_MotionControl_IncMoveLoopStart, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + + MotionNotReadyCode motion_result_code = Ros_MotionControl_StartMotionMode(MOTION_MODE_RT, &response->message); + if (motion_result_code != MOTION_READY) + { + // update response + //response->result_code.value = motion_result_code; + response->success = FALSE; + + //If it is a MOTION_NOT_READY_ERROR, then the string was already populated in the Ros_MotionControl_StartMotionMode function + if (motion_result_code != MOTION_NOT_READY_ERROR) + { + // map to human readable string + //rosidl_runtime_c__String__assign(&response->message, + // Ros_ErrorHandling_MotionNotReadyCode_ToString((MotionNotReadyCode)response->result_code.value)); + rosidl_runtime_c__String__assign(&response->message, "nope!"); + } + + //Ros_Debug_BroadcastMsg("%s: %s (%d)", __func__, + // response->message.data, response->result_code.value); + Ros_Debug_BroadcastMsg("%s: %s (%d)", __func__, + response->message.data, FALSE); + } + else + { + Ros_Debug_BroadcastMsg("%s: activated", __func__); + } +} diff --git a/src/ServiceStartRtMode.h b/src/ServiceStartRtMode.h new file mode 100644 index 00000000..3011da85 --- /dev/null +++ b/src/ServiceStartRtMode.h @@ -0,0 +1,27 @@ +//ServiceStartRtMode.h + +// SPDX-FileCopyrightText: 2025, Yaskawa America, Inc. +// SPDX-FileCopyrightText: 2025, Delft University of Technology +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef MOTOROS2_SERVICE_START_RT_MODE_H +#define MOTOROS2_SERVICE_START_RT_MODE_H + + +extern rcl_service_t g_serviceStartRtMode; + +typedef struct +{ + std_srvs__srv__Trigger_Request request; + std_srvs__srv__Trigger_Response response; +} ServiceStartRtMode_Messages; +extern ServiceStartRtMode_Messages g_messages_StartRtMode; + +extern void Ros_ServiceStartRtMode_Initialize(); +extern void Ros_ServiceStartRtMode_Cleanup(); + +extern void Ros_ServiceStartRtMode_Trigger(const void* request_msg, void* response_msg); + + +#endif // MOTOROS2_SERVICE_START_RT_MODE_H diff --git a/src/main.c b/src/main.c index 134da58b..ed84d72c 100644 --- a/src/main.c +++ b/src/main.c @@ -134,6 +134,7 @@ void RosInitTask() Ros_ServiceResetError_Initialize(); Ros_ServiceStartTrajMode_Initialize(); Ros_ServiceStartPointQueueMode_Initialize(); + Ros_ServiceStartRtMode_Initialize(); Ros_ServiceStopTrajMode_Initialize(); Ros_ServiceSelectMotionTool_Initialize(); @@ -211,6 +212,7 @@ void RosInitTask() Ros_ServiceStopTrajMode_Cleanup(); Ros_ServiceStartTrajMode_Cleanup(); Ros_ServiceStartPointQueueMode_Cleanup(); + Ros_ServiceStartRtMode_Cleanup(); Ros_ServiceResetError_Cleanup(); Ros_ServiceReadWriteIO_Cleanup(); Ros_ServiceQueueTrajPoint_Cleanup(); From 6a0f6ef0768c6cbf3216bd4bf16f4c3d9aad4bb0 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Tue, 15 Jul 2025 11:45:45 -0400 Subject: [PATCH 003/101] ignore libmicroros folders --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index 8fe86c1b..d88b6a31 100644 --- a/.gitignore +++ b/.gitignore @@ -364,3 +364,5 @@ libmicroros_dx200_foxy/ # M+ build output *.out +/libmicroros_fs100_humble +/libmicroros_yrc1000_iron From 7e07786f045d50f7f1ed7a335102bd1f21d4ab48 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Tue, 15 Jul 2025 11:48:23 -0400 Subject: [PATCH 004/101] Add motion mode `MOTION_MODE_RT` --- src/MotionControl.c | 10 +++------- src/MotionControl.h | 5 +++-- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/MotionControl.c b/src/MotionControl.c index 428bdfda..f5ee58f2 100644 --- a/src/MotionControl.c +++ b/src/MotionControl.c @@ -1608,14 +1608,10 @@ BOOL Ros_MotionControl_IsMotionMode_PointQueue() MOTION_MODE_POINTQUEUE); } -BOOL Ros_MotionControl_IsMotionMode_RawStreaming() +BOOL Ros_MotionControl_IsMotionMode_RealTime() { - return FALSE; - - //TODO - // - //return (Ros_MotionControl_ActiveMotionMode == - // STREAMING_RAW_INCREMENTS); + return (Ros_MotionControl_ActiveMotionMode == + MOTION_MODE_RT); } void Ros_MotionControl_ValidateMotionModeIsOk() diff --git a/src/MotionControl.h b/src/MotionControl.h index 14301ee3..e50b233c 100644 --- a/src/MotionControl.h +++ b/src/MotionControl.h @@ -19,7 +19,8 @@ typedef enum { MOTION_MODE_INACTIVE, MOTION_MODE_TRAJECTORY, - MOTION_MODE_POINTQUEUE + MOTION_MODE_POINTQUEUE, + MOTION_MODE_RT, } MOTION_MODE; extern Init_Trajectory_Status Ros_MotionControl_InitTrajectory(control_msgs__action__FollowJointTrajectory_SendGoal_Request* pending_ros_goal_request); @@ -38,7 +39,7 @@ extern void Ros_MotionControl_StopTrajMode(); extern BOOL Ros_MotionControl_IsMotionMode_Trajectory(); extern BOOL Ros_MotionControl_IsMotionMode_PointQueue(); -extern BOOL Ros_MotionControl_IsMotionMode_RawStreaming(); +extern BOOL Ros_MotionControl_IsMotionMode_RealTime(); extern void Ros_MotionControl_ValidateMotionModeIsOk(); From 4edb0c53cbd750b05d4ec2b7e8c324b2cd3a69b5 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Tue, 15 Jul 2025 14:58:01 -0400 Subject: [PATCH 005/101] Change `StartRtMode` to use new message type --- src/MotoROS.h | 2 ++ src/ServiceStartRtMode.c | 40 +++++++++++++++++----------------------- src/ServiceStartRtMode.h | 4 ++-- 3 files changed, 21 insertions(+), 25 deletions(-) diff --git a/src/MotoROS.h b/src/MotoROS.h index 46bffeab..cdd31601 100644 --- a/src/MotoROS.h +++ b/src/MotoROS.h @@ -71,9 +71,11 @@ #include #include #include +#include #include #include #include +#include #include #include #include diff --git a/src/ServiceStartRtMode.c b/src/ServiceStartRtMode.c index b91de351..544d2eaa 100644 --- a/src/ServiceStartRtMode.c +++ b/src/ServiceStartRtMode.c @@ -12,14 +12,15 @@ rcl_service_t g_serviceStartRtMode; ServiceStartRtMode_Messages g_messages_StartRtMode; // shorten the typename a little, locally -typedef std_srvs__srv__Trigger_Response StartRtMode_Response; +typedef motoros2_interfaces__srv__StartRtMode_Request StartRtMode_Request; +typedef motoros2_interfaces__srv__StartRtMode_Response StartRtMode_Response; void Ros_ServiceStartRtMode_Initialize() { MOTOROS2_MEM_TRACE_START(svc_start_rt_mode_init); rcl_ret_t ret = rclc_service_init_default(&g_serviceStartRtMode, &g_microRosNodeInfo.node, - ROSIDL_GET_SRV_TYPE_SUPPORT(std_srvs, srv, Trigger), + ROSIDL_GET_SRV_TYPE_SUPPORT(motoros2_interfaces, srv, StartRtMode), SERVICE_NAME_START_RT_MODE); motoRos_RCLAssertOK_withMsg(ret, SUBCODE_FAIL_INIT_SERVICE_START_RT_MODE, "Failed to init service (%d)", (int)ret); @@ -37,8 +38,7 @@ void Ros_ServiceStartRtMode_Cleanup() Ros_Debug_BroadcastMsg("Cleanup service " SERVICE_NAME_START_RT_MODE); ret = rcl_service_fini(&g_serviceStartRtMode, &g_microRosNodeInfo.node); if (ret != RCL_RET_OK) - Ros_Debug_BroadcastMsg( - "Failed cleaning up " SERVICE_NAME_START_RT_MODE " service: %d", ret); + Ros_Debug_BroadcastMsg("Failed cleaning up " SERVICE_NAME_START_RT_MODE " service: %d", ret); rosidl_runtime_c__String__fini(&g_messages_StartRtMode.response.message); MOTOROS2_MEM_TRACE_REPORT(svc_start_rt_mode_fini); @@ -46,40 +46,34 @@ void Ros_ServiceStartRtMode_Cleanup() void Ros_ServiceStartRtMode_Trigger(const void* request_msg, void* response_msg) { - RCL_UNUSED(request_msg); + StartRtMode_Request* request = (StartRtMode_Request*)request_msg; StartRtMode_Response* response = (StartRtMode_Response*)response_msg; - // trust .. - //response->result_code.value = MOTION_READY; - response->success = TRUE; + response->result_code.value = MOTION_READY; rosidl_runtime_c__String__assign(&response->message, ""); + response->period = g_Ros_Controller.interpolPeriod; - Ros_Debug_BroadcastMsg("Creating new task: IncMoveTask"); - g_Ros_Controller.tidIncMoveThread = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE, - (FUNCPTR)Ros_MotionControl_IncMoveLoopStart, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + //----------------------------------- + //TODO: Call implementation + //----------------------------------- - MotionNotReadyCode motion_result_code = Ros_MotionControl_StartMotionMode(MOTION_MODE_RT, &response->message); - if (motion_result_code != MOTION_READY) + + response->result_code.value = Ros_MotionControl_StartMotionMode(MOTION_MODE_RT, &response->message); + if (response->result_code.value != MOTION_READY) { // update response - //response->result_code.value = motion_result_code; - response->success = FALSE; //If it is a MOTION_NOT_READY_ERROR, then the string was already populated in the Ros_MotionControl_StartMotionMode function - if (motion_result_code != MOTION_NOT_READY_ERROR) + if (response->result_code.value != MOTION_NOT_READY_ERROR) { // map to human readable string - //rosidl_runtime_c__String__assign(&response->message, - // Ros_ErrorHandling_MotionNotReadyCode_ToString((MotionNotReadyCode)response->result_code.value)); - rosidl_runtime_c__String__assign(&response->message, "nope!"); + rosidl_runtime_c__String__assign(&response->message, + Ros_ErrorHandling_MotionNotReadyCode_ToString((MotionNotReadyCode)response->result_code.value)); } - //Ros_Debug_BroadcastMsg("%s: %s (%d)", __func__, - // response->message.data, response->result_code.value); Ros_Debug_BroadcastMsg("%s: %s (%d)", __func__, - response->message.data, FALSE); + response->message.data, response->result_code.value); } else { diff --git a/src/ServiceStartRtMode.h b/src/ServiceStartRtMode.h index 3011da85..fa58f630 100644 --- a/src/ServiceStartRtMode.h +++ b/src/ServiceStartRtMode.h @@ -13,8 +13,8 @@ extern rcl_service_t g_serviceStartRtMode; typedef struct { - std_srvs__srv__Trigger_Request request; - std_srvs__srv__Trigger_Response response; + motoros2_interfaces__srv__StartRtMode_Request request; + motoros2_interfaces__srv__StartRtMode_Response response; } ServiceStartRtMode_Messages; extern ServiceStartRtMode_Messages g_messages_StartRtMode; From 87a32912bdd29ba8c596c6f02cd784e1d7efdff5 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Tue, 15 Jul 2025 16:18:46 -0400 Subject: [PATCH 006/101] Manage interpolation task for each `MOTION_MODE` --- src/ActionServer_FJT.c | 2 +- src/ControllerStatusIO.c | 27 +++++------------------- src/MotionControl.c | 44 +++++++++++++++++++++++++++++++++++++++- src/MotionControl.h | 2 +- src/ServiceStartRtMode.c | 6 ------ 5 files changed, 50 insertions(+), 31 deletions(-) diff --git a/src/ActionServer_FJT.c b/src/ActionServer_FJT.c index f74de5f7..f9b08064 100644 --- a/src/ActionServer_FJT.c +++ b/src/ActionServer_FJT.c @@ -346,7 +346,7 @@ void Ros_ActionServer_FJT_ResetProgressTracker() //TODO: do multidof too } -//Called from TrajectoryMotionControl::Ros_MotionControl_IncMoveLoopStart +//Called from TrajectoryMotionControl::Ros_MotionControl_NonRtIncMoveLoopStart void Ros_ActionServer_FJT_UpdateProgressTracker(MP_EXPOS_DATA* incrementData) { if (fjt_active_goal_handle == NULL) diff --git a/src/ControllerStatusIO.c b/src/ControllerStatusIO.c index 757b8b40..c5604a68 100644 --- a/src/ControllerStatusIO.c +++ b/src/ControllerStatusIO.c @@ -171,26 +171,6 @@ BOOL Ros_Controller_Initialize() g_messages_RobotStatus.msgRobotStatus = industrial_msgs__msg__RobotStatus__create(); rosidl_runtime_c__int32__Sequence__init(&g_messages_RobotStatus.msgRobotStatus->error_codes, MAX_ALARM_COUNT + 1); - //================================== - // If not started, start the IncMoveTask (there should be only one instance of this thread) - if (g_Ros_Controller.tidIncMoveThread == INVALID_TASK) - { - Ros_Debug_BroadcastMsg("Creating new task: IncMoveTask"); - - g_Ros_Controller.tidIncMoveThread = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE, - (FUNCPTR)Ros_MotionControl_IncMoveLoopStart, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); - if (g_Ros_Controller.tidIncMoveThread == ERROR) - { - Ros_Debug_BroadcastMsg("Failed to create task for incremental-motion. Check robot parameters."); - g_Ros_Controller.tidIncMoveThread = INVALID_TASK; - Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE); - mpSetAlarm(ALARM_TASK_CREATE_FAIL, APPLICATION_NAME " FAILED TO CREATE TASK", SUBCODE_INCREMENTAL_MOTION); - - return FALSE; - } - } - //================================== // Check and report eco-mode settings ECO_MODE_INFO eco_mode_info; @@ -247,8 +227,11 @@ void Ros_Controller_Cleanup() } } - mpDeleteTask(g_Ros_Controller.tidIncMoveThread); - g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + if (g_Ros_Controller.tidIncMoveThread != INVALID_TASK) + { + mpDeleteTask(g_Ros_Controller.tidIncMoveThread); + g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + } Ros_Debug_BroadcastMsg("Cleanup publisher robot status"); ret = rcl_publisher_fini(&g_publishers_RobotStatus.robotStatus, &g_microRosNodeInfo.node); diff --git a/src/MotionControl.c b/src/MotionControl.c index f5ee58f2..a771c69a 100644 --- a/src/MotionControl.c +++ b/src/MotionControl.c @@ -669,7 +669,7 @@ UINT16 Ros_MotionControl_ProcessQueuedTrajectoryPoint(motoros2_interfaces__srv__ //------------------------------------------------------------------- // Task to move the robot at each interpolation increment //------------------------------------------------------------------- -void Ros_MotionControl_IncMoveLoopStart() //<-- IP_CLK priority task +void Ros_MotionControl_NonRtIncMoveLoopStart() //<-- IP_CLK priority task { MP_EXPOS_DATA moveData; @@ -1328,6 +1328,43 @@ static STATUS Ros_Controller_DisableEcoMode() return NG; } +BOOL StartInterpolationTask(MOTION_MODE mode) +{ + //================================== + // If not started, start the IncMoveTask (there should be only one instance of this thread) + if (g_Ros_Controller.tidIncMoveThread == INVALID_TASK) + { + Ros_Debug_BroadcastMsg("Creating new task: IncMoveTask"); + + if (mode == MOTION_MODE_TRAJECTORY || mode == MOTION_MODE_POINTQUEUE) + { + g_Ros_Controller.tidIncMoveThread = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE, + (FUNCPTR)Ros_MotionControl_NonRtIncMoveLoopStart, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + if (g_Ros_Controller.tidIncMoveThread == ERROR) + { + Ros_Debug_BroadcastMsg("Failed to create task for incremental-motion. Check robot parameters."); + g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE); + mpSetAlarm(ALARM_TASK_CREATE_FAIL, APPLICATION_NAME " FAILED TO CREATE TASK", SUBCODE_INCREMENTAL_MOTION); + + return FALSE; + } + } + else if (mode == MOTION_MODE_RT) + { + asdf; //Launch RT thread here + } + else + return FALSE; + + return TRUE; + } + + Ros_Debug_BroadcastMsg("ERROR - IncMoveTask is already allocated"); + return FALSE; +} + //----------------------------------------------------------------------- // Attempts to start playback of a job to put the controller in RosMotion mode // @@ -1512,6 +1549,8 @@ MotionNotReadyCode Ros_MotionControl_StartMotionMode(MOTION_MODE mode, rosidl_ru } } + StartInterpolationTask(mode); + // have to initialize the prevPulsePos that will be used when interpolating the traj for(grpNo = 0; grpNo < g_Ros_Controller.numGroup; ++grpNo) { @@ -1594,6 +1633,9 @@ void Ros_MotionControl_StopTrajMode() ioWriteData.ulAddr = g_Ros_Controller.ioStatusAddr[IO_ROBOTSTATUS_WAITING_ROS].ulAddr; ioWriteData.ulValue = 0; mpWriteIO(&ioWriteData, 1); + + mpDeleteTask(g_Ros_Controller.tidIncMoveThread); + g_Ros_Controller.tidIncMoveThread = INVALID_TASK; } BOOL Ros_MotionControl_IsMotionMode_Trajectory() diff --git a/src/MotionControl.h b/src/MotionControl.h index e50b233c..154772a0 100644 --- a/src/MotionControl.h +++ b/src/MotionControl.h @@ -24,7 +24,7 @@ typedef enum } MOTION_MODE; extern Init_Trajectory_Status Ros_MotionControl_InitTrajectory(control_msgs__action__FollowJointTrajectory_SendGoal_Request* pending_ros_goal_request); -extern void Ros_MotionControl_IncMoveLoopStart(); +extern void Ros_MotionControl_NonRtIncMoveLoopStart(); extern void Ros_MotionControl_AddToIncQueueProcess(CtrlGroup* ctrlGroup); extern UINT16 Ros_MotionControl_ProcessQueuedTrajectoryPoint(motoros2_interfaces__srv__QueueTrajPoint_Request* request); extern BOOL Ros_MotionControl_AddPulseIncPointToQ(CtrlGroup* ctrlGroup, Incremental_data const* dataToEnQ); diff --git a/src/ServiceStartRtMode.c b/src/ServiceStartRtMode.c index 544d2eaa..b0e1e07c 100644 --- a/src/ServiceStartRtMode.c +++ b/src/ServiceStartRtMode.c @@ -53,12 +53,6 @@ void Ros_ServiceStartRtMode_Trigger(const void* request_msg, void* response_msg) rosidl_runtime_c__String__assign(&response->message, ""); response->period = g_Ros_Controller.interpolPeriod; - - //----------------------------------- - //TODO: Call implementation - //----------------------------------- - - response->result_code.value = Ros_MotionControl_StartMotionMode(MOTION_MODE_RT, &response->message); if (response->result_code.value != MOTION_READY) { From 0f4a4efebac4895972c07f8a1bf7f2e845df54a7 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 16 Jul 2025 09:19:52 -0400 Subject: [PATCH 007/101] call library function --- src/MotionControl.c | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/src/MotionControl.c b/src/MotionControl.c index a771c69a..d83be432 100644 --- a/src/MotionControl.c +++ b/src/MotionControl.c @@ -1341,23 +1341,26 @@ BOOL StartInterpolationTask(MOTION_MODE mode) g_Ros_Controller.tidIncMoveThread = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE, (FUNCPTR)Ros_MotionControl_NonRtIncMoveLoopStart, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); - if (g_Ros_Controller.tidIncMoveThread == ERROR) - { - Ros_Debug_BroadcastMsg("Failed to create task for incremental-motion. Check robot parameters."); - g_Ros_Controller.tidIncMoveThread = INVALID_TASK; - Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE); - mpSetAlarm(ALARM_TASK_CREATE_FAIL, APPLICATION_NAME " FAILED TO CREATE TASK", SUBCODE_INCREMENTAL_MOTION); - - return FALSE; - } } else if (mode == MOTION_MODE_RT) { - asdf; //Launch RT thread here + g_Ros_Controller.tidIncMoveThread = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE, + (FUNCPTR)MotionControl_RtIncMoveLoopStart, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); } else return FALSE; + if (g_Ros_Controller.tidIncMoveThread == ERROR) + { + Ros_Debug_BroadcastMsg("Failed to create task for incremental-motion. Check robot parameters."); + g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE); + mpSetAlarm(ALARM_TASK_CREATE_FAIL, APPLICATION_NAME " FAILED TO CREATE TASK", SUBCODE_INCREMENTAL_MOTION); + + return FALSE; + } + return TRUE; } From f1ff1e0fd3961cdf2bd97ea77719582bfbcbe2f5 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 16 Jul 2025 11:39:59 -0400 Subject: [PATCH 008/101] Add RealTimeMotionControl.c to rpoject --- src/MotoROS2_AllControllers.vcxproj | 1 + src/MotoROS2_AllControllers.vcxproj.filters | 3 +++ src/RealTimeMotionControl.c | 19 +++++++++++++++++++ 3 files changed, 23 insertions(+) create mode 100644 src/RealTimeMotionControl.c diff --git a/src/MotoROS2_AllControllers.vcxproj b/src/MotoROS2_AllControllers.vcxproj index d17e4883..07ee8a55 100644 --- a/src/MotoROS2_AllControllers.vcxproj +++ b/src/MotoROS2_AllControllers.vcxproj @@ -434,6 +434,7 @@ + diff --git a/src/MotoROS2_AllControllers.vcxproj.filters b/src/MotoROS2_AllControllers.vcxproj.filters index 7fb86b6e..4eb760b9 100644 --- a/src/MotoROS2_AllControllers.vcxproj.filters +++ b/src/MotoROS2_AllControllers.vcxproj.filters @@ -372,6 +372,9 @@ Source Files\Services + + Source Files\Robot Controller + diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c new file mode 100644 index 00000000..55059bbe --- /dev/null +++ b/src/RealTimeMotionControl.c @@ -0,0 +1,19 @@ + +#include "MotoROS.h" + +void MotionControl_RtIncMoveLoopStart() +{ + MP_EXPOS_DATA moveData; + int i; + + bzero(&moveData, sizeof(moveData)); + + for (i = 0; i < g_Ros_Controller.numGroup; i++) + { + moveData.ctrl_grp |= (0x01 << i); + moveData.grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(g_Ros_Controller.ctrlGroups[i]); + + ctrlGrpData.sCtrlGrp = g_Ros_Controller.ctrlGroups[i]->groupId; + mpGetPulsePos(&ctrlGrpData, &prevPulsePosData[i]); + } +} From 6d70c33bf1ae5e0c7abab7ab34b005d49274a0e3 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Tue, 12 Aug 2025 08:36:58 -0400 Subject: [PATCH 009/101] RealTimeMotionControl header --- src/MotoROS.h | 1 + src/MotoROS2_AllControllers.vcxproj | 1 + src/MotoROS2_AllControllers.vcxproj.filters | 3 +++ src/RealTimeMotionControl.h | 13 +++++++++++++ 4 files changed, 18 insertions(+) create mode 100644 src/RealTimeMotionControl.h diff --git a/src/MotoROS.h b/src/MotoROS.h index cdd31601..ae48aa93 100644 --- a/src/MotoROS.h +++ b/src/MotoROS.h @@ -106,6 +106,7 @@ #include "ServiceStopTrajMode.h" #include "ServiceSelectMotionTool.h" #include "MotionControl.h" +#include "RealTimeMotionControl.h" #include "ConfigFile.h" #include "RosApiNameConstants.h" #include "TimeConversionUtils.h" diff --git a/src/MotoROS2_AllControllers.vcxproj b/src/MotoROS2_AllControllers.vcxproj index 07ee8a55..7fbd6120 100644 --- a/src/MotoROS2_AllControllers.vcxproj +++ b/src/MotoROS2_AllControllers.vcxproj @@ -469,6 +469,7 @@ + diff --git a/src/MotoROS2_AllControllers.vcxproj.filters b/src/MotoROS2_AllControllers.vcxproj.filters index 4eb760b9..1dabcab8 100644 --- a/src/MotoROS2_AllControllers.vcxproj.filters +++ b/src/MotoROS2_AllControllers.vcxproj.filters @@ -494,5 +494,8 @@ Header Files\Services + + Header Files\Robot Controller + \ No newline at end of file diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h new file mode 100644 index 00000000..f23daa1a --- /dev/null +++ b/src/RealTimeMotionControl.h @@ -0,0 +1,13 @@ +//RealTimeMotionControl.h + +// SPDX-FileCopyrightText: 2025, Yaskawa America, Inc. +// SPDX-FileCopyrightText: 2025, Delft University of Technology +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef MOTOROS2_REALTIME_MOTION_CONTROL_H +#define MOTOROS2_REALTIME_MOTION_CONTROL_H + +extern void MotionControl_RtIncMoveLoopStart(); + +#endif //MOTOROS2_REALTIME_MOTION_CONTROL_H From 932ced461c3fbd27410dd1c28317aaf7789bd333 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 14 Aug 2025 09:32:03 -0400 Subject: [PATCH 010/101] Make rt port number configurable --- src/ConfigFile.c | 9 +++++++++ src/ConfigFile.h | 5 +++++ 2 files changed, 14 insertions(+) diff --git a/src/ConfigFile.c b/src/ConfigFile.c index b8d09de0..410aaddc 100644 --- a/src/ConfigFile.c +++ b/src/ConfigFile.c @@ -123,6 +123,7 @@ Configuration_Item Ros_ConfigFile_Items[] = { "ignore_missing_calib_data", &g_nodeConfigSettings.ignore_missing_calib_data, Value_Bool }, { "debug_broadcast_enabled", &g_nodeConfigSettings.debug_broadcast_enabled, Value_Bool }, { "debug_broadcast_port", &g_nodeConfigSettings.debug_broadcast_port, Value_UserLanPort }, + { "rt_udp_port_number", g_nodeConfigSettings.rt_udp_port_number, Value_String }, }; void Ros_ConfigFile_SetAllDefaultValues() @@ -214,15 +215,22 @@ void Ros_ConfigFile_SetAllDefaultValues() //inform_job_name snprintf(g_nodeConfigSettings.inform_job_name, MAX_JOB_NAME_LEN, "%s", DEFAULT_INFORM_JOB_NAME); + //========= //allow_custom_inform g_nodeConfigSettings.allow_custom_inform_job = DEFAULT_ALLOW_CUSTOM_INFORM; + //========= //userlan monitoring g_nodeConfigSettings.userlan_monitor_enabled = DEFAULT_ULAN_MON_ENABLED; g_nodeConfigSettings.userlan_monitor_port = DEFAULT_ULAN_MON_LINK; + //========= //ignore_missing_calib_data g_nodeConfigSettings.ignore_missing_calib_data = DEFAULT_IGNORE_MISSING_CALIB; + + //========= + //rt_udp_port_number + sprintf(g_nodeConfigSettings.rt_udp_port_number, "%s", DEFAULT_RT_UDP_PORT_NUMBER); } void Ros_ConfigFile_CheckYamlEvent(yaml_event_t* event) @@ -743,6 +751,7 @@ void Ros_ConfigFile_PrintActiveConfiguration(Ros_Configuration_Settings const* c Ros_Debug_BroadcastMsg("Config: ignore_missing_calib_data = %d", config->ignore_missing_calib_data); Ros_Debug_BroadcastMsg("Config: debug_broadcast_enabled = %d", config->debug_broadcast_enabled); Ros_Debug_BroadcastMsg("Config: debug_broadcast_port = %d", config->debug_broadcast_port); + Ros_Debug_BroadcastMsg("Config: rt_udp_port_number = %s", config->rt_udp_port_number); } void Ros_ConfigFile_Parse() diff --git a/src/ConfigFile.h b/src/ConfigFile.h index 878d8f4d..0912adfc 100644 --- a/src/ConfigFile.h +++ b/src/ConfigFile.h @@ -109,6 +109,9 @@ typedef enum #else #define DEFAULT_ULAN_DEBUG_BROADCAST_PORT CFG_ROS_USER_LAN1 #endif + +#define DEFAULT_RT_UDP_PORT_NUMBER "8889" + typedef struct { //TODO(gavanderhoorn): add support for unsigned types @@ -154,6 +157,8 @@ typedef struct BOOL debug_broadcast_enabled; Ros_UserLan_Port_Setting debug_broadcast_port; + + char rt_udp_port_number[MAX_YAML_STRING_LEN]; } Ros_Configuration_Settings; extern Ros_Configuration_Settings g_nodeConfigSettings; From e8ccaf2548934c142b5d7ee9533b7700d81f9eb7 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 14 Aug 2025 09:32:44 -0400 Subject: [PATCH 011/101] Initial framework ready for testing --- src/ErrorHandling.h | 1 + src/MotionControl.c | 11 ++- src/MotionControl.h | 3 +- src/RealTimeMotionControl.c | 159 +++++++++++++++++++++++++++++++++++- src/RealTimeMotionControl.h | 29 ++++++- src/ServiceStartRtMode.c | 4 +- 6 files changed, 197 insertions(+), 10 deletions(-) diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h index ac1d93c7..0fae4369 100644 --- a/src/ErrorHandling.h +++ b/src/ErrorHandling.h @@ -231,6 +231,7 @@ typedef enum typedef enum { SUBCODE_OPERATION_SET_CYCLE, + SUBCODE_NOT_IMPLEMENTED, } ALARM_OPERATION_FAIL_SUBCODE; //8016 typedef enum diff --git a/src/MotionControl.c b/src/MotionControl.c index d83be432..ad30a22c 100644 --- a/src/MotionControl.c +++ b/src/MotionControl.c @@ -1342,11 +1342,11 @@ BOOL StartInterpolationTask(MOTION_MODE mode) (FUNCPTR)Ros_MotionControl_NonRtIncMoveLoopStart, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); } - else if (mode == MOTION_MODE_RT) + else if (mode == MOTION_MODE_RT_JOINT || mode == MOTION_MODE_RT_CARTESIAN) { g_Ros_Controller.tidIncMoveThread = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE, - (FUNCPTR)MotionControl_RtIncMoveLoopStart, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + (FUNCPTR)Ros_RtMotionControl_RtIncMoveLoopStart, + (int)mode, 0, 0, 0, 0, 0, 0, 0, 0, 0); } else return FALSE; @@ -1637,6 +1637,9 @@ void Ros_MotionControl_StopTrajMode() ioWriteData.ulValue = 0; mpWriteIO(&ioWriteData, 1); + if (Ros_MotionControl_IsMotionMode_RealTime()) + Ros_RtMotionControl_Cleanup(); + mpDeleteTask(g_Ros_Controller.tidIncMoveThread); g_Ros_Controller.tidIncMoveThread = INVALID_TASK; } @@ -1656,7 +1659,7 @@ BOOL Ros_MotionControl_IsMotionMode_PointQueue() BOOL Ros_MotionControl_IsMotionMode_RealTime() { return (Ros_MotionControl_ActiveMotionMode == - MOTION_MODE_RT); + (MOTION_MODE_RT_JOINT || MOTION_MODE_RT_CARTESIAN)); } void Ros_MotionControl_ValidateMotionModeIsOk() diff --git a/src/MotionControl.h b/src/MotionControl.h index 154772a0..e7dbebae 100644 --- a/src/MotionControl.h +++ b/src/MotionControl.h @@ -20,7 +20,8 @@ typedef enum MOTION_MODE_INACTIVE, MOTION_MODE_TRAJECTORY, MOTION_MODE_POINTQUEUE, - MOTION_MODE_RT, + MOTION_MODE_RT_JOINT, + MOTION_MODE_RT_CARTESIAN, } MOTION_MODE; extern Init_Trajectory_Status Ros_MotionControl_InitTrajectory(control_msgs__action__FollowJointTrajectory_SendGoal_Request* pending_ros_goal_request); diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 55059bbe..426f4f0b 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -1,10 +1,52 @@ +//RealTimeMotionControl.c + +// SPDX-FileCopyrightText: 2025, Yaskawa America, Inc. +// SPDX-FileCopyrightText: 2025, Delft University of Technology +// +// SPDX-License-Identifier: Apache-2.0 + +// Based loosely on @adv4ncr's modifications to MotoROS1 for real-time control through ROS2. +// https://github.com/adv4ncr/motoman_ROS2/blob/cdf63a592596ff711df842680a1e4c730fd547a7/controller_driver/RealTimeMotionServer.c #include "MotoROS.h" -void MotionControl_RtIncMoveLoopStart() +void Ros_RtMotionControl_JointSpace(); +void Ros_RtMotionControl_Cartesian(); +void Ros_RtMotionControl_Cleanup(); +bool Ros_RtMotionControl_OpenSocket(int* sockServer); +void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, RtReply* reply); + + +static int sockRtCommandListener = -1; + + +void Ros_RtMotionControl_RtIncMoveLoopStart(MOTION_MODE mode) +{ + if (mode == MOTION_MODE_RT_JOINT) + Ros_RtMotionControl_JointSpace(); + else + Ros_RtMotionControl_Cartesian(); +} + +void Ros_RtMotionControl_JointSpace() { MP_EXPOS_DATA moveData; - int i; + int i, groupNo, bytes_received; + + struct sockaddr_in client_addr; + int client_addr_len = sizeof(client_addr); + + MP_CTRL_GRP_SEND_DATA ctrlGrpData; + MP_PULSE_POS_RSP_DATA prevPulsePosData[MAX_CONTROLLABLE_GROUPS]; + long pulse_increments[MAX_PULSE_AXES]; + + int sockServer; + + RtPacket incomingCommand; + RtReply outgoingReply; + + UINT32 sequenceId = 0; + bzero(&moveData, sizeof(moveData)); @@ -12,8 +54,121 @@ void MotionControl_RtIncMoveLoopStart() { moveData.ctrl_grp |= (0x01 << i); moveData.grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(g_Ros_Controller.ctrlGroups[i]); + moveData.grp_pos_info[i].pos_tag.data[3] = MP_INC_PULSE_DTYPE; ctrlGrpData.sCtrlGrp = g_Ros_Controller.ctrlGroups[i]->groupId; mpGetPulsePos(&ctrlGrpData, &prevPulsePosData[i]); } + + if (!Ros_RtMotionControl_OpenSocket(&sockServer)) + mpDeleteSelf; + + //------------------------------------------------------------------------------------- + while (TRUE) + { + bzero(&incomingCommand, sizeof(incomingCommand)); + bytes_received = mpRecvFrom(sockServer, (char*)&incomingCommand, sizeof(RtPacket), 0, (struct sockaddr*)&client_addr, &client_addr_len); + + if (bytes_received > 0) + { + +#warning deal with rollover + if (incomingCommand.sequenceId < sequenceId) + continue; //drop this packet + + if ((sequenceId - incomingCommand.sequenceId) >= MAX_SEQUENCE_DIFFERENCE) + { + Ros_Debug_BroadcastMsg("ERROR: Missed too many command packets"); + break; //drop the connection + } + + // For each control group, convert radians to pulses and prepare moveData + for (groupNo = 0; groupNo < g_Ros_Controller.numGroup; groupNo += 1) + { + CtrlGroup* ctrlGroup = g_Ros_Controller.ctrlGroups[groupNo]; + + Ros_CtrlGroup_ConvertRosUnitsToMotoUnits(ctrlGroup, incomingCommand.delta_rad[groupNo], pulse_increments); + + // Copy pulse increments to moveData + for (i = 0; i < ctrlGroup->numAxes; i++) + { + moveData.grp_pos_info[groupNo].pos[i] = pulse_increments[i]; + } + } + + // Send increment to robot + mpExRcsIncrementMove(&moveData); + + sequenceId = incomingCommand.sequenceId; + + Ros_RtMotionControl_PopulateReplyMessage(sequenceId , &outgoingReply); + mpSendTo(sockServer, (char*)&outgoingReply, sizeof(RtReply), 0, (struct sockaddr*)&client_addr, client_addr_len); + } + else + break; + + // Wait for next interpolation cycle + mpClkAnnounce(MP_INTERPOLATION_CLK); + } + + mpClose(sockRtCommandListener); + sockRtCommandListener = -1; + + g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + mpDeleteSelf; +} + +void Ros_RtMotionControl_Cartesian() +{ + Ros_MotionControl_StopTrajMode(); + Ros_Debug_BroadcastMsg("ERROR: Cartesian interface not yet implemented"); + mpSetAlarm(ALARM_OPERATION_FAIL, "Cartesian not yet implemented", SUBCODE_NOT_IMPLEMENTED); + mpDeleteSelf; +} + +void Ros_RtMotionControl_Cleanup() +{ + if (sockRtCommandListener != -1) + { + mpClose(sockRtCommandListener); + sockRtCommandListener = -1; + } + + if (g_Ros_Controller.tidIncMoveThread != INVALID_TASK) + { + mpDeleteTask(g_Ros_Controller.tidIncMoveThread); + g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + } +} + +bool Ros_RtMotionControl_OpenSocket(int* sockServer) +{ + struct sockaddr_in server_addr; + + *sockServer = mpSocket(AF_INET, SOCK_DGRAM, 0); + if (*sockServer < 0) + { + Ros_Debug_BroadcastMsg("ERROR: Could not allocate socket for RT interface"); + return false; + } + + // Bind socket to port + memset(&server_addr, 0, sizeof(server_addr)); + server_addr.sin_family = AF_INET; + server_addr.sin_addr.s_addr = INADDR_ANY; + server_addr.sin_port = mpHtons(atoi(g_nodeConfigSettings.rt_udp_port_number)); + + if (mpBind(*sockServer, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) + { + Ros_Debug_BroadcastMsg("ERROR: Failed to bind UDP socket for real-time motion control"); + mpClose(*sockServer); + return false; + } + + return true; +} + +void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, RtReply* reply) +{ + reply->sequenceId = sequenceId; } diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index f23daa1a..0c21f45d 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -1,4 +1,4 @@ -//RealTimeMotionControl.h +// Corrected Code // SPDX-FileCopyrightText: 2025, Yaskawa America, Inc. // SPDX-FileCopyrightText: 2025, Delft University of Technology @@ -8,6 +8,31 @@ #ifndef MOTOROS2_REALTIME_MOTION_CONTROL_H #define MOTOROS2_REALTIME_MOTION_CONTROL_H -extern void MotionControl_RtIncMoveLoopStart(); +#define PACKED __attribute__ ((__packed__)) + +extern void Ros_RtMotionControl_RtIncMoveLoopStart(MOTION_MODE mode); +extern void Ros_RtMotionControl_Cleanup(); + +#define MAX_SEQUENCE_DIFFERENCE 50 //0.2 seconds + +struct RtPacket_ +{ + // Indentation is now done with regular spaces + UINT32 sequenceId; + double delta_rad[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM]; +} PACKED; +typedef struct RtPacket_ RtPacket; + + +struct RtReply_ +{ + // Indentation is now done with regular spaces + UINT32 sequenceId; //echo +} PACKED; +typedef struct RtReply_ RtReply; + + +// No trailing space character here +#undef PACKED #endif //MOTOROS2_REALTIME_MOTION_CONTROL_H diff --git a/src/ServiceStartRtMode.c b/src/ServiceStartRtMode.c index b0e1e07c..d3a9cfda 100644 --- a/src/ServiceStartRtMode.c +++ b/src/ServiceStartRtMode.c @@ -48,12 +48,14 @@ void Ros_ServiceStartRtMode_Trigger(const void* request_msg, void* response_msg) { StartRtMode_Request* request = (StartRtMode_Request*)request_msg; StartRtMode_Response* response = (StartRtMode_Response*)response_msg; + + MOTION_MODE mm = request->control_mode.value == motoros2_interfaces__msg__ControlModeEnum__CARTESIAN ? MOTION_MODE_RT_CARTESIAN : MOTION_MODE_RT_JOINT; response->result_code.value = MOTION_READY; rosidl_runtime_c__String__assign(&response->message, ""); response->period = g_Ros_Controller.interpolPeriod; - response->result_code.value = Ros_MotionControl_StartMotionMode(MOTION_MODE_RT, &response->message); + response->result_code.value = Ros_MotionControl_StartMotionMode(mm, &response->message); if (response->result_code.value != MOTION_READY) { // update response From 404e879dced2b1edcaabb3780ac821eca66733e0 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 14 Aug 2025 13:34:37 -0400 Subject: [PATCH 012/101] subtraction was swapped --- src/RealTimeMotionControl.c | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 426f4f0b..477d9067 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -72,13 +72,16 @@ void Ros_RtMotionControl_JointSpace() if (bytes_received > 0) { -#warning deal with rollover +#warning deal with rollover; if (incomingCommand.sequenceId < sequenceId) + { + Ros_Debug_BroadcastMsg("WARN: Received old command packet (seq: %d, new: %d)", sequenceId, incomingCommand.sequenceId); continue; //drop this packet + } - if ((sequenceId - incomingCommand.sequenceId) >= MAX_SEQUENCE_DIFFERENCE) + if ((incomingCommand.sequenceId - sequenceId) >= MAX_SEQUENCE_DIFFERENCE) { - Ros_Debug_BroadcastMsg("ERROR: Missed too many command packets"); + Ros_Debug_BroadcastMsg("ERROR: Missed too many command packets (seq: %d, new: %d)", sequenceId, incomingCommand.sequenceId); break; //drop the connection } @@ -97,7 +100,9 @@ void Ros_RtMotionControl_JointSpace() } // Send increment to robot - mpExRcsIncrementMove(&moveData); + int ret = mpExRcsIncrementMove(&moveData); + if (ret != OK) + Ros_Debug_BroadcastMsg("WARN: mpExRcsIncrementMove returned %d", ret); sequenceId = incomingCommand.sequenceId; @@ -105,7 +110,10 @@ void Ros_RtMotionControl_JointSpace() mpSendTo(sockServer, (char*)&outgoingReply, sizeof(RtReply), 0, (struct sockaddr*)&client_addr, client_addr_len); } else + { + Ros_Debug_BroadcastMsg("ERROR: recvFrom returned an error"); break; + } // Wait for next interpolation cycle mpClkAnnounce(MP_INTERPOLATION_CLK); @@ -114,6 +122,8 @@ void Ros_RtMotionControl_JointSpace() mpClose(sockRtCommandListener); sockRtCommandListener = -1; + Ros_Debug_BroadcastMsg("Ending Rt Session"); + g_Ros_Controller.tidIncMoveThread = INVALID_TASK; mpDeleteSelf; } From 67a48f34bdc7b8a97d5b535e733124f7698caad8 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 14 Aug 2025 15:22:58 -0400 Subject: [PATCH 013/101] Add timeout and clear motion mode --- src/ConfigFile.c | 6 +++ src/ConfigFile.h | 6 ++- src/RealTimeMotionControl.c | 87 +++++++++++++++++++++++-------------- 3 files changed, 65 insertions(+), 34 deletions(-) diff --git a/src/ConfigFile.c b/src/ConfigFile.c index 410aaddc..90fa2f84 100644 --- a/src/ConfigFile.c +++ b/src/ConfigFile.c @@ -124,6 +124,7 @@ Configuration_Item Ros_ConfigFile_Items[] = { "debug_broadcast_enabled", &g_nodeConfigSettings.debug_broadcast_enabled, Value_Bool }, { "debug_broadcast_port", &g_nodeConfigSettings.debug_broadcast_port, Value_UserLanPort }, { "rt_udp_port_number", g_nodeConfigSettings.rt_udp_port_number, Value_String }, + { "timeout_for_rt_msg", &g_nodeConfigSettings.timeout_for_rt_msg, Value_Int }, }; void Ros_ConfigFile_SetAllDefaultValues() @@ -231,6 +232,10 @@ void Ros_ConfigFile_SetAllDefaultValues() //========= //rt_udp_port_number sprintf(g_nodeConfigSettings.rt_udp_port_number, "%s", DEFAULT_RT_UDP_PORT_NUMBER); + + //========= + //timeout_for_rt_msg + g_nodeConfigSettings.timeout_for_rt_msg = DEFAULT_TIMEOUT_FOR_RT_MSG; } void Ros_ConfigFile_CheckYamlEvent(yaml_event_t* event) @@ -752,6 +757,7 @@ void Ros_ConfigFile_PrintActiveConfiguration(Ros_Configuration_Settings const* c Ros_Debug_BroadcastMsg("Config: debug_broadcast_enabled = %d", config->debug_broadcast_enabled); Ros_Debug_BroadcastMsg("Config: debug_broadcast_port = %d", config->debug_broadcast_port); Ros_Debug_BroadcastMsg("Config: rt_udp_port_number = %s", config->rt_udp_port_number); + Ros_Debug_BroadcastMsg("Config: timeout_for_rt_msg = %d", config->timeout_for_rt_msg); } void Ros_ConfigFile_Parse() diff --git a/src/ConfigFile.h b/src/ConfigFile.h index 0912adfc..bd0c2729 100644 --- a/src/ConfigFile.h +++ b/src/ConfigFile.h @@ -110,7 +110,9 @@ typedef enum #define DEFAULT_ULAN_DEBUG_BROADCAST_PORT CFG_ROS_USER_LAN1 #endif -#define DEFAULT_RT_UDP_PORT_NUMBER "8889" +#define DEFAULT_RT_UDP_PORT_NUMBER "8889" + +#define DEFAULT_TIMEOUT_FOR_RT_MSG 30 typedef struct { @@ -159,6 +161,8 @@ typedef struct Ros_UserLan_Port_Setting debug_broadcast_port; char rt_udp_port_number[MAX_YAML_STRING_LEN]; + + int timeout_for_rt_msg; } Ros_Configuration_Settings; extern Ros_Configuration_Settings g_nodeConfigSettings; diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 477d9067..5fe470b3 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -19,6 +19,7 @@ void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, RtReply* reply); static int sockRtCommandListener = -1; +extern MOTION_MODE Ros_MotionControl_ActiveMotionMode; void Ros_RtMotionControl_RtIncMoveLoopStart(MOTION_MODE mode) { @@ -47,6 +48,10 @@ void Ros_RtMotionControl_JointSpace() UINT32 sequenceId = 0; + struct fd_set fds; + struct timeval tv; + + //========================================================================================= bzero(&moveData, sizeof(moveData)); @@ -63,64 +68,80 @@ void Ros_RtMotionControl_JointSpace() if (!Ros_RtMotionControl_OpenSocket(&sockServer)) mpDeleteSelf; - //------------------------------------------------------------------------------------- + //========================================================================================= while (TRUE) { - bzero(&incomingCommand, sizeof(incomingCommand)); - bytes_received = mpRecvFrom(sockServer, (char*)&incomingCommand, sizeof(RtPacket), 0, (struct sockaddr*)&client_addr, &client_addr_len); - if (bytes_received > 0) - { + //------------------------------------------------------------------------------------- + FD_ZERO(&fds); + FD_SET(sockServer, &fds); -#warning deal with rollover; - if (incomingCommand.sequenceId < sequenceId) - { - Ros_Debug_BroadcastMsg("WARN: Received old command packet (seq: %d, new: %d)", sequenceId, incomingCommand.sequenceId); - continue; //drop this packet - } + tv.tv_usec = 0; + tv.tv_sec = g_nodeConfigSettings.timeout_for_rt_msg; - if ((incomingCommand.sequenceId - sequenceId) >= MAX_SEQUENCE_DIFFERENCE) - { - Ros_Debug_BroadcastMsg("ERROR: Missed too many command packets (seq: %d, new: %d)", sequenceId, incomingCommand.sequenceId); - break; //drop the connection - } + if (mpSelect(sockServer + 1, &fds, NULL, NULL, &tv) > 0) + { + bzero(&incomingCommand, sizeof(incomingCommand)); + bytes_received = mpRecvFrom(sockServer, (char*)&incomingCommand, sizeof(RtPacket), 0, (struct sockaddr*)&client_addr, &client_addr_len); - // For each control group, convert radians to pulses and prepare moveData - for (groupNo = 0; groupNo < g_Ros_Controller.numGroup; groupNo += 1) + if (bytes_received > 0) { - CtrlGroup* ctrlGroup = g_Ros_Controller.ctrlGroups[groupNo]; + #warning deal with rollover; + if (incomingCommand.sequenceId < sequenceId) + { + Ros_Debug_BroadcastMsg("WARN: Received old command packet (seq: %d, new: %d)", sequenceId, incomingCommand.sequenceId); + continue; //drop this packet + } - Ros_CtrlGroup_ConvertRosUnitsToMotoUnits(ctrlGroup, incomingCommand.delta_rad[groupNo], pulse_increments); + if ((incomingCommand.sequenceId - sequenceId) >= MAX_SEQUENCE_DIFFERENCE) + { + Ros_Debug_BroadcastMsg("ERROR: Missed too many command packets (seq: %d, new: %d)", sequenceId, incomingCommand.sequenceId); + break; //drop the connection + } - // Copy pulse increments to moveData - for (i = 0; i < ctrlGroup->numAxes; i++) + // For each control group, convert radians to pulses and prepare moveData + for (groupNo = 0; groupNo < g_Ros_Controller.numGroup; groupNo += 1) { - moveData.grp_pos_info[groupNo].pos[i] = pulse_increments[i]; + CtrlGroup* ctrlGroup = g_Ros_Controller.ctrlGroups[groupNo]; + + Ros_CtrlGroup_ConvertRosUnitsToMotoUnits(ctrlGroup, incomingCommand.delta_rad[groupNo], pulse_increments); + + // Copy pulse increments to moveData + for (i = 0; i < ctrlGroup->numAxes; i++) + { + moveData.grp_pos_info[groupNo].pos[i] = pulse_increments[i]; + } } + + // Send increment to robot + int ret = mpExRcsIncrementMove(&moveData); + if (ret != OK) + Ros_Debug_BroadcastMsg("WARN: mpExRcsIncrementMove returned %d", ret); + } + else + { + Ros_Debug_BroadcastMsg("ERROR: recvFrom returned an error"); + break; } - // Send increment to robot - int ret = mpExRcsIncrementMove(&moveData); - if (ret != OK) - Ros_Debug_BroadcastMsg("WARN: mpExRcsIncrementMove returned %d", ret); + // Wait for next interpolation cycle + mpClkAnnounce(MP_INTERPOLATION_CLK); + //send status back to the PC and notify it that I'm ready for another packet sequenceId = incomingCommand.sequenceId; - - Ros_RtMotionControl_PopulateReplyMessage(sequenceId , &outgoingReply); + Ros_RtMotionControl_PopulateReplyMessage(sequenceId, &outgoingReply); mpSendTo(sockServer, (char*)&outgoingReply, sizeof(RtReply), 0, (struct sockaddr*)&client_addr, client_addr_len); } else { - Ros_Debug_BroadcastMsg("ERROR: recvFrom returned an error"); + Ros_Debug_BroadcastMsg("No packets received for %d seconds", g_nodeConfigSettings.timeout_for_rt_msg); break; } - - // Wait for next interpolation cycle - mpClkAnnounce(MP_INTERPOLATION_CLK); } mpClose(sockRtCommandListener); sockRtCommandListener = -1; + Ros_MotionControl_ActiveMotionMode = MOTION_MODE_INACTIVE; Ros_Debug_BroadcastMsg("Ending Rt Session"); From c4d270ae9c1ae0ff89e251d5f0821d0a0b0e5d01 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 14 Aug 2025 15:31:15 -0400 Subject: [PATCH 014/101] Add RT fields to the yaml file --- config/motoros2_config.yaml | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/config/motoros2_config.yaml b/config/motoros2_config.yaml index d726861b..15f65f98 100644 --- a/config/motoros2_config.yaml +++ b/config/motoros2_config.yaml @@ -318,3 +318,20 @@ publisher_qos: # OPTIONS: USER_LAN1, USER_LAN2 # DEFAULT: (all available network ports) #debug_broadcast_port: USER_LAN1 + +#----------------------------------------------------------------------------- +# For the real time motion interface, which port should the UDP messages +# be transmitted on? +# +# DEFAULT: '8889' +#rt_udp_port_number: '8889' + +#----------------------------------------------------------------------------- +# Timeout for real time motion commands. If a command packet is not received +# within this number of seconds, the motion mode will be cancelled. +# +# Setting this to '-1' will never timeout. In that case, you must explicitly +# call '/stop_traj_mode' to stop the motion mode. +# +# DEFAULT: 30 +#timeout_for_rt_msg: 30 From be2d6a5f53b8488260f165c86a9a345068edd88c Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 14 Aug 2025 15:34:12 -0400 Subject: [PATCH 015/101] allow infinite timeout --- src/RealTimeMotionControl.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 5fe470b3..d3b31e50 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -50,6 +50,7 @@ void Ros_RtMotionControl_JointSpace() struct fd_set fds; struct timeval tv; + struct timeval* timeout; //========================================================================================= @@ -79,7 +80,12 @@ void Ros_RtMotionControl_JointSpace() tv.tv_usec = 0; tv.tv_sec = g_nodeConfigSettings.timeout_for_rt_msg; - if (mpSelect(sockServer + 1, &fds, NULL, NULL, &tv) > 0) + if (g_nodeConfigSettings.timeout_for_rt_msg != -1) + timeout = &tv; + else + timeout = NULL; + + if (mpSelect(sockServer + 1, &fds, NULL, NULL, timeout) > 0) { bzero(&incomingCommand, sizeof(incomingCommand)); bytes_received = mpRecvFrom(sockServer, (char*)&incomingCommand, sizeof(RtPacket), 0, (struct sockaddr*)&client_addr, &client_addr_len); From 7f0feb91e4a2a8b1a90518c6e8aaa36fd345363a Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 14 Aug 2025 16:56:46 -0400 Subject: [PATCH 016/101] Trying to make RT task able to be restarted --- src/MotionControl.c | 2 +- src/RealTimeMotionControl.c | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/MotionControl.c b/src/MotionControl.c index ad30a22c..89a44461 100644 --- a/src/MotionControl.c +++ b/src/MotionControl.c @@ -1395,7 +1395,7 @@ MotionNotReadyCode Ros_MotionControl_StartMotionMode(MOTION_MODE mode, rosidl_ru Ros_Controller_IoStatusUpdate(); // Check if already in the proper mode - if (Ros_Controller_IsMotionReady()) + if (Ros_Controller_IsMotionReady() && Ros_MotionControl_ActiveMotionMode != MOTION_MODE_INACTIVE) { Ros_Debug_BroadcastMsg("Already active"); return MOTION_READY; diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index d3b31e50..af6d94ee 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -69,11 +69,11 @@ void Ros_RtMotionControl_JointSpace() if (!Ros_RtMotionControl_OpenSocket(&sockServer)) mpDeleteSelf; + Ros_Debug_BroadcastMsg("Starting RT session"); + //========================================================================================= while (TRUE) { - - //------------------------------------------------------------------------------------- FD_ZERO(&fds); FD_SET(sockServer, &fds); @@ -181,6 +181,7 @@ void Ros_RtMotionControl_Cleanup() bool Ros_RtMotionControl_OpenSocket(int* sockServer) { struct sockaddr_in server_addr; + int optval = 1; *sockServer = mpSocket(AF_INET, SOCK_DGRAM, 0); if (*sockServer < 0) @@ -189,6 +190,8 @@ bool Ros_RtMotionControl_OpenSocket(int* sockServer) return false; } + Ros_setsockopt(*sockServer, SOL_SOCKET, SO_REUSEADDR, (char*)&optval, sizeof(optval)); + // Bind socket to port memset(&server_addr, 0, sizeof(server_addr)); server_addr.sin_family = AF_INET; From ff34234b2ed8c8bacb1ed707820cd89cf004f590 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 15 Aug 2025 11:02:23 -0400 Subject: [PATCH 017/101] Remove gemini comments --- src/RealTimeMotionControl.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 0c21f45d..f84ae988 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -17,7 +17,6 @@ extern void Ros_RtMotionControl_Cleanup(); struct RtPacket_ { - // Indentation is now done with regular spaces UINT32 sequenceId; double delta_rad[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM]; } PACKED; @@ -26,13 +25,10 @@ typedef struct RtPacket_ RtPacket; struct RtReply_ { - // Indentation is now done with regular spaces UINT32 sequenceId; //echo } PACKED; typedef struct RtReply_ RtReply; - -// No trailing space character here #undef PACKED #endif //MOTOROS2_REALTIME_MOTION_CONTROL_H From eb5ef134442bc77489809b7208ef6a6606dd52b1 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 15 Aug 2025 13:30:28 -0400 Subject: [PATCH 018/101] Wrong socket id was preventing restart after timeout --- src/RealTimeMotionControl.c | 36 ++++++++++++++++++++++++++++-------- 1 file changed, 28 insertions(+), 8 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index af6d94ee..1cb2dd7e 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -34,15 +34,15 @@ void Ros_RtMotionControl_JointSpace() MP_EXPOS_DATA moveData; int i, groupNo, bytes_received; + bool bFirstRecv = true; struct sockaddr_in client_addr; + struct sockaddr_in previous_client_addr; int client_addr_len = sizeof(client_addr); MP_CTRL_GRP_SEND_DATA ctrlGrpData; MP_PULSE_POS_RSP_DATA prevPulsePosData[MAX_CONTROLLABLE_GROUPS]; long pulse_increments[MAX_PULSE_AXES]; - int sockServer; - RtPacket incomingCommand; RtReply outgoingReply; @@ -66,7 +66,7 @@ void Ros_RtMotionControl_JointSpace() mpGetPulsePos(&ctrlGrpData, &prevPulsePosData[i]); } - if (!Ros_RtMotionControl_OpenSocket(&sockServer)) + if (!Ros_RtMotionControl_OpenSocket(&sockRtCommandListener)) mpDeleteSelf; Ros_Debug_BroadcastMsg("Starting RT session"); @@ -75,7 +75,7 @@ void Ros_RtMotionControl_JointSpace() while (TRUE) { FD_ZERO(&fds); - FD_SET(sockServer, &fds); + FD_SET(sockRtCommandListener, &fds); tv.tv_usec = 0; tv.tv_sec = g_nodeConfigSettings.timeout_for_rt_msg; @@ -85,15 +85,29 @@ void Ros_RtMotionControl_JointSpace() else timeout = NULL; - if (mpSelect(sockServer + 1, &fds, NULL, NULL, timeout) > 0) + if (mpSelect(sockRtCommandListener + 1, &fds, NULL, NULL, timeout) > 0) { bzero(&incomingCommand, sizeof(incomingCommand)); - bytes_received = mpRecvFrom(sockServer, (char*)&incomingCommand, sizeof(RtPacket), 0, (struct sockaddr*)&client_addr, &client_addr_len); + bytes_received = mpRecvFrom(sockRtCommandListener, (char*)&incomingCommand, sizeof(RtPacket), 0, (struct sockaddr*)&client_addr, &client_addr_len); + + if (bFirstRecv) + { + previous_client_addr = client_addr; //only allow a single commander + //flag is cleared down below + } + else + { + if (memcmp(&client_addr, &previous_client_addr, sizeof(struct sockaddr_in)) != 0) + { + Ros_Debug_BroadcastMsg("ERROR: Received command packets from multiple sources"); + break; //drop the connection + } + } if (bytes_received > 0) { #warning deal with rollover; - if (incomingCommand.sequenceId < sequenceId) + if (incomingCommand.sequenceId <= sequenceId && !bFirstRecv) { Ros_Debug_BroadcastMsg("WARN: Received old command packet (seq: %d, new: %d)", sequenceId, incomingCommand.sequenceId); continue; //drop this packet @@ -123,6 +137,8 @@ void Ros_RtMotionControl_JointSpace() int ret = mpExRcsIncrementMove(&moveData); if (ret != OK) Ros_Debug_BroadcastMsg("WARN: mpExRcsIncrementMove returned %d", ret); + + bFirstRecv = false; } else { @@ -136,7 +152,7 @@ void Ros_RtMotionControl_JointSpace() //send status back to the PC and notify it that I'm ready for another packet sequenceId = incomingCommand.sequenceId; Ros_RtMotionControl_PopulateReplyMessage(sequenceId, &outgoingReply); - mpSendTo(sockServer, (char*)&outgoingReply, sizeof(RtReply), 0, (struct sockaddr*)&client_addr, client_addr_len); + mpSendTo(sockRtCommandListener, (char*)&outgoingReply, sizeof(RtReply), 0, (struct sockaddr*)&client_addr, client_addr_len); } else { @@ -160,6 +176,10 @@ void Ros_RtMotionControl_Cartesian() Ros_MotionControl_StopTrajMode(); Ros_Debug_BroadcastMsg("ERROR: Cartesian interface not yet implemented"); mpSetAlarm(ALARM_OPERATION_FAIL, "Cartesian not yet implemented", SUBCODE_NOT_IMPLEMENTED); + + Ros_MotionControl_ActiveMotionMode = MOTION_MODE_INACTIVE; + g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + mpDeleteSelf; } From 508dafc206b8f10484d1b0eddc8360f88b54a356 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 15 Aug 2025 13:37:25 -0400 Subject: [PATCH 019/101] I didn't get to use this product name for YMConnect. This will suffice. --- src/MotionControl.c | 2 +- src/RealTimeMotionControl.c | 2 +- src/RealTimeMotionControl.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/MotionControl.c b/src/MotionControl.c index 89a44461..a64e6b7a 100644 --- a/src/MotionControl.c +++ b/src/MotionControl.c @@ -1345,7 +1345,7 @@ BOOL StartInterpolationTask(MOTION_MODE mode) else if (mode == MOTION_MODE_RT_JOINT || mode == MOTION_MODE_RT_CARTESIAN) { g_Ros_Controller.tidIncMoveThread = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE, - (FUNCPTR)Ros_RtMotionControl_RtIncMoveLoopStart, + (FUNCPTR)Ros_RtMotionControl_HyperRobotCommanderx5, (int)mode, 0, 0, 0, 0, 0, 0, 0, 0, 0); } else diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 1cb2dd7e..aa1a3f4b 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -21,7 +21,7 @@ static int sockRtCommandListener = -1; extern MOTION_MODE Ros_MotionControl_ActiveMotionMode; -void Ros_RtMotionControl_RtIncMoveLoopStart(MOTION_MODE mode) +void Ros_RtMotionControl_HyperRobotCommanderx5(MOTION_MODE mode) { if (mode == MOTION_MODE_RT_JOINT) Ros_RtMotionControl_JointSpace(); diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index f84ae988..49403f35 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -10,7 +10,7 @@ #define PACKED __attribute__ ((__packed__)) -extern void Ros_RtMotionControl_RtIncMoveLoopStart(MOTION_MODE mode); +extern void Ros_RtMotionControl_HyperRobotCommanderx5(MOTION_MODE mode); extern void Ros_RtMotionControl_Cleanup(); #define MAX_SEQUENCE_DIFFERENCE 50 //0.2 seconds From c1fb8e7395881160b79110dcfbb6003f1ebf1fc6 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 15 Aug 2025 15:33:49 -0400 Subject: [PATCH 020/101] Add fb position to reply packet --- src/CtrlGroup.h | 1 + src/RealTimeMotionControl.c | 13 ++++++++++++- src/RealTimeMotionControl.h | 10 +++++++++- 3 files changed, 22 insertions(+), 2 deletions(-) diff --git a/src/CtrlGroup.h b/src/CtrlGroup.h index 953b9e83..29a2d469 100644 --- a/src/CtrlGroup.h +++ b/src/CtrlGroup.h @@ -128,6 +128,7 @@ extern void Ros_CtrlGroup_ConvertToRosPos(CtrlGroup* ctrlGroup, long const pulse extern void Ros_CtrlGroup_ConvertToRosTorque(CtrlGroup* ctrlGroup, double const motoTorque[MAX_PULSE_AXES], double rosTorque[MAX_PULSE_AXES]); extern void Ros_CtrlGroup_ConvertToMotoPos_FromSequentialOrdering(CtrlGroup* ctrlGroup, double const radPos[MAX_PULSE_AXES], long pulsePos[MAX_PULSE_AXES]); extern void Ros_CtrlGroup_ConvertRosUnitsToMotoUnits(CtrlGroup* ctrlGroup, double const rosPos[MAX_PULSE_AXES], long motopulsePos[MAX_PULSE_AXES]); +extern void Ros_CtrlGroup_ConvertMotoUnitsToRosUnits(CtrlGroup* ctrlGroup, long const motopulsePos[MAX_PULSE_AXES], double rosPos[MAX_PULSE_AXES]); extern UCHAR Ros_CtrlGroup_GetAxisConfig(CtrlGroup* ctrlGroup); diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index aa1a3f4b..b91c2146 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -124,6 +124,7 @@ void Ros_RtMotionControl_JointSpace() { CtrlGroup* ctrlGroup = g_Ros_Controller.ctrlGroups[groupNo]; + //joints must be in moto-order Ros_CtrlGroup_ConvertRosUnitsToMotoUnits(ctrlGroup, incomingCommand.delta_rad[groupNo], pulse_increments); // Copy pulse increments to moveData @@ -230,5 +231,15 @@ bool Ros_RtMotionControl_OpenSocket(int* sockServer) void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, RtReply* reply) { - reply->sequenceId = sequenceId; + long pulsePos_moto[MAX_CONTROLLABLE_GROUPS][MAX_PULSE_AXES]; + + reply->sequenceEcho = sequenceId; + + for (int groupIndex = 0; groupIndex < g_Ros_Controller.numGroup; groupIndex += 1) + { + CtrlGroup* group = g_Ros_Controller.ctrlGroups[groupIndex]; + + Ros_CtrlGroup_GetFBPulsePos(group, pulsePos_moto[groupIndex]); + Ros_CtrlGroup_ConvertMotoUnitsToRosUnits(group, pulsePos_moto[groupIndex], reply->feedbackPosition[groupIndex]); + } } diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 49403f35..a95f091f 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -18,6 +18,11 @@ extern void Ros_RtMotionControl_Cleanup(); struct RtPacket_ { UINT32 sequenceId; + + //The order of the joints must be in the order of [S L U R B T E 8]. + //Please note that for seven axis robots, the 'E' joint is phyically + //mounted in the middle of the arm. But it must be sent at the end + //of the joint array. double delta_rad[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM]; } PACKED; typedef struct RtPacket_ RtPacket; @@ -25,7 +30,10 @@ typedef struct RtPacket_ RtPacket; struct RtReply_ { - UINT32 sequenceId; //echo + UINT32 sequenceEcho; + + double feedbackPosition[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM]; + } PACKED; typedef struct RtReply_ RtReply; From 00a0f71d017bd6780ce79b14581d8869c64c179d Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 22 Aug 2025 17:03:33 -0400 Subject: [PATCH 021/101] Add cartesian support --- src/RealTimeMotionControl.c | 156 ++++++++++++++++++++++++++---------- src/RealTimeMotionControl.h | 7 +- 2 files changed, 119 insertions(+), 44 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index b91c2146..11c9779c 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -10,8 +10,10 @@ #include "MotoROS.h" -void Ros_RtMotionControl_JointSpace(); -void Ros_RtMotionControl_Cartesian(); +void Ros_RtMotionControl_InitJointSpace(MP_EXPOS_DATA* moveData); +void Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData); +bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData); +bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData); void Ros_RtMotionControl_Cleanup(); bool Ros_RtMotionControl_OpenSocket(int* sockServer); void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, RtReply* reply); @@ -22,14 +24,6 @@ static int sockRtCommandListener = -1; extern MOTION_MODE Ros_MotionControl_ActiveMotionMode; void Ros_RtMotionControl_HyperRobotCommanderx5(MOTION_MODE mode) -{ - if (mode == MOTION_MODE_RT_JOINT) - Ros_RtMotionControl_JointSpace(); - else - Ros_RtMotionControl_Cartesian(); -} - -void Ros_RtMotionControl_JointSpace() { MP_EXPOS_DATA moveData; int i, groupNo, bytes_received; @@ -39,10 +33,6 @@ void Ros_RtMotionControl_JointSpace() struct sockaddr_in previous_client_addr; int client_addr_len = sizeof(client_addr); - MP_CTRL_GRP_SEND_DATA ctrlGrpData; - MP_PULSE_POS_RSP_DATA prevPulsePosData[MAX_CONTROLLABLE_GROUPS]; - long pulse_increments[MAX_PULSE_AXES]; - RtPacket incomingCommand; RtReply outgoingReply; @@ -54,17 +44,11 @@ void Ros_RtMotionControl_JointSpace() //========================================================================================= - bzero(&moveData, sizeof(moveData)); - - for (i = 0; i < g_Ros_Controller.numGroup; i++) - { - moveData.ctrl_grp |= (0x01 << i); - moveData.grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(g_Ros_Controller.ctrlGroups[i]); - moveData.grp_pos_info[i].pos_tag.data[3] = MP_INC_PULSE_DTYPE; + if (mode == MOTION_MODE_RT_JOINT) + Ros_RtMotionControl_InitJointSpace(&moveData); + else + Ros_RtMotionControl_InitCartesian(&moveData); - ctrlGrpData.sCtrlGrp = g_Ros_Controller.ctrlGroups[i]->groupId; - mpGetPulsePos(&ctrlGrpData, &prevPulsePosData[i]); - } if (!Ros_RtMotionControl_OpenSocket(&sockRtCommandListener)) mpDeleteSelf; @@ -119,19 +103,15 @@ void Ros_RtMotionControl_JointSpace() break; //drop the connection } - // For each control group, convert radians to pulses and prepare moveData - for (groupNo = 0; groupNo < g_Ros_Controller.numGroup; groupNo += 1) + if (mode == MOTION_MODE_RT_JOINT) { - CtrlGroup* ctrlGroup = g_Ros_Controller.ctrlGroups[groupNo]; - - //joints must be in moto-order - Ros_CtrlGroup_ConvertRosUnitsToMotoUnits(ctrlGroup, incomingCommand.delta_rad[groupNo], pulse_increments); - - // Copy pulse increments to moveData - for (i = 0; i < ctrlGroup->numAxes; i++) - { - moveData.grp_pos_info[groupNo].pos[i] = pulse_increments[i]; - } + if (!Ros_RtMotionControl_ParseJointSpace(&incomingCommand, &moveData)) + break; //drop the connection + } + else + { + if (!Ros_RtMotionControl_ParseCartesian(&incomingCommand, &moveData)) + break; //drop the connection } // Send increment to robot @@ -172,16 +152,106 @@ void Ros_RtMotionControl_JointSpace() mpDeleteSelf; } -void Ros_RtMotionControl_Cartesian() + +void Ros_RtMotionControl_InitJointSpace(MP_EXPOS_DATA* moveData) { - Ros_MotionControl_StopTrajMode(); - Ros_Debug_BroadcastMsg("ERROR: Cartesian interface not yet implemented"); - mpSetAlarm(ALARM_OPERATION_FAIL, "Cartesian not yet implemented", SUBCODE_NOT_IMPLEMENTED); + int i; - Ros_MotionControl_ActiveMotionMode = MOTION_MODE_INACTIVE; - g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + bzero(moveData, sizeof(MP_EXPOS_DATA)); - mpDeleteSelf; + for (i = 0; i < g_Ros_Controller.numGroup; i++) + { + moveData->ctrl_grp |= (0x01 << i); + moveData->grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(g_Ros_Controller.ctrlGroups[i]); + moveData->grp_pos_info[i].pos_tag.data[3] = MP_INC_PULSE_DTYPE; + } +} + +void Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) +{ + int i; + + bzero(moveData, sizeof(MP_EXPOS_DATA)); + +#warning how to specify multi group? ;;; + moveData->ctrl_grp = 1; //R1 independent operation + moveData->m_ctrl_grp = 0; + moveData->s_ctrl_grp = 0; + + for (i = 0; i < g_Ros_Controller.numGroup; i++) + { + moveData->grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(g_Ros_Controller.ctrlGroups[i]); + #warning how to specify tool ? ;;; + moveData->grp_pos_info[i].pos_tag.data[2] = 0; + moveData->grp_pos_info[i].pos_tag.data[3] = MP_INC_RF_DTYPE; + } +} + +bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData) +{ + int i, groupNo; + + long pulse_increments[MAX_PULSE_AXES]; + + // For each control group, convert radians to pulses and prepare moveData + for (groupNo = 0; groupNo < g_Ros_Controller.numGroup; groupNo += 1) + { + CtrlGroup* ctrlGroup = g_Ros_Controller.ctrlGroups[groupNo]; + + //joints must be in moto-order + Ros_CtrlGroup_ConvertRosUnitsToMotoUnits(ctrlGroup, incomingCommand->delta[groupNo], pulse_increments); + + // Copy pulse increments to moveData + for (i = 0; i < ctrlGroup->numAxes; i++) + { + moveData->grp_pos_info[groupNo].pos[i] = pulse_increments[i]; + + if (pulse_increments[i] > ctrlGroup->maxInc.maxIncrement[i]) + { + Ros_Debug_BroadcastMsg("ERROR: The increment for axis [%d] exceeds the maximum limit of [%d] pulse counts", pulse_increments[i], ctrlGroup->maxInc.maxIncrement[i]); + return false; + } + } + } + + return true; +} + +bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData) +{ + int i, groupNo; + + // For each control group, convert radians to pulses and prepare moveData + for (groupNo = 0; groupNo < g_Ros_Controller.numGroup; groupNo += 1) + { + CtrlGroup* ctrlGroup = g_Ros_Controller.ctrlGroups[groupNo]; + + + moveData->grp_pos_info[groupNo].pos[0] = incomingCommand->delta[groupNo][0] * 1000.0; + moveData->grp_pos_info[groupNo].pos[1] = incomingCommand->delta[groupNo][1] * 1000.0; + moveData->grp_pos_info[groupNo].pos[2] = incomingCommand->delta[groupNo][2] * 1000.0; + + moveData->grp_pos_info[groupNo].pos[3] = incomingCommand->delta[groupNo][3] * 10000.0; + moveData->grp_pos_info[groupNo].pos[4] = incomingCommand->delta[groupNo][4] * 10000.0; + moveData->grp_pos_info[groupNo].pos[5] = incomingCommand->delta[groupNo][5] * 10000.0; + + moveData->grp_pos_info[groupNo].pos[6] = incomingCommand->delta[groupNo][6] * 10000.0; + + moveData->grp_pos_info[groupNo].pos[7] = incomingCommand->delta[groupNo][7] * 1000.0; + + double vector = sqrt(pow(incomingCommand->delta[groupNo][0], 2) + //x^2 + pow(incomingCommand->delta[groupNo][1], 2) + //y^2 + pow(incomingCommand->delta[groupNo][2], 2)); //z^2 + if (vector > 6.0) //1500 mm/sec == 6 mm per 4 milliseconds + { + Ros_Debug_BroadcastMsg("ERROR: The increment for the TCP exceeds the maximum limit of 1500 mm/sec"); + return false; + } + + //Ros_Debug_BroadcastMsg("moveData = %d, incomingCommand = %.5f", moveData->grp_pos_info[groupNo].pos[0], incomingCommand->delta[groupNo][0] * 1000.0); + } + + return true; } void Ros_RtMotionControl_Cleanup() diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index a95f091f..df136e95 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -23,7 +23,12 @@ struct RtPacket_ //Please note that for seven axis robots, the 'E' joint is phyically //mounted in the middle of the arm. But it must be sent at the end //of the joint array. - double delta_rad[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM]; + // + //For joint-space, this will be radians of each joint. + // + //For cartesian, this will be millimeters and degrees of the TCP. + //Rotations are applied in the order of ZYX. + double delta[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM]; } PACKED; typedef struct RtPacket_ RtPacket; From 4870a167c5dcc3e9c95e228804107824543f7871 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 5 Sep 2025 16:04:04 -0400 Subject: [PATCH 022/101] Apply feedback from Jimmy - Change 8th cartesian axis to pulse - Use meters-rad instead of mm-deg - Remove `Ros_MotionControl_ActiveMotionMode` reference --- src/MotionControl.c | 2 +- src/RealTimeMotionControl.c | 22 +++++++++------------- src/RealTimeMotionControl.h | 5 +++-- 3 files changed, 13 insertions(+), 16 deletions(-) diff --git a/src/MotionControl.c b/src/MotionControl.c index a64e6b7a..30da520d 100644 --- a/src/MotionControl.c +++ b/src/MotionControl.c @@ -1345,7 +1345,7 @@ BOOL StartInterpolationTask(MOTION_MODE mode) else if (mode == MOTION_MODE_RT_JOINT || mode == MOTION_MODE_RT_CARTESIAN) { g_Ros_Controller.tidIncMoveThread = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE, - (FUNCPTR)Ros_RtMotionControl_HyperRobotCommanderx5, + (FUNCPTR)Ros_RtMotionControl_HyperRobotCommanderX5, (int)mode, 0, 0, 0, 0, 0, 0, 0, 0, 0); } else diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 11c9779c..f6cb5837 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -21,9 +21,7 @@ void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, RtReply* reply); static int sockRtCommandListener = -1; -extern MOTION_MODE Ros_MotionControl_ActiveMotionMode; - -void Ros_RtMotionControl_HyperRobotCommanderx5(MOTION_MODE mode) +void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) { MP_EXPOS_DATA moveData; int i, groupNo, bytes_received; @@ -144,7 +142,6 @@ void Ros_RtMotionControl_HyperRobotCommanderx5(MOTION_MODE mode) mpClose(sockRtCommandListener); sockRtCommandListener = -1; - Ros_MotionControl_ActiveMotionMode = MOTION_MODE_INACTIVE; Ros_Debug_BroadcastMsg("Ending Rt Session"); @@ -226,18 +223,17 @@ bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA { CtrlGroup* ctrlGroup = g_Ros_Controller.ctrlGroups[groupNo]; + moveData->grp_pos_info[groupNo].pos[0] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][0]); + moveData->grp_pos_info[groupNo].pos[1] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][1]); + moveData->grp_pos_info[groupNo].pos[2] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][2]); - moveData->grp_pos_info[groupNo].pos[0] = incomingCommand->delta[groupNo][0] * 1000.0; - moveData->grp_pos_info[groupNo].pos[1] = incomingCommand->delta[groupNo][1] * 1000.0; - moveData->grp_pos_info[groupNo].pos[2] = incomingCommand->delta[groupNo][2] * 1000.0; - - moveData->grp_pos_info[groupNo].pos[3] = incomingCommand->delta[groupNo][3] * 10000.0; - moveData->grp_pos_info[groupNo].pos[4] = incomingCommand->delta[groupNo][4] * 10000.0; - moveData->grp_pos_info[groupNo].pos[5] = incomingCommand->delta[groupNo][5] * 10000.0; + moveData->grp_pos_info[groupNo].pos[3] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][3]); + moveData->grp_pos_info[groupNo].pos[4] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][4]); + moveData->grp_pos_info[groupNo].pos[5] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][5]); - moveData->grp_pos_info[groupNo].pos[6] = incomingCommand->delta[groupNo][6] * 10000.0; + moveData->grp_pos_info[groupNo].pos[6] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][6]); - moveData->grp_pos_info[groupNo].pos[7] = incomingCommand->delta[groupNo][7] * 1000.0; + moveData->grp_pos_info[groupNo].pos[7] = incomingCommand->delta[groupNo][7]; //pulse or micron (no known manipulators use this axis) double vector = sqrt(pow(incomingCommand->delta[groupNo][0], 2) + //x^2 pow(incomingCommand->delta[groupNo][1], 2) + //y^2 diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index df136e95..03b64cd6 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -10,7 +10,7 @@ #define PACKED __attribute__ ((__packed__)) -extern void Ros_RtMotionControl_HyperRobotCommanderx5(MOTION_MODE mode); +extern void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode); extern void Ros_RtMotionControl_Cleanup(); #define MAX_SEQUENCE_DIFFERENCE 50 //0.2 seconds @@ -26,7 +26,8 @@ struct RtPacket_ // //For joint-space, this will be radians of each joint. // - //For cartesian, this will be millimeters and degrees of the TCP. + //For cartesian, this will be meters and radians of the TCP. + //The order of the joints must be in the order of [X Y Z Rx Ry Rz Re 8]. //Rotations are applied in the order of ZYX. double delta[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM]; } PACKED; From bf21d1b16151226257c3e603596179b897a5b040 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 19 Sep 2025 09:34:47 -0400 Subject: [PATCH 023/101] Allow milliseconds for timeout --- config/motoros2_config.yaml | 6 +++--- src/ConfigFile.h | 2 +- src/RealTimeMotionControl.c | 6 +++--- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/config/motoros2_config.yaml b/config/motoros2_config.yaml index 15f65f98..77c2ff99 100644 --- a/config/motoros2_config.yaml +++ b/config/motoros2_config.yaml @@ -328,10 +328,10 @@ publisher_qos: #----------------------------------------------------------------------------- # Timeout for real time motion commands. If a command packet is not received -# within this number of seconds, the motion mode will be cancelled. +# within this number of milliseconds, the motion mode will be cancelled. # # Setting this to '-1' will never timeout. In that case, you must explicitly # call '/stop_traj_mode' to stop the motion mode. # -# DEFAULT: 30 -#timeout_for_rt_msg: 30 +# DEFAULT: 30000 (30.000 seconds) +#timeout_for_rt_msg: 30000 diff --git a/src/ConfigFile.h b/src/ConfigFile.h index bd0c2729..02f0fefe 100644 --- a/src/ConfigFile.h +++ b/src/ConfigFile.h @@ -112,7 +112,7 @@ typedef enum #define DEFAULT_RT_UDP_PORT_NUMBER "8889" -#define DEFAULT_TIMEOUT_FOR_RT_MSG 30 +#define DEFAULT_TIMEOUT_FOR_RT_MSG 30000 typedef struct { diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index f6cb5837..257daf22 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -59,8 +59,8 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) FD_ZERO(&fds); FD_SET(sockRtCommandListener, &fds); - tv.tv_usec = 0; - tv.tv_sec = g_nodeConfigSettings.timeout_for_rt_msg; + tv.tv_usec = (g_nodeConfigSettings.timeout_for_rt_msg % 1000) * 1000; + tv.tv_sec = g_nodeConfigSettings.timeout_for_rt_msg / 1000; if (g_nodeConfigSettings.timeout_for_rt_msg != -1) timeout = &tv; @@ -135,7 +135,7 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) } else { - Ros_Debug_BroadcastMsg("No packets received for %d seconds", g_nodeConfigSettings.timeout_for_rt_msg); + Ros_Debug_BroadcastMsg("No packets received for %d milliseconds", g_nodeConfigSettings.timeout_for_rt_msg); break; } } From 7920aacca94b13acc03ea46856cf76a43a5e26d3 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 19 Sep 2025 09:52:29 -0400 Subject: [PATCH 024/101] Make MAX_SEQUENCE_DIFFERENCE configurable --- config/motoros2_config.yaml | 12 ++++++++++++ src/ConfigFile.c | 6 ++++++ src/ConfigFile.h | 3 +++ src/RealTimeMotionControl.c | 2 +- src/RealTimeMotionControl.h | 2 -- 5 files changed, 22 insertions(+), 3 deletions(-) diff --git a/config/motoros2_config.yaml b/config/motoros2_config.yaml index 77c2ff99..69fbab37 100644 --- a/config/motoros2_config.yaml +++ b/config/motoros2_config.yaml @@ -335,3 +335,15 @@ publisher_qos: # # DEFAULT: 30000 (30.000 seconds) #timeout_for_rt_msg: 30000 + +#----------------------------------------------------------------------------- +# When using the real time motion interface, each command packet must increment +# the sequence ID. If too many packets are lost during communication, then it +# should be assumed that the PC is not in sync with the robot. +# +# If the sequence ID of an incoming packet is different from the previous +# command by a value greater than this, then the connection will be dropped. +# The motion mode must be reactivated to be used again. +# +# DEFAULT: 10 +#max_sequence_diff_for_rt_msg: 10 diff --git a/src/ConfigFile.c b/src/ConfigFile.c index 90fa2f84..5017e84a 100644 --- a/src/ConfigFile.c +++ b/src/ConfigFile.c @@ -125,6 +125,7 @@ Configuration_Item Ros_ConfigFile_Items[] = { "debug_broadcast_port", &g_nodeConfigSettings.debug_broadcast_port, Value_UserLanPort }, { "rt_udp_port_number", g_nodeConfigSettings.rt_udp_port_number, Value_String }, { "timeout_for_rt_msg", &g_nodeConfigSettings.timeout_for_rt_msg, Value_Int }, + { "max_sequence_diff_for_rt_msg", &g_nodeConfigSettings.max_sequence_diff_for_rt_msg, Value_Int }, }; void Ros_ConfigFile_SetAllDefaultValues() @@ -236,6 +237,10 @@ void Ros_ConfigFile_SetAllDefaultValues() //========= //timeout_for_rt_msg g_nodeConfigSettings.timeout_for_rt_msg = DEFAULT_TIMEOUT_FOR_RT_MSG; + + //========= + //max_sequence_diff_for_rt_msg + g_nodeConfigSettings.max_sequence_diff_for_rt_msg = DEFAULT_MAX_SEQUENCE_DIFFERENCE; } void Ros_ConfigFile_CheckYamlEvent(yaml_event_t* event) @@ -758,6 +763,7 @@ void Ros_ConfigFile_PrintActiveConfiguration(Ros_Configuration_Settings const* c Ros_Debug_BroadcastMsg("Config: debug_broadcast_port = %d", config->debug_broadcast_port); Ros_Debug_BroadcastMsg("Config: rt_udp_port_number = %s", config->rt_udp_port_number); Ros_Debug_BroadcastMsg("Config: timeout_for_rt_msg = %d", config->timeout_for_rt_msg); + Ros_Debug_BroadcastMsg("Config: max_sequence_diff_for_rt_msg = %d", config->max_sequence_diff_for_rt_msg); } void Ros_ConfigFile_Parse() diff --git a/src/ConfigFile.h b/src/ConfigFile.h index 02f0fefe..87131c5b 100644 --- a/src/ConfigFile.h +++ b/src/ConfigFile.h @@ -114,6 +114,8 @@ typedef enum #define DEFAULT_TIMEOUT_FOR_RT_MSG 30000 +#define DEFAULT_MAX_SEQUENCE_DIFFERENCE 10 + typedef struct { //TODO(gavanderhoorn): add support for unsigned types @@ -163,6 +165,7 @@ typedef struct char rt_udp_port_number[MAX_YAML_STRING_LEN]; int timeout_for_rt_msg; + int max_sequence_diff_for_rt_msg; } Ros_Configuration_Settings; extern Ros_Configuration_Settings g_nodeConfigSettings; diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 257daf22..a100f609 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -95,7 +95,7 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) continue; //drop this packet } - if ((incomingCommand.sequenceId - sequenceId) >= MAX_SEQUENCE_DIFFERENCE) + if ((incomingCommand.sequenceId - sequenceId) > g_nodeConfigSettings.max_sequence_diff_for_rt_msg) { Ros_Debug_BroadcastMsg("ERROR: Missed too many command packets (seq: %d, new: %d)", sequenceId, incomingCommand.sequenceId); break; //drop the connection diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 03b64cd6..ac7124a8 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -13,8 +13,6 @@ extern void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode); extern void Ros_RtMotionControl_Cleanup(); -#define MAX_SEQUENCE_DIFFERENCE 50 //0.2 seconds - struct RtPacket_ { UINT32 sequenceId; From 349912039c44fe90374230db9f4c9620e987eabc Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 19 Sep 2025 09:58:52 -0400 Subject: [PATCH 025/101] User must call `stop_traj_mode` for consistency --- src/MotionControl.c | 6 +++--- src/RealTimeMotionControl.c | 6 ------ 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/src/MotionControl.c b/src/MotionControl.c index 30da520d..9f494ad5 100644 --- a/src/MotionControl.c +++ b/src/MotionControl.c @@ -1624,6 +1624,9 @@ MotionNotReadyCode Ros_MotionControl_StartMotionMode(MOTION_MODE mode, rosidl_ru void Ros_MotionControl_StopTrajMode() { + if (Ros_MotionControl_IsMotionMode_RealTime()) + Ros_RtMotionControl_Cleanup(); + Ros_MotionControl_AllGroupsInitComplete = FALSE; Ros_MotionControl_ActiveMotionMode = MOTION_MODE_INACTIVE; @@ -1637,9 +1640,6 @@ void Ros_MotionControl_StopTrajMode() ioWriteData.ulValue = 0; mpWriteIO(&ioWriteData, 1); - if (Ros_MotionControl_IsMotionMode_RealTime()) - Ros_RtMotionControl_Cleanup(); - mpDeleteTask(g_Ros_Controller.tidIncMoveThread); g_Ros_Controller.tidIncMoveThread = INVALID_TASK; } diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index a100f609..a58bab37 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -140,13 +140,7 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) } } - mpClose(sockRtCommandListener); - sockRtCommandListener = -1; - Ros_Debug_BroadcastMsg("Ending Rt Session"); - - g_Ros_Controller.tidIncMoveThread = INVALID_TASK; - mpDeleteSelf; } From fb6e89f8211d59226b2dc0e400b99ad31201b8fb Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 19 Sep 2025 10:01:57 -0400 Subject: [PATCH 026/101] Don't care about `Ros_setsockopt` return code --- src/RealTimeMotionControl.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index a58bab37..1f39db28 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -271,6 +271,9 @@ bool Ros_RtMotionControl_OpenSocket(int* sockServer) return false; } + //This should allow another connection on this port immediately after closing a + //previous connection. I don't really care if it fails or not. If it fails, then + //it is possible that the bind will fail. But maybe not... Ros_setsockopt(*sockServer, SOL_SOCKET, SO_REUSEADDR, (char*)&optval, sizeof(optval)); // Bind socket to port From 9af33bafaa351171c2f53f757cac775bb9d7e8c3 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 19 Sep 2025 10:08:45 -0400 Subject: [PATCH 027/101] Revert "ignore libmicroros folders" This reverts commit c7c46d59b678120030614bf14e4492f0a2322973. --- .gitignore | 2 -- 1 file changed, 2 deletions(-) diff --git a/.gitignore b/.gitignore index d88b6a31..8fe86c1b 100644 --- a/.gitignore +++ b/.gitignore @@ -364,5 +364,3 @@ libmicroros_dx200_foxy/ # M+ build output *.out -/libmicroros_fs100_humble -/libmicroros_yrc1000_iron From c241fdfe2263dab523e6e6795e63ced7d8ecb95f Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 19 Sep 2025 10:42:26 -0400 Subject: [PATCH 028/101] Remove `NOT_IMPLEMENTED` subcode --- src/ErrorHandling.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h index 0fae4369..ac1d93c7 100644 --- a/src/ErrorHandling.h +++ b/src/ErrorHandling.h @@ -231,7 +231,6 @@ typedef enum typedef enum { SUBCODE_OPERATION_SET_CYCLE, - SUBCODE_NOT_IMPLEMENTED, } ALARM_OPERATION_FAIL_SUBCODE; //8016 typedef enum From 4ab6157d9a584608f45cc99338a59b1928626729 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 19 Sep 2025 10:46:33 -0400 Subject: [PATCH 029/101] excess whitespace --- config/motoros2_config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/config/motoros2_config.yaml b/config/motoros2_config.yaml index 69fbab37..bfb4a351 100644 --- a/config/motoros2_config.yaml +++ b/config/motoros2_config.yaml @@ -341,7 +341,7 @@ publisher_qos: # the sequence ID. If too many packets are lost during communication, then it # should be assumed that the PC is not in sync with the robot. # -# If the sequence ID of an incoming packet is different from the previous +# If the sequence ID of an incoming packet is different from the previous # command by a value greater than this, then the connection will be dropped. # The motion mode must be reactivated to be used again. # From 14c147d458dab5910fff75840e07735033c6c94b Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 19 Sep 2025 11:24:42 -0400 Subject: [PATCH 030/101] Add enums to elaborate ordering --- src/RealTimeMotionControl.c | 22 ++++++++--------- src/RealTimeMotionControl.h | 48 +++++++++++++++++++++++++++++++++++-- 2 files changed, 57 insertions(+), 13 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 1f39db28..90658d10 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -217,21 +217,21 @@ bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA { CtrlGroup* ctrlGroup = g_Ros_Controller.ctrlGroups[groupNo]; - moveData->grp_pos_info[groupNo].pos[0] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][0]); - moveData->grp_pos_info[groupNo].pos[1] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][1]); - moveData->grp_pos_info[groupNo].pos[2] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][2]); + moveData->grp_pos_info[groupNo].pos[TCP_X] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][TCP_X]); + moveData->grp_pos_info[groupNo].pos[TCP_Y] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][TCP_Y]); + moveData->grp_pos_info[groupNo].pos[TCP_Z] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][TCP_Z]); - moveData->grp_pos_info[groupNo].pos[3] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][3]); - moveData->grp_pos_info[groupNo].pos[4] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][4]); - moveData->grp_pos_info[groupNo].pos[5] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][5]); + moveData->grp_pos_info[groupNo].pos[TCP_Rx] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][TCP_Rx]); + moveData->grp_pos_info[groupNo].pos[TCP_Ry] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][TCP_Ry]); + moveData->grp_pos_info[groupNo].pos[TCP_Rz] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][TCP_Rz]); - moveData->grp_pos_info[groupNo].pos[6] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][6]); + moveData->grp_pos_info[groupNo].pos[TCP_Re] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][TCP_Re]); - moveData->grp_pos_info[groupNo].pos[7] = incomingCommand->delta[groupNo][7]; //pulse or micron (no known manipulators use this axis) + moveData->grp_pos_info[groupNo].pos[TCP_8] = incomingCommand->delta[groupNo][TCP_8]; //pulse or micron (no known manipulators use this axis) - double vector = sqrt(pow(incomingCommand->delta[groupNo][0], 2) + //x^2 - pow(incomingCommand->delta[groupNo][1], 2) + //y^2 - pow(incomingCommand->delta[groupNo][2], 2)); //z^2 + double vector = sqrt(pow(incomingCommand->delta[groupNo][TCP_X], 2) + //x^2 + pow(incomingCommand->delta[groupNo][TCP_Y], 2) + //y^2 + pow(incomingCommand->delta[groupNo][TCP_Z], 2)); //z^2 if (vector > 6.0) //1500 mm/sec == 6 mm per 4 milliseconds { Ros_Debug_BroadcastMsg("ERROR: The increment for the TCP exceeds the maximum limit of 1500 mm/sec"); diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index ac7124a8..449d6aef 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -13,6 +13,50 @@ extern void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode); extern void Ros_RtMotionControl_Cleanup(); +typedef enum +{ + Group_1 = 0, + Group_2, + Group_3, + Group_4, + Group_5, + Group_6, + Group_7, + Group_8, + + MAX_GROUPS +} GroupIndeces; + +typedef enum +{ + Joint_S = 0, //radians + Joint_L, + Joint_U, + Joint_R, + Joint_B, + Joint_T, + Joint_E, + Joint_8, + + MAX_JOINTS +} JointIndeces; + +typedef enum +{ + TCP_X = 0, //meters + TCP_Y, + TCP_Z, + + TCP_Rx, //0.0001 degrees + TCP_Ry, + TCP_Rz, + TCP_Re, + + TCP_8, //pulse + + MAX_AXES //maxies +} CartesianIndeces; + struct RtPacket_ { UINT32 sequenceId; @@ -27,7 +71,7 @@ struct RtPacket_ //For cartesian, this will be meters and radians of the TCP. //The order of the joints must be in the order of [X Y Z Rx Ry Rz Re 8]. //Rotations are applied in the order of ZYX. - double delta[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM]; + double delta[MAX_GROUPS][MP_GRP_AXES_NUM]; } PACKED; typedef struct RtPacket_ RtPacket; @@ -36,7 +80,7 @@ struct RtReply_ { UINT32 sequenceEcho; - double feedbackPosition[MAX_CONTROLLABLE_GROUPS][MP_GRP_AXES_NUM]; + double feedbackPosition[MAX_GROUPS][MP_GRP_AXES_NUM]; } PACKED; typedef struct RtReply_ RtReply; From fa2add146f04b98ecc73133d63d9fdc185dca57c Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 19 Sep 2025 15:59:18 -0400 Subject: [PATCH 031/101] Add tool data --- src/RealTimeMotionControl.c | 13 +++++++------ src/RealTimeMotionControl.h | 7 +++++++ 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 90658d10..c3f0aecc 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -16,7 +16,7 @@ bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DAT bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData); void Ros_RtMotionControl_Cleanup(); bool Ros_RtMotionControl_OpenSocket(int* sockServer); -void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, RtReply* reply); +void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, int* tools, RtReply* reply); static int sockRtCommandListener = -1; @@ -130,7 +130,7 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) //send status back to the PC and notify it that I'm ready for another packet sequenceId = incomingCommand.sequenceId; - Ros_RtMotionControl_PopulateReplyMessage(sequenceId, &outgoingReply); + Ros_RtMotionControl_PopulateReplyMessage(sequenceId, incomingCommand.toolIndex, &outgoingReply); mpSendTo(sockRtCommandListener, (char*)&outgoingReply, sizeof(RtReply), 0, (struct sockaddr*)&client_addr, client_addr_len); } else @@ -171,9 +171,8 @@ void Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) for (i = 0; i < g_Ros_Controller.numGroup; i++) { + moveData->ctrl_grp |= (1 << i); moveData->grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(g_Ros_Controller.ctrlGroups[i]); - #warning how to specify tool ? ;;; - moveData->grp_pos_info[i].pos_tag.data[2] = 0; moveData->grp_pos_info[i].pos_tag.data[3] = MP_INC_RF_DTYPE; } } @@ -203,6 +202,8 @@ bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DAT return false; } } + + moveData->grp_pos_info[groupNo].pos_tag.data[2] = incomingCommand->toolIndex[groupNo]; } return true; @@ -215,7 +216,7 @@ bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA // For each control group, convert radians to pulses and prepare moveData for (groupNo = 0; groupNo < g_Ros_Controller.numGroup; groupNo += 1) { - CtrlGroup* ctrlGroup = g_Ros_Controller.ctrlGroups[groupNo]; + moveData->grp_pos_info[groupNo].pos_tag.data[2] = incomingCommand->toolIndex[groupNo]; moveData->grp_pos_info[groupNo].pos[TCP_X] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][TCP_X]); moveData->grp_pos_info[groupNo].pos[TCP_Y] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][TCP_Y]); @@ -292,7 +293,7 @@ bool Ros_RtMotionControl_OpenSocket(int* sockServer) return true; } -void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, RtReply* reply) +void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, int* tools, RtReply* reply) { long pulsePos_moto[MAX_CONTROLLABLE_GROUPS][MAX_PULSE_AXES]; diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 449d6aef..638cf457 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -72,6 +72,13 @@ struct RtPacket_ //The order of the joints must be in the order of [X Y Z Rx Ry Rz Re 8]. //Rotations are applied in the order of ZYX. double delta[MAX_GROUPS][MP_GRP_AXES_NUM]; + + //Set tool that will be used by motion API (ie: passed by us to mpExRcsIncrementMove(..)) + //NOTE: this will change the 'motion tool' ONLY for those increments which + // haven't yet been added to the increment queue. See also the ROS 2 + // 'select_tool' service definition file in motoros2_interfaces. + int toolIndex[MAX_GROUPS]; //TOOL 0 - 63 + } PACKED; typedef struct RtPacket_ RtPacket; From 62d9a8c53aaf68ad35fb9396711786e512ffb526 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 19 Sep 2025 15:59:50 -0400 Subject: [PATCH 032/101] Unused variables --- src/RealTimeMotionControl.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index c3f0aecc..b342b80e 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -24,7 +24,7 @@ static int sockRtCommandListener = -1; void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) { MP_EXPOS_DATA moveData; - int i, groupNo, bytes_received; + int bytes_received; bool bFirstRecv = true; struct sockaddr_in client_addr; @@ -211,7 +211,7 @@ bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DAT bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData) { - int i, groupNo; + int groupNo; // For each control group, convert radians to pulses and prepare moveData for (groupNo = 0; groupNo < g_Ros_Controller.numGroup; groupNo += 1) From 3c2a5f9796a13ef85686b5e5469a836f14e3e2a2 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 19 Sep 2025 16:00:34 -0400 Subject: [PATCH 033/101] Add cartesian and command positions to reply message --- src/RealTimeMotionControl.c | 69 +++++++++++++++++++++++++++++++++---- src/RealTimeMotionControl.h | 33 ++++++++++++++++-- 2 files changed, 94 insertions(+), 8 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index b342b80e..10853b24 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -164,11 +164,9 @@ void Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) bzero(moveData, sizeof(MP_EXPOS_DATA)); -#warning how to specify multi group? ;;; - moveData->ctrl_grp = 1; //R1 independent operation moveData->m_ctrl_grp = 0; moveData->s_ctrl_grp = 0; - + for (i = 0; i < g_Ros_Controller.numGroup; i++) { moveData->ctrl_grp |= (1 << i); @@ -295,7 +293,15 @@ bool Ros_RtMotionControl_OpenSocket(int* sockServer) void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, int* tools, RtReply* reply) { - long pulsePos_moto[MAX_CONTROLLABLE_GROUPS][MAX_PULSE_AXES]; + long pulsePos_moto[MAX_PULSE_AXES]; + long degrees[MP_GRP_AXES_NUM]; + BITSTRING figure; + MP_COORD coord; + MP_CTRL_GRP_SEND_DATA ctrlGroup; + MP_PULSE_POS_RSP_DATA cmdPulse; + + bzero(reply, sizeof(RtReply)); + bzero(degrees, sizeof(long) * MP_GRP_AXES_NUM); reply->sequenceEcho = sequenceId; @@ -303,7 +309,58 @@ void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, int* tools, RtRepl { CtrlGroup* group = g_Ros_Controller.ctrlGroups[groupIndex]; - Ros_CtrlGroup_GetFBPulsePos(group, pulsePos_moto[groupIndex]); - Ros_CtrlGroup_ConvertMotoUnitsToRosUnits(group, pulsePos_moto[groupIndex], reply->feedbackPosition[groupIndex]); + //================================================================================ + //FB pos + //================================================================================ + Ros_CtrlGroup_GetFBPulsePos(group, pulsePos_moto); + + //Angles + Ros_CtrlGroup_ConvertMotoUnitsToRosUnits(group, pulsePos_moto, reply->feedbackPositionJoints[groupIndex]); + + for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) + degrees[axis] = RAD_TO_DEG_0001(reply->feedbackPositionJoints[groupIndex][axis]); + + //Cart + mpConvAxesToCartPos(groupIndex, degrees, tools[groupIndex], &figure, &coord); + + reply->feedbackPositionCartesian[groupIndex][TCP_X] = MICROMETERS_TO_METERS(coord.x); + reply->feedbackPositionCartesian[groupIndex][TCP_Y] = MICROMETERS_TO_METERS(coord.y); + reply->feedbackPositionCartesian[groupIndex][TCP_Z] = MICROMETERS_TO_METERS(coord.z); + + reply->feedbackPositionCartesian[groupIndex][TCP_Rx] = DEG_0001_TO_RAD(coord.rx); + reply->feedbackPositionCartesian[groupIndex][TCP_Ry] = DEG_0001_TO_RAD(coord.ry); + reply->feedbackPositionCartesian[groupIndex][TCP_Rz] = DEG_0001_TO_RAD(coord.rz); + reply->feedbackPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); + + //================================================================================ + //CMD pos + //================================================================================ +#warning Should this be Ros_CtrlGroup_GetPulsePosCmd??? See https://github.com/Yaskawa-Global/motoros2/discussions/455 ; + ctrlGroup.sCtrlGrp = groupIndex; + mpGetPulsePos(&ctrlGroup, &cmdPulse); + + //rad + Ros_CtrlGroup_ConvertMotoUnitsToRosUnits(group, cmdPulse.lPos, reply->previousCommandPositionJoints[groupIndex]); + + //deg + for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) + degrees[axis] = RAD_TO_DEG_0001(reply->previousCommandPositionJoints[groupIndex][axis]); + + //Cart + mpConvAxesToCartPos(groupIndex, degrees, tools[groupIndex], &figure, &coord); + + reply->previousCommandPositionCartesian[groupIndex][TCP_X] = MICROMETERS_TO_METERS(coord.x); + reply->previousCommandPositionCartesian[groupIndex][TCP_Y] = MICROMETERS_TO_METERS(coord.y); + reply->previousCommandPositionCartesian[groupIndex][TCP_Z] = MICROMETERS_TO_METERS(coord.z); + + reply->previousCommandPositionCartesian[groupIndex][TCP_Rx] = DEG_0001_TO_RAD(coord.rx); + reply->previousCommandPositionCartesian[groupIndex][TCP_Ry] = DEG_0001_TO_RAD(coord.ry); + reply->previousCommandPositionCartesian[groupIndex][TCP_Rz] = DEG_0001_TO_RAD(coord.rz); + reply->previousCommandPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); + + //================================================================================ + //FSU speed limit + //================================================================================ +#warning todo } } diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 638cf457..5a8f7bf5 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -57,6 +57,11 @@ typedef enum MAX_AXES //maxies } CartesianIndeces; + +//########################################################################## +// !All data is little-endian! +//########################################################################## + struct RtPacket_ { UINT32 sequenceId; @@ -64,12 +69,13 @@ struct RtPacket_ //The order of the joints must be in the order of [S L U R B T E 8]. //Please note that for seven axis robots, the 'E' joint is phyically //mounted in the middle of the arm. But it must be sent at the end - //of the joint array. + //of the joint array. See JointIndeces enum. // //For joint-space, this will be radians of each joint. // //For cartesian, this will be meters and radians of the TCP. //The order of the joints must be in the order of [X Y Z Rx Ry Rz Re 8]. + //See CartesianIndeces enum. //Rotations are applied in the order of ZYX. double delta[MAX_GROUPS][MP_GRP_AXES_NUM]; @@ -83,12 +89,35 @@ struct RtPacket_ typedef struct RtPacket_ RtPacket; +//########################################################################## +// !All data is little-endian! +//########################################################################## + struct RtReply_ { UINT32 sequenceEcho; - double feedbackPosition[MAX_GROUPS][MP_GRP_AXES_NUM]; + //This is indicative of where the robot is physically located. + //Please note that this will trail behind the commanded position. + //The joint ordering will match that of the original command + //packet. See JointIndeces and CartesianIndeces enums. + double feedbackPositionJoints[MAX_GROUPS][MP_GRP_AXES_NUM]; + double feedbackPositionCartesian[MAX_GROUPS][MP_GRP_AXES_NUM]; + + //The command position is the target destination you are instructing + //the robot to reach. It's the calculated endpoint based on the sum + //of all position increments received from the user. + // + //This does NOT include the commanded delta from the most recent + //command packet. + // + //This is used to track if the robot's speed is being limited + //by the Functional Safety Unit (FSU). It can also be used to + //monitor the latency between command and feedback. + double previousCommandPositionJoints[MAX_GROUPS][MP_GRP_AXES_NUM]; + double previousCommandPositionCartesian[MAX_GROUPS][MP_GRP_AXES_NUM]; + bool fsuInterferenceDetected; } PACKED; typedef struct RtReply_ RtReply; From a6497319c1cef3e404ee324cfc200749e12d0593 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 26 Sep 2025 11:06:53 -0400 Subject: [PATCH 034/101] Clarify `Ros_CtrlGroup_GetPulsePosCmd` comment --- src/RealTimeMotionControl.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 10853b24..5478798c 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -335,7 +335,10 @@ void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, int* tools, RtRepl //================================================================================ //CMD pos //================================================================================ -#warning Should this be Ros_CtrlGroup_GetPulsePosCmd??? See https://github.com/Yaskawa-Global/motoros2/discussions/455 ; + //Should this be Ros_CtrlGroup_GetPulsePosCmd? + //Answer: No, it should not. That should only be used when converting incoming + // positional commands that contain an absolute position. + // See https://github.com/Yaskawa-Global/motoros2/discussions/455 ctrlGroup.sCtrlGrp = groupIndex; mpGetPulsePos(&ctrlGroup, &cmdPulse); From db6b9b1fc287f9c8fb9a3eb92ebc7c700ccd20fb Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 26 Sep 2025 12:35:44 -0400 Subject: [PATCH 035/101] FSU detection --- src/RealTimeMotionControl.c | 56 +++++++++++++++++++++++++++++++------ src/RealTimeMotionControl.h | 10 +++++++ 2 files changed, 58 insertions(+), 8 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 5478798c..b7a913c0 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -16,11 +16,13 @@ bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DAT bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData); void Ros_RtMotionControl_Cleanup(); bool Ros_RtMotionControl_OpenSocket(int* sockServer); -void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, int* tools, RtReply* reply); - +void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* command, RtReply* reply); static int sockRtCommandListener = -1; +static LONG prevRtIncrementAmount[MAX_GROUPS][MAX_AXES]; +static LONG toProcessRtIncrements[MAX_GROUPS][MAX_AXES]; + void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) { MP_EXPOS_DATA moveData; @@ -47,6 +49,7 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) else Ros_RtMotionControl_InitCartesian(&moveData); + bzero(prevRtIncrementAmount, MAX_GROUPS * MAX_AXES * sizeof(LONG)); if (!Ros_RtMotionControl_OpenSocket(&sockRtCommandListener)) mpDeleteSelf; @@ -130,8 +133,19 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) //send status back to the PC and notify it that I'm ready for another packet sequenceId = incomingCommand.sequenceId; - Ros_RtMotionControl_PopulateReplyMessage(sequenceId, incomingCommand.toolIndex, &outgoingReply); + Ros_RtMotionControl_PopulateReplyMessage(mode, &incomingCommand, &outgoingReply); mpSendTo(sockRtCommandListener, (char*)&outgoingReply, sizeof(RtReply), 0, (struct sockaddr*)&client_addr, client_addr_len); + + //track how big the increment SHOULD have been + //we'll compare next cycle to see what actually happened + bzero(toProcessRtIncrements, MAX_GROUPS * MAX_AXES * sizeof(LONG)); + for (int groupIndex = 0; groupIndex < g_Ros_Controller.numGroup; groupIndex += 1) + { + for (int axis = 0; axis < MAX_AXES; axis += 1) + { + toProcessRtIncrements[groupIndex][axis] = moveData.grp_pos_info[groupIndex].pos[axis]; + } + } } else { @@ -291,7 +305,7 @@ bool Ros_RtMotionControl_OpenSocket(int* sockServer) return true; } -void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, int* tools, RtReply* reply) +void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* command, RtReply* reply) { long pulsePos_moto[MAX_PULSE_AXES]; long degrees[MP_GRP_AXES_NUM]; @@ -303,7 +317,7 @@ void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, int* tools, RtRepl bzero(reply, sizeof(RtReply)); bzero(degrees, sizeof(long) * MP_GRP_AXES_NUM); - reply->sequenceEcho = sequenceId; + reply->sequenceEcho = command->sequenceId; for (int groupIndex = 0; groupIndex < g_Ros_Controller.numGroup; groupIndex += 1) { @@ -321,7 +335,7 @@ void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, int* tools, RtRepl degrees[axis] = RAD_TO_DEG_0001(reply->feedbackPositionJoints[groupIndex][axis]); //Cart - mpConvAxesToCartPos(groupIndex, degrees, tools[groupIndex], &figure, &coord); + mpConvAxesToCartPos(groupIndex, degrees, command->toolIndex[groupIndex], &figure, &coord); reply->feedbackPositionCartesian[groupIndex][TCP_X] = MICROMETERS_TO_METERS(coord.x); reply->feedbackPositionCartesian[groupIndex][TCP_Y] = MICROMETERS_TO_METERS(coord.y); @@ -350,7 +364,7 @@ void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, int* tools, RtRepl degrees[axis] = RAD_TO_DEG_0001(reply->previousCommandPositionJoints[groupIndex][axis]); //Cart - mpConvAxesToCartPos(groupIndex, degrees, tools[groupIndex], &figure, &coord); + mpConvAxesToCartPos(groupIndex, degrees, command->toolIndex[groupIndex], &figure, &coord); reply->previousCommandPositionCartesian[groupIndex][TCP_X] = MICROMETERS_TO_METERS(coord.x); reply->previousCommandPositionCartesian[groupIndex][TCP_Y] = MICROMETERS_TO_METERS(coord.y); @@ -364,6 +378,32 @@ void Ros_RtMotionControl_PopulateReplyMessage(int sequenceId, int* tools, RtRepl //================================================================================ //FSU speed limit //================================================================================ -#warning todo + + LONG* coordAsArray = (LONG*)&coord; + LONG processedIncrement[MAX_AXES]; + bzero(processedIncrement, sizeof(LONG) * MAX_AXES); + BOOL fsuDetected = FALSE; + + // Check if pulses/mm's are missing from last increment. + // Get the current controller command position and substract the previous command position + // and check if it matches the amount if increment sent last cycle + for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) + { + if (mode == MOTION_MODE_RT_JOINT) + { + processedIncrement[axis] = cmdPulse.lPos[axis] - prevRtIncrementAmount[groupIndex][axis]; + prevRtIncrementAmount[groupIndex][axis] = cmdPulse.lPos[axis]; + } + else if (mode == MOTION_MODE_RT_CARTESIAN) + { + processedIncrement[axis] = coordAsArray[axis] - prevRtIncrementAmount[groupIndex][axis]; + prevRtIncrementAmount[groupIndex][axis] = coordAsArray[axis]; + } + + toProcessRtIncrements[groupIndex][axis] -= processedIncrement[axis]; + if (toProcessRtIncrements[groupIndex][axis] > MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION) + fsuDetected = TRUE; + } + reply->fsuInterferenceDetected = fsuDetected; } } diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 5a8f7bf5..50129cf9 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -117,10 +117,20 @@ struct RtReply_ double previousCommandPositionJoints[MAX_GROUPS][MP_GRP_AXES_NUM]; double previousCommandPositionCartesian[MAX_GROUPS][MP_GRP_AXES_NUM]; + //If the FSU speed limit is enabled, it can truncate the commanded + //delta increments. This flag is an indicator that the *previous* + //command cycle was truncated. It does NOT indicate that this most + //recent command packet was truncated. bool fsuInterferenceDetected; } PACKED; typedef struct RtReply_ RtReply; +//When checking for interference from the FSU speed limit, there will +//likely be some small rounding errors. So, the deviation must exceed +//this amount before the system will report that the FSU has limited +//the incoming motion command. +#define MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION START_MAX_PULSE_DEVIATION + #undef PACKED #endif //MOTOROS2_REALTIME_MOTION_CONTROL_H From afc8470a2d401290b20729831ce8e309991b4125 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 26 Sep 2025 13:48:13 -0400 Subject: [PATCH 036/101] End session if API errors out (alarm or estop) --- src/RealTimeMotionControl.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index b7a913c0..e11b47ed 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -118,7 +118,10 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) // Send increment to robot int ret = mpExRcsIncrementMove(&moveData); if (ret != OK) + { Ros_Debug_BroadcastMsg("WARN: mpExRcsIncrementMove returned %d", ret); + break; //drop the connection + } bFirstRecv = false; } From b61eb1709dadfac9511b0a563034735244b66c55 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 26 Sep 2025 15:08:38 -0400 Subject: [PATCH 037/101] Socket must persist for multiple sessions to work --- src/RealTimeMotionControl.c | 31 ++++++++++++------------------- src/RealTimeMotionControl.h | 1 + src/main.c | 8 ++++++++ 3 files changed, 21 insertions(+), 19 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index e11b47ed..99361342 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -15,7 +15,6 @@ void Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData); bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData); bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData); void Ros_RtMotionControl_Cleanup(); -bool Ros_RtMotionControl_OpenSocket(int* sockServer); void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* command, RtReply* reply); static int sockRtCommandListener = -1; @@ -51,11 +50,11 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) bzero(prevRtIncrementAmount, MAX_GROUPS * MAX_AXES * sizeof(LONG)); - if (!Ros_RtMotionControl_OpenSocket(&sockRtCommandListener)) - mpDeleteSelf; - Ros_Debug_BroadcastMsg("Starting RT session"); + Ros_Debug_BroadcastMsg("Flushing stale packets from socket buffer..."); + mpIoctl(sockRtCommandListener, FIOFLUSH, 1); + //========================================================================================= while (TRUE) { @@ -262,46 +261,40 @@ bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA void Ros_RtMotionControl_Cleanup() { - if (sockRtCommandListener != -1) - { - mpClose(sockRtCommandListener); - sockRtCommandListener = -1; - } + //Do not close sockRtCommandListener. Allow it to persist + //indefinitely and be reused. if (g_Ros_Controller.tidIncMoveThread != INVALID_TASK) { mpDeleteTask(g_Ros_Controller.tidIncMoveThread); g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + Ros_Debug_BroadcastMsg("Deleting old R/T task"); } } -bool Ros_RtMotionControl_OpenSocket(int* sockServer) +bool Ros_RtMotionControl_OpenSocket() { struct sockaddr_in server_addr; int optval = 1; - *sockServer = mpSocket(AF_INET, SOCK_DGRAM, 0); - if (*sockServer < 0) + sockRtCommandListener = mpSocket(AF_INET, SOCK_DGRAM, 0); + if (sockRtCommandListener < 0) { Ros_Debug_BroadcastMsg("ERROR: Could not allocate socket for RT interface"); return false; } - //This should allow another connection on this port immediately after closing a - //previous connection. I don't really care if it fails or not. If it fails, then - //it is possible that the bind will fail. But maybe not... - Ros_setsockopt(*sockServer, SOL_SOCKET, SO_REUSEADDR, (char*)&optval, sizeof(optval)); - // Bind socket to port memset(&server_addr, 0, sizeof(server_addr)); server_addr.sin_family = AF_INET; server_addr.sin_addr.s_addr = INADDR_ANY; server_addr.sin_port = mpHtons(atoi(g_nodeConfigSettings.rt_udp_port_number)); - if (mpBind(*sockServer, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) + if (mpBind(sockRtCommandListener, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) { Ros_Debug_BroadcastMsg("ERROR: Failed to bind UDP socket for real-time motion control"); - mpClose(*sockServer); + mpClose(sockRtCommandListener); + sockRtCommandListener = -1; return false; } diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 50129cf9..5e96aec2 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -11,6 +11,7 @@ #define PACKED __attribute__ ((__packed__)) extern void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode); +extern bool Ros_RtMotionControl_OpenSocket(); extern void Ros_RtMotionControl_Cleanup(); typedef enum diff --git a/src/main.c b/src/main.c index ed84d72c..7c1eae50 100644 --- a/src/main.c +++ b/src/main.c @@ -105,6 +105,14 @@ void RosInitTask() Ros_Controller_SetIOState(IO_FEEDBACK_RESERVED_7, FALSE); Ros_Controller_SetIOState(IO_FEEDBACK_RESERVED_8, FALSE); + //================================== + //REUSEADDR isn't working (not officially supported anyway), so + //I need to keep the R/T socket open forever. But I also can't + //open it on a subtask which will be deleted as connections come + //and go. So, I'm opening on the one persistent task that never + //ends. + Ros_RtMotionControl_OpenSocket(); + //================================== FOREVER { From cd4d0e9175c840efdd1789c0786e4bef71262c42 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 26 Sep 2025 16:27:16 -0400 Subject: [PATCH 038/101] use absolute value --- src/RealTimeMotionControl.c | 43 ++++++++++++++++++++++--------------- 1 file changed, 26 insertions(+), 17 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 99361342..ba0b5f44 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -19,7 +19,7 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman static int sockRtCommandListener = -1; -static LONG prevRtIncrementAmount[MAX_GROUPS][MAX_AXES]; +static LONG prevRtCmdPosition[MAX_GROUPS][MAX_AXES]; static LONG toProcessRtIncrements[MAX_GROUPS][MAX_AXES]; void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) @@ -48,7 +48,7 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) else Ros_RtMotionControl_InitCartesian(&moveData); - bzero(prevRtIncrementAmount, MAX_GROUPS * MAX_AXES * sizeof(LONG)); + bzero(prevRtCmdPosition, MAX_GROUPS * MAX_AXES * sizeof(LONG)); Ros_Debug_BroadcastMsg("Starting RT session"); @@ -275,7 +275,6 @@ void Ros_RtMotionControl_Cleanup() bool Ros_RtMotionControl_OpenSocket() { struct sockaddr_in server_addr; - int optval = 1; sockRtCommandListener = mpSocket(AF_INET, SOCK_DGRAM, 0); if (sockRtCommandListener < 0) @@ -307,8 +306,10 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman long degrees[MP_GRP_AXES_NUM]; BITSTRING figure; MP_COORD coord; + LONG* coordAsArray = (LONG*)&coord; MP_CTRL_GRP_SEND_DATA ctrlGroup; MP_PULSE_POS_RSP_DATA cmdPulse; + BOOL fsuDetected = FALSE; bzero(reply, sizeof(RtReply)); bzero(degrees, sizeof(long) * MP_GRP_AXES_NUM); @@ -375,30 +376,38 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman //FSU speed limit //================================================================================ - LONG* coordAsArray = (LONG*)&coord; LONG processedIncrement[MAX_AXES]; bzero(processedIncrement, sizeof(LONG) * MAX_AXES); - BOOL fsuDetected = FALSE; // Check if pulses/mm's are missing from last increment. // Get the current controller command position and substract the previous command position // and check if it matches the amount if increment sent last cycle for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) { - if (mode == MOTION_MODE_RT_JOINT) + if (toProcessRtIncrements[groupIndex][axis] != 0) { - processedIncrement[axis] = cmdPulse.lPos[axis] - prevRtIncrementAmount[groupIndex][axis]; - prevRtIncrementAmount[groupIndex][axis] = cmdPulse.lPos[axis]; - } - else if (mode == MOTION_MODE_RT_CARTESIAN) - { - processedIncrement[axis] = coordAsArray[axis] - prevRtIncrementAmount[groupIndex][axis]; - prevRtIncrementAmount[groupIndex][axis] = coordAsArray[axis]; - } + if (mode == MOTION_MODE_RT_JOINT) + { + processedIncrement[axis] = cmdPulse.lPos[axis] - prevRtCmdPosition[groupIndex][axis]; + prevRtCmdPosition[groupIndex][axis] = cmdPulse.lPos[axis]; + } + else if (mode == MOTION_MODE_RT_CARTESIAN) + { + processedIncrement[axis] = coordAsArray[axis] - prevRtCmdPosition[groupIndex][axis]; + prevRtCmdPosition[groupIndex][axis] = coordAsArray[axis]; + } + + toProcessRtIncrements[groupIndex][axis] -= processedIncrement[axis]; + if (abs(toProcessRtIncrements[groupIndex][axis]) > MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION) + { + fsuDetected = TRUE; - toProcessRtIncrements[groupIndex][axis] -= processedIncrement[axis]; - if (toProcessRtIncrements[groupIndex][axis] > MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION) - fsuDetected = TRUE; + //Ros_Debug_BroadcastMsg("current CMD coordAsArray[%d] = %d", axis, coordAsArray[axis]); + //Ros_Debug_BroadcastMsg("toProcessRtIncrements[%d][%d] = %d", groupIndex, axis, toProcessRtIncrements[groupIndex][axis]); + //Ros_Debug_BroadcastMsg("processedIncrement = %d", processedIncrement[axis]); + //Ros_Debug_BroadcastMsg("---------"); + } + } } reply->fsuInterferenceDetected = fsuDetected; } From 2358575c179bd97ee935fb50ec98f4072b699e83 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 29 Sep 2025 11:57:53 -0400 Subject: [PATCH 039/101] Attempt (fail) to fix initialization problem. - Properly init the compared pos at startup - Use Cartesian API for command position --- src/RealTimeMotionControl.c | 108 ++++++++++++++++++++++++++---------- src/RealTimeMotionControl.h | 6 +- 2 files changed, 82 insertions(+), 32 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index ba0b5f44..9aa97396 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -16,11 +16,12 @@ bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DAT bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData); void Ros_RtMotionControl_Cleanup(); void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* command, RtReply* reply); +bool Ros_CheckForFsuInterference(MOTION_MODE mode, int* tools); static int sockRtCommandListener = -1; static LONG prevRtCmdPosition[MAX_GROUPS][MAX_AXES]; -static LONG toProcessRtIncrements[MAX_GROUPS][MAX_AXES]; +static LONG howMuchShouldIHaveMoved[MAX_GROUPS][MAX_AXES]; void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) { @@ -35,21 +36,23 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) RtPacket incomingCommand; RtReply outgoingReply; - UINT32 sequenceId = 0; + UINT32 previousSequenceId = 0; struct fd_set fds; struct timeval tv; struct timeval* timeout; + bool fsuLimitingDetected; + //========================================================================================= + bzero(prevRtCmdPosition, MAX_GROUPS * MAX_AXES * sizeof(LONG)); + if (mode == MOTION_MODE_RT_JOINT) Ros_RtMotionControl_InitJointSpace(&moveData); else Ros_RtMotionControl_InitCartesian(&moveData); - bzero(prevRtCmdPosition, MAX_GROUPS * MAX_AXES * sizeof(LONG)); - Ros_Debug_BroadcastMsg("Starting RT session"); Ros_Debug_BroadcastMsg("Flushing stale packets from socket buffer..."); @@ -91,15 +94,15 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) if (bytes_received > 0) { #warning deal with rollover; - if (incomingCommand.sequenceId <= sequenceId && !bFirstRecv) + if (incomingCommand.sequenceId <= previousSequenceId && !bFirstRecv) { - Ros_Debug_BroadcastMsg("WARN: Received old command packet (seq: %d, new: %d)", sequenceId, incomingCommand.sequenceId); + Ros_Debug_BroadcastMsg("WARN: Received old command packet (seq: %d, new: %d)", previousSequenceId, incomingCommand.sequenceId); continue; //drop this packet } - if ((incomingCommand.sequenceId - sequenceId) > g_nodeConfigSettings.max_sequence_diff_for_rt_msg) + if ((incomingCommand.sequenceId - previousSequenceId) > g_nodeConfigSettings.max_sequence_diff_for_rt_msg) { - Ros_Debug_BroadcastMsg("ERROR: Missed too many command packets (seq: %d, new: %d)", sequenceId, incomingCommand.sequenceId); + Ros_Debug_BroadcastMsg("ERROR: Missed too many command packets (seq: %d, new: %d)", previousSequenceId, incomingCommand.sequenceId); break; //drop the connection } @@ -114,6 +117,8 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) break; //drop the connection } + fsuLimitingDetected = Ros_CheckForFsuInterference(mode, incomingCommand.toolIndex); + // Send increment to robot int ret = mpExRcsIncrementMove(&moveData); if (ret != OK) @@ -134,18 +139,19 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) mpClkAnnounce(MP_INTERPOLATION_CLK); //send status back to the PC and notify it that I'm ready for another packet - sequenceId = incomingCommand.sequenceId; + previousSequenceId = incomingCommand.sequenceId; Ros_RtMotionControl_PopulateReplyMessage(mode, &incomingCommand, &outgoingReply); + outgoingReply.fsuInterferenceDetected = fsuLimitingDetected; mpSendTo(sockRtCommandListener, (char*)&outgoingReply, sizeof(RtReply), 0, (struct sockaddr*)&client_addr, client_addr_len); //track how big the increment SHOULD have been //we'll compare next cycle to see what actually happened - bzero(toProcessRtIncrements, MAX_GROUPS * MAX_AXES * sizeof(LONG)); + bzero(howMuchShouldIHaveMoved, MAX_GROUPS * MAX_AXES * sizeof(LONG)); for (int groupIndex = 0; groupIndex < g_Ros_Controller.numGroup; groupIndex += 1) { for (int axis = 0; axis < MAX_AXES; axis += 1) { - toProcessRtIncrements[groupIndex][axis] = moveData.grp_pos_info[groupIndex].pos[axis]; + howMuchShouldIHaveMoved[groupIndex][axis] = moveData.grp_pos_info[groupIndex].pos[axis]; } } } @@ -163,6 +169,8 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) void Ros_RtMotionControl_InitJointSpace(MP_EXPOS_DATA* moveData) { int i; + MP_CTRL_GRP_SEND_DATA ctrlGroup; + MP_PULSE_POS_RSP_DATA cmdPulse; bzero(moveData, sizeof(MP_EXPOS_DATA)); @@ -171,12 +179,20 @@ void Ros_RtMotionControl_InitJointSpace(MP_EXPOS_DATA* moveData) moveData->ctrl_grp |= (0x01 << i); moveData->grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(g_Ros_Controller.ctrlGroups[i]); moveData->grp_pos_info[i].pos_tag.data[3] = MP_INC_PULSE_DTYPE; + + + ctrlGroup.sCtrlGrp = i; + mpGetPulsePos(&ctrlGroup, &cmdPulse); + memcpy(prevRtCmdPosition[i], cmdPulse.lPos, sizeof(cmdPulse.lPos)); } } void Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) { int i; + MP_CARTPOS_EX_SEND_DATA cartSendData; + MP_CART_POS_RSP_DATA_EX cartRespData; + MP_GET_TOOL_NO_RSP_DATA getToolResp; bzero(moveData, sizeof(MP_EXPOS_DATA)); @@ -188,6 +204,14 @@ void Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) moveData->ctrl_grp |= (1 << i); moveData->grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(g_Ros_Controller.ctrlGroups[i]); moveData->grp_pos_info[i].pos_tag.data[3] = MP_INC_RF_DTYPE; + + mpGetToolNo(MP_R1_GID, &getToolResp); + + cartSendData.sRobotNo = i; + cartSendData.sFrame = 1; //1 = RF + cartSendData.sToolNo = getToolResp.sToolNo; + mpGetCartPosEx(&cartSendData, &cartRespData); + memcpy(prevRtCmdPosition[i], cartRespData.lPos, sizeof(LONG) * MAX_AXES); } } @@ -306,10 +330,8 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman long degrees[MP_GRP_AXES_NUM]; BITSTRING figure; MP_COORD coord; - LONG* coordAsArray = (LONG*)&coord; MP_CTRL_GRP_SEND_DATA ctrlGroup; MP_PULSE_POS_RSP_DATA cmdPulse; - BOOL fsuDetected = FALSE; bzero(reply, sizeof(RtReply)); bzero(degrees, sizeof(long) * MP_GRP_AXES_NUM); @@ -371,44 +393,74 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman reply->previousCommandPositionCartesian[groupIndex][TCP_Ry] = DEG_0001_TO_RAD(coord.ry); reply->previousCommandPositionCartesian[groupIndex][TCP_Rz] = DEG_0001_TO_RAD(coord.rz); reply->previousCommandPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); + } +} +bool Ros_CheckForFsuInterference(MOTION_MODE mode, int* tools) +{ + MP_CTRL_GRP_SEND_DATA ctrlGroup; + MP_PULSE_POS_RSP_DATA cmdPulse; + MP_CARTPOS_EX_SEND_DATA cartSendData; + MP_CART_POS_RSP_DATA_EX cartRespData; + + for (int groupIndex = 0; groupIndex < g_Ros_Controller.numGroup; groupIndex += 1) + { //================================================================================ //FSU speed limit //================================================================================ - LONG processedIncrement[MAX_AXES]; - bzero(processedIncrement, sizeof(LONG) * MAX_AXES); + LONG difference; + LONG howMuchDidIActuallyMove[MAX_AXES]; + bzero(howMuchDidIActuallyMove, sizeof(LONG) * MAX_AXES); + + if (mode == MOTION_MODE_RT_JOINT) + { + //Should this be Ros_CtrlGroup_GetPulsePosCmd? + //Answer: No, it should not. That should only be used when converting incoming + // positional commands that contain an absolute position. + // See https://github.com/Yaskawa-Global/motoros2/discussions/455 + ctrlGroup.sCtrlGrp = groupIndex; + mpGetPulsePos(&ctrlGroup, &cmdPulse); + } + else if (mode == MOTION_MODE_RT_CARTESIAN) + { + cartSendData.sRobotNo = groupIndex; + cartSendData.sFrame = 1; //1 = RF + cartSendData.sToolNo = tools[groupIndex]; + mpGetCartPosEx(&cartSendData, &cartRespData); + } - // Check if pulses/mm's are missing from last increment. + // Check if pulses (or mm's) are missing from last increment. // Get the current controller command position and substract the previous command position // and check if it matches the amount if increment sent last cycle for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) { - if (toProcessRtIncrements[groupIndex][axis] != 0) + if (howMuchShouldIHaveMoved[groupIndex][axis] != 0) { if (mode == MOTION_MODE_RT_JOINT) { - processedIncrement[axis] = cmdPulse.lPos[axis] - prevRtCmdPosition[groupIndex][axis]; + howMuchDidIActuallyMove[axis] = cmdPulse.lPos[axis] - prevRtCmdPosition[groupIndex][axis]; prevRtCmdPosition[groupIndex][axis] = cmdPulse.lPos[axis]; } else if (mode == MOTION_MODE_RT_CARTESIAN) { - processedIncrement[axis] = coordAsArray[axis] - prevRtCmdPosition[groupIndex][axis]; - prevRtCmdPosition[groupIndex][axis] = coordAsArray[axis]; + howMuchDidIActuallyMove[axis] = cartRespData.lPos[axis] - prevRtCmdPosition[groupIndex][axis]; + prevRtCmdPosition[groupIndex][axis] = cartRespData.lPos[axis]; } - toProcessRtIncrements[groupIndex][axis] -= processedIncrement[axis]; - if (abs(toProcessRtIncrements[groupIndex][axis]) > MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION) + difference = howMuchShouldIHaveMoved[groupIndex][axis] - howMuchDidIActuallyMove[axis]; + if (abs(difference) > MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION) { - fsuDetected = TRUE; +#warning remove this; + Ros_Debug_BroadcastMsg("howMuchShouldIHaveMoved[%d][%d] = %d", groupIndex, axis, howMuchShouldIHaveMoved[groupIndex][axis]); + Ros_Debug_BroadcastMsg("howMuchDidIActuallyMove[%d] = %d", axis, howMuchDidIActuallyMove[axis]); + Ros_Debug_BroadcastMsg("difference = %d", difference); + Ros_Debug_BroadcastMsg("---------"); - //Ros_Debug_BroadcastMsg("current CMD coordAsArray[%d] = %d", axis, coordAsArray[axis]); - //Ros_Debug_BroadcastMsg("toProcessRtIncrements[%d][%d] = %d", groupIndex, axis, toProcessRtIncrements[groupIndex][axis]); - //Ros_Debug_BroadcastMsg("processedIncrement = %d", processedIncrement[axis]); - //Ros_Debug_BroadcastMsg("---------"); + return TRUE; } } } - reply->fsuInterferenceDetected = fsuDetected; } + return FALSE; } diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 5e96aec2..2542d4f8 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -65,6 +65,7 @@ typedef enum struct RtPacket_ { + //Must increment sequentially with each new command packet. UINT32 sequenceId; //The order of the joints must be in the order of [S L U R B T E 8]. @@ -109,9 +110,6 @@ struct RtReply_ //the robot to reach. It's the calculated endpoint based on the sum //of all position increments received from the user. // - //This does NOT include the commanded delta from the most recent - //command packet. - // //This is used to track if the robot's speed is being limited //by the Functional Safety Unit (FSU). It can also be used to //monitor the latency between command and feedback. @@ -130,7 +128,7 @@ typedef struct RtReply_ RtReply; //likely be some small rounding errors. So, the deviation must exceed //this amount before the system will report that the FSU has limited //the incoming motion command. -#define MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION START_MAX_PULSE_DEVIATION +#define MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION 50 //50 pulse, 0.05 millimeters, or 0.005 degrees #undef PACKED From e95e41293bcb7b0550ecd1fe46af084c19dc76f0 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 29 Sep 2025 11:59:01 -0400 Subject: [PATCH 040/101] `mpGetToolNo` was always getting R1 --- src/RealTimeMotionControl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 9aa97396..461564cd 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -205,7 +205,7 @@ void Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) moveData->grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(g_Ros_Controller.ctrlGroups[i]); moveData->grp_pos_info[i].pos_tag.data[3] = MP_INC_RF_DTYPE; - mpGetToolNo(MP_R1_GID, &getToolResp); + mpGetToolNo(MP_R1_GID + i, &getToolResp); cartSendData.sRobotNo = i; cartSendData.sFrame = 1; //1 = RF From 7b6d54c8fdcc7a17247a76f37afb3e7ffc48598a Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 29 Sep 2025 15:02:56 -0400 Subject: [PATCH 041/101] Purge stale packets at start of connection --- src/RealTimeMotionControl.c | 44 ++++++++++++++++++++++++++++++------- src/RealTimeMotionControl.h | 2 +- 2 files changed, 37 insertions(+), 9 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 461564cd..e553fe98 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -56,7 +56,27 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) Ros_Debug_BroadcastMsg("Starting RT session"); Ros_Debug_BroadcastMsg("Flushing stale packets from socket buffer..."); - mpIoctl(sockRtCommandListener, FIOFLUSH, 1); + //---------------------------- + //mpIoctl(sockRtCommandListener, FIOFLUSH, 1); + //UPDATE: mpIoctl isn't working! We'll manually purge the buffer with a draining loop. + //---------------------------- + while (TRUE) + { + FD_ZERO(&fds); + FD_SET(sockRtCommandListener, &fds); + + //no wait + tv.tv_usec = 0; + tv.tv_sec = 0; + + if (mpSelect(sockRtCommandListener + 1, &fds, NULL, NULL, &tv) > 0) + { + mpRecvFrom(sockRtCommandListener, (char*)&incomingCommand, sizeof(RtPacket), 0, (struct sockaddr*)&client_addr, &client_addr_len); + } + else + break; + } + //---------------------------- //========================================================================================= while (TRUE) @@ -84,9 +104,11 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) } else { - if (memcmp(&client_addr, &previous_client_addr, sizeof(struct sockaddr_in)) != 0) + if (memcmp(&client_addr.sin_addr.s_addr, &previous_client_addr.sin_addr.s_addr, sizeof(UINT32)) != 0) { - Ros_Debug_BroadcastMsg("ERROR: Received command packets from multiple sources"); + Ros_Debug_BroadcastMsg("ERROR: Received command packets from multiple sources (0x%08X and 0x%08X)", + (UINT32)previous_client_addr.sin_addr.s_addr, + (UINT32)client_addr.sin_addr.s_addr); break; //drop the connection } } @@ -97,6 +119,9 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) if (incomingCommand.sequenceId <= previousSequenceId && !bFirstRecv) { Ros_Debug_BroadcastMsg("WARN: Received old command packet (seq: %d, new: %d)", previousSequenceId, incomingCommand.sequenceId); + + //send a copy of the previous reply to trigger next packet + mpSendTo(sockRtCommandListener, (char*)&outgoingReply, sizeof(RtReply), 0, (struct sockaddr*)&client_addr, client_addr_len); continue; //drop this packet } @@ -205,6 +230,10 @@ void Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) moveData->grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(g_Ros_Controller.ctrlGroups[i]); moveData->grp_pos_info[i].pos_tag.data[3] = MP_INC_RF_DTYPE; + //NOTE: This isn't the best method for this. During testing, I had tool #2 selected + // on the pendant, but I was commanding increments on tool #0. Because of this, + // the first motion on each axis would trigger the FSU detection mechanism. But + // it immediately recovers after one cycle. mpGetToolNo(MP_R1_GID + i, &getToolResp); cartSendData.sRobotNo = i; @@ -451,11 +480,10 @@ bool Ros_CheckForFsuInterference(MOTION_MODE mode, int* tools) difference = howMuchShouldIHaveMoved[groupIndex][axis] - howMuchDidIActuallyMove[axis]; if (abs(difference) > MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION) { -#warning remove this; - Ros_Debug_BroadcastMsg("howMuchShouldIHaveMoved[%d][%d] = %d", groupIndex, axis, howMuchShouldIHaveMoved[groupIndex][axis]); - Ros_Debug_BroadcastMsg("howMuchDidIActuallyMove[%d] = %d", axis, howMuchDidIActuallyMove[axis]); - Ros_Debug_BroadcastMsg("difference = %d", difference); - Ros_Debug_BroadcastMsg("---------"); + //Ros_Debug_BroadcastMsg("howMuchShouldIHaveMoved[%d][%d] = %d", groupIndex, axis, howMuchShouldIHaveMoved[groupIndex][axis]); + //Ros_Debug_BroadcastMsg("howMuchDidIActuallyMove[%d] = %d", axis, howMuchDidIActuallyMove[axis]); + //Ros_Debug_BroadcastMsg("difference = %d", difference); + //Ros_Debug_BroadcastMsg("---------"); return TRUE; } diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 2542d4f8..3b68132e 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -128,7 +128,7 @@ typedef struct RtReply_ RtReply; //likely be some small rounding errors. So, the deviation must exceed //this amount before the system will report that the FSU has limited //the incoming motion command. -#define MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION 50 //50 pulse, 0.05 millimeters, or 0.005 degrees +#define MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION 50 //50 pulse, 0.050 millimeters, or 0.0050 degrees #undef PACKED From b35083b8d615ebd64ab1d58b8f38ffcdee8847b8 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 29 Sep 2025 16:00:02 -0400 Subject: [PATCH 042/101] Don't monitor speed limit for rotations --- src/RealTimeMotionControl.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index e553fe98..625955aa 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -473,6 +473,15 @@ bool Ros_CheckForFsuInterference(MOTION_MODE mode, int* tools) } else if (mode == MOTION_MODE_RT_CARTESIAN) { + //When working in cartesian space, we're only going to monitor the translation. + //1. There is no FSU speed limit for rotation. So it's moot. + //2. When rotating by some increment, that rotation gets 'spread out' over multiple + // axes. Even if I put all of my commanded increment into a single axis, all + // three of them are going to react. So, the cmd-value of my intended axis may + // not be the value I expect. + if (axis >= TCP_Rx) + break; + howMuchDidIActuallyMove[axis] = cartRespData.lPos[axis] - prevRtCmdPosition[groupIndex][axis]; prevRtCmdPosition[groupIndex][axis] = cartRespData.lPos[axis]; } From c0895e97c2f99511fd2540442e300e78bc9390a5 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 29 Sep 2025 16:13:46 -0400 Subject: [PATCH 043/101] Handle rollover of sequence id --- src/RealTimeMotionControl.c | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 625955aa..b446bd59 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -115,20 +115,23 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) if (bytes_received > 0) { - #warning deal with rollover; - if (incomingCommand.sequenceId <= previousSequenceId && !bFirstRecv) + //Check for old or same sequence ID (wraparound safe) + if ((int32_t)(incomingCommand.sequenceId - previousSequenceId) <= 0) { - Ros_Debug_BroadcastMsg("WARN: Received old command packet (seq: %d, new: %d)", previousSequenceId, incomingCommand.sequenceId); + // This packet is old or a duplicate. + Ros_Debug_BroadcastMsg("WARN: Received old command packet (prev: %u, new: %u)", previousSequenceId, incomingCommand.sequenceId); - //send a copy of the previous reply to trigger next packet + // Resend the previous reply and drop this packet. mpSendTo(sockRtCommandListener, (char*)&outgoingReply, sizeof(RtReply), 0, (struct sockaddr*)&client_addr, client_addr_len); - continue; //drop this packet + continue; } + //Check if the sequence ID jumped too far ahead (wraparound safe) if ((incomingCommand.sequenceId - previousSequenceId) > g_nodeConfigSettings.max_sequence_diff_for_rt_msg) { - Ros_Debug_BroadcastMsg("ERROR: Missed too many command packets (seq: %d, new: %d)", previousSequenceId, incomingCommand.sequenceId); - break; //drop the connection + Ros_Debug_BroadcastMsg("ERROR: Missed too many command packets (prev: %u, new: %u)", + previousSequenceId, incomingCommand.sequenceId); + break; // Drop the connection } if (mode == MOTION_MODE_RT_JOINT) From 6f75377b2b0d8d3eff5cdabe402e25292ebbeb5b Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Tue, 30 Sep 2025 12:03:53 -0400 Subject: [PATCH 044/101] Documentation --- CHANGELOG.md | 12 +- README.md | 30 ++++- doc/img/RtFlow.png | Bin 0 -> 10251 bytes doc/img/RtFlow.vsdx | Bin 0 -> 35376 bytes doc/ros_api.md | 19 ++- doc/rt_control.md | 140 ++++++++++++++++++++ doc/troubleshooting.md | 4 +- src/MotoROS2_AllControllers.vcxproj | 1 + src/MotoROS2_AllControllers.vcxproj.filters | 3 + src/RealTimeMotionControl.h | 2 +- 10 files changed, 198 insertions(+), 13 deletions(-) create mode 100644 doc/img/RtFlow.png create mode 100644 doc/img/RtFlow.vsdx create mode 100644 doc/rt_control.md diff --git a/CHANGELOG.md b/CHANGELOG.md index 6cbf8db9..51825262 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,12 +1,20 @@ # Changelog +## Forthcoming + +MotoROS2 is now built against `micro_ros_motoplus` version TODO + +New functionality: + +- Add new motion mode for real-time control of the robot. This pipes the user commands directly to the motion API with minimal overhead. ([#449]https://github.com/Yaskawa-Global/motoros2/pull/449) + ## 0.2.1 (2025-06-26) MotoROS2 is now built against `micro_ros_motoplus` version `20250328`. diff --git a/README.md b/README.md index 51b7870f..5376e55c 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ @@ -605,7 +605,12 @@ Instead, write a `FollowJointTrajectory` action *client* script or use a motion ### Commanding motion -The ROS API of MotoROS2 for commanding motion is similar to that of `motoman_driver` (with MotoROS1), and client applications are recommended to implement a similar flow of control to keep track of the state of the robot before, during and after trajectory and motion execution. +There are three methods of commanding motion using MotoROS2. +`FollowJointTrajectory` action server, point streaming, and real-time incremental control. + +#### - [FollowJointTrajectory](doc/ros_api.md#follow_joint_trajectory) action server. + +The ROS API of MotoROS2 for commanding motion is similar to that of motoman_driver (with MotoROS1), and client applications are recommended to implement a similar flow of control to keep track of the state of the robot before, during and after trajectory and motion execution. The following provides a high-level overview of the behaviour a client application should implement to successfully interact with the [FollowJointTrajectory](doc/ros_api.md#follow_joint_trajectory) action server offered by MotoROS2. While not all steps are absolutely necessary, checking for errors and monitoring execution progress facilitates achieving robust execution and minimises unexpected behaviour in both client and server. @@ -632,7 +637,22 @@ As a final check, inspect the `error_code` field of the result to ascertain exec 1. if there are more trajectories to execute, return to step 2. Otherwise call the [stop_traj_mode](doc/ros_api.md#stop_traj_mode) service to exit MotoROS2's trajectory execution mode -Interaction with the *point streaming* interface (MotoROS2 `0.0.15` and newer) would be similar, although no `FollowJointTrajectory` action client would be created, no goals would be submitted and monitoring robot status would be done purely by subscribing to the [robot_status](doc/ros_api.md#robot_status) topic (instead of relying on an action client to report trajectory execution status). +#### - [QueueTrajectoryPoint](doc/ros_api.md#queue_traj_point) point streaming + +Interaction with the *point streaming* interface (MotoROS2 `0.0.15` and newer) would be similar to the process above, although no `FollowJointTrajectory` action client would be created, no goals would be submitted and monitoring robot status would be done purely by subscribing to the [robot_status](doc/ros_api.md#robot_status) topic (instead of relying on an action client to report trajectory execution status). + +Rather than submitting a complete trajectory in a single goal, an indefinite number of points are submitted to the robot one at a time. +The execution of the robot will be identical to the behavior of `FollowJointTrajectory`. + +#### - [StartRtMode](doc/ros_api.md#start_rt_mode) real-time incremental motion + +This activates a separate UDP server which listens for real-time position increments. +It is intended to be used in a closed loop system which requires low level control of the motion. +This UDP server is activated using a ROS2 service. +However, the protocol for commanding/monitoring motion is *not* based on ROS2 communication. + +This control mode minimizes overhead as much as possible by routing the user commands directly to the motion API. +See [R/T Motion Control](doc/rt_control.md) for information on the protocol implementation. ### With MoveIt @@ -844,9 +864,7 @@ The following items are on the MotoROS2 roadmap, and are listed here in no parti - native (ie: Agent-less) communication - support asynchronous motion / partial goals - complete ROS parameter server support (there is currently no support for `string`s in RCL) -- real-time position streaming interface (skipping MotoROS2's internal motion queue) - Cartesian motion interfaces -- velocity control (based on `mpExRcsIncrementMove(..)`) - integration with ROS logging (`rosout`) - publishing static transforms to `tf_static` - integrate a UI into the teach pendant / Smart Pendant diff --git a/doc/img/RtFlow.png b/doc/img/RtFlow.png new file mode 100644 index 0000000000000000000000000000000000000000..89d3f39378e1668176879efe7f3a5f7618a93b83 GIT binary patch literal 10251 zcmbuFWmKEdmiJqrxE2jspg?h#0)+y>rMSDhdy(PD*cK z-gW1Gm|5?KCt2rYm0!JjO$Vt8U;_jT5qXhUuWDn_9*y3>#wdNmm>`n zQT5b6T0!%~yK3h>Jy~_`{1Ny09ih~_0Bn}PoOc^v+Y~E17CMm1Ps)#|t7olaI|k1d zI(C@JKNeSCD`7eK3eGJV+Z^Sq9mH!}B{ncbELT&;eHJ}N30SU9x4k|e_fJ6)8>LN; zTO2+16Bc|p9QAiM$93OMU-P%OIhXnjETFDCmjf1%AY$O`#fXRjThJDB)~o2xixmyl zM6E4qMz%zCbV#%(X7eiN1A0@W-WMuJ8XHqF5k#Id7vEB_F4 z_C>qT0T8p)R$5@|wGzh5)6^7i=JVR%^Fu8-T`fh?F_MSPIb+oMlvZ~o6w&~)F1CYG z&LO(H2xj0C3U^uGH%kwK;Rj_Bs(lvW_JW9Ybzi(@Y3ywfyr&3qJ?4mA`^~8+yMyWX z>WKI^aov2&%UWM)_!AN@RW5k+2dKE&^JNT2a&=eyZUdNCq~M0`p(to}o5XXQSSwaL zh~RRWU=TWlhAQ|pp$u^n@w+zHhDP#=i4Bkv1RbW9iZph-aRtk%HhDssUB7?-Zpd@- zO|K-i#PO_OXBVO8p1@Hg-=oR12aF=`9Tv&kMZo_WhwBGE%W6sPG`=d|HbknUY`6R{aRClRr)_90!b4 zws~gEv3yAk9FD$BkUHdFVrs-6jp#}18&bYcKRqtKa)vkp!&BrxTgf+0ovXcEwODz& zSTH|Nuqqf`M-f>y)1N=I)tP-15^^MueX2+9Oe|Wfc`yE~wV!@?{r$#35I zg8ags02-IKjwhYuqKzvch=zO*QfoDnW$#P+?TdiVT*xO2K}&0o-&?z)9yXuGx!t5h zOglEN?BwvHCm9JHfa1`F7oPj;_K@AL4GfipS8dHo0FDmDa@Wf;-8P&=Q7t>(<2WAu zUrV0*&puqgu10ce{4V^O<#l|+BR0IP(zJ~dR6UHKk>bvtz2Z7sJU9rfod%R)lN5zU zN&(?pr~@NQTQ!P|L?XeE?UT)s6S~zAo!OrXs8zWa3I(7FR|$-?&lrTvcJo8C%0c7V zRbq$m)jf-jsMln|@eugvMzQUop(%s4SKB9!lgdG~9_CSxOx+>~p$mUzEpQ0JWFcEt z#$ug2MNwVu+`wMvrB(5<=)G;C|%qt#C{W zio;p{RLLg0QuvwX`K50sj@Fla!$stdutdlO^;e=rq6PwOZAT_k^QaAZ&9y)r8$mZc>4F3)^& z@X2(Y@s>A}mLNlrreCt4Z9%@mYVZ=EfB#-x>npjvB=0_idBNZO70ng-9_wLnRwAWJ zt3sYJ4(y>?&pc6BYe*cSmd+eUA!8^NmONO79uMFI z4?>dBlZUpLT?4PWW%Gk0=L!&_)oVgA9o)_2x`_GPdRB=Lym#4j;be2dFi!x2uiG?_y(v?I9bec}TX1a25xjwpaf9#>* z!oVQ&*OMnlU(dsbC6huA*pgDRCt#EY$4S)PoJr!AFBWaN`Jp4=lZ9;|!l{2AV>Vnd zTLwzV>Y@0wYt4flrNh~bWcYhReDbRtI#w%eESvr*(*d}&_hrD|RRnhJn7s{IBCbdJ zK%T_kS>Izr3aFf5->EFo#TxBxo&;{ffQrqL=3^)3n+kiKZ zZ?iZg#1dDoIT7Wt(XMig=0V}E1F(x&VYdvI@(4vu3#Rai=w+7N+yP|2 zj*8{DI3EH%)ge%FFQmCtH0;_(mg>ZCZWnec2Vb}(dce7$u`bbvwMaHAp+x2#Vsxp1 zVq$Hw(wUn%b)a$T{y}JYLbzyjE~Z0dv{vx9;zszRiPd+FplCH!YB3OoW}5^?MwNk& zjvoPZJXFP5vvfqIbOh&!N|oQgMS+}fWZ&<=sdxG57Ahy=58AI9-g~Tz+1lSPUf(}CA|YLm;A>=`Kx`3+?pCiK)#`h zqT;vjXzpfAmqh1+#BO2)BsDyiz*g-mEWoBHmRNxs|F{RxI7i-?G12!h2CsOwz;IPrZYMeZ=02sK=H$HmJKe$Xy$ zX<{IcOs`N_{5+t61$N&eJq|u$V-bUcdJJuC#pN<-u zX=>!ZiEha)n~B!8tvB|U$)3T-A^Ej{L|w8{o-0BvDsz~RBj7B4BTn*RX+Gi)Hqb-r zhy!GZU5%3%QQ}iClFy<6c0Zcg7q3UL{%qw>cve>~oXso9$=7uD79j{hzQhEKz^fk! z<7Li&6VRRHZh7%%B`T*I>qpp6M`BMwbT1q0c2rcE{~3ws#8gvcL81Q~od4G;fkOcD zNey9}XV@oUM2ybQ;{tvGx{gq?>+o>b{{Ien7~oFc*d|hlc4G;NZzNF%#fIa1j2xNZ zJp4Wgx{zGGE|`)5LUHQxk328&P7XB1AsFVGT+ohJn%#})0PU-CY(fA|I$cb_h z_M|6*?|dmNq8$^CMQ?{65|SoUq0?-N9UA2B>J%ZxjV5v;;=p9Iv>u3cW9iOVwDWxF zL*>9$95yh7s#{DB7pNhl`cXUY94GgExc!g1b-Y6XKdRV)PYq=4kZf|@Eb=mq7vzwT zkYS=X>4AZ83A#0mCb@L)10@8;2*rZTchAH7sjWtE%syNq_=3Y(z8r?yop%s$e~>7* z1impHX-3Qx)}x!CaJlKse_ABjwDL9*$6c-{{V2$(37AXl43an;RkIT7*T`!%{foRd zVp6RC*wNs3C~2Pua!~Bs5CX!bEdI@A_*qbjwI8Zc9#QEZK*$>l}yAseC~D zG?WqZ^mu`lh_;V)aR~_pawzEkd5V?}(0Y$#j8QBen?MX-;@qTJ@vAj`>X5PJk5Y1S zs@l=9VT~Z#6y*5hV|y+ho(&j?JbIpXAx+NB(Q2@Z-Yv?I3JI_k#o;ide_Qig?@r~k zaFi(vy}!RtpzrUG2(=6hCu`>Zb!BPQ@on24cVAo3a?iDg`iV3BTZWZ-0i8E?c3f~w zj6&Pxj$gfNtOHpTh|Ng63NSy;O-(8E+QqrfsBST#w7(~Zk~oA9Zf0>26f$g_DL=-n4CYgGACE4h2P9{WShVR;25!<|YbSFln`OZbPm#fD;oDBVNMAhAu{xI7bjryhIRNuS9aP zV*$ptN|7h;mH0qyQgbya>hrglvOOP_;qJ0F2V(=YWrp!gzi`u=ba>s4YgwLvcUToH?BnyC0ZEBvSbaV8WM5t3{kB28c0K*KE}3&!6~3*YVaRN< zWSsgzy>*7=p4~%MUNqEGZCm~OnsppQUG!?q-PY6m(13%`o^zc*<79_mK&CyC*)aS$Zs9soBVP48$U%bS;J*GBKR%tFt`I z9ZYbMXggH?RVNn;h=>>=Tux5V+9|}gYiWuG-;Lj0b@nrja4ZzD_K`9`-;%@;O*@}j zsjM%m0v+D2m2YIXdyQrz`obe4RkWicMZ_p&GZxgf7X!0T4sY&y+d$VdPj}AT)a1q# z(bf8^d*2xfSu#G5H$%+vmH~2$-bWQqq{BL9$;wkf;{CDrrBspQ!MPUiHX5mTOTM=+SQ12v*_-Adz44e^Ce$k zqUEE#30_l@|H8GNo6Z#a3?~YUGhhXpv9P4sm=_=7{p&_Ff*)u)t1jJEe{7gkpaGqo zbD<$#obl|?4ZwgzFYE_oJkb&4c?^%_UAWASS_cS6K`QD^tNkz%*r!EaAfF8yO`T5E zAi@m_9gGYLi#+WLASeC6+uv@LWSU-Up9)}!1@Pz)DSt11$IK1D!GSg{FHgYaO#kxw zI&ZfZrL7qfJ&n;5CU49Ip39Gl60vSWiCs26+e6$sQ=-)so(K%l;<6%Pt1j|6<@N-{=wqEvjcLXiiP)zh`Sa~|uMgK&%3`h9irwQeD$HL& zvBH+)<|JH~Sjp~WKtVpx3op*m6uNc|A7hT~ZF6b8;@^Ubqp&*-X!_auvkCzJ0xm9&T0# ziUuget(}d3YtTWvVAmZ4v*0{Y;Qq^(``_U+lm;0u;rgWWapZch{Lw4c2od8A4GvfO z8vCtYAKhfqm3k*4MytXx*v!8n9%4H$PW^1gzW+{5KgWqvJW!ig~ta#<)$F7T- z=eb_DCI1Op*+JYPT(@vVC(gqOkJePh+BiGSBxKBjmEVXERP(+e zJL9w^o}@pX_rKn{E+TD43~SWJH*`hpduj=6tjdjhpTg-1vR%N~ys*}{tBV@wdL7+09c-hbn8+N6r^dLMa6aqfiW}-; zPP6z$7Y+#KZZc3wDZlMuMMK9y-4`NepSX%t*^I-Uu=cQNx|2X9p2WY;pTSoSGUr;# z0UYmKzX|I9qB)7s*uRf!vI-D%4I-i za`(F1_!Y=A1 z9nmZwupA<>gggdr64;5$4jZ!BEdw=6vK6|jaQ`_318FVuP8J!2ga50zLZrZ9JWhfP z$c&b={0Q%7wPZ}IvLgXEc%dBndt=5o3ca@3Ha3fJ01jRr`Rf3v^B8e1^@=-u9lDfS z(AF7GRJ47lMI*v#CBUSs>aBdA9PATX6;>QRVOm@%H8Y<#&>6*i`B99L$4FB~%B*x3 zt~f=^pPls&a_sNCVT}GFBJx^>Wg)Q^jQdt3Ka+>x878fM(#jr!s&8P}=*wA4(lGR3 zJM+g#To3`=o9?wX2JHW&(Yi1{tyI|A2-UXq$uC zM1L7zx~O_B^&c=^V43uvC$cE%}XKKwxrm zG1|UWHQsaC`D;qOQL!9IOjR{Ow=^M(r6;$J<&b|%v^Y6^Pr97UKE!~TK8&xE49{IpNN29R9sFjilgr@QR}O!&F7Xji=z@F4Uab~&2IL8n}!cVb8L~5 zin@Ns+!efN#nUU%blUofC8I1o!_C?$3B^O-t}o+bbLg_iUuNyLZ_IDnWU?NIobBXe z&NO_$m3YSx`I`P4t%_d@iiZJV$kqq}J#&9oW*3k+FI`ntHJA*k^>p9B7Y$(aQ%vGR z=}k_3l|fZ+RH%qpfs&p|*wfbURZha29)vWg8p{k17rjY8OyQ?2?T>onW#AL&psJ9K zcphu$(<3a7FzSYa7Ld(M(f_LNUv-U=&(bQT)f|I;CMGML8hBVrYc|S@we}J1Xa#=v z5C9qSz&)~wtZXvT;@cieFRmv-isi?#3g@(oC1dSxeLz&yg+lz&{>u0L*aK4bFJK`q zp`Ez5bAjM<*Re~J$t;)GZQ>C=Oa_~~WJ)>~V=yM7o@BRStpy*bHQG5xUahNAmd|YC zWKABWa)z)!dGUx%L8qvBg~2+ch%(T0q*^wib{!;Raa6=wdt=Ht$D|M+CjLj46{EV< zo>-==b+h3tcr=fRlgD=@bn8N;#@H%=QcS`StT~zR5o7hK?M?M_Ure$8 zi4N}vVNQa-X{JgBxW%#Tmj=s1cg>}h6o2i@b;MF{9H2OL_#KdNRAb1AcDZ#a6p}1% z=(Mi8IdSE?I`lgyH;$IYUQm7M+q}?TPs0~dKF_)6d-dO2&`CQbPzlyAXFz#0E5XnO z$a+8{w(6JRPzGOxb}f;Ulaoxj6T9UHQxGmhhEeDABJ)WoJB4>Lt3}wL%Dh+XF#l|J zL_klua3%z|kJZ`UGzRyZy8@ellJH#B2G^5LQ{e5A@|q{ZTNnpkx%Vp+)^^ za`ro#=lwv;(fd!-AHS}`*A3)G0dQ|%E819EewMc*V<^CADCrMDBa7<<<&;mrr}rTY zY-XJblDXI8~HSk&Zk9(J~v(d;`1wIU)XftcU)0_FF z8a4-74s%^W=tB74AJE~V2lj;TpU>3^0un|%@a*fq6Eryq%(QCwMuO`1IO9LMw49@w zz);4^>C_QNhz}eDzt!KwJNDg{bB>v_XN$eZ98FB-9Iv2|mYT|#G&%hN@E0RjSM~8S z`2u)7TAf;qZfm5Y5SP3Y;@PF5dDr80n1ohx1cio!4|cR_*hXj#?6Jw1+pWIA@>cX` zpVqE`9y-OfjqeO3bS$o(;K>6K{T2Wk2h*VZ#UCsrJwDx!4Q_k6>2^}t#(jG&evj*M zBvVV_3J-x+qN7@lsJ`YLVz%k++;+-Cr z=A@%ZtJ|5&t%G4UikO5eh>3vH?`8F2;qo?L zN?rl|cHdMGwnKi{`vlxK>gAd5GW$X z1*g{X3DLmSMQ!hw`JOa@7AvfQjtO zKGbibk)woxuhhA^Dr@h1Qxq0RSS&+rU@t)NdE5H&A zO>lvp=8lde2dQ0DT=jZ%wfSlYQOMDcqngb}@TL3MdWA3-lL27IS8`!(6(r$Wr)oS< z!(-}0*}aEJ|4NN!R?lSBq6fx{i1LoG8h{#h0P$R5q4KW={GVZ zA8oh?vJo_~a`wqADwLu1$O1j>9l?IFO3eWf*z(^sM{5pB8mtF7j77PQluhL{65k~W z>l<%f*M@(!efaY@H`*t|h!ZUdR_D@#@Z`W8^o5s8QkctaEN4U+I5ewDKe5*MVh3D( z>`b5sa*>Oyx$Q%42uKTCu{+L%3e8-Nv$A-38BBn#g@N0r4NG=!UsJaHD+H3DV=2{! zGf2v+3Q-w`zSS#PJ^F&hBFj2k?jPLUz3vb$A5fi>^NNNLZil=Rm-(-aXvbts;YkKF z$7@b*K~th!e$qNDos#k&Q0{YZ9?Xq-ZGYS>mUKa7#iru2)~gKP@}M^%1d;W4Z~Xs&qNiv2|1cb#EZE{>iPIhwbteN>{gT_B zM^gbo&73Ai*XtA9vHUO#S;DoY0nNcjH8QBsE*!n?9Y;5M@pRC*J{?uTS$a~@pCYr; zlysc@(K9u4Vwe-4K|#S63yWW9SsQSICuz&K;ZX7l{gkMy z7aFo=&JJQe^fbe}qfAV`iA0&sctBCOrsj9fM3f#AFxXP)_MG$?D$IB~#yx$u0;x-! zTR#$-4D=8#m9&y4?C`TAK5nOhM`7nNE9*FbP?k>J?-*os53p9KYDdxi3F@EKM10uk z49?$FvFa_Yhjl$tYqlySHisV!BbqNYr22Al<9+kn%wh4uI2Ti1W0 zV{Ojn3~b`5-SUqUyZ6%q>02rsvMFr(W7qhiD_<1Dy|ij!+>BrghSe`f?E+ciy}w?_ zlx%qED-C9Ewj2y!&M`a>^_2fCN0xA}B@UCNuLYXt*kZM;-ug+crSQjPRGxHU8GHhT z)IrZCZpPHK2a2x)$vWf9NN^Cx)E>@8EO-r_jtg~0_34X8f(G=NM9r=7p7*kJ3ujZ{ zI4nu&UILFBvw@o!Si`YO&3c6J7V|tQ-T8%H9g*Q?FJr7C(|1eoY?8h2B$|ua0UBd> z`d$W--p&YFUXC4)1C~^zLMt&yFE`a}GzZp9BeK1j${zb=&B{&JZ>N@;&kN=^nF5-DR&TTCsST{g!BEn)@!uKI-f@8j{?a! zNGQdfP-1$sA@GIj*#8+-z4WVO|J7bRZQErwehT*|Vqrf+@@`w)uPo)r%(c_kH`%k$ z!BqFr3}O&TvO?dQUSeQEu;Z3m2aaB=!DFqAQ1(vg$tE>5w{ohn?rVIms{ZL+pC_Y* zpGj2PRcHPf5#?v-eh*kSPG9}tDi&q8z^Li|`5bP6O^D#L{YIlqezeK8N8ZBtsQn~) zgN>NP>C-{J>!Djwh#KJvr<2sIKGQ8+doW0;t{v_A6alT>9Kr;32wT zyPU|fE%vKhV@f`BMWPmeK^R}ZSr0&253w;~Yyud$XpBq`bZa?bw+l3OLwChSS6F|4 z=QzJ3#3P2a&*qwwgka8Ue&I;KoWdE==O!4s>XpwFILc+e4QP{tl4}u5e@2i$*rqur zY!P39HHnrN=4iNo literal 0 HcmV?d00001 diff --git a/doc/img/RtFlow.vsdx b/doc/img/RtFlow.vsdx new file mode 100644 index 0000000000000000000000000000000000000000..79a7a56cf4d4e73bb880b8322f0730c4b7386e94 GIT binary patch literal 35376 zcmeEt|6{~Jsr%P z^%y*CZHS7%K&T3UK>mLJ|F8dt5ok^ww;N!>3b{^wLrAPsGcJg(jOr_#Y&aAm?2Gyi zSn^1;ZEd^0MzX|h&{AU%?$3K2apc2}TWOR|Gt8Z%q5Q0tKv4`m5}9_oFLd~0tiYh$D1=~9^Ovs7P)x;-e!ki` z26TIJnQT>TnsaVDhM3O&6A37z1N+MD0P*L*cmNE-c!<{b44HYkOyFdZE+%Xw?5zBI zNWnsB3&nm~jRPyoc*r_AuY6zmwIl1#dt6W}vW6HxK2i+k69UH#{=Hi7Azw_W;L(}g z*J0}zUwW=yfZ&x9qjEwn=VUww-J)bLE;Xk&Efz4jMwZwoh2`J;x- z13VDW_cthz;{QSAa?ODj^1oEp2L%Fx{Yzy7Co>yoMuvZk|DVGD5A*iFOuagJQhtyL zN$gqjBlysFzl1MDG1(4D*&Qkr5$i(A7+KI%q4Mdv0S26e+-#nKpJ3bDytYgGT6VS- ztH`bOKrGEUOr59HR_Ctm&GWWTi>w~T%c9}HJH|&(&!9%X0a7~NxZKxNYIr2c{l}M5 z1QjnE66^7NS@sAxXi8imI>7;CGgX?WALD=lynZa(ns_Ro%_6+T;~&an=jKph5O6S& z-vO|MC|X3h=0@xx7!?N-EaJFQY5(zize2qM7pWIW}->%tfb0Zpm`C z2sqMDA0D&K_X+L4?*$PALV!+3_Yb=>O5r8hb?`W@7hJw3kB*FeD6fKtM|`>D_r&<* zI7edm5t@7|g-UNZ7S;YhxVwY;{ zS@TE}lDr_(WPW;}%!J(UGsTb`kGfAfB#E{aj=9e(9}IVI39hpB5XPAIcNMrjxg&7I z8j$(ku%oluL=11#*n$%YN=drA;YJQqsNNW11hj9H@0v2f+L17OZV0TJ`ti3*?y1J8 z$zj^-KxQ8yf`2k(zy{a)jh&YFlZ4udu#EYSBTMAlS)js<`2n>y+NnV86ktj)o0fgy zPc0l2I)2>NudUf}Fl^)FbL3yc-_}91rz(Q1jT6(q?h4)773O_kZUc-MeC(xhR)1+Std^sk#Az*{Biq4yicql zU;R05-XD#cFn{dIp>W`?G$9=kIbu)Qfy=ViwYfh(yJ^=v>F>LCw!nY){JgYbf5Ii& z{G*p3)C;mWvwW4^{p4l=>)!)qehVV*RzCmd=LH+joqcBm_;(Omn34l5b*tM`!uxa% zqM;PR{d{f!-f8-Xqk%j=cCm${C`fkfzL*@hA)8K&YJgU+n7q3la zU^FWNoK()F01ok54)8;7X29qe#c~Z^JKfEAp`MULJ?)&lulAd+!?a1{Do3U zhTsmFxwYG2^rcSepiC<`QO)ibzc}^|uMApqcp45ADD6Hps%VaN^^W@DF zySq?xtCWZzG|s`2tGBA`iHJG}E41Z6t9o$x^DWuq zB_XEg!%8y61iykO>ZnTs^W~I8F?_hIDaqsY6SFDD1GhW7HFOKc3pd)x=E} z;Y&go<__qQd|i?&(HHEuHljC|#E1$>!^u%7Xoy)gls|u>d|GYbN{xg0O|ZdTOa~1s zVZg`lF8V;Cy=AmOvz~8@5W8K`7*XTKn_kF1=o(`Y0h)8T!c`xlIZ+2`75iFnqQrTP za?qf%QxwAb-bER?0mfdN={Zqr=jX^~8nvXoQQ?e^EJ`1WH0p;wVLD13>f>}VYB0Zj z1SNy3@jp=@@lN6VHg{OZn*C89i|SACY3B&T%WqA|L1|3*^=&dWLdN@{O zYI~|$6gZREEC3WO5ikR;{TWLqnn{^#>vL7g9+l0s2~}7s;$Ilrdo{XePturF%AqrW zRL?Ufh7xU(Gc`>IXG;b=B>NhZ*?ALr>%s!-_Z<3TR4byUKy#`xYGSl+3brd#Ag(_h zfG7Jzh1s+XrWrH})*`E&n(grz=VV2HiI0p%`xreFzY%dPdU&}Tu>mjZQIDPOiI6>m z)NsX^n}T3{j)Cdk!2T)y$sWv~RrP@|SZBO2DR#2JSo9(Tjs4_V!4mOm6IF_* zVY!WC9p7ZK6j;j&*8weXFQzq;50uqccqTdffT8-{Fo0sb?J4GtooKcXgbL#J+ZkdN zhlpr45O@bmjrWER(^fv{+lfvZC*MLl?ppo}{2g<%v>hKqOXZoSg=5-nQuBcgF4hd)a88P%{l~ysSapq{pdu_yUqUBpifYd~Dsv}zV9xave= zV?u6IhtGm;H<6jzM8`Y5?DauP5l))=o)@q(aIpe{>m;uTK9D(#{uB3-N&e)x0B&wc zl~fs-KR6Yw2(h$GVl*E%gepuDJTz7H78re}5*f zn-MzEZw>6Ov1Le=^HKDNZH>8a3-H`yq*lxYKbSC-HqJofObEm8ad`h6tp^R_qxPjh zXI63aE;N;94v|N`BV+iT4PihccDk6{@pH`e!AMDl>rX)%NrL5s8CG@>!LWX?WpTvU zWXVx?Ql`$1k2tbvUUVv_X9D?TRWo>gIVHzE=J=zSVD7oZos@@qPe(D@s^q6D5H23I z9NXq4DSLAlMWT)(Y@)10JSEbHGbDoZN^NOrVkVwc$2PQ3SUGB;^#Fcdw5n)R4|K`e z7lJt;SqUXHhO0TS(|*%s(`Nh0j_lRbF^+_Sh`>0=7n=($0?X?Gz>1H1D7~MHpC1V1 zs(>innXHR8g9HwsiSR6MPp$tbtAH?i%8J!e=ILa==^H<7u`VCXz>wl=M75rmgta7} zSzEdkY_zHKhC+5;Ger*aYpP*+HGFC?GRig@`dy};IOY*sEwp51^rY7nHRy8Jh&*^X zv$mI_(V(5O=@8l!kjScS>1J;~vo5T~DUEAff(SX~aZZ zVtXQbB8wbZSL}!~1As{o5wC-vcRXgwo;%tBB)dX6?9KD6li#zKM9$wdN;D$|mvI(N7IAH9exR$Rv&qt0Rt-f=;iW0y}ClQAf zn}I>@*r7b~velB!AL=Q2G+1Xz9JT)v#!UBli%R=a%PLNy=zy`q5Vsb}H{U=A)v}3v z3CvBNKNUta6;($V0=QBm(BNj-p^!fodJ7lpuUADMeg3ksoA@cq)5hIno4Joeg|*rf zdyoAV{hG7EhxDLtZOA+B)Ypw*?zWnEZl6*6QU+dxW+r=bYpen^RfR_6O_9S?dAefx z-14zgGQyoWiBf&;vO^UAdwjxik#jO0tg6^8?tXB`XX`0tPz_+L(AjQg65oHo{fgX} zlu59EESau0g_^6Jc$#=LyLj7hFWSHv@TSYPnM&GHtjb-hU0nD_rN-um`egEAc~l7! z1Fagk>`M(q8~*!;NHQ*Q1tbb%?}S5(*e{T_mMNH@HeyX8NdsMqz%rX)nMUUMkaJo? z+)xES&cOnbQlx5rcNP>1?Hv4)2o%0+hh=FQknK~gVx#$nQ%))M&R)2yd4;ja@?oE% z@ADh~OVZ%iTdp6K$MW-PPUV}5G79W|m8A*lgCZhj)2jvu)X(%s$6WVOH2?qGG7 zNMm@xRKNNP@``4L_{H)=4IT91yE6B{MAjQ+%(h}zTUeb3Gfr(z$Ms=|DpYjaRhLs5 zb%&-`{A;ZHP(dk=zrGjYkL#r(x}W=2_eHN|cZqXd8nwD{t}-2JSLw{TPqZ^Pcd+z^ zMdlT7SdqwcfFjn7{ev|j!`8Ckjwu?hAS+ux=VD2ey+1`$@+Ir?dV|0ec3qR_=C7oQ zbv1SAeLGHC^8JD3j9UE*mcFr^G-ynIk+ZNDDQ zVL_e^q5zuVTzcu67Gs)NZCbmxCEJ`u>txFE5wxi1S$5B5ZDtQbz9@AYo%Byw)~37K zLSd!~dbyw({n`PciduccoXBtN$3P#A{t!^sB;d&mWDFZTp}b*vO{VNnzWtTmez-7r zRC5RQcBWv5!7%Iubb%?NRVHJIXRO^J*@pVEvwGvOnGj4?Dvqg){y=|q!*Ezse~}mG-FFmGEmb2Qs}50*T+gP$TxH)+b3rHs#}H*bPAI_X35pb73@py zF{F9*;w}9((&D`z6=tItEHlBf$Z4p=!Bo27>0hF1fJS31}}-O{Rdg`s!P* z*}ay)~b%87;ikY8LN|gGqa9!?D&H;L*m`uzpTs_+tHri78sn^Dfv_5JUa= z%MjN`KL?(dIet$gO=h`4(4RtrIa$Q&6gOHFFBi^jl||w8Gm7g!Cj@=&az3E6oL7XS zEo3pZX0(ar`DfIleog82I|px#?sFe16l|%hJCSu#cF5bB1tkFl@(Png1qFaB?r@5R zRsBpL4Nxj3amXvoGW7S=g)OI+?9R)a;vjufCnjkefprp_hv*tf$p02tl5v>cdM8ro ze9pM(&E&{OxxHi~!ii+&E43=r8YK*~#5}Cp3-pxn78G^1=9?DXL(<{$2@{KGlls(_ zNtKg?>Io@H8}$~pi4WzQ5t`d1Lhwbp8W9xv4@Jt1^!a_$Q#ZvrL`YsN*7mk6v_{im zV-5OLR4`roOjN8c{YLhYbc8@X6RVZT%4pGYYGX$Yb?Nd70RxP{j;FXtLN9Qi*fZRQ zZd%&ik@s48<53`1U#-xMTK1+2GBSa75>hV6uVP+mDZ&p@aM+~DG@^@P-Whvp@ zj?zF{i|gFo-CE%WAqdYe`wWi`_q*{+Sj;l!4-q8Ye%#Q`Ts^s*h&`H;R|1ueSA(5I z0y9c~$22g_oA0+U%XEb_HMuG zgI0a)2r=lM7pSI~;;*lb93M>Z_^V|n`S|Eg+PI~f%um;~erxf{SXWIZyQb9=lnEnv zEkn%`RaDo*`3pfw?T=WiN*_Tu)zAF$KMFu#b5`MOK2DB2iCIducQR8p0*nq~j*`B1 zfA1Yu&F@rtyK-Y82v(E%({Boyna9GP z?7SJu0IdN+03To7>EYY8)O2*J1TzckZ#4^CEw8}_728y@TJr_JFCQAgELWEmA8`bW zrHRe8HP6(~S+pOK8o4b~31uek6L6c?5*C3{HI)bOOHBkqK#PHVLfycIU+|1Nl1_(Sd6h=jm-LZkA@{Y6r&JTdxlB{qG_Nh(K_5=U6Ns zo3L^~hU_9Uo}aLkn69rHesyi&TwgOw&hq-eCH44^CCqovc|a!iGhGw2P-*gybw$c$A!teS#dkdK_Qc83ba(5QR*-j`%vDlsJ^O6Ju{w<_sj4 ziG*HMinFP8Z6>9~z_PZ)t1QOPjn4CzZzi?(8KiFCd?^!EJ3K3E)k^dG@CZz_y-&s^ zAMbgwzo;{BgL&iTB+yPz806+ zdj+L}7o0N=&dN^iIfyER9eneonENA8z8|~WLkHm;ej#x34x-jIKb8KvB1Z^D7R(sE zU&dmU@tRH&C2s;aK0TE^@em=3uL3yF5vARaVSZ28;PKqX6$umn7{Yk}4i}6bLwg!H z)1nDaajp>o2Ny1fYTlzv_(Z20GqfL@iB{vNZR>7m^O%;kya>?$$3b}$^{}b zOdYeT$rap)nMWrFCk{39e^|DqzmbvH?Idooz??)f(`{Kgk$s-T439g5GaWwe)NCq% z>E-`OiRphM3(D<1OO8N%A)qhD`}>cB9ic7j0zyyo12wFeohb41K^xWcI$B5XFHv_A_} z?lZ&u@gq$FG>}vJ$A&=5UNY`#cmZZw^mOG;{bi7GaP{lU-#YkjulE>#m(xrqQDO&- zr|Vg8F)Mt+T{APKVYHUi&9}bKb!Lm9YQ~rhmfP~8|GAWs=TdzK!n*T%LDG&+%b-4T zn)DX^J{=yhNU)X349FrnIG8k0wjz!cZOpO%I9xWLT(>y|%+J3m^C~){0!f#-6~rC; znL&`N@+z+UeHplJ0u)HzL0AHeE!@R@=RKX4X0GTh$7!uYZk8F{TKIJ|gJJTPICJwlDX>d)-Z9Z=4p(!p-T}%sAnt1G*=CYg(1sK|9y^t?(&sxD%O{VEgjn2GGbB(X>I3Pk+NQ@U2zmE^|Q|wmS64916bIvJ54cw->iYP^1*_ZV-+;WDT$(8PdOwU-#e0Oj-cCa z_vw@XltWoX8gnp7v{t{D)&0FzL^(`Gj=pN}%1XsfHkOS~bqu9MV6BI~tSkUP6ew!d z0-^DHE^@8;N|SJeb9k*%T$LzSJ%LUO+n)0a#3(Bv8! zVE3!a7B03Y17eb@8^-QtD^NC%hNqupq_7E$ubyJ$CzeIfu{dq}#Mh&N`~EvUkyBRn8f&V~#iL3Br_M48gf%`O}X&{`n?f1T2ZUp#Y7o zhdrrCO(;d&LM{3rB4N(glj|!Ahr?WPtRdEz0a@a`ha5UlCbm*z!yi_f=c&q8$4ZjL z++7)L?UB-CDwi%2g7F#|E+wWPb$5q4BTiUrS*f7&?pkNrhfD`?J&ckqmU@tzaW&%1 z_DEihpnudYo~-;`>}23Q&52+`A2-&= zIicJT(E(eH`5n!aQFDIW%h1E*;{OdVCaw$g+UVgu7e6ZVFpR$iop=Pv^UGw2TmjU< z<(&!=91>Ls0~~0(z8G`6S>fT8C6`eJu9WubicFZcz2djz(6OS=_VzEBOF^An(f zfK1qcfUy6)^<``1>|*BR%=ka=|J(u7{&vo8NBQn%{Um&eB{T+EU1iSY;slvmbC%2E zxYYCR?IIR$JvF)kuFdZ9{C>Zz1%mbencVvPr#mKB7vJyzWH^PZG=SlT1`#_nl;K&cluZ-*(UM)#nd<(@e6c z1Yw1b;c397dGqd{ar3V0-%99|W8nVV?6Yq@X4aRxb8~b}faM%kz&y{Oe707c1)R6& zPA>wC=K4^4Fm^SaB~#Qu!r(cOSi^<=by~^!@GHCQvH6T=VOI=_pUUR&9FUJ6+GzH= z&R54TYs_PYc#}h`0^{TDqWX-gi8|0F9_C7h*!P8?#FJ|J>+uXayhgL??kze$hl5-UITs zm+o*{b)IBWLOSI*+tK}Z-Ot%zcMAgC;fMNE*qthwM;EQ9|6=z$dHJ2ffZ8dG{k82 z&@+rU>rvOAA&!7db~(JUH*ewM1~^Z(v3o}^JJt+6CKAMaGEk<)_3=1kC)ustv%O)| zjmTx!H0``%&wRgtF zn^8+Be*}7^(g^7CR&fdOo*1%tq_u{igDV=z+n=bOc~jXjWBq_n@Q?Vn8&={ zm@o*`k4%gZv#4Y>IBF%aptMzVDh?4w0GXqRcE}uJ!;LfWw~B|I_q;GGtgPh4k@z96O%Xo(aaxFg=@ucXjZ8T5SkIl((Iqc<5=H)o9M2BzA9F;3 z$QwX)gb11~2xH+#I{+4XD;PyOqZ-e3U$HMM1n%_4+ZRjs{XQXa*|X(m+9|Ok3mc9j zRZyq3i3pd?r_;hHwaIveG-Ctkt@Cy;ZyU)RoIN>&H3sjZ<)htVh0HB5F!>{v*K+O* z%>WI)Ss;u>BU}`~SdWA0iy>Vg+`g)SfFT5-hpbBLfn#@nCQH ztcZ_;LUw6$6L6g$${W}EEsh8j_YHaSHvfWNep!9X-4B@=c*e9wKaqZRB5Ot{8Pp7- z+|ba-{)yywi8oN!jnmsxbnP1MTk^i{u5{*quhPAX0(Hp`6w>h zah~laanBFna<*A<%I74t#Oic7GwA;47N4|HL6&NR4?u zrP$rlU*XUUanP147kTx?Apssr|4GJkSnACZCPLr+J97BZN4cZMq@spnabciF#phjM&(*`dZo1HZP7A ze9ekv2uo(C8FPd3#yygoAQx;jn8tdl^+5mwmJtzz7_U0?A7o~$?yRCvmqJe?-}|Bh zjsfFTu&Kb`O9pmZ)XD8u`T{iUpyY;Yfe=OvDriTiTs$KsOz{w1;>-l6@?MVB@%0GI zG(QS4-*KYjsEXu^?$M-eezNQSRSfqjF)*? z&a)|!awey#lz%1NECxzGAX0{!O$m~OFP^pmoWLr+JrXH@SKH$5kS>McAN?#lZDHqV z_Br{(N_4jP)H&q`#xiW!Z0&&bT|lJ#)?4`7Rk0S0f8DZpZrEh?1OS6uiZw7QF7TUV z2i2_=t?osYgb-0dKF)_PjAsv>wDfJ}ptwUUJ#~?agcE%1*Z7~M&%@824Ea{KIe7AN zc73UIl7gn9_#2GN_@8`u&LQv&nA`D2YZfha|1H9sx3gwyZ?ky}W=0xOE8CYV? z6)+MixEita6suB)U8^1vUkSA+9MRiF4oDW@kf$@@5hqCt0K!AA;I1ix?3xnSt8|OD zxeDcP&8!1nD-djdlexx#nHW!eowoN@giFO0-e*_eJ)!RDKceqbm?*Bl28B3!Qg|Z3 zFzMl-cPe{>QxJn_VX*2%@h3(6ZZZF(jZ5NzL%sc*-nIRk_(lJ>jdL)vFmq=7=lBl` zx2ZO7x6Xv-o8Rm|rjsX)0)sCaQW@GnOe_@Qb_qBL!!@36U>yx?tlMruC8GNI-Xoye z=?Q;Zs+>F*5*y$w$*v+;=OWIrwVHfWHwC|E`-m9 zPoqF(a=e72zx0zGFD8K5T3_`p*-kZvB@6Gce*JXQje%J(z|O=>h@e$z5bV~xH=+n` zF&VgnzHV#Cb%Y;xb4f?YHlv|z&L#m*rg1c2QMi|4=`5b<4RbjhE$S=_=#3x?S%m_)~3Yo(!;cqY^cSt!tT)s$aLcA}vOgx6bN ziv7-#$NZ;us82Kr)Y5xK;trlLp^_ZfcYx)Vs#L%tSKWh-f8T4!y)pTdSE?zmvmr63 z`8yXf0rVuUaZ?J36fCMk^UIU2CTdP9H|cK!NU2+YY7RVARf_|Mq})A2=Z28tG=jL1 z7HHIWCJJS>Y1Xf-*~=@j7!WblH~=PPnhw8Vo^0l8v<*G^Av~r>`0`F)?d{32?lwRF zH|*K9L>q(K^y0CLh2|eSJ9_B42c`C3*Alpg?AWd_GA-BN!@t_Ht=Z1-ILkdMUvMLC zAN4cvC&K=7t5Y4HAP4d{I~($sC;0#7$iEV_|26CP&opgGQoq&T z6y=ZW(68WCZ+c$((v-1|6ro!3PaxttTO}DRZ1F^`qkRX0e`c~IBiD7Y zoSf{rfwI*P7gY(W?N1U`e~cnD1u0Q^FecoO4cm_Oy)p!pCptHIz-w%@oeE(oak+?R&-oUP#q@dB?^#DtiAy^nXVr5lAaA*Z+dH z@)ta^e}nhG`ow>M_+Prke`46YF#Q)pxW3zW!)rqqgOrj>tWtUVV10>apb;h1rWer3 zl&4EttYEX4O((RG?|e7`JbOQ7jfo56xj9vKDj1F@1~?pn*=t7og7%ELY-)%F2g)#$ z=MR^cf^oEtp<kF zN1VHg?Es$b{Y7!`eTQ9iQ+!{~n zywXg>w9fW=g0@+EyoC&=hDzB~5&`o^lIi5KtarzlhH(CZ6UOJl6rgw3yz)I zh5tGvx`~o+E_? zICB+_0rMRm?+4!Nq$R{8?WG%GrPfHDrg1lor4lwuynvRrbir>IW)R}wW*Mg7j8Ghn zK_eLhW{jL{^OM>FM3F%W26AF8ra|%^DGb;Hk$YsveeNF4v}hp<>z-IZn5_>114s)JzYO$c)rkU(?yBH( z()_Be0j_l*nS`L}JG#AUe^+`8Rq!99)Iq>Mrws4_|7rWrMp(ASSr)|(!A4n(1} zM^VBIt3v>c0IK$zt1x;TSEoWiolVDdc=1$7C6wlNRp*Ra%iyncewPB}Y+ib|pfh$x z;{O1Q7-bVI%tXPGtGh3ni^q2M*+|6b3jax;8G@+dzbbo$w@p2m{xg$30V3$-b+&P; z&EnTAL3U8uEmhOPw9j)ElfPm%2uMYA>rEN=3rrG&{bKi zDjXLTSeJ<$uI%9?J=neVMGgAjAo|M}r!yo2a!(N%Odk9C^D>$77W&=7T%~;^wPsu( zA?E=vL$dr3R==-6Yr-FQNdbVt$g@0krMnMSV_=FPq(?d95uY@^4FT8d@w$M!Ud1e} zq|X=?WaTPjOk_p_S%-Q7J}PshCq^)HX7$*y?+}v*!94RT@7}C#c#v7?cnKs0qOn0AUsZSt0Kc!#6J5`U^3_yH~V3SWdv3Bsh^2#9O?jH7ZJ_}WmF52VH$U8nY) zbHQq9b5g5G#o!icv2?z-!+ou9**-I{NS0Q=Z0Z+|G<*tBW}fjz`?hsD?kl3S9VfW) zgfLhwn8%cJ6WseUFgjD%(uj zY625@62w560&f(uCaR8;8ERzONR&KlFBxi6foH4MDPZ)lQM~tR3Uk?{+v|w3JRQZA z7|BnO-HieYrP!oLl|zFF;cJMqQ=CfFv9s26qYmjRCe-pU@?M!*L@}0bl``_SG<|c% z(+Rf+pgzIhO_fP1K8Jj+`CeM%H%DcWIgj7U__129B?vo%Ckp8;Ro5`O^vfdoPP&6-O5$8hr=Lhm*zvZ7^C8#&;%){q+mEV7{;W~T~TDw zwL=~yJQ>{po$~OIK!q2^DpJ4^S7tE)t8GU`3&oaaA>FQz zGMtpfe3Q(l+nxoi#8d4?CXg_1Y8=EQriIs)Qwd2>z$l9$T|#$AB0TY3MdvPQ`GK1Q zn@1mnS)V^Pr8iZzH%Ma7ku4a8CZ|GQ&0)`13ToU(vrZ}-Y>xXAaYt(f*-ccp8+XEO ztuNdI`iS zMJRfW?a{}+OAAM)AF`#>h^}m(j;|r3NKasoG4boRS#IwhV5C)tHZ4m*?D&?)cww=-iwjnaCca?_mb8~Ay7=)N^s)r3+2WuKFNxdKN!D*TAhp$jo>}mo zfe$x-b(FFL_*66}!un@|YpD`}=Tg8fQ)J5f5(i{yCyiWj}fZW$)H_fpuM8(ta& z4-J^ZE_PJ^BdwFWCp3n^gYs{d@fFqQp(1AY*ub#^ES561}Mno zX?p=npUhF#A^Z{|^oc%&cVviOCyKY#QTKO>+@p=*@uVs>XPP%m)11f^pe;>&ft(CD zhT3`Y?8r>$g+wK%Rk0Exq>k79%}^+e;I#`88={k+hsgN-K#^&KDybageDeJOI!@Ay zfSVj+U;H(5)JTvW3HjK0p$PU**)`a%j-U#dEC#>RCsSRS=_^1!SZezK)JL24p zR`<68X0&!s;gp}^T(n?F&Nu9T^3EG?pf)~#T_ii&e=nn~|I0@+;@@2x*#Ns?p)8iBkiOf6AYs9z30Y-K!PYI0}g)nQ{43YUH zL$eD-K3_FOB4QYbJ?4s1u`YKE@v%Hd4#rvqdAM)d#S=q1R;W2|eJrbuO-oFWPIsvZ zhlYD)K{6N;^x}eAwkLg=TCsNr^OqG`e{tG$V#JX@FEVGB*KSFtpRIDusu8bo^wDy^ zG%3Gg@2VT<1{E%VUw?zAc>c{{ckZg!X??yGXlF;)Q>E(@P3eJ_9+lI97U#LJyp2mO);{8nQ0CM3!hCl95YtH~#Jo!GqakX<2YIOk*HXk9rst`KL z+HrbRO%k>QtD8%apMz8+zj4#cd98It;5fONtU#1Mgl{#2!M4!(|)r(qtAYQrYGX2 zWmu3)LNz-Kni((NxLf;Mnd|k&KYXA2wcCGj2!xm|$(F*4b)(qYU`@Sk%JC}ISCWfx6 zRtGp+2SvX!v%dTWBHag*0EK?pGfuBSjbx{sJd^L!$i1SR)y&SYbi3d z-S4pps|>6W+b%8I$exwGrkI&+EN>TO<-Goy`Fs5OKeYQkTU8{af5q&K`oEVfwtq^N zwx0dQ7`iXx+;>5dWh%YR&mO@AuFg19a&cSA*j$?@YD3d?iZm3psEJyghu&wnib8Xn zi6;{1AoF|_nS+}d{^LCFD@BE#Z`ZVEfwDsu+9PjY9->cuo_vhswGXGd-bw~Hx!&hb zfZ*5exMO4Wf|Kf0m5V;Tce`!R=DC5|(%!8@dyal-lPPzG8K6-U19Qnxdn#;rSd2q1 zoRF1Hz%-|aJZ};G>zQL2X#R{yhn*Y)@zS;dl^Y58k z=qIl6JD>X%tvBnI*Yjz<_Z1pf>kkQ~@tEcJPPd||+6=|8@5o?1Kc-zbW4djPhhC2l zs_FYb9H+`{LZg1f4X_9ZMhCB zG27-VK=?FC+c&P%t$gERk1-V;#Zmmyac4v9@|^toU{T1X8c3z^Y_)WgxAW`L7x;K_ z{XO-SOMrQdPmAvZcDIlCN;O+1SVwrkadWrSKfc!A+4OhAg1{G(;fZ7#(_)w;dl|EL zI_+9vH2K!hi~SvtPhPkfh8Z6?4E&zooAYBW%vzj3+({Jh}~@S({!k>uE$q@6xc$;V;Y6MxW-L>m8KB1pP&*Iy_iEu4@v zr#`gw%KCLRV?n(VBQ2e(PG$wun;wF&!cS=>fV3i}t-dl=Y0dA-tF0~^w@vYS=11SF zQ|#+WNKUxWwfwFW7!Gzc#-3F&|3Wx0s}SAzCgG*G4IuvV+il%Gxj7o_U@a!p9&GF$ z?vk6lDDs@p%eBl=g^oFxJ$>z#JEzJtAc#AkTzI|81~AnsC6(Xb5AlX>7}L;xH~8Q_(7TJ3)0waPPZ*70`)JJf`} z9Q&RS#{*s~YfpQ--mF=4KDv=-;eJ7CvQ>e>F#B=y5C|6KG+H`g>y-jxQ@~(jn#@3! z?nqF5?s?>93})`%Q_pr!_+%KHUhf+<>4PFV;or%68Pu0fXeNCv+e)j{4i4_fGTCGI z5e_|CD=Uo!`uPk5KjUbx=QG4vc$IvERf6D4ETI*>-GLUgD%i3TuI;Sd`R+Znrxq%~ z6gkV=k_Yews-5wLyp`KYqr929sO9UT3<#Xo_&NyZllSszr`GmaIyQfo(1OdBj1G@J zR9ilwriJ8-Q_ruX$kFkoE$ki~(9@$k7!JWuccjo%XXIyvBmOduqOtlP?Y)D0WLwwn z9jjwI>8LBVZM$Q2Y}+E-D0HcgyJFDWr#{GQuki==Ym(6=jbL7sdnQvs3Xt9 zl8BWHub3Mo(kflHcfbwf8FTWFqmooZqf3;NLrm(A-|h=-FS^OMo5gfR*AEiNiMMnL z=pbg5Fh}d|DTF>_=8-T3j-O;w;~_q1mQ0#)|ZeFWv@#|tpvs_ zWMhiDtx*bVYZh^^#6c6G>h4eF>*EL#-V2g(A_TV@DKfR_=FZEYrJqHYe8h(Aw+0j8 zsC6vY>!&59Izk;zF{Cd@;sU6FFc2T6Uh*k~T>*y;R4n|qO3gC-MOiL+HE>(Q&(}=% zfi*-aw0OFH2@tN2B;GdvF7 zXv#QuViLuT@&f7(Q~)SgUxG+?lQj538`j5(hM5G;^YAeR~$0GK5SYVSxx4v2eD6eQJ?diN3hHyfW57IV#mn3f3#OeSKu zw}UMYDS8Ch!-ZT$IVS4Ie3t~4&jNmfVqCSrrk_w|^z^mB6)7F`d3)O%mu!OA2*rk} z*%0&R_5DjxLGa)@`+B#9HO&0C5ryjIMoHXc>!tkRsh|cj`GCRs`5`R;tLG|fV^n@7 z#oe}q{rW?zaBS>L@E$%YSphcVx+%$p?Qbu>4}tK>eD#Bfp_b4pygBA}7yGw^h^bgw zFC{M;g~!=RgUjvum~?^ogZ(B*1A}tX0X=2OAsCt3Ch}rLHeWR5YQW_k090GgpQ-HX zBZ`v9Qu!ZA2|=`1g+jJ1K{hbEl}eAae2XwF(8Ts5s4d_S$X8+|iBLgKY~(T=;(3Pm z&u}^gdHrCZm1$?CD_v^O5u^an+Cv@;dMp<@zwJ$>D?rqao`4a7}GHvrqo=yiYvoa4Qq@u}(v*jeM!kJ`|=y$QxAray9r_y}?2^1TIy z_0Q@1_aTBNMPQ@@oaRiMN6AoKVA`PuaAcwwUg3K`VE;iws$p4s|E{k+Ic2&PB;W(=Wk_cw#J`2X(!;>O&c(%s`$n1Qm}XM7`n$ znIgsW|G*Y$n>}9K4>9|SD7eZV%pYwl1RfleN8vCcBQ(w1gUdCP5-ekv!!H&EjU9x1 z@QIpTbBGa7guuuL1-c6Tx_9=|SuYlb)zHy*Xfn+_LyTc?XUOkgzG3CzRV;&Zh6;jy zF#=#RkvT!Ob-Uu85e6QqC%gPeBE~qQFQ@L2-?8L;V%HBDzMtx&PiI;qTYS*pJUk_~ z;c&qqZ(b}r5AI0g^bWhY!vPH|8F27R@TM-9PoC=O=ts~qg+ zM138EWTmzTH5l#ee1%Pb?>X{b;~Ki{mHc=&;!xuix&_O3J6-)Qj`e=W>w!DSyX5iA z`>!=sCjgRN?MKcX@*zn5|Ej4n|B+coC~MiTu%UWxro17nT=-{8TJ$xp%aG;o`a%By z60ce9o{iE6TNq5_iWSiHemm)dN1mE^{){F0Gu{2Nh2Y>|wv(zGo<55FxUUj}&qqvT zG1=nfO4y>3N8sxCVNu7*h#VsX4uBK_1y2%j#q%SuxY9Grn4LoLB7sdmem3%oLH*gG zt_SPJSvB^hBYX9B3>C$sdhrZano@}5GN_gTZ7t72LlM^WK|!J^mgI4Z@oT<7(3riZ z?YPr;8Iz!=Dt$lF>Sq^slta@%mp1~bAkRm9Klh-fOI z@Cb z-*PV~HS*yT)SHk+B38_?P@*n|C|pSWfTotI0~M`BFT*pHsJD2dDpN2S?3GImSErth zJ)3=yh*L%(u}1J2U~)s87(d;kt0)*vEPBf_dA7(rM$X;0L#I#u*Igzei;?DKP$G@$ zGqd)B8+GA@^$QC{MULc_s|K`DZYLhQ0O&YjCbN=j-TJM}lOo8`niKsm?4Xqc%=0ho zEB4{!*F#n@1GZTRh#m>jX-CCI+qDToV|%s?QiByFq7=nWhi|L=nGCdvD;Xk2j>=p2 z8!g`tzIyDM*s;oB_sXD5lc%gYb#$d4=HZ$2Wj~(?nFuO4`!Alie1|rrjk0tFX~LO3 z0U3F=*k5UoKe38NU*3IecONV+MpoaJ((&wjy=1$oZGSy?ZfW-GF2tHTA#9T8E~>KT z!*=Q^SIhgxrzV`D?TinUqsf8Y&);#pkSWbkr0d(X$GDy~#KIMgByd{9#|LbmYKb$>d2QRj~MDy7Wt_;s)D0k6J6HPVbt|ZfI=vyj2Vp`4(G=1d4O+P@9 zI7iu>o6;_c_Q=`s+hhcnrE+zOr0Ck>b&XkTe9nT`984=)sMBlNVCO~-2Jpsc5LH2> zlP(WKN<^4l5BU2TJHOMV_sIt4?D}H#mjQBAj?+vYRfRGAvJZ|h7nV_vXS(|pw zgqNmrD9*sxg?vL1 zmG2SIKMtL3K(0snr%JAarg0SP zIwnr3`(;|qE{K3*0*X768_%$ByFAWzc3J9IBAka`OPFzj>)o{_x1t-%Qw+1dyjk|p zj?5Eb-0d<)qU3`2>gA0UgEF;{x6DkL0sAkmAG%4tJJQde*~RbmwKFPPb=7!XWu&?& z9$T%!@rqztFIM@6T9B5^7WSQeMS(Bk_GWLEXGa_%cBbq4Ft*x=-(2#Ki1$s{LdS8( zM>ZC2n4W4mRqXLpcI}QJh|u|^Mi|5-C+F3h)nz?T$;k?CnhNZcZl>Rsl3D~30vx^w z@7GufiNM(LJ*vrirUhz+rzlW(X`H-}X%&ig;fJ$epY1cbShwRMppQ9ZVjBAKloT|g z^4cLuvq`)M7w+0k5E()Dus6Wrk?XkHq6QJ&cueMw1m2P17DUnclF@|k_{3i5z-$wQ zg%>!fu0SW8H4_{1Lm0{gto>{cxpB@zIEGK9rYkrb#!txv2~k2zSYLi3-GV?nfp&h4 z4z%40BaAqA?66Wa_If+zMIJUoq)8HwLSRo;pD0J#E(H^3lj1K>x*1m%$1wa%lA>O7 zkBu|}hy-@_v@oXjS;#1qKvw%{TDrv7SnlYkbkYKWXw5^-Ehh4M;ZuZN%$nr{?n^(E zGsbTcXeNh5Ty~gxHZ^zvM|tbS@gCXiW?WuTzcbh;)*^TxP!2kGf{KU!W%B)5J+7nq zVyU%ez$v*_)k8`8+8({-7j){og$H|bZy4a-%al8o0yaLrRi!~H4!=a~b2szguo2{lTM*tRK+<=Co?(FF>xH5AGuT_H3nt9_oYdM(RP@pGw2 zb85uFCHE-Bs=5%W)<|CkhDEYP)h4y&=!rNhE8Ljjb@T~KeWmW}%%10g^U-qSvF5r3 z;yMe@vopw_EHzDmhr`5AUDz{ezb9JH%;rm1`SmOxF@^W2x14l5gmXv z;i%#y+qV(gR~lpum^RP6&3z5$Nkn9OopwT2;tFnW)k*ppF=q3l*QOx??Q=LMi1JA0 z?%9kQh+$2e8!WWmS7pi95||8gxOH5doEd7sIqjKwi{%pLDV1~80C7G-AiO|@*$*TJ z5+Hb&i$vB(orkvPz{632dL&yglH?wh52D=*2rV1&-gKO(i1q? zzaynAM?N^R6352F2SWrnST^^<9L|ZZei#2cC_32DwuoaS924KVAQBi?gdi1d0Q^jI zBQEw5Rx>&YEc*j?x7CNj7=BR;h5bb4lb$^nSZdEA*qYC5fmJ9wSHLM_{db$*-e}SD ztE)9O5yZ4B%ufX;bM4Kz_(Wlz6Fx8Wjd*Jn1(PB`0!nK3>K)y;en7CQl}zv}vf-CE zfRHyrAP(Q7dJwRXz3t+D&3>*ZgVn)xGN5gGKp_bMPcyQKntoO!PymcL1YzeXv`!!+ z2*cxpGHeg)j^z4qWFSL*V>CjjgcWM5F78VVZHeCQik%B#6jcZx5r4KSrGzCIw$de} zJ>mkyXX*z&t~EgMPp{NlkQJnFktP)j>YvyJc|3sYL33{P#C+I7@v?htVRa`Sdb=U3 zZhcB3fjn}Gk5Q&9zEMER=t|arWy+I#2+b3l^Bwu4xgHJh)uxyWr&Jbp%MbYGgBfK9 z8(`ZwxgmA7akKz`_6usU!TQNx?a5ndndXnkg)ZF}-9qG)M*}qg3eARoI|T`6RnUzE z1)lpEbeL`}Tu%jxHBbH=5scsVvxu**A9J>7&Bi=pggN1naLg4^mj9{`IAjG;6sl2= zCJ`bYZhFY<@)$1OXAmG=et3e2`W^ChQIt|hhzuL4UC*cQWL_yP*g{Y?jmWU|CN3Bc0 z4Uef*MCmK-Zy}lL|GCFZeSj#cBItnt3W_tNzv+PWiiOVL^WLYB%xGj{h!S>Uf(ijf z0q1J|ZjCEsA2NS2%cS%PNkWL|d*2pBPP>a0m_eY=tTj+r%Al=!F)Q%j%HtYXi=Mu2 zg}g$6q1HgT0sG}+0+b;g;!)Y+Td-*E)Q9yxL}&z)0xIP$9PK^xx#LlRvW-$oGfa6m zI8H&xq0t?3uVj#u-gOER8x$Kf67Sa1720@!o7Q_ScF~|wgqOP6rthNn*9I|pTD!6A z#}AhVFU|!If&1Z{SY1EJ*x%2O49q2Eu7zZOK5p(nKD}_hy>Pz1C$DrB+eYv-o$FBm zNgnhS6L@OZV$C;sAo%94aOPQ?-MeMlwGnN>KFO%wUW{D$W@oiYCkYt4KzC?>u8$VR zd>^kTIX(oaN&QL_vrby@Jr={3S$;p3V*>|3-qD$kV(UH6LU4rI@ImnX`h0ozZ6KLkq20kPY zO!cJ)(9BF!`_O6?rG>f|0`k;edct&^1^%Q(+a*lUN(0a>gte>Va7wL$%y<#WdkZ#c z88=&BBThRH>9mee0J4Qf4&c&X;*`$4jq2bPTF96~z$g8`3Vh{m0u1n{erZm~njBno zJV)a=8y`9x7^~DZ07n}cCMD$y&{m_;_HwQhuaSDX?_QXp1jQh%7!2kw?&1_FPI<+S zm}-8$WF`J#!N1A!1@?qJ=dC#qWu~H$J%?LpLjQbdKL&?gS7=gm0($(6`%CAlB^f)PoG1cR8OoeSZ zSQab=>Kt%zV+{`2sh;zxz+6mvdQWN*!X*a8=2p$}I4SPfF1zB^#4j5HgJI<6!tsq{ z%rhbj!_ZciboQ(B0cVc>sXk5U!InxWO!EbEsMrAz;($-M)2tn4KDjpisfu#4;)8(u zdcXc~qnjmX6J^p`>0Rz>KSNeVlQmNu{2-XdW;;bc{dhH^lr>yb z&xMXb!Sf$C6b$PTP%$?)`GAE!X{2Ng^Vi1l&Vz=dV4nvSvc#to#y{?@NN*7)FHJnE z*i0i~7CzL{Q>laX&G?VLqj0uXNS5km-MbbccP=cpk#1qDbxuAr4J4qE7vweo8)w}w z8+#hn*G@jiCrn7ofHA~Bm&?L@=Q&l>A&Mb}3h?7Ojl)g;W0UmpURI(OamY`#tp5FP zU~?a@*T>@g^FBg}#DJrCPV2}jS|^{+>UioT+m-i=F^;Kb$Sk#%Ybj0@(X~v%ugA4n z*)Fde7ux5McUQk{2sE}^Zq)uz9%U5447Us@brF=_8eRMf-SYj68n=4Cdu@Yzy{)JL z#qh2?*P+X-+Qw6{St1qxkztBzCUb`60D@heNQWdL*}qHb~&= zW^%%z+w+{~T(G0Os^doKZ{Ss8M^-gXwMv}%_5 zG+KgE_ayK_MB}_fRJqLaUX0o)oD*nQCt11K1!z%XIQPZI%W_u%5@QjuIp?s|>ugL0fC(QJB&h`^;bWc0 zR(Tg$(g0ZUqAv4$mg8xgU&*~;@rg8@?gm>sl7BS`S{+}9a)`dKwPkT1*#VY7$9>m( zo;}LV4J47E}0jU|ub3qG@NWvgf& z2&icAuKnCVDs>zd;?2K{fSulchA)1UvSU7G#Snfx|HnA|_n9AmR5FJq$z!7<5M%M_5 z1mqWrzXs@ziOu^g{6yZ|RUQlS`8C#-uzybM+E(jvE7qx{w(&M>doA|%uWJ;{uLJEl ze_W7;4|O!s|6CMjQ!4{&XZt^@_dgWJtXAm|+cA!Kj|LU*8>w+({pMuWK%n)6tY$$# zU_=TnbCJoT?})uON$-o-z_{<_2w>08Xhu>pBC95pOz}daOUTaM{Q%I?;a-vkVx=LfFx9#WlN~~y}tq`NPvwgh+ zu=V3{`o*gP+8#z#QPaIM5-`)9mjSNG8X8CY*0^OP2Tf9nYajix)dM4?xFDm~#Gfd@a{&qe$xn5i>#szMgO8 zpJwfUg5iM*_Up_YLTuJK8)wEiN!oycFEmELylW@Er1Jm`pLPs%Bt(I*UaF|Z9%%@x z&oS%1h}dNpk^U*Ijz}s^xsZPe6_!xZZrq79Mh2T!8x!KxG?o>XH_0IH-UT#r@sYhu zvXt)_Evdx3Z{7P_&FqhtUctAyG+m>sM!&@MHFtz3y1K$P$M3v;$MA2n1am7x2w$}L zFY!BcGWJoJvi2!^oIz9;sKGm*Jev#_{MD*8Z7OdLR&frE*2_WGAcID^S)p!DhlYZS z&W12v0Q+iba>;OuFO}m^B4&?N;JkAsMcj;FG&AZfmW}+rK^tGsS)J=#t;<+4T`riy z3Nv7TCD?_Z-k-1cbtz8#O0%oDHhBJ5;b}=I+Tb$`5RexS5D@zRv{DBXLrX*2Kezv> zLE|i2t&ZMz@cr_g;?%zgN80x*JMYJ7&mZ1Z%_Y%R%$HDz@(h1L66P&B+Po|9o@VS5 zX51TApc;_2ItM~Bb~*U5Jed-B>Ft{SrB^T>!!{8f@`lrtQ0N$dGj#00*4zOtY)*(u zDlr5YnK5B=2@C6?C3t@=`gv5HE_^HIb8TuH66CL@MgxxkABNGuM zlOl^S$o>rDaya5kBtF=KJ8;Y=HjO)_Ek$+>K(?YA?Foi(jpk#d{0PHM09bnmSrQ+TM%6!{wJ8K5|)< z%*xU_R?VS*Fy<>vxt20~NRtmO5)S;#z6aaXd7-C+`wl37 zLW@Lf2m!}0=1Z@t6i^mbD-sPjDoz`rzDBq$5f~v>SYOil8bQc+O~s0qmbR?IT`jfa z5|}o*b0I={2FU9+{DRI)%1*;`aB>6_$CFx?A|np%Zn^2GtOzIag8du zUhiWGU!#dpU>9462UU2bRW@o4>N>|3?B5RD;=ZNsfA-P60%NeEh?fmF@e`3tcL#)C z-e3j;l0V5?msZM8pY}2eRKyK^DvpoJxIG01zsd4`f7|@^EXv&Y$wd$}lbJc;$`-v~ zcDK0D7whhNhTRxK>r;}b8hg0tD^`@rR-Pg9B)iQ-!tRU>eEer<$O|@`sz%>7b|)DV zHrlZvdgO_`D%|{@ae7dX_Wt?BcRBE_j%CzWgMtAocN(6dR3 zw#adW|Lttc$$nBP8#vO7IOfio`MS8Xx?^)!=2p2C%p zeCE0+Qt%G#moE{jDhv0d=$feZq;~c@mGmRKF7RfqJ4jF~Mh5l|YBqHNLk$ETMxT+y zVvJ2#d&r3giR-DR(}QmY**Ocqy~0MZz7L<$s^G(6pc=y0iHehf?G^)E^l2(P>Gr(* zhPzc>mXDJdv0uK#CtY(R9G2MI@(VsMcd!cHf+IXU@4Y4mag$%vQStX0D&KTZ(x^hR zkIj%D0T-ChzdezBoAX4O%mZmFu}K)0AFU}}jUx`qPf%JPoLy!(gk^`J!F=iPk{|;R z4Sx<{%yGdlvbIf;ApO`F>@_vNq9+~0oKn-U8d_;MT=3G&=3r7>Y&hkXxy72QRU2Bl zXBNA5QgXB&ha;DpcT71`2|Ib8oeMcZ`<9)V&GQ9tZdL~*_lw(U@sP?>CxbAYvjxa- zr7kBFvA}G@fetbqWxU?&7qS!0LdB72!$Oi3-`QkgC;4_-tj31GzD zvXzf?6|rpR2fK%j%#+9_JF8?Cg)Qg9YTuM@7+}e-Zk?x+x~XgR1Xli(w%}HdGe@aU zuLrhzf)SB_S|~2TX&DQn6AY@Ma_37Yuq88~v#r|+G)q2XDtsjJsYhUC&=pgQ_9pjE zp1uq=puMz$Q=GoHLvFBUT_xqG;XxOkX{kl0lp33t1JrD2%%XzXYjS_$;-XR#lircH z=1e!Uno0y2O2|~by#lu?n-YqE7b)t62E*%jDA=7`%*xx1b<{F`a!reEvj)Xkv0Y*7!zhcI6z~bsLuXa@#qlY+4YtrY`Kj=fq1G)-w4v~5EXa$f+gl<2fSWC5_ zIl6~C2tH_2gswpGu>z!)f?U>`_KwTRHf+<70`MN}Ne61U)fhE^nS_OEFdTPN_ZTM8 z5e4PcMA$pyBT<+h#tF92I)i5trROX4X<0BccUM?kg7=cYano?Mv|5I>>OBVKViz8o zHPrDH;KuB)WCko!I?Ap>hd`}9t)Qz?#yeIkg|SXcu;puWYYlbvY}EMC_V+ztv%cNcPD3OFxH@y_jKT!{%aewV+Syq z7~YZJBwvSuw9_Z-*FI`FRX(Hb%W2z8`bO;WmT56nzN)mcGX)N!78x8;nR4>WR*%Z( z(4o3c;>v00egDm~+KJ_T0Z-y0F5}?GL($N(i-JC#MH(3F`-(svGp)~!+MOYug!$}G zAq@no=@~dJXxuUSAK8`gtFjKmB#!N?KM#3Ptv;u23X3CSFuu-zrg;+!0b+oP_W86K zcfD%^goa)Dka}n)SZumot;Vb3=N<@_`1;+rWK*upKn%WU3d?eg@b+X|m4^1*Bv>jF` z-Nh$o^xW17WH^bCD=BCs^$Nt1q~brm*-JUmRfo9PdRr-V#^I&n{0<`hY(1SnB{dIdy_iv zsA4XcCa;B_d@g$_7#8N67=;^e-;$>_D(8beTYu>Iwn{D zP_Qo9b)PZjGW>`6BQ^h;M`65LPr31i{sLVYvMI*)CDk6Ut=L%a5beOF_#kf#m^bTq z#+_8_<$41;A{wHF5)WA40Rj@OTzGf3q4xh`b-e%=>QhMy~B9c z_ni60w&8NgxrVoI^oYcHKM={V+nv>fR|?R7nV0W^(=gIwHKy}K#uv*xBg|$%(y<6F8B?SXB@sx#t2GFnB4(mZ5P_y_b zUwv!kSaq2H*2b~wbIYp)JQ$cD7~8Gsf>BjDUOgR0Ca9B8ukSaV`HhO@TGNOw)y3di zjqg19=g;fGNSt`xiq6gr0|t3y%{C`$(_3iY-Q8V(fBzCYG0w`uaUB=L0pFsw(~8PV z3e@zAK6m%l2{{H?oPx<$D3K}tuZlLcI#nuTOE&GM6|thm8tP7~ZfnQ@EkvtwSXBzp z0#aQ`!|ED_5c5|dLS(QqiF74W{!d~3?zU###bd0sQ&Hc*Fm(6ul(?uSF%kACbHWZ` zSQAl+cH_MXF4tP(%%%igR&3FBOjdjYWBIb^gB5^;N|N@Eo(?nU(Zkg$l%JZ=I+oN1 z4#JUUFx*U?MSbj<>b^wP!oN&SA+|#WbVs8Jtn@!Kf_$Dsxx;($O@eH!l$JuoNp$gE z0i^|Dch?^x-~850Yq`xRmCd+2RRB!b-1U~8UC>&KM(HWt6%iF>B*rgW$e1^|>x7+D zAb`@_vvlF8$5e>H)2Et&B}(Mo;JUt+v);XJh!4e;C}9_fxkWWFN04Mmp$xGG4=akL zFco+FBcwOr6t0i76k?DmJ?bygYyIv&Rx)$Ga z`W^>mi8lO7DG3MV?)$AO^YF2n!mgX=yb<%d;6~KQy`2*PKaS4o?OIa?IEEcFsY9`p znz+L|O#f3sl^&0syGPhidSjHmnOX9h`_ql#0C;yKhWlh0xj&CRyXe!92+=)m={q|^;a0kf4Jpjoll`4qw@}8)uzu=Px|45Nafh9y z;a*Aw%awad)!e`DrUhw%;`^HrhywqK&hW_kPHF(nW zi(r|#wc}||hER=^*z)AY;0tDF_DP(?(aCcYvFlxhM$>S0D0@w1Wo2$|?)v2|-f}oA z(xTHu+Psf{IU#Zp$IMD4pAykmu_#*CCZFUf1ZY)yt2;`1*ReeEc zj4&S22|yDx6&Z`~$k~2jgfwBXYg%_RR(`YLr((RK*O!ndz$J@s{5FXE$q=sF`%@~x zKtZYz>S7SX&pDGJkXZY8W|WK6MEm}_oHZ@JTjH2~STYOO5Z&*BHKChGKGX;Yj|^q& z_>8N19wBg?gt>?iKaBMaz5`)?5}OE>WZ(K|HDrkGwo1Q@u2m*B zSJDkfGul&~f@W2>9muQ=5N|1904ckn;E}7Q{&1Z}qJjYem(lsWu1e2%SLD*OXbQq zF;B#g#unedynkNRhyL01QHA{9nb-IlJyz6*u@wA=E*=R83iy9AFa3|@82+fHf2{ip z49Euh_{jgiAMvq0GP`t$L6>4L@G+Mqaf#(i#nK3EJN05>sJ_iu?ofqMmdlC)n_flm z;u@yxHn%@sZw(J7sTK(`Mm9bRou7pz)cF~HD^Of%q;%?PnW$1lmscf?sH@g<53yK( zZB~`gk;K?yVM73B?vD^-S(vF>qRg#g|9*qUfn=Ot{nf)Z2SrTRqy@u{Qhn<-_i==i zF9!_93+TX$s8%*V7G>trPaKGp9rLJLZ*RYRPn25VXvZ5zVP9z3bj^L`*nUm1F;>u4 z!CQzM`lU1LKAZ_+T<2yet;^YTUUSu71zEN3YpqBmL@FsS&oP?wq&1?)b=sEJcyZ+}&-gv)Pxn0;WDzQad9;12p!B6Lr3G##o?yju^f`4tqq1@1NXdgpD2_H@m zi2rQFx;8fduMPj*Z_~d@e_x?P>`*!J4zUa7a}K94s9ovJ<)#8L1uYVrw_pHn3bkCG zg?c{al#zg#b5tl(W5bW?1yU-sFL|i5Nuz?y>g`4<#+9hB zLC>5DM7Vx&*3)m`mqga;MjRdQ1&v7&i5Y`e^~JH%uq~Qu1lgIpAl5=<=nI_aR4E2L z(_u>!aUqxD-ufN`^`YCU&;If|;JAHh(XwM`x1mBVmx2zs_DHoew$6b`tQAVCG%#wI z1qVMhYMV2YHpTh=#gtktg>W{kDc^yswM`P^`^b&qn;~a89-iR9fo+cBZwMM-HmDzB|}ky*(t*y3{qWkrx{rycMKU{w%7Df07PI1MpNDj;ez$mR zsxj|kxOQQ{y{>XaK1>av*^GPq@~>S%FoBr-i z?Qyc!esq|Fmy!>7VwYmTrvX3^;3#pXKL)wZ^bo^Xa;Qzq($pT-RctX4I4zBuI=ydR zGrzdvL~NK^;#Nlo)FCc$L&5%l(ME2B>ycz(4(^VoFgDb-4-&NLM~=vSY5PX*;vYz) zimmQ9f_N5h<}nsK06(kwaF={6J^((RKr^tgqa+NIex)Z%ub2qB-A(bJU~9&L!EUo~ zi*V8uOG+zqu}8tlvSNkm5l#>pMxhY2Ma&;Z!jG6Db0RH;v!Xw1-vNmN|Jh-1!r)}! z_zPX?Y%M5kq9UGWhx$aU<7;w(Y&e@F#xExHLKL^cPmDNE!}oZ1E7&a-nrSsMO)l@`(|_9$1A|Zl zp#nhweK-&UeFk#R?~RE4Xr>S!mYFDj|4RPvo`#^cmBYu<>N;OtZ4B+T{%EvvlK%}T zQtfRZ{QzixwDXSv+&Mair)i&U(EgQ$j=|g^1qjM|2_Wq6*2yf$0hnJ z{x9Fte>k50j``h@`ESfFFM9ZZ}fi=e;301ss8v^+V5tff76s1{-XVLeE)g=b9nyu z)BN3(?{AC}>%U|EwCMXC^Sde4- @@ -133,6 +133,21 @@ Check the relevant fields of the `RobotStatus` messages to determine overall con The `reset_error` service can be used to attempt to reset errors and alarms +### start_rt_mode + +Type: [motoros2_interfaces/srv/StartRtMode](https://github.com/yaskawa-global/motoros2_interfaces/srv/StartRtMode.srv) + +Attempts to enable servo drives, activate the real-time UDP server, and set the job-cycle mode to allow execution of INIT_ROS. +This allows the user to send incremental motion to the robot at the rate returned by the service (`period`). + +See [R/T Motion Control](rt_control.md) for information on the protocol implementation. + +Note: this service may fail if controller state prevents it from transitioning to trajectory mode. +Inspect the `result_code` to determine the cause. +Check the relevant fields of the `RobotStatus` messages to determine overall controller status. + +The `reset_error` service can be used to attempt to reset errors and alarms + ### stop_traj_mode Type: [std_srvs/srv/Trigger](https://github.com/ros2/common_interfaces/blob/37ebe90cbfa91bcdaf69d6ed39c08859c4c3bcd4/std_srvs/srv/Trigger.srv) diff --git a/doc/rt_control.md b/doc/rt_control.md new file mode 100644 index 00000000..ff3b02cf --- /dev/null +++ b/doc/rt_control.md @@ -0,0 +1,140 @@ + + +# R/T Motion Control + +The real-time motion control server is intended to be used in a closed loop system. +It allows the user to command incremental offsets at the rate of the robot controller's interpolation clock. +This control mode minimizes overhead as much as possible by routing the user commands directly to the MotoPlus motion API. + +## Activation + +This control mode is activated using the [StartRtMode](ros_api.md#start_rt_mode) service. +The user must specify the `control_mode` to indicate whether the increments will be joint offsets (radians) or cartesian TCP offsets (meters / radians). + +If this service is successful, it will return a `result_code` of `Ready (1)`. +Otherwise, please examine the `result_code` and `message` for more information. + +The service will also return a `period` in milliseconds. +This indicates the rate at which increment commands will be expected by the robot. +The default `period` for a single robot arm is 4 milliseconds. +However, that value will increase as additional axes or arms are added to the system. + +## Usage + +### Command Flow + +Once activated, a UDP server will listen on port `8889` (default). +The user then sends the first increment with a `sequenceId = 0`. +After that, the user must wait until the robot replies before sending the next increment. +Each subsequent command must increment the `sequenceId`. Additionally, each subsequent command must not be sent until the robot replies to the previous command. +This will occur at the rate of the `period` from the [StartRtMode](ros_api.md#start_rt_mode) service. + +If a command is not received with 30 seconds (default), then the session times out and is dropped. +At that point, the server must be reactivated by calling `stop_traj_mode` and `start_rt_mode`. +A "keep-alive" can be used by sending a command with zero increments. + +Command Flow + +### Data format (command) + +The command packet is a *packed* `RtPacket` structure. +This contains a sequence ID, the increments for each control group, and the tool number to use for each control group. + +``` +//########################################################################## +// !All data is little-endian! +//########################################################################## +struct RtPacket +{ + UINT32 sequenceId; + double delta[MAX_GROUPS][MAX_AXES]; //[8][8] + int toolIndex[MAX_GROUPS]; //[8] +} +``` + +#### Joints + +When the `control_mode` is `JOINT_ANGLES (1)`, the order of the joints in the `delta` array must be in the order of `S L U R B T E 8`. +Please note that for seven axis robots, the `E` joint is phyically mounted in the middle of the arm. +But it must be sent at the end of the joint array. + +See `JointIndeces` enum. + +``` +enum JointIndeces +{ + Joint_S = 0, //radians + Joint_L, + Joint_U, + Joint_R, + Joint_B, + Joint_T, + Joint_E, + Joint_8, + + MAX_JOINTS +} +``` + +#### Cartesian + +When the `control_mode` is `CARTESIAN (2)`, the order of the joints in the `delta` array must be in order of `X Y Z Rx Ry Rz Re 8`. + +See `CartesianIndeces` enum. + +``` +enum CartesianIndeces +{ + TCP_X = 0, //meters + TCP_Y, + TCP_Z, + + TCP_Rx, //radians + TCP_Ry, + TCP_Rz, + TCP_Re, + + TCP_8, //pulse + + MAX_AXES +} +``` + +Please note that rotations are applied in the order of `Z Y X`. + +### Data format (reply) + +The command packet is a *packed* `RtReply` structure. +This will echo the sequence ID, provide feedback position, and provide command position. + +Additionally, there is a flag to indicate if the Functional Safety Unit (FSU) reduced the speed of the **previous** command cycle. +This indicates that the robot did not complete the full increment as commanded. + +``` +//########################################################################## +// !All data is little-endian! +//########################################################################## +struct RtReply +{ + UINT32 sequenceEcho; + + double feedbackPositionJoints[MAX_GROUPS][MAX_JOINTS]; //[8][8] + double feedbackPositionCartesian[MAX_GROUPS][MAX_JOINTS]; //[8][8] + + double previousCommandPositionJoints[MAX_GROUPS][MAX_AXES]; //[8][8] + double previousCommandPositionCartesian[MAX_GROUPS][MAX_AXES]; //[8][8] + + bool fsuInterferenceDetected; +} +``` + +## Deactivation + +Other motion modes may not be used at the same time as the real-time motion control server. +By calling `stop_traj_mode`, the r/t server will be disposed. +At that time, another motion mode may be used. \ No newline at end of file diff --git a/doc/troubleshooting.md b/doc/troubleshooting.md index e2506cde..5c778f24 100644 --- a/doc/troubleshooting.md +++ b/doc/troubleshooting.md @@ -582,7 +582,7 @@ The name must not be blank. After correcting the configuration, the [changes will need to be propagated to the Yaskawa controller](../README.md#updating-the-configuration). -### Alarm: 8011[23 - 54] +### Alarm: 8011[23 - 54] or `[56 - 58]` or `[65 - 66]` *Example:* @@ -611,7 +611,7 @@ ALARM 8011 [xx] ``` -Where `[xx]` is a subcode in the ranges `[23 - 54]` or `[56 - 58]`. +Where `[xx]` is a subcode in the ranges `[23 - 54]` or `[56 - 58]` or `[65 - 66]`. *Solution:* These alarms are often caused by version incompatibilities between ROS 2 (on the client PC), micro-ROS (as part of MotoROS2) and/or the micro-ROS Agent. diff --git a/src/MotoROS2_AllControllers.vcxproj b/src/MotoROS2_AllControllers.vcxproj index 7fbd6120..63237c51 100644 --- a/src/MotoROS2_AllControllers.vcxproj +++ b/src/MotoROS2_AllControllers.vcxproj @@ -407,6 +407,7 @@ + diff --git a/src/MotoROS2_AllControllers.vcxproj.filters b/src/MotoROS2_AllControllers.vcxproj.filters index 1dabcab8..d0c4ba62 100644 --- a/src/MotoROS2_AllControllers.vcxproj.filters +++ b/src/MotoROS2_AllControllers.vcxproj.filters @@ -277,6 +277,9 @@ MotoPlus Libraries\micro-ROS + + Docs\doc + diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 3b68132e..c7d4c6f1 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -48,7 +48,7 @@ typedef enum TCP_Y, TCP_Z, - TCP_Rx, //0.0001 degrees + TCP_Rx, //radians TCP_Ry, TCP_Rz, TCP_Re, From 4e3cbce577a298a7799f00e5553582780eedc0d4 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Tue, 30 Sep 2025 12:46:04 -0400 Subject: [PATCH 045/101] Satisfy REUSE and md_lint --- .reuse/dep5 | 4 ++++ CHANGELOG.md | 2 +- README.md | 4 ++-- doc/rt_control.md | 11 ++++++----- doc/troubleshooting.md | 4 ++-- src/MotoROS2_AllControllers.vcxproj | 2 +- 6 files changed, 16 insertions(+), 11 deletions(-) diff --git a/.reuse/dep5 b/.reuse/dep5 index 9de29b9c..e1345077 100644 --- a/.reuse/dep5 +++ b/.reuse/dep5 @@ -46,3 +46,7 @@ License: CC0-1.0 Files: doc/img/logo.png Copyright: 2023 Yaskawa America, Inc. License: CC-BY-NC-ND-4.0 + +Files: doc/img/RtFlow.png doc/img/RtFlow.vsdx +Copyright: 2023 Yaskawa America, Inc. +License: CC-BY-NC-ND-4.0 diff --git a/CHANGELOG.md b/CHANGELOG.md index 51825262..45c2b174 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,7 +13,7 @@ MotoROS2 is now built against `micro_ros_motoplus` version TODO New functionality: -- Add new motion mode for real-time control of the robot. This pipes the user commands directly to the motion API with minimal overhead. ([#449]https://github.com/Yaskawa-Global/motoros2/pull/449) +- Add new motion mode for real-time control of the robot. This pipes the user commands directly to the motion API with minimal overhead. ([#449](https://github.com/Yaskawa-Global/motoros2/pull/449)) ## 0.2.1 (2025-06-26) diff --git a/README.md b/README.md index 5376e55c..f8c5eaae 100644 --- a/README.md +++ b/README.md @@ -608,7 +608,7 @@ Instead, write a `FollowJointTrajectory` action *client* script or use a motion There are three methods of commanding motion using MotoROS2. `FollowJointTrajectory` action server, point streaming, and real-time incremental control. -#### - [FollowJointTrajectory](doc/ros_api.md#follow_joint_trajectory) action server. +#### - [FollowJointTrajectory](doc/ros_api.md#follow_joint_trajectory) action server The ROS API of MotoROS2 for commanding motion is similar to that of motoman_driver (with MotoROS1), and client applications are recommended to implement a similar flow of control to keep track of the state of the robot before, during and after trajectory and motion execution. @@ -642,7 +642,7 @@ Otherwise call the [stop_traj_mode](doc/ros_api.md#stop_traj_mode) service to ex Interaction with the *point streaming* interface (MotoROS2 `0.0.15` and newer) would be similar to the process above, although no `FollowJointTrajectory` action client would be created, no goals would be submitted and monitoring robot status would be done purely by subscribing to the [robot_status](doc/ros_api.md#robot_status) topic (instead of relying on an action client to report trajectory execution status). Rather than submitting a complete trajectory in a single goal, an indefinite number of points are submitted to the robot one at a time. -The execution of the robot will be identical to the behavior of `FollowJointTrajectory`. +The execution of the robot will be identical to the behavior of `FollowJointTrajectory`. #### - [StartRtMode](doc/ros_api.md#start_rt_mode) real-time incremental motion diff --git a/doc/rt_control.md b/doc/rt_control.md index ff3b02cf..7b1232c8 100644 --- a/doc/rt_control.md +++ b/doc/rt_control.md @@ -45,7 +45,7 @@ A "keep-alive" can be used by sending a command with zero increments. The command packet is a *packed* `RtPacket` structure. This contains a sequence ID, the increments for each control group, and the tool number to use for each control group. -``` +```c //########################################################################## // !All data is little-endian! //########################################################################## @@ -65,7 +65,7 @@ But it must be sent at the end of the joint array. See `JointIndeces` enum. -``` +```c enum JointIndeces { Joint_S = 0, //radians @@ -87,7 +87,7 @@ When the `control_mode` is `CARTESIAN (2)`, the order of the joints in the `delt See `CartesianIndeces` enum. -``` +```c enum CartesianIndeces { TCP_X = 0, //meters @@ -115,7 +115,7 @@ This will echo the sequence ID, provide feedback position, and provide command p Additionally, there is a flag to indicate if the Functional Safety Unit (FSU) reduced the speed of the **previous** command cycle. This indicates that the robot did not complete the full increment as commanded. -``` +```c //########################################################################## // !All data is little-endian! //########################################################################## @@ -137,4 +137,5 @@ struct RtReply Other motion modes may not be used at the same time as the real-time motion control server. By calling `stop_traj_mode`, the r/t server will be disposed. -At that time, another motion mode may be used. \ No newline at end of file +At that time, another motion mode may be used. + diff --git a/doc/troubleshooting.md b/doc/troubleshooting.md index 5c778f24..fe36722a 100644 --- a/doc/troubleshooting.md +++ b/doc/troubleshooting.md @@ -582,7 +582,7 @@ The name must not be blank. After correcting the configuration, the [changes will need to be propagated to the Yaskawa controller](../README.md#updating-the-configuration). -### Alarm: 8011[23 - 54] or `[56 - 58]` or `[65 - 66]` +### Alarm: 8011[23 - 54] or [56 - 58] or [65 - 66] *Example:* @@ -662,7 +662,7 @@ After correcting the configuration, the [changes will need to be propagated to t ### Alarm: 8011[56 - 58] -Please refer to [Alarm: 8011[23 - 54]](#alarm-801123---54). +Please refer to [Alarm: 8011[23 - 54]](#alarm-801123---54-or-56---58-or-65---66). ### Alarm: 8011[59] diff --git a/src/MotoROS2_AllControllers.vcxproj b/src/MotoROS2_AllControllers.vcxproj index 63237c51..8f134b57 100644 --- a/src/MotoROS2_AllControllers.vcxproj +++ b/src/MotoROS2_AllControllers.vcxproj @@ -368,6 +368,7 @@ + @@ -407,7 +408,6 @@ - From 1c717f3452fa75dc7b60468c89abfe9633614a2d Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Tue, 30 Sep 2025 12:48:03 -0400 Subject: [PATCH 046/101] Whitespace --- doc/rt_control.md | 1 - 1 file changed, 1 deletion(-) diff --git a/doc/rt_control.md b/doc/rt_control.md index 7b1232c8..51adb0ae 100644 --- a/doc/rt_control.md +++ b/doc/rt_control.md @@ -138,4 +138,3 @@ struct RtReply Other motion modes may not be used at the same time as the real-time motion control server. By calling `stop_traj_mode`, the r/t server will be disposed. At that time, another motion mode may be used. - From 3c8a1f5e2c6a8f638bf42ebbd1e7d8e2b3c973f8 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 2 Oct 2025 12:04:34 -0400 Subject: [PATCH 047/101] Spelling --- doc/rt_control.md | 8 ++++---- src/RealTimeMotionControl.h | 12 ++++++------ 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/doc/rt_control.md b/doc/rt_control.md index 51adb0ae..23c24b7f 100644 --- a/doc/rt_control.md +++ b/doc/rt_control.md @@ -63,10 +63,10 @@ When the `control_mode` is `JOINT_ANGLES (1)`, the order of the joints in the `d Please note that for seven axis robots, the `E` joint is phyically mounted in the middle of the arm. But it must be sent at the end of the joint array. -See `JointIndeces` enum. +See `JointIndices` enum. ```c -enum JointIndeces +enum JointIndices { Joint_S = 0, //radians Joint_L, @@ -85,10 +85,10 @@ enum JointIndeces When the `control_mode` is `CARTESIAN (2)`, the order of the joints in the `delta` array must be in order of `X Y Z Rx Ry Rz Re 8`. -See `CartesianIndeces` enum. +See `CartesianIndices` enum. ```c -enum CartesianIndeces +enum CartesianIndices { TCP_X = 0, //meters TCP_Y, diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index c7d4c6f1..7f7ebada 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -26,7 +26,7 @@ typedef enum Group_8, MAX_GROUPS -} GroupIndeces; +} GroupIndices; typedef enum { @@ -40,7 +40,7 @@ typedef enum Joint_8, MAX_JOINTS -} JointIndeces; +} JointIndices; typedef enum { @@ -56,7 +56,7 @@ typedef enum TCP_8, //pulse MAX_AXES //maxies -} CartesianIndeces; +} CartesianIndices; //########################################################################## @@ -71,13 +71,13 @@ struct RtPacket_ //The order of the joints must be in the order of [S L U R B T E 8]. //Please note that for seven axis robots, the 'E' joint is phyically //mounted in the middle of the arm. But it must be sent at the end - //of the joint array. See JointIndeces enum. + //of the joint array. See JointIndices enum. // //For joint-space, this will be radians of each joint. // //For cartesian, this will be meters and radians of the TCP. //The order of the joints must be in the order of [X Y Z Rx Ry Rz Re 8]. - //See CartesianIndeces enum. + //See CartesianIndices enum. //Rotations are applied in the order of ZYX. double delta[MAX_GROUPS][MP_GRP_AXES_NUM]; @@ -102,7 +102,7 @@ struct RtReply_ //This is indicative of where the robot is physically located. //Please note that this will trail behind the commanded position. //The joint ordering will match that of the original command - //packet. See JointIndeces and CartesianIndeces enums. + //packet. See JointIndices and CartesianIndices enums. double feedbackPositionJoints[MAX_GROUPS][MP_GRP_AXES_NUM]; double feedbackPositionCartesian[MAX_GROUPS][MP_GRP_AXES_NUM]; From be07e285e7015cd86716d2956c52a6a0d5a39ac6 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 2 Oct 2025 12:28:08 -0400 Subject: [PATCH 048/101] Report robot's configuration to the invoker - timeout_for_rt_msg - max_sequence_diff_for_rt_msg --- config/motoros2_config.yaml | 2 +- src/ServiceStartRtMode.c | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/config/motoros2_config.yaml b/config/motoros2_config.yaml index bfb4a351..2342d5ad 100644 --- a/config/motoros2_config.yaml +++ b/config/motoros2_config.yaml @@ -339,7 +339,7 @@ publisher_qos: #----------------------------------------------------------------------------- # When using the real time motion interface, each command packet must increment # the sequence ID. If too many packets are lost during communication, then it -# should be assumed that the PC is not in sync with the robot. +# will be assumed that the PC is not in sync with the robot. # # If the sequence ID of an incoming packet is different from the previous # command by a value greater than this, then the connection will be dropped. diff --git a/src/ServiceStartRtMode.c b/src/ServiceStartRtMode.c index d3a9cfda..f26c124b 100644 --- a/src/ServiceStartRtMode.c +++ b/src/ServiceStartRtMode.c @@ -54,6 +54,8 @@ void Ros_ServiceStartRtMode_Trigger(const void* request_msg, void* response_msg) response->result_code.value = MOTION_READY; rosidl_runtime_c__String__assign(&response->message, ""); response->period = g_Ros_Controller.interpolPeriod; + response->timeout_for_rt_msg = g_nodeConfigSettings.timeout_for_rt_msg; + response->max_sequence_diff_for_rt_msg = g_nodeConfigSettings.max_sequence_diff_for_rt_msg; response->result_code.value = Ros_MotionControl_StartMotionMode(mm, &response->message); if (response->result_code.value != MOTION_READY) From a47be777ddd31bc976e15881092120143e7fbc2b Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 2 Oct 2025 12:32:56 -0400 Subject: [PATCH 049/101] incoming increment can be negative --- src/RealTimeMotionControl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index b446bd59..d86c01a3 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -266,7 +266,7 @@ bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DAT { moveData->grp_pos_info[groupNo].pos[i] = pulse_increments[i]; - if (pulse_increments[i] > ctrlGroup->maxInc.maxIncrement[i]) + if (abs(pulse_increments[i]) > ctrlGroup->maxInc.maxIncrement[i]) { Ros_Debug_BroadcastMsg("ERROR: The increment for axis [%d] exceeds the maximum limit of [%d] pulse counts", pulse_increments[i], ctrlGroup->maxInc.maxIncrement[i]); return false; From 4941a803c2570bb09c9a84fa178f2677d4fa912e Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 2 Oct 2025 12:36:24 -0400 Subject: [PATCH 050/101] Ensure `prevRtCmdPosition` is set for all axes --- src/RealTimeMotionControl.c | 60 +++++++++++++++++++------------------ 1 file changed, 31 insertions(+), 29 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index d86c01a3..3ee65831 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -467,38 +467,40 @@ bool Ros_CheckForFsuInterference(MOTION_MODE mode, int* tools) // and check if it matches the amount if increment sent last cycle for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) { - if (howMuchShouldIHaveMoved[groupIndex][axis] != 0) + if (mode == MOTION_MODE_RT_CARTESIAN) { - if (mode == MOTION_MODE_RT_JOINT) - { - howMuchDidIActuallyMove[axis] = cmdPulse.lPos[axis] - prevRtCmdPosition[groupIndex][axis]; - prevRtCmdPosition[groupIndex][axis] = cmdPulse.lPos[axis]; - } - else if (mode == MOTION_MODE_RT_CARTESIAN) - { - //When working in cartesian space, we're only going to monitor the translation. - //1. There is no FSU speed limit for rotation. So it's moot. - //2. When rotating by some increment, that rotation gets 'spread out' over multiple - // axes. Even if I put all of my commanded increment into a single axis, all - // three of them are going to react. So, the cmd-value of my intended axis may - // not be the value I expect. - if (axis >= TCP_Rx) - break; - - howMuchDidIActuallyMove[axis] = cartRespData.lPos[axis] - prevRtCmdPosition[groupIndex][axis]; - prevRtCmdPosition[groupIndex][axis] = cartRespData.lPos[axis]; - } + howMuchDidIActuallyMove[axis] = cartRespData.lPos[axis] - prevRtCmdPosition[groupIndex][axis]; + prevRtCmdPosition[groupIndex][axis] = cartRespData.lPos[axis]; + } + else if (mode == MOTION_MODE_RT_JOINT) + { + howMuchDidIActuallyMove[axis] = cmdPulse.lPos[axis] - prevRtCmdPosition[groupIndex][axis]; + prevRtCmdPosition[groupIndex][axis] = cmdPulse.lPos[axis]; + } + } + + for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) + { + //When working in cartesian space, we're only going to monitor the translation. + //1. There is no FSU speed limit for rotation. So it's moot. + //2. When rotating by some increment, that rotation gets 'spread out' over multiple + // axes. Even if I put all of my commanded increment into a single axis, all + // three of them are going to react. So, the cmd-value of my intended axis may + // not be the value I expect. + if (mode == MOTION_MODE_RT_CARTESIAN && axis >= TCP_Rx) + { + break; + } - difference = howMuchShouldIHaveMoved[groupIndex][axis] - howMuchDidIActuallyMove[axis]; - if (abs(difference) > MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION) - { - //Ros_Debug_BroadcastMsg("howMuchShouldIHaveMoved[%d][%d] = %d", groupIndex, axis, howMuchShouldIHaveMoved[groupIndex][axis]); - //Ros_Debug_BroadcastMsg("howMuchDidIActuallyMove[%d] = %d", axis, howMuchDidIActuallyMove[axis]); - //Ros_Debug_BroadcastMsg("difference = %d", difference); - //Ros_Debug_BroadcastMsg("---------"); + difference = howMuchShouldIHaveMoved[groupIndex][axis] - howMuchDidIActuallyMove[axis]; + if (abs(difference) > MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION) + { + //Ros_Debug_BroadcastMsg("howMuchShouldIHaveMoved[%d][%d] = %d", groupIndex, axis, howMuchShouldIHaveMoved[groupIndex][axis]); + //Ros_Debug_BroadcastMsg("howMuchDidIActuallyMove[%d] = %d", axis, howMuchDidIActuallyMove[axis]); + //Ros_Debug_BroadcastMsg("difference = %d", difference); + //Ros_Debug_BroadcastMsg("---------"); - return TRUE; - } + return TRUE; } } } From 7c678f9f22368c68229b30cb4c1ccc8ec633af89 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 2 Oct 2025 12:38:14 -0400 Subject: [PATCH 051/101] Tighten tolerance for `MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION` --- src/RealTimeMotionControl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 7f7ebada..42ce00be 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -128,7 +128,7 @@ typedef struct RtReply_ RtReply; //likely be some small rounding errors. So, the deviation must exceed //this amount before the system will report that the FSU has limited //the incoming motion command. -#define MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION 50 //50 pulse, 0.050 millimeters, or 0.0050 degrees +#define MAX_INCREMENT_DEVIATION_FOR_FSU_DETECTION 20 //20 pulse, 0.020 millimeters, or 0.0020 degrees #undef PACKED From 42ebb95670cf02670069952a2c030e0b9b9599d6 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 2 Oct 2025 12:41:32 -0400 Subject: [PATCH 052/101] Fix warning on first cmd ID received --- src/RealTimeMotionControl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 3ee65831..dbed033c 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -116,7 +116,7 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) if (bytes_received > 0) { //Check for old or same sequence ID (wraparound safe) - if ((int32_t)(incomingCommand.sequenceId - previousSequenceId) <= 0) + if (((int32_t)(incomingCommand.sequenceId - previousSequenceId) <= 0) && !bFirstRecv) { // This packet is old or a duplicate. Ros_Debug_BroadcastMsg("WARN: Received old command packet (prev: %u, new: %u)", previousSequenceId, incomingCommand.sequenceId); From 28d7b11d929ef06c2d267d5b28ac6330729f6557 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 2 Oct 2025 12:43:03 -0400 Subject: [PATCH 053/101] Invalid boolean logic --- src/MotionControl.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/MotionControl.c b/src/MotionControl.c index 9f494ad5..0d4af441 100644 --- a/src/MotionControl.c +++ b/src/MotionControl.c @@ -1658,8 +1658,8 @@ BOOL Ros_MotionControl_IsMotionMode_PointQueue() BOOL Ros_MotionControl_IsMotionMode_RealTime() { - return (Ros_MotionControl_ActiveMotionMode == - (MOTION_MODE_RT_JOINT || MOTION_MODE_RT_CARTESIAN)); + return (Ros_MotionControl_ActiveMotionMode == MOTION_MODE_RT_JOINT || + Ros_MotionControl_ActiveMotionMode == MOTION_MODE_RT_CARTESIAN); } void Ros_MotionControl_ValidateMotionModeIsOk() From e96194fb216b4e4d09d9502e92f5e9979e4d813c Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 2 Oct 2025 12:53:31 -0400 Subject: [PATCH 054/101] Client should use the same timeout as the server --- config/motoros2_config.yaml | 3 +++ doc/rt_control.md | 2 ++ src/MotoROS2_AllControllers.vcxproj.filters | 2 +- 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/config/motoros2_config.yaml b/config/motoros2_config.yaml index 2342d5ad..32dee204 100644 --- a/config/motoros2_config.yaml +++ b/config/motoros2_config.yaml @@ -330,6 +330,9 @@ publisher_qos: # Timeout for real time motion commands. If a command packet is not received # within this number of milliseconds, the motion mode will be cancelled. # +# Additionally, if the client does not receive a reply packet within this +# amount of time, then it should be assumed that the session is dead. +# # Setting this to '-1' will never timeout. In that case, you must explicitly # call '/stop_traj_mode' to stop the motion mode. # diff --git a/doc/rt_control.md b/doc/rt_control.md index 23c24b7f..cb0bb111 100644 --- a/doc/rt_control.md +++ b/doc/rt_control.md @@ -38,6 +38,8 @@ If a command is not received with 30 seconds (default), then the session times o At that point, the server must be reactivated by calling `stop_traj_mode` and `start_rt_mode`. A "keep-alive" can be used by sending a command with zero increments. +Additionally, if the client does not receive a reply packet within this amount of time, then it should be assumed that the session is dead. + Command Flow ### Data format (command) diff --git a/src/MotoROS2_AllControllers.vcxproj.filters b/src/MotoROS2_AllControllers.vcxproj.filters index d0c4ba62..e3b77646 100644 --- a/src/MotoROS2_AllControllers.vcxproj.filters +++ b/src/MotoROS2_AllControllers.vcxproj.filters @@ -277,7 +277,7 @@ MotoPlus Libraries\micro-ROS - + Docs\doc From cec6bf55ffa60016ff654c979155c2d41211bc55 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 2 Oct 2025 13:01:38 -0400 Subject: [PATCH 055/101] Purge UDP buffer before triggering next command --- src/RealTimeMotionControl.c | 61 ++++++++++++++++++++++--------------- 1 file changed, 37 insertions(+), 24 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index dbed033c..29df545a 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -16,7 +16,8 @@ bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DAT bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData); void Ros_RtMotionControl_Cleanup(); void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* command, RtReply* reply); -bool Ros_CheckForFsuInterference(MOTION_MODE mode, int* tools); +bool Ros_RtMotionControl_CheckForFsuInterference(MOTION_MODE mode, int* tools); +void Ros_RtMotionControl_PurgeBufferedPackets(); static int sockRtCommandListener = -1; @@ -56,27 +57,7 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) Ros_Debug_BroadcastMsg("Starting RT session"); Ros_Debug_BroadcastMsg("Flushing stale packets from socket buffer..."); - //---------------------------- - //mpIoctl(sockRtCommandListener, FIOFLUSH, 1); - //UPDATE: mpIoctl isn't working! We'll manually purge the buffer with a draining loop. - //---------------------------- - while (TRUE) - { - FD_ZERO(&fds); - FD_SET(sockRtCommandListener, &fds); - - //no wait - tv.tv_usec = 0; - tv.tv_sec = 0; - - if (mpSelect(sockRtCommandListener + 1, &fds, NULL, NULL, &tv) > 0) - { - mpRecvFrom(sockRtCommandListener, (char*)&incomingCommand, sizeof(RtPacket), 0, (struct sockaddr*)&client_addr, &client_addr_len); - } - else - break; - } - //---------------------------- + Ros_RtMotionControl_PurgeBufferedPackets(); //========================================================================================= while (TRUE) @@ -145,7 +126,7 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) break; //drop the connection } - fsuLimitingDetected = Ros_CheckForFsuInterference(mode, incomingCommand.toolIndex); + fsuLimitingDetected = Ros_RtMotionControl_CheckForFsuInterference(mode, incomingCommand.toolIndex); // Send increment to robot int ret = mpExRcsIncrementMove(&moveData); @@ -170,6 +151,8 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) previousSequenceId = incomingCommand.sequenceId; Ros_RtMotionControl_PopulateReplyMessage(mode, &incomingCommand, &outgoingReply); outgoingReply.fsuInterferenceDetected = fsuLimitingDetected; + + Ros_RtMotionControl_PurgeBufferedPackets(); //in case user sent multiple commands while this was sleeping mpSendTo(sockRtCommandListener, (char*)&outgoingReply, sizeof(RtReply), 0, (struct sockaddr*)&client_addr, client_addr_len); //track how big the increment SHOULD have been @@ -428,7 +411,7 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman } } -bool Ros_CheckForFsuInterference(MOTION_MODE mode, int* tools) +bool Ros_RtMotionControl_CheckForFsuInterference(MOTION_MODE mode, int* tools) { MP_CTRL_GRP_SEND_DATA ctrlGroup; MP_PULSE_POS_RSP_DATA cmdPulse; @@ -506,3 +489,33 @@ bool Ros_CheckForFsuInterference(MOTION_MODE mode, int* tools) } return FALSE; } + +void Ros_RtMotionControl_PurgeBufferedPackets() +{ + struct fd_set fds; + struct timeval tv; + struct sockaddr_in client_addr; + int client_addr_len = sizeof(client_addr); + RtPacket incomingCommand; + + //---------------------------- + //mpIoctl(sockRtCommandListener, FIOFLUSH, 1); + //UPDATE: mpIoctl isn't working! We'll manually purge the buffer with a draining loop. + //---------------------------- + while (TRUE) + { + FD_ZERO(&fds); + FD_SET(sockRtCommandListener, &fds); + + //no wait + tv.tv_usec = 0; + tv.tv_sec = 0; + + if (mpSelect(sockRtCommandListener + 1, &fds, NULL, NULL, &tv) > 0) + { + mpRecvFrom(sockRtCommandListener, (char*)&incomingCommand, sizeof(RtPacket), 0, (struct sockaddr*)&client_addr, &client_addr_len); + } + else + break; + } +} From 66eb98edfe4ddcd7d170dacd9d878de8de5355dd Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 10 Oct 2025 13:46:55 -0400 Subject: [PATCH 056/101] header format --- src/RealTimeMotionControl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 42ce00be..863c18d3 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -1,4 +1,4 @@ -// Corrected Code +// RealTimeMotionControl.h // SPDX-FileCopyrightText: 2025, Yaskawa America, Inc. // SPDX-FileCopyrightText: 2025, Delft University of Technology From 7dad1ed75be828681d2779ad14d64bd24f61b951 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 10 Oct 2025 13:55:12 -0400 Subject: [PATCH 057/101] scale cartesian speed limit with interpolation clock --- src/RealTimeMotionControl.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 29df545a..3f19ecd7 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -286,13 +286,15 @@ bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA double vector = sqrt(pow(incomingCommand->delta[groupNo][TCP_X], 2) + //x^2 pow(incomingCommand->delta[groupNo][TCP_Y], 2) + //y^2 pow(incomingCommand->delta[groupNo][TCP_Z], 2)); //z^2 - if (vector > 6.0) //1500 mm/sec == 6 mm per 4 milliseconds + + // Assuming 'elapsed_ms' is your variable for time in milliseconds. + const double max_speed_mm_per_ms = 1.5; // 1500 mm/sec is 1.5 mm/ms + + if (vector > (max_speed_mm_per_ms * g_Ros_Controller.interpolPeriod)) { Ros_Debug_BroadcastMsg("ERROR: The increment for the TCP exceeds the maximum limit of 1500 mm/sec"); return false; } - - //Ros_Debug_BroadcastMsg("moveData = %d, incomingCommand = %.5f", moveData->grp_pos_info[groupNo].pos[0], incomingCommand->delta[groupNo][0] * 1000.0); } return true; From 8d6eae1a903414f399cc10074d64c2b51dcf0db6 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 10 Oct 2025 13:57:20 -0400 Subject: [PATCH 058/101] Consistent quotations for port numbers --- config/motoros2_config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/motoros2_config.yaml b/config/motoros2_config.yaml index 32dee204..90f6a52d 100644 --- a/config/motoros2_config.yaml +++ b/config/motoros2_config.yaml @@ -323,8 +323,8 @@ publisher_qos: # For the real time motion interface, which port should the UDP messages # be transmitted on? # -# DEFAULT: '8889' -#rt_udp_port_number: '8889' +# DEFAULT: 8889 +#rt_udp_port_number: 8889 #----------------------------------------------------------------------------- # Timeout for real time motion commands. If a command packet is not received From a213192d7db5860cd9f28ab965f974b58a5f065e Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 10 Oct 2025 14:05:54 -0400 Subject: [PATCH 059/101] Repeat code. Remove it and explain what's happening --- src/RealTimeMotionControl.c | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 3f19ecd7..d9eea1b4 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -305,12 +305,7 @@ void Ros_RtMotionControl_Cleanup() //Do not close sockRtCommandListener. Allow it to persist //indefinitely and be reused. - if (g_Ros_Controller.tidIncMoveThread != INVALID_TASK) - { - mpDeleteTask(g_Ros_Controller.tidIncMoveThread); - g_Ros_Controller.tidIncMoveThread = INVALID_TASK; - Ros_Debug_BroadcastMsg("Deleting old R/T task"); - } + //Do not delete interpolation task. This is handled in Ros_MotionControl_StopTrajMode. } bool Ros_RtMotionControl_OpenSocket() From fefa69d53dc075e7b809a26ad8158cc6ae4c53ef Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 20 Oct 2025 13:37:25 -0400 Subject: [PATCH 060/101] Reply with error code if user requests invalid mode --- src/ErrorHandling.c | 2 ++ src/ErrorHandling.h | 1 + src/ServiceStartRtMode.c | 15 ++++++++++++--- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/src/ErrorHandling.c b/src/ErrorHandling.c index 7852e64b..dc0454ac 100644 --- a/src/ErrorHandling.c +++ b/src/ErrorHandling.c @@ -78,6 +78,8 @@ const char* const Ros_ErrorHandling_MotionNotReadyCode_ToString(MotionNotReadyCo return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_ECO_MODE_STR; case MOTION_NOT_READY_SERVO_ON_TIMEOUT: return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_SERVO_ON_TIMEOUT_STR; + case MOTION_NOT_READY_INVALID_SELECTION: + return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_INVALID_SELECTION; default: return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_UNSPECIFIED_STR; } diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h index ac1d93c7..c56df670 100644 --- a/src/ErrorHandling.h +++ b/src/ErrorHandling.h @@ -35,6 +35,7 @@ typedef enum MOTION_NOT_READY_MAJOR_ALARM = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_MAJOR_ALARM, MOTION_NOT_READY_ECO_MODE = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_ECO_MODE, MOTION_NOT_READY_SERVO_ON_TIMEOUT = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_SERVO_ON_TIMEOUT, + MOTION_NOT_READY_INVALID_SELECTION = motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_INVALID_SELECTION, } MotionNotReadyCode; typedef enum diff --git a/src/ServiceStartRtMode.c b/src/ServiceStartRtMode.c index f26c124b..f297e3b9 100644 --- a/src/ServiceStartRtMode.c +++ b/src/ServiceStartRtMode.c @@ -48,16 +48,25 @@ void Ros_ServiceStartRtMode_Trigger(const void* request_msg, void* response_msg) { StartRtMode_Request* request = (StartRtMode_Request*)request_msg; StartRtMode_Response* response = (StartRtMode_Response*)response_msg; - - MOTION_MODE mm = request->control_mode.value == motoros2_interfaces__msg__ControlModeEnum__CARTESIAN ? MOTION_MODE_RT_CARTESIAN : MOTION_MODE_RT_JOINT; response->result_code.value = MOTION_READY; rosidl_runtime_c__String__assign(&response->message, ""); response->period = g_Ros_Controller.interpolPeriod; response->timeout_for_rt_msg = g_nodeConfigSettings.timeout_for_rt_msg; response->max_sequence_diff_for_rt_msg = g_nodeConfigSettings.max_sequence_diff_for_rt_msg; + + MOTION_MODE mm = MOTION_MODE_INACTIVE; + + if (request->control_mode.value == motoros2_interfaces__msg__ControlModeEnum__CARTESIAN) + mm = MOTION_MODE_RT_CARTESIAN; + else if (request->control_mode.value == motoros2_interfaces__msg__ControlModeEnum__JOINT_ANGLES) + mm = MOTION_MODE_RT_JOINT; + + if (mm != MOTION_MODE_INACTIVE) + response->result_code.value = Ros_MotionControl_StartMotionMode(mm, &response->message); + else + response->result_code.value = MOTION_NOT_READY_INVALID_SELECTION; - response->result_code.value = Ros_MotionControl_StartMotionMode(mm, &response->message); if (response->result_code.value != MOTION_READY) { // update response From b57fc9e724bb0773b90c5baab98d6957dead89ed Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 20 Oct 2025 14:18:00 -0400 Subject: [PATCH 061/101] rebase changed error codes --- doc/troubleshooting.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/troubleshooting.md b/doc/troubleshooting.md index fe36722a..d25d7415 100644 --- a/doc/troubleshooting.md +++ b/doc/troubleshooting.md @@ -582,7 +582,7 @@ The name must not be blank. After correcting the configuration, the [changes will need to be propagated to the Yaskawa controller](../README.md#updating-the-configuration). -### Alarm: 8011[23 - 54] or [56 - 58] or [65 - 66] +### Alarm: 8011[23 - 54] or [56 - 58] or [66 - 67] *Example:* @@ -611,7 +611,7 @@ ALARM 8011 [xx] ``` -Where `[xx]` is a subcode in the ranges `[23 - 54]` or `[56 - 58]` or `[65 - 66]`. +Where `[xx]` is a subcode in the ranges `[23 - 54]` or `[56 - 58]` or `[66 - 67]`. *Solution:* These alarms are often caused by version incompatibilities between ROS 2 (on the client PC), micro-ROS (as part of MotoROS2) and/or the micro-ROS Agent. From c51090e3960b12267204bd250c7933f7cbc0b926 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 20 Oct 2025 14:21:11 -0400 Subject: [PATCH 062/101] Correct subcode in link --- doc/troubleshooting.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/troubleshooting.md b/doc/troubleshooting.md index d25d7415..28eda526 100644 --- a/doc/troubleshooting.md +++ b/doc/troubleshooting.md @@ -662,7 +662,7 @@ After correcting the configuration, the [changes will need to be propagated to t ### Alarm: 8011[56 - 58] -Please refer to [Alarm: 8011[23 - 54]](#alarm-801123---54-or-56---58-or-65---66). +Please refer to [Alarm: 8011[23 - 54]](#alarm-801123---54-or-56---58-or-66---67). ### Alarm: 8011[59] From 2f710a7036dd9265340fff9a6df21fafcac9bcf8 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 29 Oct 2025 12:42:25 -0400 Subject: [PATCH 063/101] Use string; not enum --- src/ErrorHandling.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ErrorHandling.c b/src/ErrorHandling.c index dc0454ac..af79088e 100644 --- a/src/ErrorHandling.c +++ b/src/ErrorHandling.c @@ -79,7 +79,7 @@ const char* const Ros_ErrorHandling_MotionNotReadyCode_ToString(MotionNotReadyCo case MOTION_NOT_READY_SERVO_ON_TIMEOUT: return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_SERVO_ON_TIMEOUT_STR; case MOTION_NOT_READY_INVALID_SELECTION: - return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_INVALID_SELECTION; + return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_INVALID_SELECTION_STR; default: return motoros2_interfaces__msg__MotionReadyEnum__NOT_READY_UNSPECIFIED_STR; } From c2593aff30bc1f6a0e732be3c1510dfec1c28d2a Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Tue, 25 Nov 2025 11:34:19 -0500 Subject: [PATCH 064/101] Documentation clarification based on review --- config/motoros2_config.yaml | 3 ++- doc/ros_api.md | 6 +++--- doc/rt_control.md | 19 ++++++++++--------- 3 files changed, 15 insertions(+), 13 deletions(-) diff --git a/config/motoros2_config.yaml b/config/motoros2_config.yaml index 90f6a52d..00807010 100644 --- a/config/motoros2_config.yaml +++ b/config/motoros2_config.yaml @@ -328,7 +328,8 @@ publisher_qos: #----------------------------------------------------------------------------- # Timeout for real time motion commands. If a command packet is not received -# within this number of milliseconds, the motion mode will be cancelled. +# within this number of milliseconds, the motion mode will be cancelled. This +# applies to all packets within R/T session. # # Additionally, if the client does not receive a reply packet within this # amount of time, then it should be assumed that the session is dead. diff --git a/doc/ros_api.md b/doc/ros_api.md index b096fdbd..bb7a0988 100644 --- a/doc/ros_api.md +++ b/doc/ros_api.md @@ -137,13 +137,13 @@ The `reset_error` service can be used to attempt to reset errors and alarms Type: [motoros2_interfaces/srv/StartRtMode](https://github.com/yaskawa-global/motoros2_interfaces/srv/StartRtMode.srv) -Attempts to enable servo drives, activate the real-time UDP server, and set the job-cycle mode to allow execution of INIT_ROS. +Attempts to enable servo drives, activate the [real-time UDP server](rt_control.md), and set the job-cycle mode to allow execution of `INIT_ROS`. This allows the user to send incremental motion to the robot at the rate returned by the service (`period`). See [R/T Motion Control](rt_control.md) for information on the protocol implementation. -Note: this service may fail if controller state prevents it from transitioning to trajectory mode. -Inspect the `result_code` to determine the cause. +Note: this service may fail if controller state prevents it from transitioning to R/T mode. +Inspect the `result_code` field to determine the cause. Check the relevant fields of the `RobotStatus` messages to determine overall controller status. The `reset_error` service can be used to attempt to reset errors and alarms diff --git a/doc/rt_control.md b/doc/rt_control.md index cb0bb111..be2c898d 100644 --- a/doc/rt_control.md +++ b/doc/rt_control.md @@ -9,30 +9,30 @@ SPDX-License-Identifier: CC-BY-SA-4.0 The real-time motion control server is intended to be used in a closed loop system. It allows the user to command incremental offsets at the rate of the robot controller's interpolation clock. -This control mode minimizes overhead as much as possible by routing the user commands directly to the MotoPlus motion API. +This control mode minimizes overhead as much as possible by routing the user commands directly to the MotoPlus motion API, mpExRcsIncrementMove. ## Activation -This control mode is activated using the [StartRtMode](ros_api.md#start_rt_mode) service. +This control mode is activated using the [start_rt_mode](ros_api.md#start_rt_mode) service. The user must specify the `control_mode` to indicate whether the increments will be joint offsets (radians) or cartesian TCP offsets (meters / radians). If this service is successful, it will return a `result_code` of `Ready (1)`. -Otherwise, please examine the `result_code` and `message` for more information. +Otherwise, please examine the `result_code` and `message` files in the response for more information. The service will also return a `period` in milliseconds. This indicates the rate at which increment commands will be expected by the robot. -The default `period` for a single robot arm is 4 milliseconds. -However, that value will increase as additional axes or arms are added to the system. +The default period for a single manipulator is 4 milliseconds. +However, that value will increase as additional axes or manipulators are added to the system. ## Usage ### Command Flow Once activated, a UDP server will listen on port `8889` (default). -The user then sends the first increment with a `sequenceId = 0`. +The user then sends the first increment with a the `sequenceId` field set to `0`. After that, the user must wait until the robot replies before sending the next increment. Each subsequent command must increment the `sequenceId`. Additionally, each subsequent command must not be sent until the robot replies to the previous command. -This will occur at the rate of the `period` from the [StartRtMode](ros_api.md#start_rt_mode) service. +This will occur at the rate of the `period` from the [start_rt_mode](ros_api.md#start_rt_mode) service. If a command is not received with 30 seconds (default), then the session times out and is dropped. At that point, the server must be reactivated by calling `stop_traj_mode` and `start_rt_mode`. @@ -112,7 +112,7 @@ Please note that rotations are applied in the order of `Z Y X`. ### Data format (reply) The command packet is a *packed* `RtReply` structure. -This will echo the sequence ID, provide feedback position, and provide command position. +This will echo the sequence ID, provide feedback position, and provide commanded position. Additionally, there is a flag to indicate if the Functional Safety Unit (FSU) reduced the speed of the **previous** command cycle. This indicates that the robot did not complete the full increment as commanded. @@ -138,5 +138,6 @@ struct RtReply ## Deactivation Other motion modes may not be used at the same time as the real-time motion control server. -By calling `stop_traj_mode`, the r/t server will be disposed. +The service to start those modes will fail when invoked. +By calling `stop_traj_mode`, the R/T server will be disposed. At that time, another motion mode may be used. From b4108504ff74414fbe0c31b58d981139acb24e89 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 15 Dec 2025 09:34:58 -0500 Subject: [PATCH 065/101] decrease `Timeout for real time motion commands` --- config/motoros2_config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/motoros2_config.yaml b/config/motoros2_config.yaml index 00807010..8060eb0e 100644 --- a/config/motoros2_config.yaml +++ b/config/motoros2_config.yaml @@ -337,8 +337,8 @@ publisher_qos: # Setting this to '-1' will never timeout. In that case, you must explicitly # call '/stop_traj_mode' to stop the motion mode. # -# DEFAULT: 30000 (30.000 seconds) -#timeout_for_rt_msg: 30000 +# DEFAULT: 5000 (5.000 seconds) +#timeout_for_rt_msg: 5000 #----------------------------------------------------------------------------- # When using the real time motion interface, each command packet must increment From dcfe324e00fcac8d256f2479c861083c2f6b4bf9 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 15 Dec 2025 09:37:19 -0500 Subject: [PATCH 066/101] decrease `max_sequence_diff_for_rt_msg` --- config/motoros2_config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/motoros2_config.yaml b/config/motoros2_config.yaml index 8060eb0e..7a95b134 100644 --- a/config/motoros2_config.yaml +++ b/config/motoros2_config.yaml @@ -349,5 +349,5 @@ publisher_qos: # command by a value greater than this, then the connection will be dropped. # The motion mode must be reactivated to be used again. # -# DEFAULT: 10 -#max_sequence_diff_for_rt_msg: 10 +# DEFAULT: 3 +#max_sequence_diff_for_rt_msg: 3 From 184cdd8ea9d5cab70fa1e87754ecf122da211b76 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 15 Dec 2025 09:46:34 -0500 Subject: [PATCH 067/101] Move `rt_udp_port_number` to range specified in manual --- config/motoros2_config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/motoros2_config.yaml b/config/motoros2_config.yaml index 7a95b134..73dd918f 100644 --- a/config/motoros2_config.yaml +++ b/config/motoros2_config.yaml @@ -323,8 +323,8 @@ publisher_qos: # For the real time motion interface, which port should the UDP messages # be transmitted on? # -# DEFAULT: 8889 -#rt_udp_port_number: 8889 +# DEFAULT: 22000 +#rt_udp_port_number: 22000 #----------------------------------------------------------------------------- # Timeout for real time motion commands. If a command packet is not received From 92e34755e70c2ca1db57ae7b392d623cfc3755ff Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 15 Dec 2025 10:40:09 -0500 Subject: [PATCH 068/101] reflect port change in docs --- doc/rt_control.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/rt_control.md b/doc/rt_control.md index be2c898d..fab924f1 100644 --- a/doc/rt_control.md +++ b/doc/rt_control.md @@ -28,7 +28,7 @@ However, that value will increase as additional axes or manipulators are added t ### Command Flow -Once activated, a UDP server will listen on port `8889` (default). +Once activated, a UDP server will listen on port `22000` (default). The user then sends the first increment with a the `sequenceId` field set to `0`. After that, the user must wait until the robot replies before sending the next increment. Each subsequent command must increment the `sequenceId`. Additionally, each subsequent command must not be sent until the robot replies to the previous command. From 4339067bf992397d54623548047265a5c3a676cd Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 15 Dec 2025 13:07:01 -0500 Subject: [PATCH 069/101] Implement `version` field --- doc/rt_control.md | 9 ++++++++- src/RealTimeMotionControl.c | 7 +++++++ src/RealTimeMotionControl.h | 9 +++++++++ 3 files changed, 24 insertions(+), 1 deletion(-) diff --git a/doc/rt_control.md b/doc/rt_control.md index fab924f1..35986862 100644 --- a/doc/rt_control.md +++ b/doc/rt_control.md @@ -45,7 +45,6 @@ Additionally, if the client does not receive a reply packet within this amount o ### Data format (command) The command packet is a *packed* `RtPacket` structure. -This contains a sequence ID, the increments for each control group, and the tool number to use for each control group. ```c //########################################################################## @@ -53,12 +52,20 @@ This contains a sequence ID, the increments for each control group, and the tool //########################################################################## struct RtPacket { + int version; + UINT32 sequenceId; double delta[MAX_GROUPS][MAX_AXES]; //[8][8] int toolIndex[MAX_GROUPS]; //[8] } ``` +The `version` must match the version number expected by the server. +If it does not match the expected value, the packet will be rejected and the connection will be dropped. +The current version is `1`. + +This contains a sequence ID, the increments for each control group, and the tool number to use for each control group. + #### Joints When the `control_mode` is `JOINT_ANGLES (1)`, the order of the joints in the `delta` array must be in the order of `S L U R B T E 8`. diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index d9eea1b4..61d0c41b 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -96,6 +96,13 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) if (bytes_received > 0) { + //Verify version of the command packet + if (incomingCommand.version != VERSION_REAL_TIME_INTERFACE) + { + Ros_Debug_BroadcastMsg("ERROR: The command packet must be version [%d]", VERSION_REAL_TIME_INTERFACE); + break; //drop the connection + } + //Check for old or same sequence ID (wraparound safe) if (((int32_t)(incomingCommand.sequenceId - previousSequenceId) <= 0) && !bFirstRecv) { diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 863c18d3..b858050b 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -8,6 +8,8 @@ #ifndef MOTOROS2_REALTIME_MOTION_CONTROL_H #define MOTOROS2_REALTIME_MOTION_CONTROL_H +#define VERSION_REAL_TIME_INTERFACE 1 + #define PACKED __attribute__ ((__packed__)) extern void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode); @@ -65,6 +67,10 @@ typedef enum struct RtPacket_ { + //The version of the command packet must match the value expected + //by MotoROS2. + int version; + //Must increment sequentially with each new command packet. UINT32 sequenceId; @@ -87,6 +93,9 @@ struct RtPacket_ // 'select_tool' service definition file in motoros2_interfaces. int toolIndex[MAX_GROUPS]; //TOOL 0 - 63 + //Reserved for future expansion + char reserved[64]; + } PACKED; typedef struct RtPacket_ RtPacket; From b337a78b049b5287ac20974b023b0ad1468b8a83 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 15 Dec 2025 13:19:09 -0500 Subject: [PATCH 070/101] Verify incoming packet type --- src/RealTimeMotionControl.c | 24 ++++++++++++++++++++++++ src/RealTimeMotionControl.h | 10 ++++++++++ 2 files changed, 34 insertions(+) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 61d0c41b..103d7442 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -45,6 +45,8 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) bool fsuLimitingDetected; + bool packetTypeOK; + //========================================================================================= bzero(prevRtCmdPosition, MAX_GROUPS * MAX_AXES * sizeof(LONG)); @@ -103,6 +105,28 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) break; //drop the connection } + //Verify the packet type + packetTypeOK = TRUE; + switch (mode) + { + case MOTION_MODE_RT_JOINT: + if (incomingCommand.packetType != PacketType_Joint_Increments) + { + packetTypeOK = false; + Ros_Debug_BroadcastMsg("ERROR: The packet type does not match the control_mode specified in start_rt_mode (Joint Increments)"); + } + break; + case MOTION_MODE_RT_CARTESIAN: + if (incomingCommand.packetType != PacketType_Cart_Increments) + { + packetTypeOK = false; + Ros_Debug_BroadcastMsg("ERROR: The packet type does not match the control_mode specified in start_rt_mode (Cartesian Increments)"); + } + break; + } + if (!packetTypeOK) + break; //drop the connection + //Check for old or same sequence ID (wraparound safe) if (((int32_t)(incomingCommand.sequenceId - previousSequenceId) <= 0) && !bFirstRecv) { diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index b858050b..e5aba2be 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -16,6 +16,12 @@ extern void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode); extern bool Ros_RtMotionControl_OpenSocket(); extern void Ros_RtMotionControl_Cleanup(); +typedef enum +{ + PacketType_Joint_Increments = 0, + PacketType_Cart_Increments +} PacketType; + typedef enum { Group_1 = 0, @@ -71,6 +77,10 @@ struct RtPacket_ //by MotoROS2. int version; + //The packet type must match the control_mode which was specified + //in when invoking the start_rt_mode service. + PacketType packetType; + //Must increment sequentially with each new command packet. UINT32 sequenceId; From 47290e76f5b07f28ba7397dfe8eb6f618eb67cb7 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Mon, 15 Dec 2025 13:39:20 -0500 Subject: [PATCH 071/101] Implement feedback state --- src/RealTimeMotionControl.c | 17 +++++++++++++++++ src/RealTimeMotionControl.h | 14 ++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 103d7442..7fe14b85 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -367,6 +367,21 @@ bool Ros_RtMotionControl_OpenSocket() return true; } +//Essentially a clone of the /robot_status topic. But decoupled from the industrial_msgs/RobotStatus type. +void Ros_RtMotionControl_ConvertRobotStatusToRobotState(RobotState* state) +{ + state->drives_powered = g_messages_RobotStatus.msgRobotStatus->drives_powered.val; + state->e_stopped = g_messages_RobotStatus.msgRobotStatus->e_stopped.val; + state->in_motion = g_messages_RobotStatus.msgRobotStatus->in_motion.val; + state->play_mode = (g_messages_RobotStatus.msgRobotStatus->mode.val == industrial_msgs__msg__RobotMode__AUTO); + state->motion_possible = g_messages_RobotStatus.msgRobotStatus->motion_possible.val; + state->error = g_messages_RobotStatus.msgRobotStatus->in_error.val; + if (g_messages_RobotStatus.msgRobotStatus->error_codes.size > 0) + state->error_code = g_messages_RobotStatus.msgRobotStatus->error_codes.data[0]; + else + state->error_code = 0; +} + void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* command, RtReply* reply) { long pulsePos_moto[MAX_PULSE_AXES]; @@ -437,6 +452,8 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman reply->previousCommandPositionCartesian[groupIndex][TCP_Rz] = DEG_0001_TO_RAD(coord.rz); reply->previousCommandPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); } + + Ros_RtMotionControl_ConvertRobotStatusToRobotState(&reply->state); } bool Ros_RtMotionControl_CheckForFsuInterference(MOTION_MODE mode, int* tools) diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index e5aba2be..005c6b2f 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -66,6 +66,16 @@ typedef enum MAX_AXES //maxies } CartesianIndices; +typedef struct +{ + BOOL drives_powered; + BOOL e_stopped; + BOOL in_motion; + BOOL play_mode; + BOOL motion_possible; + BOOL error; + int error_code; +} RobotState; //########################################################################## // !All data is little-endian! @@ -118,6 +128,10 @@ struct RtReply_ { UINT32 sequenceEcho; + //Essentially a clone of the /robot_status topic. But decoupled + //from the industrial_msgs/RobotStatus type. + RobotState state; + //This is indicative of where the robot is physically located. //Please note that this will trail behind the commanded position. //The joint ordering will match that of the original command From efc8a8852bf18749bf64ea80bd26fdf09dfa409d Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 26 Mar 2026 14:36:52 -0400 Subject: [PATCH 072/101] Explicitly check packet size --- src/RealTimeMotionControl.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 7fe14b85..fcaf278c 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -96,7 +96,7 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) } } - if (bytes_received > 0) + if (bytes_received == sizeof(RtPacket)) { //Verify version of the command packet if (incomingCommand.version != VERSION_REAL_TIME_INTERFACE) @@ -123,6 +123,9 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) Ros_Debug_BroadcastMsg("ERROR: The packet type does not match the control_mode specified in start_rt_mode (Cartesian Increments)"); } break; + default: + packetTypeOK = false; + Ros_Debug_BroadcastMsg("ERROR: Unexpected motion mode is active"); } if (!packetTypeOK) break; //drop the connection From 303cec87a1e346fc74b5119e1c3607e3e9a36ea8 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 26 Mar 2026 15:26:49 -0400 Subject: [PATCH 073/101] Put robot state onto separate socket/thread --- config/motoros2_config.yaml | 16 +++++++- src/ConfigFile.c | 20 ++++++++-- src/ConfigFile.h | 8 +++- src/RealTimeMotionControl.c | 78 ++++++++++++++++++++++++++++--------- src/RealTimeMotionControl.h | 33 +++++++++------- 5 files changed, 115 insertions(+), 40 deletions(-) diff --git a/config/motoros2_config.yaml b/config/motoros2_config.yaml index 73dd918f..03a8d9e3 100644 --- a/config/motoros2_config.yaml +++ b/config/motoros2_config.yaml @@ -324,7 +324,21 @@ publisher_qos: # be transmitted on? # # DEFAULT: 22000 -#rt_udp_port_number: 22000 +#rt_listener_udp_port_number: 22000 + +#----------------------------------------------------------------------------- +# For the real time robot status interface, which port should the UDP messages +# be transmitted on? +# +# DEFAULT: 22001 +#rt_status_udp_port_number: 22001 + +#----------------------------------------------------------------------------- +# For the real time robot status interface, how much delay should there be +# between messages? +# +# DEFAULT: 10 +#rt_status_sleep_period: 10 #----------------------------------------------------------------------------- # Timeout for real time motion commands. If a command packet is not received diff --git a/src/ConfigFile.c b/src/ConfigFile.c index 5017e84a..415ffe71 100644 --- a/src/ConfigFile.c +++ b/src/ConfigFile.c @@ -123,7 +123,9 @@ Configuration_Item Ros_ConfigFile_Items[] = { "ignore_missing_calib_data", &g_nodeConfigSettings.ignore_missing_calib_data, Value_Bool }, { "debug_broadcast_enabled", &g_nodeConfigSettings.debug_broadcast_enabled, Value_Bool }, { "debug_broadcast_port", &g_nodeConfigSettings.debug_broadcast_port, Value_UserLanPort }, - { "rt_udp_port_number", g_nodeConfigSettings.rt_udp_port_number, Value_String }, + { "rt_listener_udp_port_number", g_nodeConfigSettings.rt_listener_udp_port_number, Value_String }, + { "rt_status_udp_port_number", g_nodeConfigSettings.rt_status_udp_port_number, Value_String }, + { "rt_status_sleep_period", &g_nodeConfigSettings.rt_status_sleep_period, Value_Int }, { "timeout_for_rt_msg", &g_nodeConfigSettings.timeout_for_rt_msg, Value_Int }, { "max_sequence_diff_for_rt_msg", &g_nodeConfigSettings.max_sequence_diff_for_rt_msg, Value_Int }, }; @@ -231,8 +233,16 @@ void Ros_ConfigFile_SetAllDefaultValues() g_nodeConfigSettings.ignore_missing_calib_data = DEFAULT_IGNORE_MISSING_CALIB; //========= - //rt_udp_port_number - sprintf(g_nodeConfigSettings.rt_udp_port_number, "%s", DEFAULT_RT_UDP_PORT_NUMBER); + //rt_listener_udp_port_number + sprintf(g_nodeConfigSettings.rt_listener_udp_port_number, "%s", DEFAULT_RT_LISTENER_UDP_PORT_NUMBER); + + //========= + //rt_status_udp_port_number + sprintf(g_nodeConfigSettings.rt_status_udp_port_number, "%s", DEFAULT_RT_STATUS_UDP_PORT_NUMBER); + + //========= + //rt_status_sleep_period + g_nodeConfigSettings.rt_status_sleep_period = DEFAULT_RT_STATUS_SLEEP_PERIOD; //========= //timeout_for_rt_msg @@ -761,7 +771,9 @@ void Ros_ConfigFile_PrintActiveConfiguration(Ros_Configuration_Settings const* c Ros_Debug_BroadcastMsg("Config: ignore_missing_calib_data = %d", config->ignore_missing_calib_data); Ros_Debug_BroadcastMsg("Config: debug_broadcast_enabled = %d", config->debug_broadcast_enabled); Ros_Debug_BroadcastMsg("Config: debug_broadcast_port = %d", config->debug_broadcast_port); - Ros_Debug_BroadcastMsg("Config: rt_udp_port_number = %s", config->rt_udp_port_number); + Ros_Debug_BroadcastMsg("Config: rt_listener_udp_port_number = %s", config->rt_listener_udp_port_number); + Ros_Debug_BroadcastMsg("Config: rt_status_udp_port_number = %s", config->rt_status_udp_port_number); + Ros_Debug_BroadcastMsg("Config: rt_status_sleep_period = %d", config->rt_status_sleep_period); Ros_Debug_BroadcastMsg("Config: timeout_for_rt_msg = %d", config->timeout_for_rt_msg); Ros_Debug_BroadcastMsg("Config: max_sequence_diff_for_rt_msg = %d", config->max_sequence_diff_for_rt_msg); } diff --git a/src/ConfigFile.h b/src/ConfigFile.h index 87131c5b..f8fa6e53 100644 --- a/src/ConfigFile.h +++ b/src/ConfigFile.h @@ -110,7 +110,9 @@ typedef enum #define DEFAULT_ULAN_DEBUG_BROADCAST_PORT CFG_ROS_USER_LAN1 #endif -#define DEFAULT_RT_UDP_PORT_NUMBER "8889" +#define DEFAULT_RT_LISTENER_UDP_PORT_NUMBER "22000" +#define DEFAULT_RT_STATUS_UDP_PORT_NUMBER "22001" +#define DEFAULT_RT_STATUS_SLEEP_PERIOD 10 #define DEFAULT_TIMEOUT_FOR_RT_MSG 30000 @@ -162,7 +164,9 @@ typedef struct BOOL debug_broadcast_enabled; Ros_UserLan_Port_Setting debug_broadcast_port; - char rt_udp_port_number[MAX_YAML_STRING_LEN]; + char rt_listener_udp_port_number[MAX_YAML_STRING_LEN]; + char rt_status_udp_port_number[MAX_YAML_STRING_LEN]; + int rt_status_sleep_period; int timeout_for_rt_msg; int max_sequence_diff_for_rt_msg; diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index fcaf278c..5f7ec7f1 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -18,8 +18,12 @@ void Ros_RtMotionControl_Cleanup(); void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* command, RtReply* reply); bool Ros_RtMotionControl_CheckForFsuInterference(MOTION_MODE mode, int* tools); void Ros_RtMotionControl_PurgeBufferedPackets(); +void Ros_RtMotionControl_SendRobotStatus(); static int sockRtCommandListener = -1; +static int sockStatusSender = -1; + +static struct sockaddr_in client_addr_status_messages; static LONG prevRtCmdPosition[MAX_GROUPS][MAX_AXES]; static LONG howMuchShouldIHaveMoved[MAX_GROUPS][MAX_AXES]; @@ -357,7 +361,7 @@ bool Ros_RtMotionControl_OpenSocket() memset(&server_addr, 0, sizeof(server_addr)); server_addr.sin_family = AF_INET; server_addr.sin_addr.s_addr = INADDR_ANY; - server_addr.sin_port = mpHtons(atoi(g_nodeConfigSettings.rt_udp_port_number)); + server_addr.sin_port = mpHtons(atoi(g_nodeConfigSettings.rt_listener_udp_port_number)); if (mpBind(sockRtCommandListener, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) { @@ -367,22 +371,34 @@ bool Ros_RtMotionControl_OpenSocket() return false; } - return true; -} + //========================================================================================= + sockStatusSender = mpSocket(AF_INET, SOCK_DGRAM, 0); + if (sockStatusSender < 0) + { + Ros_Debug_BroadcastMsg("ERROR: Could not allocate Status socket for RT interface"); + return false; + } -//Essentially a clone of the /robot_status topic. But decoupled from the industrial_msgs/RobotStatus type. -void Ros_RtMotionControl_ConvertRobotStatusToRobotState(RobotState* state) -{ - state->drives_powered = g_messages_RobotStatus.msgRobotStatus->drives_powered.val; - state->e_stopped = g_messages_RobotStatus.msgRobotStatus->e_stopped.val; - state->in_motion = g_messages_RobotStatus.msgRobotStatus->in_motion.val; - state->play_mode = (g_messages_RobotStatus.msgRobotStatus->mode.val == industrial_msgs__msg__RobotMode__AUTO); - state->motion_possible = g_messages_RobotStatus.msgRobotStatus->motion_possible.val; - state->error = g_messages_RobotStatus.msgRobotStatus->in_error.val; - if (g_messages_RobotStatus.msgRobotStatus->error_codes.size > 0) - state->error_code = g_messages_RobotStatus.msgRobotStatus->error_codes.data[0]; - else - state->error_code = 0; + // Bind socket to port + memset(&client_addr_status_messages, 0, sizeof(client_addr_status_messages)); + client_addr_status_messages.sin_family = AF_INET; + client_addr_status_messages.sin_addr.s_addr = INADDR_ANY; + client_addr_status_messages.sin_port = mpHtons(atoi(g_nodeConfigSettings.rt_status_udp_port_number)); + + if (mpBind(sockStatusSender, (struct sockaddr*)&client_addr_status_messages, sizeof(client_addr_status_messages)) < 0) + { + Ros_Debug_BroadcastMsg("ERROR: Failed to bind UDP socket for real-time motion control"); + mpClose(sockStatusSender); + sockStatusSender = -1; + return false; + } + + //Spin up a separate normal-priorty thread to send out the robot status info + mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, + (FUNCPTR)Ros_RtMotionControl_SendRobotStatus, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + + return true; } void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* command, RtReply* reply) @@ -455,8 +471,6 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman reply->previousCommandPositionCartesian[groupIndex][TCP_Rz] = DEG_0001_TO_RAD(coord.rz); reply->previousCommandPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); } - - Ros_RtMotionControl_ConvertRobotStatusToRobotState(&reply->state); } bool Ros_RtMotionControl_CheckForFsuInterference(MOTION_MODE mode, int* tools) @@ -567,3 +581,31 @@ void Ros_RtMotionControl_PurgeBufferedPackets() break; } } + +//Essentially a clone of the /robot_status topic. But decoupled from the industrial_msgs/RobotStatus type. +void Ros_RtMotionControl_SendRobotStatus() +{ + RobotState stateMsg; + + int client_addr_len = sizeof(client_addr_status_messages); + + while (TRUE) + { + Ros_Sleep(g_nodeConfigSettings.rt_status_sleep_period); + + //------------------------------------------------------------------------------- + stateMsg.drives_powered = g_messages_RobotStatus.msgRobotStatus->drives_powered.val; + stateMsg.e_stopped = g_messages_RobotStatus.msgRobotStatus->e_stopped.val; + stateMsg.in_motion = g_messages_RobotStatus.msgRobotStatus->in_motion.val; + stateMsg.play_mode = (g_messages_RobotStatus.msgRobotStatus->mode.val == industrial_msgs__msg__RobotMode__AUTO); + stateMsg.motion_possible = g_messages_RobotStatus.msgRobotStatus->motion_possible.val; + stateMsg.error = g_messages_RobotStatus.msgRobotStatus->in_error.val; + if (g_messages_RobotStatus.msgRobotStatus->error_codes.size > 0) + stateMsg.error_code = g_messages_RobotStatus.msgRobotStatus->error_codes.data[0]; + else + stateMsg.error_code = 0; + + //------------------------------------------------------------------------------- + mpSendTo(sockStatusSender, (char*)&stateMsg, sizeof(RobotState), 0, (struct sockaddr*)&client_addr_status_messages, client_addr_len); + } +} diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 005c6b2f..d0c601f7 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -66,17 +66,6 @@ typedef enum MAX_AXES //maxies } CartesianIndices; -typedef struct -{ - BOOL drives_powered; - BOOL e_stopped; - BOOL in_motion; - BOOL play_mode; - BOOL motion_possible; - BOOL error; - int error_code; -} RobotState; - //########################################################################## // !All data is little-endian! //########################################################################## @@ -128,10 +117,6 @@ struct RtReply_ { UINT32 sequenceEcho; - //Essentially a clone of the /robot_status topic. But decoupled - //from the industrial_msgs/RobotStatus type. - RobotState state; - //This is indicative of where the robot is physically located. //Please note that this will trail behind the commanded position. //The joint ordering will match that of the original command @@ -157,6 +142,24 @@ struct RtReply_ } PACKED; typedef struct RtReply_ RtReply; + +//########################################################################## +// !All data is little-endian! +//########################################################################## + +//Essentially a clone of the /robot_status topic. But decoupled +//from the industrial_msgs/RobotStatus type. +typedef struct +{ + BOOL drives_powered; + BOOL e_stopped; + BOOL in_motion; + BOOL play_mode; + BOOL motion_possible; + BOOL error; + int error_code; +} RobotState; + //When checking for interference from the FSU speed limit, there will //likely be some small rounding errors. So, the deviation must exceed //this amount before the system will report that the FSU has limited From 1370af6c4a9eb235a7c0eeb7b208e5cd3d00cc98 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 26 Mar 2026 15:49:15 -0400 Subject: [PATCH 074/101] Set the dest address for SendRobotStatus routine --- src/RealTimeMotionControl.c | 74 +++++++++++++++++++++---------------- 1 file changed, 42 insertions(+), 32 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 5f7ec7f1..4eb1eef5 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -21,13 +21,15 @@ void Ros_RtMotionControl_PurgeBufferedPackets(); void Ros_RtMotionControl_SendRobotStatus(); static int sockRtCommandListener = -1; -static int sockStatusSender = -1; +static int sockRtStatusSender = -1; static struct sockaddr_in client_addr_status_messages; static LONG prevRtCmdPosition[MAX_GROUPS][MAX_AXES]; static LONG howMuchShouldIHaveMoved[MAX_GROUPS][MAX_AXES]; +static BOOL rtMotionControlConnected = FALSE; + void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) { MP_EXPOS_DATA moveData; @@ -88,6 +90,22 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) { previous_client_addr = client_addr; //only allow a single commander //flag is cleared down below + + //setup destination for status messages + memset(&client_addr_status_messages, 0, sizeof(client_addr_status_messages)); + client_addr_status_messages.sin_family = AF_INET; + client_addr_status_messages.sin_addr.s_addr = client_addr.sin_addr.s_addr; + client_addr_status_messages.sin_port = mpHtons(atoi(g_nodeConfigSettings.rt_status_udp_port_number)); + + if (mpConnect(sockRtStatusSender, (struct sockaddr*)&client_addr_status_messages, sizeof(client_addr_status_messages)) == ERROR) + { + Ros_Debug_BroadcastMsg("ERROR: Failed to set destination address for RT status"); + mpClose(sockRtStatusSender); + sockRtStatusSender = -1; + break; + } + + rtMotionControlConnected = TRUE; } else { @@ -211,6 +229,7 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) } } + rtMotionControlConnected = FALSE; Ros_Debug_BroadcastMsg("Ending Rt Session"); } @@ -372,27 +391,13 @@ bool Ros_RtMotionControl_OpenSocket() } //========================================================================================= - sockStatusSender = mpSocket(AF_INET, SOCK_DGRAM, 0); - if (sockStatusSender < 0) + sockRtStatusSender = mpSocket(AF_INET, SOCK_DGRAM, 0); + if (sockRtStatusSender < 0) { Ros_Debug_BroadcastMsg("ERROR: Could not allocate Status socket for RT interface"); return false; } - // Bind socket to port - memset(&client_addr_status_messages, 0, sizeof(client_addr_status_messages)); - client_addr_status_messages.sin_family = AF_INET; - client_addr_status_messages.sin_addr.s_addr = INADDR_ANY; - client_addr_status_messages.sin_port = mpHtons(atoi(g_nodeConfigSettings.rt_status_udp_port_number)); - - if (mpBind(sockStatusSender, (struct sockaddr*)&client_addr_status_messages, sizeof(client_addr_status_messages)) < 0) - { - Ros_Debug_BroadcastMsg("ERROR: Failed to bind UDP socket for real-time motion control"); - mpClose(sockStatusSender); - sockStatusSender = -1; - return false; - } - //Spin up a separate normal-priorty thread to send out the robot status info mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)Ros_RtMotionControl_SendRobotStatus, @@ -591,21 +596,26 @@ void Ros_RtMotionControl_SendRobotStatus() while (TRUE) { - Ros_Sleep(g_nodeConfigSettings.rt_status_sleep_period); - - //------------------------------------------------------------------------------- - stateMsg.drives_powered = g_messages_RobotStatus.msgRobotStatus->drives_powered.val; - stateMsg.e_stopped = g_messages_RobotStatus.msgRobotStatus->e_stopped.val; - stateMsg.in_motion = g_messages_RobotStatus.msgRobotStatus->in_motion.val; - stateMsg.play_mode = (g_messages_RobotStatus.msgRobotStatus->mode.val == industrial_msgs__msg__RobotMode__AUTO); - stateMsg.motion_possible = g_messages_RobotStatus.msgRobotStatus->motion_possible.val; - stateMsg.error = g_messages_RobotStatus.msgRobotStatus->in_error.val; - if (g_messages_RobotStatus.msgRobotStatus->error_codes.size > 0) - stateMsg.error_code = g_messages_RobotStatus.msgRobotStatus->error_codes.data[0]; - else - stateMsg.error_code = 0; + Ros_Sleep(10); - //------------------------------------------------------------------------------- - mpSendTo(sockStatusSender, (char*)&stateMsg, sizeof(RobotState), 0, (struct sockaddr*)&client_addr_status_messages, client_addr_len); + while (rtMotionControlConnected) + { + Ros_Sleep(g_nodeConfigSettings.rt_status_sleep_period); + + //------------------------------------------------------------------------------- + stateMsg.drives_powered = g_messages_RobotStatus.msgRobotStatus->drives_powered.val; + stateMsg.e_stopped = g_messages_RobotStatus.msgRobotStatus->e_stopped.val; + stateMsg.in_motion = g_messages_RobotStatus.msgRobotStatus->in_motion.val; + stateMsg.play_mode = (g_messages_RobotStatus.msgRobotStatus->mode.val == industrial_msgs__msg__RobotMode__AUTO); + stateMsg.motion_possible = g_messages_RobotStatus.msgRobotStatus->motion_possible.val; + stateMsg.error = g_messages_RobotStatus.msgRobotStatus->in_error.val; + if (g_messages_RobotStatus.msgRobotStatus->error_codes.size > 0) + stateMsg.error_code = g_messages_RobotStatus.msgRobotStatus->error_codes.data[0]; + else + stateMsg.error_code = 0; + + //------------------------------------------------------------------------------- + mpSendTo(sockRtStatusSender, (char*)&stateMsg, sizeof(RobotState), 0, (struct sockaddr*)&client_addr_status_messages, client_addr_len); + } } } From 3e8f8ba08aa29bec9f57b44734df703d00848c7c Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 26 Mar 2026 16:03:41 -0400 Subject: [PATCH 075/101] Update documentation --- doc/rt_control.md | 90 +++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 80 insertions(+), 10 deletions(-) diff --git a/doc/rt_control.md b/doc/rt_control.md index 35986862..b723ab92 100644 --- a/doc/rt_control.md +++ b/doc/rt_control.md @@ -34,7 +34,7 @@ After that, the user must wait until the robot replies before sending the next i Each subsequent command must increment the `sequenceId`. Additionally, each subsequent command must not be sent until the robot replies to the previous command. This will occur at the rate of the `period` from the [start_rt_mode](ros_api.md#start_rt_mode) service. -If a command is not received with 30 seconds (default), then the session times out and is dropped. +If a command is not received with 5 seconds (default), then the session times out and is dropped. At that point, the server must be reactivated by calling `stop_traj_mode` and `start_rt_mode`. A "keep-alive" can be used by sending a command with zero increments. @@ -52,11 +52,44 @@ The command packet is a *packed* `RtPacket` structure. //########################################################################## struct RtPacket { + //The version of the command packet must match the value expected + //by MotoROS2. + int version; + //The packet type must match the control_mode which was specified + //in when invoking the start_rt_mode service. + + PacketType packetType; + + //Must increment sequentially with each new command packet. + UINT32 sequenceId; - double delta[MAX_GROUPS][MAX_AXES]; //[8][8] - int toolIndex[MAX_GROUPS]; //[8] + + //The order of the joints must be in the order of [S L U R B T E 8]. + //Please note that for seven axis robots, the 'E' joint is phyically + //mounted in the middle of the arm. But it must be sent at the end + //of the joint array. See JointIndices enum. + // + //For joint-space, this will be radians of each joint. + // + //For cartesian, this will be meters and radians of the TCP. + //The order of the joints must be in the order of [X Y Z Rx Ry Rz Re 8]. + //See CartesianIndices enum. + //Rotations are applied in the order of ZYX. + + double delta[MAX_GROUPS][MP_GRP_AXES_NUM]; + + //Set tool that will be used by motion API (ie: passed by us to mpExRcsIncrementMove(..)) + //NOTE: this will change the 'motion tool' ONLY for those increments which + // haven't yet been added to the increment queue. See also the ROS 2 + // 'select_tool' service definition file in motoros2_interfaces. + + int toolIndex[MAX_GROUPS]; //TOOL 0 - 63 + + //Reserved for future expansion + + char reserved[64]; } ``` @@ -64,8 +97,6 @@ The `version` must match the version number expected by the server. If it does not match the expected value, the packet will be rejected and the connection will be dropped. The current version is `1`. -This contains a sequence ID, the increments for each control group, and the tool number to use for each control group. - #### Joints When the `control_mode` is `JOINT_ANGLES (1)`, the order of the joints in the `delta` array must be in the order of `S L U R B T E 8`. @@ -131,12 +162,30 @@ This indicates that the robot did not complete the full increment as commanded. struct RtReply { UINT32 sequenceEcho; - - double feedbackPositionJoints[MAX_GROUPS][MAX_JOINTS]; //[8][8] - double feedbackPositionCartesian[MAX_GROUPS][MAX_JOINTS]; //[8][8] - double previousCommandPositionJoints[MAX_GROUPS][MAX_AXES]; //[8][8] - double previousCommandPositionCartesian[MAX_GROUPS][MAX_AXES]; //[8][8] + //This is indicative of where the robot is physically located. + //Please note that this will trail behind the commanded position. + //The joint ordering will match that of the original command + //packet. See JointIndices and CartesianIndices enums. + + double feedbackPositionJoints[MAX_GROUPS][MP_GRP_AXES_NUM]; + double feedbackPositionCartesian[MAX_GROUPS][MP_GRP_AXES_NUM]; + + //The command position is the target destination you are instructing + //the robot to reach. It's the calculated endpoint based on the sum + //of all position increments received from the user. + // + //This is used to track if the robot's speed is being limited + //by the Functional Safety Unit (FSU). It can also be used to + //monitor the latency between command and feedback. + + double previousCommandPositionJoints[MAX_GROUPS][MP_GRP_AXES_NUM]; + double previousCommandPositionCartesian[MAX_GROUPS][MP_GRP_AXES_NUM]; + + //If the FSU speed limit is enabled, it can truncate the commanded + //delta increments. This flag is an indicator that the *previous* + //command cycle was truncated. It does NOT indicate that this most + //recent command packet was truncated. bool fsuInterferenceDetected; } @@ -148,3 +197,24 @@ Other motion modes may not be used at the same time as the real-time motion cont The service to start those modes will fail when invoked. By calling `stop_traj_mode`, the R/T server will be disposed. At that time, another motion mode may be used. + +# R/T Status Monitoring + +When the R/T Motion Control is activated, MotoROS2 will begin to send the `RobotState` structure on port UDP `22001` (default). +This is essentially a clone of the `/robot_status topic`. +But decoupled from the `industrial_msgs/RobotStatus` type. + +``` +struct RobotState +{ + BOOL drives_powered; + BOOL e_stopped; + BOOL in_motion; + BOOL play_mode; + BOOL motion_possible; + BOOL error; + int error_code; +} +``` + +This will be sent every `rt_status_sleep_period` milliseconds. From 84217043215ee390340af8b3253f0c4bc57840b1 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 26 Mar 2026 16:18:55 -0400 Subject: [PATCH 076/101] Remove control flag that terminates status info --- src/RealTimeMotionControl.c | 40 ++++++++++++++----------------------- 1 file changed, 15 insertions(+), 25 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 4eb1eef5..ddaa4d53 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -28,8 +28,6 @@ static struct sockaddr_in client_addr_status_messages; static LONG prevRtCmdPosition[MAX_GROUPS][MAX_AXES]; static LONG howMuchShouldIHaveMoved[MAX_GROUPS][MAX_AXES]; -static BOOL rtMotionControlConnected = FALSE; - void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) { MP_EXPOS_DATA moveData; @@ -104,8 +102,6 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) sockRtStatusSender = -1; break; } - - rtMotionControlConnected = TRUE; } else { @@ -229,7 +225,6 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) } } - rtMotionControlConnected = FALSE; Ros_Debug_BroadcastMsg("Ending Rt Session"); } @@ -596,26 +591,21 @@ void Ros_RtMotionControl_SendRobotStatus() while (TRUE) { - Ros_Sleep(10); - - while (rtMotionControlConnected) - { - Ros_Sleep(g_nodeConfigSettings.rt_status_sleep_period); - - //------------------------------------------------------------------------------- - stateMsg.drives_powered = g_messages_RobotStatus.msgRobotStatus->drives_powered.val; - stateMsg.e_stopped = g_messages_RobotStatus.msgRobotStatus->e_stopped.val; - stateMsg.in_motion = g_messages_RobotStatus.msgRobotStatus->in_motion.val; - stateMsg.play_mode = (g_messages_RobotStatus.msgRobotStatus->mode.val == industrial_msgs__msg__RobotMode__AUTO); - stateMsg.motion_possible = g_messages_RobotStatus.msgRobotStatus->motion_possible.val; - stateMsg.error = g_messages_RobotStatus.msgRobotStatus->in_error.val; - if (g_messages_RobotStatus.msgRobotStatus->error_codes.size > 0) - stateMsg.error_code = g_messages_RobotStatus.msgRobotStatus->error_codes.data[0]; - else - stateMsg.error_code = 0; + Ros_Sleep(g_nodeConfigSettings.rt_status_sleep_period); + + //------------------------------------------------------------------------------- + stateMsg.drives_powered = g_messages_RobotStatus.msgRobotStatus->drives_powered.val; + stateMsg.e_stopped = g_messages_RobotStatus.msgRobotStatus->e_stopped.val; + stateMsg.in_motion = g_messages_RobotStatus.msgRobotStatus->in_motion.val; + stateMsg.play_mode = (g_messages_RobotStatus.msgRobotStatus->mode.val == industrial_msgs__msg__RobotMode__AUTO); + stateMsg.motion_possible = g_messages_RobotStatus.msgRobotStatus->motion_possible.val; + stateMsg.error = g_messages_RobotStatus.msgRobotStatus->in_error.val; + if (g_messages_RobotStatus.msgRobotStatus->error_codes.size > 0) + stateMsg.error_code = g_messages_RobotStatus.msgRobotStatus->error_codes.data[0]; + else + stateMsg.error_code = 0; - //------------------------------------------------------------------------------- - mpSendTo(sockRtStatusSender, (char*)&stateMsg, sizeof(RobotState), 0, (struct sockaddr*)&client_addr_status_messages, client_addr_len); - } + //------------------------------------------------------------------------------- + mpSendTo(sockRtStatusSender, (char*)&stateMsg, sizeof(RobotState), 0, (struct sockaddr*)&client_addr_status_messages, client_addr_len); } } From cb5eaa986b8a2e200cf46659d5081ce67bec4938 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 27 Mar 2026 08:45:30 -0400 Subject: [PATCH 077/101] `mpConnect` is not needed since I'm using `sockaddr` --- src/RealTimeMotionControl.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index ddaa4d53..62dca5e9 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -95,13 +95,13 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) client_addr_status_messages.sin_addr.s_addr = client_addr.sin_addr.s_addr; client_addr_status_messages.sin_port = mpHtons(atoi(g_nodeConfigSettings.rt_status_udp_port_number)); - if (mpConnect(sockRtStatusSender, (struct sockaddr*)&client_addr_status_messages, sizeof(client_addr_status_messages)) == ERROR) - { - Ros_Debug_BroadcastMsg("ERROR: Failed to set destination address for RT status"); - mpClose(sockRtStatusSender); - sockRtStatusSender = -1; - break; - } + //if (mpConnect(sockRtStatusSender, (struct sockaddr*)&client_addr_status_messages, sizeof(client_addr_status_messages)) == ERROR) + //{ + // Ros_Debug_BroadcastMsg("ERROR: Failed to set destination address for RT status"); + // mpClose(sockRtStatusSender); + // sockRtStatusSender = -1; + // break; + //} } else { From 2b18361dc0e530357bf1fc0c09bc4179b3540343 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 27 Mar 2026 14:21:19 -0400 Subject: [PATCH 078/101] Formatting errors caught by linter --- doc/rt_control.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/rt_control.md b/doc/rt_control.md index b723ab92..c3f10883 100644 --- a/doc/rt_control.md +++ b/doc/rt_control.md @@ -198,13 +198,13 @@ The service to start those modes will fail when invoked. By calling `stop_traj_mode`, the R/T server will be disposed. At that time, another motion mode may be used. -# R/T Status Monitoring +## R/T Status Monitoring When the R/T Motion Control is activated, MotoROS2 will begin to send the `RobotState` structure on port UDP `22001` (default). This is essentially a clone of the `/robot_status topic`. But decoupled from the `industrial_msgs/RobotStatus` type. -``` +```c struct RobotState { BOOL drives_powered; From 9a493ee1e3751bc25f5eb9c556a53c65a8b83089 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Thu, 30 Jul 2026 11:19:43 -0400 Subject: [PATCH 079/101] Check return code when starting motion --- src/MotionControl.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/MotionControl.c b/src/MotionControl.c index 0d4af441..8dfd90f3 100644 --- a/src/MotionControl.c +++ b/src/MotionControl.c @@ -1552,7 +1552,8 @@ MotionNotReadyCode Ros_MotionControl_StartMotionMode(MOTION_MODE mode, rosidl_ru } } - StartInterpolationTask(mode); + if (!StartInterpolationTask(mode)) + return MOTION_NOT_READY_ERROR; // have to initialize the prevPulsePos that will be used when interpolating the traj for(grpNo = 0; grpNo < g_Ros_Controller.numGroup; ++grpNo) From 2c55b27eb8961ac634f2ac3f28f9bf524c709d77 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 12 Aug 2026 08:52:52 -0400 Subject: [PATCH 080/101] Ensure the default values match the default in the yaml --- src/ConfigFile.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ConfigFile.h b/src/ConfigFile.h index f8fa6e53..c3dcfa0b 100644 --- a/src/ConfigFile.h +++ b/src/ConfigFile.h @@ -114,9 +114,9 @@ typedef enum #define DEFAULT_RT_STATUS_UDP_PORT_NUMBER "22001" #define DEFAULT_RT_STATUS_SLEEP_PERIOD 10 -#define DEFAULT_TIMEOUT_FOR_RT_MSG 30000 +#define DEFAULT_TIMEOUT_FOR_RT_MSG 5000 -#define DEFAULT_MAX_SEQUENCE_DIFFERENCE 10 +#define DEFAULT_MAX_SEQUENCE_DIFFERENCE 3 typedef struct { From f6d32eece8c2e7fbc92e8c28ca8a51eb39c2c919 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 12 Aug 2026 10:42:51 -0400 Subject: [PATCH 081/101] Add assertions when allocating RT socket --- doc/troubleshooting.md | 75 +++++++++++++++++++++++++++++++++++++ src/ErrorHandling.h | 4 ++ src/RealTimeMotionControl.c | 12 +++--- src/RealTimeMotionControl.h | 2 +- 4 files changed, 85 insertions(+), 8 deletions(-) diff --git a/doc/troubleshooting.md b/doc/troubleshooting.md index 28eda526..f297ae73 100644 --- a/doc/troubleshooting.md +++ b/doc/troubleshooting.md @@ -757,6 +757,81 @@ Describe the problem and include the following items: - copy of `motoros2_config.yaml` copied from the robot controller. - verbatim copy of the alarm text as seen on the teach pendant (alarm number and `[subcode]`). +### Alarm: 8011[66] + +*Example:* + +```text +ALARM 8011 + Failed to init service (x) +[66] +``` + +*Solution:* +Save a copy of the output of the [debug-listener script](#debug-log-client) and the `PANELBOX.LOG` from the robot's teach pendant. +Open a new issue on the [Issue tracker](https://github.com/yaskawa-global/motoros2/issues), describe the problem and attach `PANELBOX.LOG` and the debug log to the issue. +Include a verbatim copy of the alarm text as seen on the teach pendant (alarm number and `[subcode]`). + +### Alarm: 8011[67] + +*Example:* + +```text +ALARM 8011 + Failed adding service (x) +[67] +``` + +*Solution:* +Save a copy of the output of the [debug-listener script](#debug-log-client) and the `PANELBOX.LOG` from the robot's teach pendant. +Open a new issue on the [Issue tracker](https://github.com/yaskawa-global/motoros2/issues), describe the problem and attach `PANELBOX.LOG` and the debug log to the issue. +Include a verbatim copy of the alarm text as seen on the teach pendant (alarm number and `[subcode]`). + +### Alarm: 8011[68] + +*Example:* + +```text +ALARM 8011 + Failed to allocate RT socket +[68] +``` + +*Solution:* +Save a copy of the output of the [debug-listener script](#debug-log-client) and the `PANELBOX.LOG` from the robot's teach pendant. +Open a new issue on the [Issue tracker](https://github.com/yaskawa-global/motoros2/issues), describe the problem and attach `PANELBOX.LOG` and the debug log to the issue. +Include a verbatim copy of the alarm text as seen on the teach pendant (alarm number and `[subcode]`). + +### Alarm: 8011[69] + +*Example:* + +```text +ALARM 8011 + Failed to bind RT socket +[69] +``` + +*Solution:* +Save a copy of the output of the [debug-listener script](#debug-log-client) and the `PANELBOX.LOG` from the robot's teach pendant. +Open a new issue on the [Issue tracker](https://github.com/yaskawa-global/motoros2/issues), describe the problem and attach `PANELBOX.LOG` and the debug log to the issue. +Include a verbatim copy of the alarm text as seen on the teach pendant (alarm number and `[subcode]`). + +### Alarm: 8011[70] + +*Example:* + +```text +ALARM 8011 + Failed to allocate RT socket +[70] +``` + +*Solution:* +Save a copy of the output of the [debug-listener script](#debug-log-client) and the `PANELBOX.LOG` from the robot's teach pendant. +Open a new issue on the [Issue tracker](https://github.com/yaskawa-global/motoros2/issues), describe the problem and attach `PANELBOX.LOG` and the debug log to the issue. +Include a verbatim copy of the alarm text as seen on the teach pendant (alarm number and `[subcode]`). + ### Alarm: 8012[xx] *Example:* diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h index c56df670..88a0b425 100644 --- a/src/ErrorHandling.h +++ b/src/ErrorHandling.h @@ -179,6 +179,10 @@ typedef enum SUBCODE_CONFIGURATION_FILE_YAML_PARSING_ERROR, SUBCODE_FAIL_INIT_SERVICE_START_RT_MODE, SUBCODE_FAIL_ADD_SERVICE_START_RT_MODE, + SUBCODE_FAIL_ALLOCATE_RT_CMD_SOCKET, + SUBCODE_FAIL_BIND_RT_SOCKET, + + SUBCODE_FAIL_ALLOCATE_RT_FB_SOCKET } ALARM_ASSERTION_FAIL_SUBCODE; //8011 diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 62dca5e9..1bbbac56 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -360,7 +360,7 @@ void Ros_RtMotionControl_Cleanup() //Do not delete interpolation task. This is handled in Ros_MotionControl_StopTrajMode. } -bool Ros_RtMotionControl_OpenSocket() +void Ros_RtMotionControl_OpenSocket() { struct sockaddr_in server_addr; @@ -368,7 +368,7 @@ bool Ros_RtMotionControl_OpenSocket() if (sockRtCommandListener < 0) { Ros_Debug_BroadcastMsg("ERROR: Could not allocate socket for RT interface"); - return false; + motoRosAssert_withMsg(false, SUBCODE_FAIL_ALLOCATE_RT_CMD_SOCKET, "Failed to allocate RT socket"); } // Bind socket to port @@ -381,8 +381,8 @@ bool Ros_RtMotionControl_OpenSocket() { Ros_Debug_BroadcastMsg("ERROR: Failed to bind UDP socket for real-time motion control"); mpClose(sockRtCommandListener); - sockRtCommandListener = -1; - return false; + sockRtCommandListener = -1; + motoRosAssert_withMsg(false, SUBCODE_FAIL_BIND_RT_SOCKET, "Failed to bind RT socket"); } //========================================================================================= @@ -390,15 +390,13 @@ bool Ros_RtMotionControl_OpenSocket() if (sockRtStatusSender < 0) { Ros_Debug_BroadcastMsg("ERROR: Could not allocate Status socket for RT interface"); - return false; + motoRosAssert_withMsg(false, SUBCODE_FAIL_ALLOCATE_RT_FB_SOCKET, "Failed to allocate RT socket"); } //Spin up a separate normal-priorty thread to send out the robot status info mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)Ros_RtMotionControl_SendRobotStatus, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); - - return true; } void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* command, RtReply* reply) diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index d0c601f7..5fa19ce8 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -13,7 +13,7 @@ #define PACKED __attribute__ ((__packed__)) extern void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode); -extern bool Ros_RtMotionControl_OpenSocket(); +extern void Ros_RtMotionControl_OpenSocket(); extern void Ros_RtMotionControl_Cleanup(); typedef enum From 450d6883bcd7dbc199d56caead57e9a712aeb86a Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 12 Aug 2026 10:44:40 -0400 Subject: [PATCH 082/101] Reword error message for clarity --- src/RealTimeMotionControl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 1bbbac56..a31142d1 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -303,7 +303,7 @@ bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DAT if (abs(pulse_increments[i]) > ctrlGroup->maxInc.maxIncrement[i]) { - Ros_Debug_BroadcastMsg("ERROR: The increment for axis [%d] exceeds the maximum limit of [%d] pulse counts", pulse_increments[i], ctrlGroup->maxInc.maxIncrement[i]); + Ros_Debug_BroadcastMsg("ERROR: Group [%d] Axis [%d] has been commanded to move [%d] pulse counts this increment, exceeding the maximum limit of [%d] pulse counts", groupNo, i, pulse_increments[i], ctrlGroup->maxInc.maxIncrement[i]); return false; } } From cb418bef28963a2909b5b6d5c0bb01cc6742d915 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 12 Aug 2026 10:50:06 -0400 Subject: [PATCH 083/101] Ensure that msgRobotStatus is allocated before usage --- src/ControllerStatusIO.c | 7 +++++-- src/RealTimeMotionControl.c | 6 ++++++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/ControllerStatusIO.c b/src/ControllerStatusIO.c index c5604a68..0efc23d1 100644 --- a/src/ControllerStatusIO.c +++ b/src/ControllerStatusIO.c @@ -168,8 +168,11 @@ BOOL Ros_Controller_Initialize() //================================== //create message for robot status //TODO(gavanderhoorn): use micro_ros_utilities_create_message_memory(..) instead - g_messages_RobotStatus.msgRobotStatus = industrial_msgs__msg__RobotStatus__create(); - rosidl_runtime_c__int32__Sequence__init(&g_messages_RobotStatus.msgRobotStatus->error_codes, MAX_ALARM_COUNT + 1); + if (g_messages_RobotStatus.msgRobotStatus == NULL) //may already be allocated in RealTimeMotionControl.c + { + g_messages_RobotStatus.msgRobotStatus = industrial_msgs__msg__RobotStatus__create(); + rosidl_runtime_c__int32__Sequence__init(&g_messages_RobotStatus.msgRobotStatus->error_codes, MAX_ALARM_COUNT + 1); + } //================================== // Check and report eco-mode settings diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index a31142d1..f17380c3 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -587,6 +587,12 @@ void Ros_RtMotionControl_SendRobotStatus() int client_addr_len = sizeof(client_addr_status_messages); + if (g_messages_RobotStatus.msgRobotStatus == NULL) //may already be allocated in ControllerStatusIO.c + { + g_messages_RobotStatus.msgRobotStatus = industrial_msgs__msg__RobotStatus__create(); + rosidl_runtime_c__int32__Sequence__init(&g_messages_RobotStatus.msgRobotStatus->error_codes, MAX_ALARM_COUNT + 1); + } + while (TRUE) { Ros_Sleep(g_nodeConfigSettings.rt_status_sleep_period); From cd45d09fa716e834b0aed770711461586986ec89 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 12 Aug 2026 10:51:09 -0400 Subject: [PATCH 084/101] `magnitude` instead of `vector` --- src/RealTimeMotionControl.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index f17380c3..e0013297 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -335,14 +335,14 @@ bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA moveData->grp_pos_info[groupNo].pos[TCP_8] = incomingCommand->delta[groupNo][TCP_8]; //pulse or micron (no known manipulators use this axis) - double vector = sqrt(pow(incomingCommand->delta[groupNo][TCP_X], 2) + //x^2 + double magnitude = sqrt(pow(incomingCommand->delta[groupNo][TCP_X], 2) + //x^2 pow(incomingCommand->delta[groupNo][TCP_Y], 2) + //y^2 pow(incomingCommand->delta[groupNo][TCP_Z], 2)); //z^2 // Assuming 'elapsed_ms' is your variable for time in milliseconds. const double max_speed_mm_per_ms = 1.5; // 1500 mm/sec is 1.5 mm/ms - if (vector > (max_speed_mm_per_ms * g_Ros_Controller.interpolPeriod)) + if (magnitude > (max_speed_mm_per_ms * g_Ros_Controller.interpolPeriod)) { Ros_Debug_BroadcastMsg("ERROR: The increment for the TCP exceeds the maximum limit of 1500 mm/sec"); return false; From d27168b1c69aa4421ff10af16b9d4e9dc9256126 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 12 Aug 2026 10:57:18 -0400 Subject: [PATCH 085/101] `magnitude` should be mm instead of m --- src/MathConstants.h | 1 + src/RealTimeMotionControl.c | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/MathConstants.h b/src/MathConstants.h index 62f5e76f..2d2e059b 100644 --- a/src/MathConstants.h +++ b/src/MathConstants.h @@ -16,6 +16,7 @@ #define DEGREES_PER_RAD (57.295779513082) // macro +#define METERS_TO_MILLIMETERS(x) (x * 0.001) #define MICROMETERS_TO_METERS(x) (x * 0.000001) #define METERS_TO_MICROMETERS(x) (x * 1000000) #define RAD_TO_DEG_0001(x) (x * DEGREES_PER_RAD * 10000) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index e0013297..dad4c958 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -335,9 +335,9 @@ bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA moveData->grp_pos_info[groupNo].pos[TCP_8] = incomingCommand->delta[groupNo][TCP_8]; //pulse or micron (no known manipulators use this axis) - double magnitude = sqrt(pow(incomingCommand->delta[groupNo][TCP_X], 2) + //x^2 + double magnitude = METERS_TO_MILLIMETERS(sqrt(pow(incomingCommand->delta[groupNo][TCP_X], 2) + //x^2 pow(incomingCommand->delta[groupNo][TCP_Y], 2) + //y^2 - pow(incomingCommand->delta[groupNo][TCP_Z], 2)); //z^2 + pow(incomingCommand->delta[groupNo][TCP_Z], 2))); //z^2 // Assuming 'elapsed_ms' is your variable for time in milliseconds. const double max_speed_mm_per_ms = 1.5; // 1500 mm/sec is 1.5 mm/ms From 9bdf8b836c97f432de7b49cb48e04b17cb338dfd Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 12 Aug 2026 11:04:42 -0400 Subject: [PATCH 086/101] Ensure `tidIncMoveThread` is valid before deleting it --- src/MotionControl.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/MotionControl.c b/src/MotionControl.c index 8dfd90f3..8ee575c8 100644 --- a/src/MotionControl.c +++ b/src/MotionControl.c @@ -1641,8 +1641,11 @@ void Ros_MotionControl_StopTrajMode() ioWriteData.ulValue = 0; mpWriteIO(&ioWriteData, 1); - mpDeleteTask(g_Ros_Controller.tidIncMoveThread); - g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + if (g_Ros_Controller.tidIncMoveThread != INVALID_TASK) + { + mpDeleteTask(g_Ros_Controller.tidIncMoveThread); + g_Ros_Controller.tidIncMoveThread = INVALID_TASK; + } } BOOL Ros_MotionControl_IsMotionMode_Trajectory() From 4619da41ebfb0949bc1a412bd7b0a8bca158514a Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 19 Aug 2026 14:57:38 -0400 Subject: [PATCH 087/101] Ensure `howMuchShouldIHaveMoved` is initialized from previous cycle --- src/RealTimeMotionControl.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index dad4c958..bf079af4 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -54,6 +54,7 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) //========================================================================================= bzero(prevRtCmdPosition, MAX_GROUPS * MAX_AXES * sizeof(LONG)); + bzero(howMuchShouldIHaveMoved, MAX_GROUPS * MAX_AXES * sizeof(LONG)); if (mode == MOTION_MODE_RT_JOINT) Ros_RtMotionControl_InitJointSpace(&moveData); @@ -426,7 +427,10 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman Ros_CtrlGroup_ConvertMotoUnitsToRosUnits(group, pulsePos_moto, reply->feedbackPositionJoints[groupIndex]); for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) - degrees[axis] = RAD_TO_DEG_0001(reply->feedbackPositionJoints[groupIndex][axis]); + { + //if (group->axisType.type[axis] == AXIS_ROTATION) + degrees[axis] = RAD_TO_DEG_0001(reply->feedbackPositionJoints[groupIndex][axis]); + } //Cart mpConvAxesToCartPos(groupIndex, degrees, command->toolIndex[groupIndex], &figure, &coord); From e8eb2c7d99fb1e9de9145aab71272a3f2a3f0e9b Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 19 Aug 2026 15:18:27 -0400 Subject: [PATCH 088/101] Exclude external axes from cartesian feedback --- src/RealTimeMotionControl.c | 67 +++++++++++++++++++++---------------- 1 file changed, 38 insertions(+), 29 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index bf079af4..552957d5 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -423,26 +423,29 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman //================================================================================ Ros_CtrlGroup_GetFBPulsePos(group, pulsePos_moto); - //Angles + //Angles (or meters for a linear track) Ros_CtrlGroup_ConvertMotoUnitsToRosUnits(group, pulsePos_moto, reply->feedbackPositionJoints[groupIndex]); - - for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) + + if (group->groupId <= MP_R8_GID) //is a robot and not an external axis { - //if (group->axisType.type[axis] == AXIS_ROTATION) - degrees[axis] = RAD_TO_DEG_0001(reply->feedbackPositionJoints[groupIndex][axis]); - } + for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) + { + //if (group->axisType.type[axis] == AXIS_ROTATION) + degrees[axis] = RAD_TO_DEG_0001(reply->feedbackPositionJoints[groupIndex][axis]); + } - //Cart - mpConvAxesToCartPos(groupIndex, degrees, command->toolIndex[groupIndex], &figure, &coord); + //Cart + mpConvAxesToCartPos(groupIndex, degrees, command->toolIndex[groupIndex], &figure, &coord); - reply->feedbackPositionCartesian[groupIndex][TCP_X] = MICROMETERS_TO_METERS(coord.x); - reply->feedbackPositionCartesian[groupIndex][TCP_Y] = MICROMETERS_TO_METERS(coord.y); - reply->feedbackPositionCartesian[groupIndex][TCP_Z] = MICROMETERS_TO_METERS(coord.z); + reply->feedbackPositionCartesian[groupIndex][TCP_X] = MICROMETERS_TO_METERS(coord.x); + reply->feedbackPositionCartesian[groupIndex][TCP_Y] = MICROMETERS_TO_METERS(coord.y); + reply->feedbackPositionCartesian[groupIndex][TCP_Z] = MICROMETERS_TO_METERS(coord.z); - reply->feedbackPositionCartesian[groupIndex][TCP_Rx] = DEG_0001_TO_RAD(coord.rx); - reply->feedbackPositionCartesian[groupIndex][TCP_Ry] = DEG_0001_TO_RAD(coord.ry); - reply->feedbackPositionCartesian[groupIndex][TCP_Rz] = DEG_0001_TO_RAD(coord.rz); - reply->feedbackPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); + reply->feedbackPositionCartesian[groupIndex][TCP_Rx] = DEG_0001_TO_RAD(coord.rx); + reply->feedbackPositionCartesian[groupIndex][TCP_Ry] = DEG_0001_TO_RAD(coord.ry); + reply->feedbackPositionCartesian[groupIndex][TCP_Rz] = DEG_0001_TO_RAD(coord.rz); + reply->feedbackPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); + } //================================================================================ //CMD pos @@ -454,24 +457,30 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman ctrlGroup.sCtrlGrp = groupIndex; mpGetPulsePos(&ctrlGroup, &cmdPulse); - //rad + //rad (or meter for linear track) Ros_CtrlGroup_ConvertMotoUnitsToRosUnits(group, cmdPulse.lPos, reply->previousCommandPositionJoints[groupIndex]); - - //deg - for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) - degrees[axis] = RAD_TO_DEG_0001(reply->previousCommandPositionJoints[groupIndex][axis]); - //Cart - mpConvAxesToCartPos(groupIndex, degrees, command->toolIndex[groupIndex], &figure, &coord); + if (group->groupId <= MP_R8_GID) //is a robot and not an external axis + { + //deg + for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) + degrees[axis] = RAD_TO_DEG_0001(reply->previousCommandPositionJoints[groupIndex][axis]); + + if (group->groupId <= MP_R8_GID) //is a robot and not an external axis + { + //Cart + mpConvAxesToCartPos(groupIndex, degrees, command->toolIndex[groupIndex], &figure, &coord); - reply->previousCommandPositionCartesian[groupIndex][TCP_X] = MICROMETERS_TO_METERS(coord.x); - reply->previousCommandPositionCartesian[groupIndex][TCP_Y] = MICROMETERS_TO_METERS(coord.y); - reply->previousCommandPositionCartesian[groupIndex][TCP_Z] = MICROMETERS_TO_METERS(coord.z); + reply->previousCommandPositionCartesian[groupIndex][TCP_X] = MICROMETERS_TO_METERS(coord.x); + reply->previousCommandPositionCartesian[groupIndex][TCP_Y] = MICROMETERS_TO_METERS(coord.y); + reply->previousCommandPositionCartesian[groupIndex][TCP_Z] = MICROMETERS_TO_METERS(coord.z); - reply->previousCommandPositionCartesian[groupIndex][TCP_Rx] = DEG_0001_TO_RAD(coord.rx); - reply->previousCommandPositionCartesian[groupIndex][TCP_Ry] = DEG_0001_TO_RAD(coord.ry); - reply->previousCommandPositionCartesian[groupIndex][TCP_Rz] = DEG_0001_TO_RAD(coord.rz); - reply->previousCommandPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); + reply->previousCommandPositionCartesian[groupIndex][TCP_Rx] = DEG_0001_TO_RAD(coord.rx); + reply->previousCommandPositionCartesian[groupIndex][TCP_Ry] = DEG_0001_TO_RAD(coord.ry); + reply->previousCommandPositionCartesian[groupIndex][TCP_Rz] = DEG_0001_TO_RAD(coord.rz); + reply->previousCommandPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); + } + } } } From 3c300f9b296db130053ae738b9dde402aba00c7d Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 19 Aug 2026 15:25:50 -0400 Subject: [PATCH 089/101] Set the cartesian frame based on the group type --- src/RealTimeMotionControl.c | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 552957d5..053db852 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -11,7 +11,7 @@ #include "MotoROS.h" void Ros_RtMotionControl_InitJointSpace(MP_EXPOS_DATA* moveData); -void Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData); +bool Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData); bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData); bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData); void Ros_RtMotionControl_Cleanup(); @@ -59,7 +59,10 @@ void Ros_RtMotionControl_HyperRobotCommanderX5(MOTION_MODE mode) if (mode == MOTION_MODE_RT_JOINT) Ros_RtMotionControl_InitJointSpace(&moveData); else - Ros_RtMotionControl_InitCartesian(&moveData); + { + if (!Ros_RtMotionControl_InitCartesian(&moveData)) + return; //abort rt session + } Ros_Debug_BroadcastMsg("Starting RT session"); @@ -251,7 +254,7 @@ void Ros_RtMotionControl_InitJointSpace(MP_EXPOS_DATA* moveData) } } -void Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) +bool Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) { int i; MP_CARTPOS_EX_SEND_DATA cartSendData; @@ -276,11 +279,24 @@ void Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) mpGetToolNo(MP_R1_GID + i, &getToolResp); cartSendData.sRobotNo = i; - cartSendData.sFrame = 1; //1 = RF + + CtrlGroup* group = g_Ros_Controller.ctrlGroups[i]; + if (group->groupId <= MP_R8_GID) //is a robot and not an external axis + cartSendData.sFrame = 1; //1 = RF + else if (group->groupId <= MP_B8_GID) //is a base track + cartSendData.sFrame = 0; //0 = BF + else + { + Ros_Debug_BroadcastMsg("ERROR: Group [%d] is an external positioner. Cartesian control mode is not supported for this group."); + return false; + } + cartSendData.sToolNo = getToolResp.sToolNo; mpGetCartPosEx(&cartSendData, &cartRespData); memcpy(prevRtCmdPosition[i], cartRespData.lPos, sizeof(LONG) * MAX_AXES); } + + return true; } bool Ros_RtMotionControl_ParseJointSpace(RtPacket* incomingCommand, MP_EXPOS_DATA* moveData) From c4da270861accfa87008a36f55cd5010fd724e75 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 19 Aug 2026 15:42:44 -0400 Subject: [PATCH 090/101] Pack and version the `RobotState` packets --- doc/rt_control.md | 2 ++ src/RealTimeMotionControl.c | 2 ++ src/RealTimeMotionControl.h | 8 ++++++-- 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/doc/rt_control.md b/doc/rt_control.md index c3f10883..c627cfe3 100644 --- a/doc/rt_control.md +++ b/doc/rt_control.md @@ -207,6 +207,8 @@ But decoupled from the `industrial_msgs/RobotStatus` type. ```c struct RobotState { + int version; + BOOL drives_powered; BOOL e_stopped; BOOL in_motion; diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 053db852..27196380 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -622,6 +622,8 @@ void Ros_RtMotionControl_SendRobotStatus() rosidl_runtime_c__int32__Sequence__init(&g_messages_RobotStatus.msgRobotStatus->error_codes, MAX_ALARM_COUNT + 1); } + stateMsg.version = VERSION_OF_ROBOT_STATE_PACKET; + while (TRUE) { Ros_Sleep(g_nodeConfigSettings.rt_status_sleep_period); diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 5fa19ce8..31d0dab2 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -149,8 +149,11 @@ typedef struct RtReply_ RtReply; //Essentially a clone of the /robot_status topic. But decoupled //from the industrial_msgs/RobotStatus type. -typedef struct +#define VERSION_OF_ROBOT_STATE_PACKET 1 +struct RobotState_ { + int version; + BOOL drives_powered; BOOL e_stopped; BOOL in_motion; @@ -158,7 +161,8 @@ typedef struct BOOL motion_possible; BOOL error; int error_code; -} RobotState; +} PACKED; +typedef struct RobotState_ RobotState; //When checking for interference from the FSU speed limit, there will //likely be some small rounding errors. So, the deviation must exceed From 6afbe03c64e4f2916749ad45886d265c2c806ea3 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Wed, 19 Aug 2026 15:47:31 -0400 Subject: [PATCH 091/101] Use `groupId` instead of `groupIndex`. My testing just happened to work because I was only using two robots. --- src/RealTimeMotionControl.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 27196380..6887b708 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -248,7 +248,7 @@ void Ros_RtMotionControl_InitJointSpace(MP_EXPOS_DATA* moveData) moveData->grp_pos_info[i].pos_tag.data[3] = MP_INC_PULSE_DTYPE; - ctrlGroup.sCtrlGrp = i; + ctrlGroup.sCtrlGrp = g_Ros_Controller.ctrlGroups[i]->groupId;; mpGetPulsePos(&ctrlGroup, &cmdPulse); memcpy(prevRtCmdPosition[i], cmdPulse.lPos, sizeof(cmdPulse.lPos)); } @@ -470,7 +470,7 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman //Answer: No, it should not. That should only be used when converting incoming // positional commands that contain an absolute position. // See https://github.com/Yaskawa-Global/motoros2/discussions/455 - ctrlGroup.sCtrlGrp = groupIndex; + ctrlGroup.sCtrlGrp = g_Ros_Controller.ctrlGroups[groupIndex]->groupId; mpGetPulsePos(&ctrlGroup, &cmdPulse); //rad (or meter for linear track) @@ -523,7 +523,7 @@ bool Ros_RtMotionControl_CheckForFsuInterference(MOTION_MODE mode, int* tools) //Answer: No, it should not. That should only be used when converting incoming // positional commands that contain an absolute position. // See https://github.com/Yaskawa-Global/motoros2/discussions/455 - ctrlGroup.sCtrlGrp = groupIndex; + ctrlGroup.sCtrlGrp = g_Ros_Controller.ctrlGroups[groupIndex]->groupId; mpGetPulsePos(&ctrlGroup, &cmdPulse); } else if (mode == MOTION_MODE_RT_CARTESIAN) From 900c2145dd7f87013287362766effd4d6d17d2d2 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 21 Aug 2026 11:44:24 -0400 Subject: [PATCH 092/101] `METERS_TO_MILLIMETERS` was inverted --- src/MathConstants.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/MathConstants.h b/src/MathConstants.h index 2d2e059b..f9b57a48 100644 --- a/src/MathConstants.h +++ b/src/MathConstants.h @@ -16,7 +16,7 @@ #define DEGREES_PER_RAD (57.295779513082) // macro -#define METERS_TO_MILLIMETERS(x) (x * 0.001) +#define METERS_TO_MILLIMETERS(x) (x * 1000) #define MICROMETERS_TO_METERS(x) (x * 0.000001) #define METERS_TO_MICROMETERS(x) (x * 1000000) #define RAD_TO_DEG_0001(x) (x * DEGREES_PER_RAD * 10000) From 0191f08b070c2956822fa5fb578e42898c5de34a Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 21 Aug 2026 12:54:53 -0400 Subject: [PATCH 093/101] Missing argument for debug msg --- src/RealTimeMotionControl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 6887b708..a3003347 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -287,7 +287,7 @@ bool Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) cartSendData.sFrame = 0; //0 = BF else { - Ros_Debug_BroadcastMsg("ERROR: Group [%d] is an external positioner. Cartesian control mode is not supported for this group."); + Ros_Debug_BroadcastMsg("ERROR: Group [%d] is an external positioner. Cartesian control mode is not supported for this group.", i); return false; } From 0eb069b5f28878ac23ad172aacf61aa579853ecc Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 21 Aug 2026 13:45:53 -0400 Subject: [PATCH 094/101] Ensure `prevRtCmdPosition` gets processed for all groups --- src/RealTimeMotionControl.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index a3003347..a2f6b5ba 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -506,6 +506,7 @@ bool Ros_RtMotionControl_CheckForFsuInterference(MOTION_MODE mode, int* tools) MP_PULSE_POS_RSP_DATA cmdPulse; MP_CARTPOS_EX_SEND_DATA cartSendData; MP_CART_POS_RSP_DATA_EX cartRespData; + bool returnValue = FALSE; for (int groupIndex = 0; groupIndex < g_Ros_Controller.numGroup; groupIndex += 1) { @@ -572,11 +573,11 @@ bool Ros_RtMotionControl_CheckForFsuInterference(MOTION_MODE mode, int* tools) //Ros_Debug_BroadcastMsg("difference = %d", difference); //Ros_Debug_BroadcastMsg("---------"); - return TRUE; + returnValue = TRUE; } } } - return FALSE; + return returnValue; } void Ros_RtMotionControl_PurgeBufferedPackets() From fc799d4ebb2b5ac25639d426906434d02a072faf Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 21 Aug 2026 13:47:09 -0400 Subject: [PATCH 095/101] Redundant `if` statement --- src/RealTimeMotionControl.c | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index a2f6b5ba..7f23a139 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -482,20 +482,17 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) degrees[axis] = RAD_TO_DEG_0001(reply->previousCommandPositionJoints[groupIndex][axis]); - if (group->groupId <= MP_R8_GID) //is a robot and not an external axis - { - //Cart - mpConvAxesToCartPos(groupIndex, degrees, command->toolIndex[groupIndex], &figure, &coord); + //Cart + mpConvAxesToCartPos(groupIndex, degrees, command->toolIndex[groupIndex], &figure, &coord); - reply->previousCommandPositionCartesian[groupIndex][TCP_X] = MICROMETERS_TO_METERS(coord.x); - reply->previousCommandPositionCartesian[groupIndex][TCP_Y] = MICROMETERS_TO_METERS(coord.y); - reply->previousCommandPositionCartesian[groupIndex][TCP_Z] = MICROMETERS_TO_METERS(coord.z); + reply->previousCommandPositionCartesian[groupIndex][TCP_X] = MICROMETERS_TO_METERS(coord.x); + reply->previousCommandPositionCartesian[groupIndex][TCP_Y] = MICROMETERS_TO_METERS(coord.y); + reply->previousCommandPositionCartesian[groupIndex][TCP_Z] = MICROMETERS_TO_METERS(coord.z); - reply->previousCommandPositionCartesian[groupIndex][TCP_Rx] = DEG_0001_TO_RAD(coord.rx); - reply->previousCommandPositionCartesian[groupIndex][TCP_Ry] = DEG_0001_TO_RAD(coord.ry); - reply->previousCommandPositionCartesian[groupIndex][TCP_Rz] = DEG_0001_TO_RAD(coord.rz); - reply->previousCommandPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); - } + reply->previousCommandPositionCartesian[groupIndex][TCP_Rx] = DEG_0001_TO_RAD(coord.rx); + reply->previousCommandPositionCartesian[groupIndex][TCP_Ry] = DEG_0001_TO_RAD(coord.ry); + reply->previousCommandPositionCartesian[groupIndex][TCP_Rz] = DEG_0001_TO_RAD(coord.rz); + reply->previousCommandPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); } } } From 675f0f01cded76134ed71cd54550a86185bee795 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 21 Aug 2026 13:48:58 -0400 Subject: [PATCH 096/101] Utilize existing helper function --- src/RealTimeMotionControl.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 7f23a139..95b520c5 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -442,7 +442,7 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman //Angles (or meters for a linear track) Ros_CtrlGroup_ConvertMotoUnitsToRosUnits(group, pulsePos_moto, reply->feedbackPositionJoints[groupIndex]); - if (group->groupId <= MP_R8_GID) //is a robot and not an external axis + if (Ros_CtrlGroup_IsRobot(group)) //is a robot and not an external axis { for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) { @@ -476,7 +476,7 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman //rad (or meter for linear track) Ros_CtrlGroup_ConvertMotoUnitsToRosUnits(group, cmdPulse.lPos, reply->previousCommandPositionJoints[groupIndex]); - if (group->groupId <= MP_R8_GID) //is a robot and not an external axis + if (Ros_CtrlGroup_IsRobot(group)) //is a robot and not an external axis { //deg for (int axis = 0; axis < MP_GRP_AXES_NUM; axis += 1) From 4a78657d1fbe7c569336ea517f8984d4c66066d9 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 21 Aug 2026 14:02:07 -0400 Subject: [PATCH 097/101] Verify only robots are being used for cartesian --- src/RealTimeMotionControl.c | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 95b520c5..42d2fe2a 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -268,6 +268,18 @@ bool Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) for (i = 0; i < g_Ros_Controller.numGroup; i++) { + CtrlGroup* group = g_Ros_Controller.ctrlGroups[i]; + + if (Ros_CtrlGroup_IsRobot(group)) //is a robot and not an external axis + cartSendData.sFrame = 1; //1 = RF + else if (Ros_CtrlGroup_IsBase(group)) //is a base track + cartSendData.sFrame = 0; //0 = BF + else + { + Ros_Debug_BroadcastMsg("ERROR: Group [%d] is an external positioner. Cartesian control mode is not supported for this group.", i); + return false; + } + moveData->ctrl_grp |= (1 << i); moveData->grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(g_Ros_Controller.ctrlGroups[i]); moveData->grp_pos_info[i].pos_tag.data[3] = MP_INC_RF_DTYPE; @@ -280,17 +292,6 @@ bool Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) cartSendData.sRobotNo = i; - CtrlGroup* group = g_Ros_Controller.ctrlGroups[i]; - if (group->groupId <= MP_R8_GID) //is a robot and not an external axis - cartSendData.sFrame = 1; //1 = RF - else if (group->groupId <= MP_B8_GID) //is a base track - cartSendData.sFrame = 0; //0 = BF - else - { - Ros_Debug_BroadcastMsg("ERROR: Group [%d] is an external positioner. Cartesian control mode is not supported for this group.", i); - return false; - } - cartSendData.sToolNo = getToolResp.sToolNo; mpGetCartPosEx(&cartSendData, &cartRespData); memcpy(prevRtCmdPosition[i], cartRespData.lPos, sizeof(LONG) * MAX_AXES); From 27c20ce0dca730a63c2336e6a9111de54a5c80ed Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 21 Aug 2026 14:02:58 -0400 Subject: [PATCH 098/101] Check group id instead of index --- src/RealTimeMotionControl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 42d2fe2a..d3720415 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -288,7 +288,7 @@ bool Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) // on the pendant, but I was commanding increments on tool #0. Because of this, // the first motion on each axis would trigger the FSU detection mechanism. But // it immediately recovers after one cycle. - mpGetToolNo(MP_R1_GID + i, &getToolResp); + mpGetToolNo(group->groupId, &getToolResp); cartSendData.sRobotNo = i; From e8ed9aff424eadd69bd9a78fee20b8c7eb7d7900 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 21 Aug 2026 14:09:10 -0400 Subject: [PATCH 099/101] Ensure `msgRobotStatus` gets NULL'ed out --- src/ControllerStatusIO.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ControllerStatusIO.c b/src/ControllerStatusIO.c index 0efc23d1..5b71fea6 100644 --- a/src/ControllerStatusIO.c +++ b/src/ControllerStatusIO.c @@ -242,6 +242,7 @@ void Ros_Controller_Cleanup() Ros_Debug_BroadcastMsg("Failed cleaning up robot status publisher: %d", ret); industrial_msgs__msg__RobotStatus__destroy(g_messages_RobotStatus.msgRobotStatus); + g_messages_RobotStatus.msgRobotStatus = NULL; MOTOROS2_MEM_TRACE_REPORT(ctrlr_fini); } From 4020c6c1095aa6d8a71ab91defce4e89b9282380 Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 21 Aug 2026 14:15:49 -0400 Subject: [PATCH 100/101] Exclude base tracks from cartesian mode. More testing is needed. I don't have the hardware. We can revisit this topic in the future. --- src/RealTimeMotionControl.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index d3720415..2f9bf031 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -272,8 +272,10 @@ bool Ros_RtMotionControl_InitCartesian(MP_EXPOS_DATA* moveData) if (Ros_CtrlGroup_IsRobot(group)) //is a robot and not an external axis cartSendData.sFrame = 1; //1 = RF - else if (Ros_CtrlGroup_IsBase(group)) //is a base track - cartSendData.sFrame = 0; //0 = BF + //TODO: Test on an actual track. I don't think you can get the cartesian of the track alone. + // I think that you can only get robot position, but in Base Frame. +// else if (Ros_CtrlGroup_IsBase(group)) //is a base track +// cartSendData.sFrame = 0; //0 = BF else { Ros_Debug_BroadcastMsg("ERROR: Group [%d] is an external positioner. Cartesian control mode is not supported for this group.", i); From 15513c5e1b706e9896c05bacc059808b3818db5b Mon Sep 17 00:00:00 2001 From: Ted Miller Date: Fri, 21 Aug 2026 14:45:39 -0400 Subject: [PATCH 101/101] Use quaternion for rotation instead of euler. --- doc/rt_control.md | 21 ++++++++---------- src/RealTimeMotionControl.c | 43 ++++++++++++++++++++++++++----------- src/RealTimeMotionControl.h | 13 ++++++----- 3 files changed, 45 insertions(+), 32 deletions(-) diff --git a/doc/rt_control.md b/doc/rt_control.md index c627cfe3..369b6cb0 100644 --- a/doc/rt_control.md +++ b/doc/rt_control.md @@ -14,7 +14,7 @@ This control mode minimizes overhead as much as possible by routing the user com ## Activation This control mode is activated using the [start_rt_mode](ros_api.md#start_rt_mode) service. -The user must specify the `control_mode` to indicate whether the increments will be joint offsets (radians) or cartesian TCP offsets (meters / radians). +The user must specify the `control_mode` to indicate whether the increments will be joint offsets (radians) or cartesian TCP offsets (meters / quaternion). If this service is successful, it will return a `result_code` of `Ready (1)`. Otherwise, please examine the `result_code` and `message` files in the response for more information. @@ -73,10 +73,9 @@ struct RtPacket // //For joint-space, this will be radians of each joint. // - //For cartesian, this will be meters and radians of the TCP. - //The order of the joints must be in the order of [X Y Z Rx Ry Rz Re 8]. + //For cartesian, this will be meters and quaternion of the TCP. + //The order of the joints must be in the order of [X Y Z Qx Qy Qz Qw Re]. //See CartesianIndices enum. - //Rotations are applied in the order of ZYX. double delta[MAX_GROUPS][MP_GRP_AXES_NUM]; @@ -123,7 +122,7 @@ enum JointIndices #### Cartesian -When the `control_mode` is `CARTESIAN (2)`, the order of the joints in the `delta` array must be in order of `X Y Z Rx Ry Rz Re 8`. +When the `control_mode` is `CARTESIAN (2)`, the order of the joints in the `delta` array must be in order of `X Y Z Qx Qy Qz Qw Re`. See `CartesianIndices` enum. @@ -134,19 +133,17 @@ enum CartesianIndices TCP_Y, TCP_Z, - TCP_Rx, //radians - TCP_Ry, - TCP_Rz, - TCP_Re, + TCP_Qx, //quaternion + TCP_Qy, + TCP_Qz, + TCP_Qw, - TCP_8, //pulse + TCP_Re, //radians MAX_AXES } ``` -Please note that rotations are applied in the order of `Z Y X`. - ### Data format (reply) The command packet is a *packed* `RtReply` structure. diff --git a/src/RealTimeMotionControl.c b/src/RealTimeMotionControl.c index 2f9bf031..09b5e8d0 100644 --- a/src/RealTimeMotionControl.c +++ b/src/RealTimeMotionControl.c @@ -338,7 +338,7 @@ bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA { int groupNo; - // For each control group, convert radians to pulses and prepare moveData + // For each control group, convert incoming command and prepare moveData for (groupNo = 0; groupNo < g_Ros_Controller.numGroup; groupNo += 1) { moveData->grp_pos_info[groupNo].pos_tag.data[2] = incomingCommand->toolIndex[groupNo]; @@ -347,13 +347,22 @@ bool Ros_RtMotionControl_ParseCartesian(RtPacket* incomingCommand, MP_EXPOS_DATA moveData->grp_pos_info[groupNo].pos[TCP_Y] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][TCP_Y]); moveData->grp_pos_info[groupNo].pos[TCP_Z] = METERS_TO_MICROMETERS(incomingCommand->delta[groupNo][TCP_Z]); - moveData->grp_pos_info[groupNo].pos[TCP_Rx] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][TCP_Rx]); - moveData->grp_pos_info[groupNo].pos[TCP_Ry] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][TCP_Ry]); - moveData->grp_pos_info[groupNo].pos[TCP_Rz] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][TCP_Rz]); + Quaternion q; + q.x = incomingCommand->delta[groupNo][TCP_Qx]; + q.y = incomingCommand->delta[groupNo][TCP_Qy]; + q.z = incomingCommand->delta[groupNo][TCP_Qz]; + q.w = incomingCommand->delta[groupNo][TCP_Qw]; + + LONG rx_deg = 0, ry_deg = 0, rz_deg = 0; + QuatConversion_GeomMsgsQuaternion_To_MpCoordOrient(&q, &rx_deg, &ry_deg, &rz_deg); + + moveData->grp_pos_info[groupNo].pos[3] = rx_deg; + moveData->grp_pos_info[groupNo].pos[4] = ry_deg; + moveData->grp_pos_info[groupNo].pos[5] = rz_deg; - moveData->grp_pos_info[groupNo].pos[TCP_Re] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][TCP_Re]); + moveData->grp_pos_info[groupNo].pos[6] = RAD_TO_DEG_0001(incomingCommand->delta[groupNo][TCP_Re]); - moveData->grp_pos_info[groupNo].pos[TCP_8] = incomingCommand->delta[groupNo][TCP_8]; //pulse or micron (no known manipulators use this axis) + moveData->grp_pos_info[groupNo].pos[7] = 0; double magnitude = METERS_TO_MILLIMETERS(sqrt(pow(incomingCommand->delta[groupNo][TCP_X], 2) + //x^2 pow(incomingCommand->delta[groupNo][TCP_Y], 2) + //y^2 @@ -456,13 +465,17 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman //Cart mpConvAxesToCartPos(groupIndex, degrees, command->toolIndex[groupIndex], &figure, &coord); + Quaternion qFb; + QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion(coord.rx, coord.ry, coord.rz, &qFb); + reply->feedbackPositionCartesian[groupIndex][TCP_X] = MICROMETERS_TO_METERS(coord.x); reply->feedbackPositionCartesian[groupIndex][TCP_Y] = MICROMETERS_TO_METERS(coord.y); reply->feedbackPositionCartesian[groupIndex][TCP_Z] = MICROMETERS_TO_METERS(coord.z); - reply->feedbackPositionCartesian[groupIndex][TCP_Rx] = DEG_0001_TO_RAD(coord.rx); - reply->feedbackPositionCartesian[groupIndex][TCP_Ry] = DEG_0001_TO_RAD(coord.ry); - reply->feedbackPositionCartesian[groupIndex][TCP_Rz] = DEG_0001_TO_RAD(coord.rz); + reply->feedbackPositionCartesian[groupIndex][TCP_Qx] = qFb.x; + reply->feedbackPositionCartesian[groupIndex][TCP_Qy] = qFb.y; + reply->feedbackPositionCartesian[groupIndex][TCP_Qz] = qFb.z; + reply->feedbackPositionCartesian[groupIndex][TCP_Qw] = qFb.w; reply->feedbackPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); } @@ -488,13 +501,17 @@ void Ros_RtMotionControl_PopulateReplyMessage(MOTION_MODE mode, RtPacket* comman //Cart mpConvAxesToCartPos(groupIndex, degrees, command->toolIndex[groupIndex], &figure, &coord); + Quaternion qCmd; + QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion(coord.rx, coord.ry, coord.rz, &qCmd); + reply->previousCommandPositionCartesian[groupIndex][TCP_X] = MICROMETERS_TO_METERS(coord.x); reply->previousCommandPositionCartesian[groupIndex][TCP_Y] = MICROMETERS_TO_METERS(coord.y); reply->previousCommandPositionCartesian[groupIndex][TCP_Z] = MICROMETERS_TO_METERS(coord.z); - reply->previousCommandPositionCartesian[groupIndex][TCP_Rx] = DEG_0001_TO_RAD(coord.rx); - reply->previousCommandPositionCartesian[groupIndex][TCP_Ry] = DEG_0001_TO_RAD(coord.ry); - reply->previousCommandPositionCartesian[groupIndex][TCP_Rz] = DEG_0001_TO_RAD(coord.rz); + reply->previousCommandPositionCartesian[groupIndex][TCP_Qx] = qCmd.x; + reply->previousCommandPositionCartesian[groupIndex][TCP_Qy] = qCmd.y; + reply->previousCommandPositionCartesian[groupIndex][TCP_Qz] = qCmd.z; + reply->previousCommandPositionCartesian[groupIndex][TCP_Qw] = qCmd.w; reply->previousCommandPositionCartesian[groupIndex][TCP_Re] = DEG_0001_TO_RAD(coord.ex1); } } @@ -560,7 +577,7 @@ bool Ros_RtMotionControl_CheckForFsuInterference(MOTION_MODE mode, int* tools) // axes. Even if I put all of my commanded increment into a single axis, all // three of them are going to react. So, the cmd-value of my intended axis may // not be the value I expect. - if (mode == MOTION_MODE_RT_CARTESIAN && axis >= TCP_Rx) + if (mode == MOTION_MODE_RT_CARTESIAN && axis > TCP_Z) { break; } diff --git a/src/RealTimeMotionControl.h b/src/RealTimeMotionControl.h index 31d0dab2..19adaa11 100644 --- a/src/RealTimeMotionControl.h +++ b/src/RealTimeMotionControl.h @@ -56,10 +56,10 @@ typedef enum TCP_Y, TCP_Z, - TCP_Rx, //radians - TCP_Ry, - TCP_Rz, - TCP_Re, + TCP_Qx, //quaternion + TCP_Qy, + TCP_Qz, + TCP_Qw, TCP_8, //pulse @@ -90,10 +90,9 @@ struct RtPacket_ // //For joint-space, this will be radians of each joint. // - //For cartesian, this will be meters and radians of the TCP. - //The order of the joints must be in the order of [X Y Z Rx Ry Rz Re 8]. + //For cartesian, this will be meters and quaternion of the TCP. + //The order of the joints must be in the order of [X Y Z Qx Qy Qz Qw 8]. //See CartesianIndices enum. - //Rotations are applied in the order of ZYX. double delta[MAX_GROUPS][MP_GRP_AXES_NUM]; //Set tool that will be used by motion API (ie: passed by us to mpExRcsIncrementMove(..))