Feature/safety functions

[patrick-benito]

Checking out safety features from Paul's branch on origin/control-augmented-stack.

[Copilot]

Pull Request Overview

This PR introduces safety features for motor operations by integrating secure mode functionality and improving motor command safety checks.

• Added a declared parameter for secure mode and utilized it to conditionally subscribe to mocap data and perform extra safety checks.
• Revised motor position handling by computing delta changes and enhancing error reporting for unsafe conditions.
• Updated the launch file to pass the secure mode parameter to the motor node.

Reviewed Changes

Copilot reviewed 2 out of 2 changed files in this pull request and generated 2 comments.

File	Description
stack/motors/src/trunk_motors/trunk_motors/scripts/motor_node.py	Introduces secure mode, safety checks with delta limits, and improved handling of motor commands and mocap data.
stack/motors/src/trunk_motors/trunk_motors/launch/launch_motors.py	Adds a launch argument for secure mode and passes it to the motor node.

Comments suppressed due to low confidence (2)

stack/motors/src/trunk_motors/trunk_motors/scripts/motor_node.py:152

• Reword the error message to accurately reflect the issue without referencing indices, for example: 'Unsafe trunk position (norm: {np.linalg.norm(y_centered_tip)}) exceeds safe threshold. Shutting down motor node.'

self.get_logger().error(f"Unsafe trunk position at value commands at indices {np.linalg.norm(y_centered_tip)}. Shutting down the motor node.")

stack/motors/src/trunk_motors/trunk_motors/launch/launch_motors.py:12

• Ensure that the 'secure_mode' parameter is interpreted as a boolean in the motor node. Consider converting the string value from the launch argument to a boolean to avoid type mismatches.

DeclareLaunchArgument('secure_mode', default_value='true', description='Enable or disable secure mode for motor_node')

[Copilot]

Add explicit parentheses to clarify the intended logic of the condition, e.g., 'if np.any(mask_low | mask_high) or (self.MPC_SECURITY_MODE and np.any(mask_delta_low | mask_delta_high)):' to improve readability.

Suggested change

	if np.any(mask_low | mask_high) or self.MPC_SECURITY_MODE and np.any(mask_delta_low | mask_delta_high):
	if np.any(mask_low | mask_high) or (self.MPC_SECURITY_MODE and np.any(mask_delta_low | mask_delta_high)):

[Copilot]

Replace the print statement with self.get_logger().debug() to adhere to standard logging practices in ROS nodes.

Suggested change

	print(f"Positions: {positions}, Delta Positions: {delta_positions}")
	self.get_logger().debug(f"Positions: {positions}, Delta Positions: {delta_positions}")