Trajectory doesn't Complete with Speed Limit set on Robot FSU

Question

Trajectory doesn't Complete with Speed Limit set on Robot FSU

Aneeer opened this issue a year ago · 6 comments

ROS2 Version: Humble
MotoROS2 Version: Latest (0.1.1)
MicroROS Version: Latest

Hi team. So I'm in a bit of predicament where I don't know the best way to slow my robot from a functional safety standpoint without the controller throwing me an error. Wondering the best approach to robustly slow down the robot's movements in certain regions of the workspace from a safety standpoint.

Debug log:

Click to expand

[1970-01-01 00:00:00.000000] [192.168.1.31:55509]: Found micro-ROS PC Agent
[2023-09-18 12:18:20.120000] [192.168.1.31:55509]: rclc_support_init_with_options = 0
[2023-09-18 12:18:20.130000] [192.168.1.31:55509]: remap_rules str: ''
[2023-09-18 12:18:20.143200] [192.168.1.31:55509]: len(remap_rules str): 0
[2023-09-18 12:18:20.151400] [192.168.1.31:55509]: num parsed remap rules: 0
[2023-09-18 12:18:20.161400] [192.168.1.31:55509]: faux argc: 2
[2023-09-18 12:18:20.171000] [192.168.1.31:55509]: rcl_parse_arguments = 0
[2023-09-18 12:18:20.193600] [192.168.1.31:55509]: rclc_node_init_with_options = 0
[2023-09-18 12:18:20.204200] [192.168.1.31:55509]: Ros_Communication_Initialize(..):comm_exec_init: memory free: 1045936 bytes, delta: -760
[2023-09-18 12:18:20.224200] [192.168.1.31:55509]: Initializing controller
[2023-09-18 12:18:20.234600] [192.168.1.31:55509]: Number of motion groups: 1 (max supported: 8)
[2023-09-18 12:18:20.253000] [192.168.1.31:55509]: axisType[0]: Rot	Rot	Rot	Rot	Rot	Rot	---	---	
[2023-09-18 12:18:20.263600] [192.168.1.31:55509]: pulse->unit[0]: 109518.9727	144069.6010	131031.6280	108028.9186	101406.4562	74132.5027	--	--	
[2023-09-18 12:18:20.281400] [192.168.1.31:55509]: maxInc[0] (in motoman joint order): 955, 1156, 1143, 1372, 1238, 1371, 0
[2023-09-18 12:18:20.302400] [192.168.1.31:55509]: maxSpeed[0] (in ros joint order): 2.179988, 2.005975, 2.180771, 3.175076, 3.052074, 4.623478, 0.000000
[2023-09-18 12:18:20.322400] [192.168.1.31:55509]: Creating new task: Add To Inc Q (Group 1)
[2023-09-18 12:18:20.332600] [192.168.1.31:55509]: Created ctrl group 0, memfree = (1008168) bytes
[2023-09-18 12:18:20.356000] [192.168.1.31:55509]: Creating new task: IncMoveTask
[2023-09-18 12:18:20.366800] [192.168.1.31:55509]: IncMoveTask Started
[2023-09-18 12:18:20.370800] [192.168.1.31:55509]: Eco-mode: enabled
[2023-09-18 12:18:20.379400] [192.168.1.31:55509]: Eco-mode: timeout: 10 min
[2023-09-18 12:18:20.390000] [192.168.1.31:55509]: Ros_Controller_Initialize(..):ctrlr_init: memory free: 1007848 bytes, delta: -38088
[2023-09-18 12:18:20.410600] [192.168.1.31:55509]: Checking validity of robot job: INIT_ROS
[2023-09-18 12:18:20.471200] [192.168.1.31:55509]: Job is OK
[2023-09-18 12:18:20.477600] [192.168.1.31:55509]: Initializing PositionMonitor publishers
[2023-09-18 12:18:20.510400] [192.168.1.31:55509]: PositionMonitor: TF topic absolute
[2023-09-18 12:18:20.520200] [192.168.1.31:55509]: PositionMonitor: publishing TF to '/tf'
[2023-09-18 12:18:20.542400] [192.168.1.31:55509]: Ros_PositionMonitor_Initialize(..):pos_mon_init: memory free: 1005336 bytes, delta: -2512
[2023-09-18 12:18:20.563200] [192.168.1.31:55509]: Initializing ActionServer FollowJointTrajectory
[2023-09-18 12:18:20.607000] [192.168.1.31:55509]: Allocating FollowJointTrajectory goal request
[2023-09-18 12:18:20.620400] [192.168.1.31:55509]: Maximum length of trajectories: 200 points
[2023-09-18 12:18:20.639000] [192.168.1.31:55509]: g_actionServer_FJT_SendGoal_Request__sizeof = 451008
[2023-09-18 12:18:20.675800] [192.168.1.31:55509]: Ros_ActionServer_FJT_Initialize(..):fjt_init: memory free: 1003480 bytes, delta: -1856
[2023-09-18 12:18:20.700200] [192.168.1.31:55509]: Ros_ServiceQueueTrajPoint_Initialize(..):svc_queue_point_init: memory free: 996232 bytes, delta: -7248
[2023-09-18 12:18:20.745000] [192.168.1.31:55509]: Ros_ServiceReadWriteIO_Initialize(..):svc_rw_io_init: memory free: 994520 bytes, delta: -1712
[2023-09-18 12:18:20.770600] [192.168.1.31:55509]: Ros_ServiceResetError_Initialize(..):svc_reset_error_init: memory free: 994240 bytes, delta: -280
[2023-09-18 12:18:20.795400] [192.168.1.31:55509]: Ros_ServiceStartTrajMode_Initialize(..):svc_start_traj_mode_init: memory free: 993944 bytes, delta: -296
[2023-09-18 12:18:20.819000] [192.168.1.31:55509]: Ros_ServiceStartPointQueueMode_Initialize(..):svc_start_point_queue_mode_init: memory free: 993648 bytes, delta: -296
[2023-09-18 12:18:20.844400] [192.168.1.31:55509]: Ros_ServiceStopTrajMode_Initialize(..):svc_stop_traj_mode_init: memory free: 993360 bytes, delta: -288
[2023-09-18 12:18:20.866000] [192.168.1.31:55509]: Ros_ServiceSelectMotionTool_Initialize(..):svc_select_tool_init: memory free: 993064 bytes, delta: -296
[2023-09-18 12:18:20.883200] [192.168.1.31:55509]: Initialization complete. Memory available: (993064) bytes. Memory in use: (55512) bytes
[2023-09-18 12:18:20.886400] [192.168.1.31:55509]: Starting UserLan link state monitor (port: 1)
[2023-09-18 20:19:26.686816] [192.168.1.31:55509]: Ros_MotionControl_StartMotionMode: enter
[2023-09-18 20:19:29.167816] [192.168.1.31:55509]: Robot job is ready for ROS commands.
[2023-09-18 20:19:29.179216] [192.168.1.31:55509]: Ros_MotionControl_StartMotionMode: exit
[2023-09-18 20:19:29.188816] [192.168.1.31:55509]: Ros_MotionControl_ActiveTrajMode = 1
[2023-09-18 20:19:29.198816] [192.168.1.31:55509]: Ros_ServiceStartTrajMode_Trigger: activated
[2023-09-18 20:19:34.371881] [192.168.1.31:55509]: Trajectory contains 11 points
[2023-09-18 20:19:34.383081] [192.168.1.31:55509]: Initializing trajectory for group #0
[2023-09-18 20:19:34.396881] [192.168.1.31:55509]: Group #0 - Trajectory is ready for processing
[2023-09-18 20:19:34.411081] [192.168.1.31:55509]: feedback_msg_alloc_cfg size = 41352 bytes
[2023-09-18 20:19:34.414881] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=0.336: (0.0031, -0.8077, -0.8551, -0.0022, -0.0190, 1.5701)]
[2023-09-18 20:19:34.482881] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=0.480: (0.0055, -0.7987, -0.8982, -0.0045, -0.0325, 1.5702)]
[2023-09-18 20:19:34.528481] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=0.687: (0.0082, -0.7915, -0.9420, -0.0071, -0.0477, 1.5703)]
[2023-09-18 20:19:34.576881] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=0.853: (0.0076, -0.7655, -0.9786, -0.0067, -0.0439, 1.5703)]
[2023-09-18 20:19:34.616481] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=1.070: (0.0000, -0.7000, -1.0000, 0.0000, 0.0000, 1.5700)]
[2023-09-18 20:19:34.735881] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=1.532: (-0.0468, -0.4110, -0.9356, 0.0424, 0.2706, 1.5684)]
[2023-09-18 20:19:35.192081] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=1.856: (-0.1005, -0.0823, -0.8560, 0.0911, 0.5815, 1.5665)]
[2023-09-18 20:19:35.516881] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=2.141: (-0.1575, 0.2650, -0.7692, 0.1428, 0.9112, 1.5645)]
[2023-09-18 20:19:35.796081] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=2.487: (-0.2142, 0.6106, -0.6831, 0.1942, 1.2392, 1.5625)]
[2023-09-18 20:19:36.146481] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=4.153: (-0.2708, 0.9558, -0.5971, 0.2456, 1.5669, 1.5606)]
[2023-09-18 20:19:37.839281] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:37.872281] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:37.914481] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:37.950881] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:37.995281] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.027081] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.060081] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.091081] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.130481] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.174481] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.207481] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.247281] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.290681] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.323281] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.363481] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.413681] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.448881] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.487681] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.523281] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.562681] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.594481] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.623081] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.665481] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.699481] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.740881] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.772081] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.824763] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.862963] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.899563] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.935163] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:38.974163] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:39.009963] [192.168.1.31:55509]: Trajectory complete
[2023-09-18 20:19:39.021363] [192.168.1.31:55509]: FJT using DEFAULT goal time tolerance: 500000000 ns
[2023-09-18 20:19:39.032963] [192.168.1.31:55509]: FJT action failed
[2023-09-18 20:19:39.042563] [192.168.1.31:55509]: Final position was outside tolerance. Check robot safety-limits that could be inhibiting motion. [joint_1_s: 0.0000 deviation] [joint_2_l: 0.0000 deviation] [joint_3_u: 0.0000 deviation] [joint_4_r: 0.0000 deviation] [joint_5_b: 0.0000 deviation]
[2023-09-18 20:19:39.091163] [192.168.1.31:55509]: FJT action complete
[2023-09-18 20:19:44.235420] [192.168.1.31:55509]: Trajectory contains 11 points
[2023-09-18 20:19:44.245220] [192.168.1.31:55509]: Initializing trajectory for group #0
[2023-09-18 20:19:44.256220] [192.168.1.31:55509]: Group #0 - Trajectory is ready for processing
[2023-09-18 20:19:44.272220] [192.168.1.31:55509]: feedback_msg_alloc_cfg size = 41352 bytes
[2023-09-18 20:19:44.272620] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=0.702: (-0.1167, 0.0016, -0.8078, 0.1059, 0.6743, 1.5653)]
[2023-09-18 20:19:44.380420] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=0.959: (-0.0858, -0.1828, -0.8609, 0.0779, 0.4958, 1.5665)]
[2023-09-18 20:19:44.477420] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=1.185: (-0.0548, -0.3680, -0.9145, 0.0497, 0.3164, 1.5678)]
[2023-09-18 20:19:44.708820] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=1.467: (-0.0255, -0.5439, -0.9628, 0.0231, 0.1473, 1.5690)]
[2023-09-18 20:19:44.997220] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=1.920: (0.0000, -0.7000, -1.0000, 0.0000, 0.0000, 1.5700)]
[2023-09-18 20:19:45.445020] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=2.077: (0.0042, -0.7445, -0.9743, -0.0037, -0.0244, 1.5702)]
[2023-09-18 20:19:45.601620] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=2.195: (0.0046, -0.7691, -0.9374, -0.0039, -0.0269, 1.5702)]
[2023-09-18 20:19:45.720820] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=2.299: (0.0032, -0.7844, -0.8953, -0.0025, -0.0192, 1.5701)]
[2023-09-18 20:19:45.822220] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=2.416: (0.0020, -0.8006, -0.8537, -0.0012, -0.0125, 1.5701)]
[2023-09-18 20:19:45.945820] [192.168.1.31:55509]: Processing next point in trajectory [Group #0 - T=2.967: (0.0008, -0.8170, -0.8121, 0.0000, -0.0060, 1.5700)]
[2023-09-18 20:19:46.505020] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:46.540220] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:46.571620] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:46.604820] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:46.652820] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:46.684420] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:46.723020] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:46.767020] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:46.809220] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:46.856420] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:46.888620] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:46.937020] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:46.973820] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:47.005820] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:47.048420] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:47.083820] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:47.116220] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:47.156220] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:47.197420] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:47.228820] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:47.267420] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:47.301820] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:47.333220] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:47.363420] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:47.395620] [192.168.1.31:55509]: Robot still in motion
[2023-09-18 20:19:47.425220] [192.168.1.31:55509]: Trajectory complete
[2023-09-18 20:19:47.436220] [192.168.1.31:55509]: FJT using DEFAULT goal time tolerance: 500000000 ns
[2023-09-18 20:19:47.447420] [192.168.1.31:55509]: FJT action failed
[2023-09-18 20:19:47.458020] [192.168.1.31:55509]: Final position was outside tolerance. Check robot safety-limits that could be inhibiting motion. [joint_1_s: 0.0000 deviation] [joint_2_l: 0.0000 deviation] [joint_4_r: 0.0000 deviation] [joint_5_b: 0.0000 deviation]
[2023-09-18 20:19:47.504220] [192.168.1.31:55509]: FJT action complete

Configuration file:

Click to expand

---
# SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc.
# SPDX-FileCopyrightText: 2022-2023, Delft University of Technology
#
# SPDX-License-Identifier: Apache-2.0


#-----------------------------------------------------------------------------
# REQUIRED
# IP address and UDP port number of the Micro-ROS Agent PC. All communication
# to/from MotoROS2 will route through the Agent application.
# (There is no default value for these fields. They must be specified by
# the user.)
agent_ip_address: "192.168.1.10"
agent_port_number: 8888


# Any settings that are not specified will be set to their DEFAULT value.


#-----------------------------------------------------------------------------
# The (DDS) domain to join
#
# Please refer to the ROS 2 documentation on DDS domain IDs for more
# information. This setting works exactly like its ROS 2 analogue.
#
#DEFAULT: 0 (the default ROS 2 domain ID)
ros_domain_id: 0

#-----------------------------------------------------------------------------
# Name under which MotoROS2 should register with the ROS 2 node graph.
#
# DEFAULT: "motoman_xx_yy_zz"  (xyz: last three bytes of robot's MAC address)
node_name: "gp180_120"

#-----------------------------------------------------------------------------
# Namespace to use for the MotoROS2 node and all topics.
#
# DEFAULT: ""  (empty string)
node_namespace: ""

#-----------------------------------------------------------------------------
# Remap rules to apply to ROS 2 resource names.
#
# This configures the micro-ROS equivalent of the ROS 2 remap functionality.
#
# The current implementation expects all remap rules as a single,
# space-separated string. Whitespace in resource names is not allowed, so
# this should not pose any issues.
#
# Maximum total length of the remap_rules string: 255 chars. Any characters
# beyond that will be ignored (and likely result in parsing failures).
#
# Please refer to the ROS 2 documentation on remapping for more information
# on syntax and contraints.
#
# Example: the following remaps the 'joint_states' topic to 'my_joint_states',
# and the 'read_single_io' service to 'io/read_single':
#
#   "joint_states:=my_joint_states read_single_io:=io/read_single"
#
# DEFAULT: ""  (empty string)
remap_rules: ""

#-----------------------------------------------------------------------------
# This will ensure that when timestamps are sampled, they will match the clock
# of the Agent PC. This is useful if the date/time of the robot controller is
# not synchronized.
#
# DEFAULT: true
sync_timeclock_with_agent: true

#-----------------------------------------------------------------------------
# Should MotoROS2 monitor the link state of the ethernet port used to
# communicate with the Agent?
#
# If enabled, this will cause immediate notification of loss-of-connection
# when the link goes down, causing MotoROS2 to activate it's shutdown
# behaviour (ie: stop all motion (if enabled), release resources, wait for
# link up and attempt reconnection with the Agent).
#
# If this is disabled, Agent disconnection detection uses an application
# layer based timeout mechanism which takes longer to detect disconnects (but
# might be more robust against intermittent link-layer disconnects which do not
# cause network disconnections).
#
# DEFAULT: true
userlan_monitor_enabled: true

#-----------------------------------------------------------------------------
# Which port should MotoROS2 monitor, if 'userlan_monitor_enabled' is 'true'?
#
# If not specified, MotoROS2 will attempt to auto-detect the network port
# that is used to connect to the micro-ROS Agent. It will do this by
# comparing the 'agent_ip_address' setting against the IP addresses (and
# netmasks) of all available network interfaces.
#
# If the Agent can be reached directly over a particular interface, or via a
# configured default gateway on the same interface, the associated port will be
# monitored for link drops.
#
# To disable auto-detection, uncomment 'userlan_monitor_port' below and set
# it to the desired port.
#
# NOTE:   this setting only applies to YRC1000 and YRC1000u controllers.
#         DX200 and FS100 controllers only have a single ethernet port, and any
#         value other than 1 will be ignored.
#
# NOTE 2: auto-detection is not perfect. It can't determine whether the Agent
#         might be reachable using multiple hops. Nor can it determine whether
#         any routers have been configured to forward particular types of
#         traffic or firewalls exist between MotoROS2 and the micro-ROS Agent.
#         For deployments with complex network configurations, disable
#         auto-detection by configuring 'userlan_monitor_port' below.
#
# NOTE 3: the port specified here is NOT checked against 'agent_ip_address'.
#         In other words: if 'agent_ip_address' is on 'USER_LAN1', but
#         'userlan_monitor_port' is set to 'USER_LAN2', MotoROS2 will detect
#         link down events on 'USER_LAN2', whether or not that port is used
#         to communicate with the Agent.
#
# OPTIONS: USER_LAN1, USER_LAN2
# DEFAULT: USER_LAN1
userlan_monitor_port: USER_LAN1

#-----------------------------------------------------------------------------
# If the Agent PC disconnects from the robot while it is in motion, should the
# robot come to a stop?
#
# DEFAULT: true
stop_motion_on_disconnect: true

#-----------------------------------------------------------------------------
# Should MotoROS2 broadcast transforms on '/tf'? This can be disabled if
# the data will interfere with applications such as robot_state_publisher.
#
# DEFAULT: true
publish_tf: true

#-----------------------------------------------------------------------------
# Should the 'tf' topic be namespaced if 'node_namespace' is configured with a
# non-empty string?
#
# Setting this to 'false' will make MotoROS2 broadcast transforms on the '/tf'
# global topic, which cannot be namespaced (due to being an absolute name).
# Otherwise, it will broadcast on 'tf', which can be namespaced.
#
# DEFAULT: true
namespace_tf: false

#-----------------------------------------------------------------------------
# Similar to the 'frame_prefix' parameter of the ROS 2 'robot_state_publisher'
# node. All published TF frames will be prefixed with this string.
#
# Example: with this parameter set to "left/", the "r1/tool0" frame would be
# published as "left/r1/tool0".
#
# NOTE: the prefix must include a separator (fi: '/') if one should be included
#       in the final name of the TF frames. Such a separator is not added
#       automatically.
#
# DEFAULT: ""  (empty string)
tf_frame_prefix: ""

#-----------------------------------------------------------------------------
# Joints in this configuration file must be listed in the order of Robots,
# Base-axes, Station-axes.
#
# Example: R1+B1+R2+S1 system
#
# joint_names:
# - [r1_s_axis, r1_l_axis, r1_u_axis, r1_r_axis, r1_b_axis, r1_t_axis]
# - [r2_s_axis, r2_l_axis, r2_u_axis, r2_r_axis, r2_b_axis, r2_t_axis, r2_e_axis]
# - [b1_axis]
# - [s1_axis_1, s1_axis_2]
#
# When using a 7 axis robot arm with an elbow joint (E) in the middle of the
# arm, the elbow axis should be listed last (SLURBTE).
#
# DEFAULT: "group_x/joint_y"
joint_names:
- [joint_1_s, joint_2_l, joint_3_u, joint_4_r, joint_5_b, joint_6_t]

#-----------------------------------------------------------------------------
# Logging settings
logging:
  # All log messages are broadcast on the network on port UDP 21789.
  # Additionally, the messages can be printed to the standard output stream of
  # the robot controller. This would be visible over a telnet connection, or by
  # attaching a VGA debug monitor to the robot controller.
  #
  # NOTE: this WILL slow down the application.
  #
  # DEFAULT: false
  log_to_stdout: true

#-----------------------------------------------------------------------------
update_periods:
  # The delay between checks for incoming activity on the network. A lower
  # number will result in quicker responsiveness to received messages.
  # Additionally, it determines the rate at which the feedback_publisher timers
  # are checked.
  # This value should be <= to action_feedback_publisher_period as executor_sleep_period
  # is the rate at which the action-feedback timer is checked. If the value for
  # action_feedback_publisher_period is < executor_sleep_period, it will effectively
  # be treated as having the same value executor_sleep_period at runtime.
  #
  # DEFAULT: 10 milliseconds
  executor_sleep_period: 10

  # The delay between each publish of feedback position and status information.
  # A lower number will publish the feedback data more frequently.
  # This value should be >= to executor_sleep_period and
  # controller_status_monitor_period.
  #
  # DEFAULT: 20 milliseconds
  action_feedback_publisher_period: 20

  # The delay between each poll of the robot I/O and controller status data.
  # This value should be <= to action_feedback_publisher_period.
  #
  # DEFAULT: 10 milliseconds
  controller_status_monitor_period: 10

#-----------------------------------------------------------------------------
# QoS profile to use for various publishers MotoROS2 creates.
# The default values here are based on tests and inspection of the source code
# of typical consumers of messages on these topics.
#
# NOTE : RViz2 expects/requires 'tf' to use the 'default' profile (ie: reliable).
# NOTE2: MoveIt2 expects/requires 'joint_states' to use the 'default' profile.
publisher_qos:
  # DEFAULT: sensor_data
  robot_status: sensor_data

  # DEFAULT: sensor_data
  joint_states: sensor_data

  # DEFAULT: default
  tf: default

#-----------------------------------------------------------------------------
# Name of the INFORM job to be used (and monitored) by MotoROS2.
#
# Maximum length: 32 UTF-8 characters. 16 Japanese (Shift-JIS) characters.
#
# Set this to a custom value when using a custom INFORM job with a different
# name.
#
# NOTE: do NOT include the file extension here (ie: '.JBI'). Only the name
#       of the job should be specified.
#
# DEFAULT: "INIT_ROS"
inform_job_name: "INIT_ROS"

#-----------------------------------------------------------------------------
# Allow custom INFORM job.
#
# If MotoROS2 detects that the specified INFORM job is not formatted properly
# then an alarm will be raised at startup.
# This flag indicates that the job is intentionally configured and will
# suppress the alarm.
#
# When this flag is set to 'true', then MotoROS2 will not validate whether the
# job is compatible. It is the responsibility of the user to make sure the
# custom job includes the required INFORM statements and in the expected order
# (refer to the provided INFORM job files for the general structure).
#
# DEFAULT: false
allow_custom_inform: false

Answer 1 · 2023-09-18T20:44:26.000Z

Hi @Aneeer
Unfortunately, this is known issue. #50
At this time, there is no single solution that works for all use-cases.

From a safety perspective, you are correct to be using the FSU in such a way that it satisfies the risk-assessment for your application. But MotoROS2 cannot detect when a limit has been enacted. It can only see that the intended trajectory-end was not reached.

The most basic answer is to monitor the feedback position (/joint_states, /tf) during execution to ensure that your trajectory is being followed as expected. If a significant deviation is detected, you can abort the existing trajectory and submit a new one.

If you are using the FollowJointTrajectory action server (as shown above), then aborting the trajectory will force the robot to come to a complete stop before another trajectory can be submitted.

If you are using the QueueTrajPoint service, then you may be able to react to the deviation in real time without halting. But please note that any data which has already been submitted to the queue will still be executed and potentially limited.

@gavanderhoorn: I kept this as an issue instead of discussion due to the output in the log.

Final position was outside tolerance. Check robot safety-limits that could be inhibiting motion. [joint_1_s: 0.0000 deviation] [joint_2_l: 0.0000 deviation] [joint_4_r: 0.0000 deviation] [joint_5_b: 0.0000 deviation]

It seems to indicate that there was no deviation on the final point. So, that makes me wonder if this isn't hitting the a time-tolerance rather than a positional-tolerance.

This should be investigated.

Answer 2 · 2023-09-19T06:27:20.000Z

@Aneeer: you have log_to_stdout: true. Unless you have a specific reason, I would strongly recommend to set that to false. The debug logger logs the exact same output, and incurs much less overhead.

@ted-miller: agreed. But perhaps a new issue to track it would be better in that case.

Edit: I've marked this as a duplicate, as it technically is one (of #50). I've not (yet) labelled it as a bug, as although it may not be expected, MotoROS2 is correct in that with FSU enabled, trajectories do sometimes fail to complete on time, and the FJT server reporting the failure is expected behaviour.

As mentioned earlier, there might still be a bug in the implementation of the detection of time-overrun in the FJT server, but that would/should be a separate issue.

Answer 3 · 2023-09-19T06:40:50.000Z

I've opened #156 to track the need to document the current state of PFL/FSU support more clearly.

Answer 4 · 2023-09-19T15:19:53.000Z

Thanks for the support. I'll stay tuned on what happens with PFL/FSU developments.

Answer 5 · 2023-09-19T21:06:08.000Z

In case you haven't seen this post, someone has updated MotoROS2 to include better behavior with the FSU.

Once we integrate those changes, then the robot should follow the trajectory properly at the safety-reduced speeds. Then the only concern would be MoveIt potentially aborting the trajectory due to a time-constraint.

Answer 6 · 2023-09-20T16:36:48.000Z

Hi, the first version of FSU Speed Limit Handling integrated to motoros2 is here #157.