diff --git a/linear_feedback_controller_msgs_py/numpy_conversions.py b/linear_feedback_controller_msgs_py/numpy_conversions.py index 69e1885..0b71b93 100644 --- a/linear_feedback_controller_msgs_py/numpy_conversions.py +++ b/linear_feedback_controller_msgs_py/numpy_conversions.py @@ -180,39 +180,47 @@ def matrix_numpy_to_msg(input: npt.NDArray[np.float64]) -> Float64MultiArray: """Converts Numpy array into ROS array message. Args: - input (npt.NDArray[np.float64]]): Input matrix. + input (npt.NDArray[np.float64]]): Input matrix of size (N,2) or (N,). Returns: std_msgs.msg.Float64MultiArray: ROS message with the matrix. """ + # In case vector is passed consider is (N,1) array. + rows, cols = input.shape if input.ndim != 1 else (input.shape[0], 1) assert ( - input.ndim == 2 - ), f"Input matrix is dimension '{input.ndim}'. EXpected 2D matrix!" + input.ndim == 2 or input.ndim == 1 + ), f"Input matrix is dimension '{input.ndim}'. Expected 2D matrix or 1D vector!" m = Float64MultiArray() m.layout.data_offset = 0 m.layout.dim = [MultiArrayDimension(), MultiArrayDimension()] m.layout.dim[0].label = "rows" - m.layout.dim[0].size = input.shape[0] + m.layout.dim[0].size = rows m.layout.dim[0].stride = input.size m.layout.dim[1].label = "cols" - m.layout.dim[1].stride = input.shape[1] - m.layout.dim[1].size = input.shape[1] + m.layout.dim[1].stride = cols + m.layout.dim[1].size = cols # Flatten the matrix to a vector m.data = input.reshape(-1).tolist() return m -def matrix_msg_to_numpy(msg: Float64MultiArray) -> npt.NDArray[np.float64]: +def matrix_msg_to_numpy( + msg: Float64MultiArray, return_vector: bool = True +) -> npt.NDArray[np.float64]: """Converts ROS array message into numpy array. Args: msg (std_msgs.msg.Float64MultiArray): Input ROS message with array. + return_vector (bool, optional): If ``True`` vector is returned in a shape (N,) + otherwise the shape is (N,1). Defaults to True. Returns: npt.NDArray[np.float64]: Output numpy matrix. """ - assert len(msg.layout.dim) == 2, "The ROS message must be a 2D matrix." + assert len(msg.layout.dim) == 2, "The ROS message must be a 2D matrix!" + if return_vector and msg.layout.dim[1].size == 1: + return np.array(msg.data) return np.array(msg.data).reshape(msg.layout.dim[0].size, msg.layout.dim[1].size) diff --git a/tests/test_numpy_conversions.py b/tests/test_numpy_conversions.py index 55c98de..7285fe1 100644 --- a/tests/test_numpy_conversions.py +++ b/tests/test_numpy_conversions.py @@ -1,12 +1,8 @@ #!/usr/bin/env python import numpy as np -import numpy.typing as npt -from typing import List from copy import deepcopy -from geometry_msgs.msg import Transform, Vector3, Quaternion - from linear_feedback_controller_msgs_py.numpy_conversions import * import linear_feedback_controller_msgs_py.lfc_py_types as lfc_py_types @@ -50,6 +46,25 @@ def test_check_ros_numpy_matrix_conversion() -> None: + "Numpy is not equal the initial matrix!", ) + numpy_random_vector = np.random.rand(20) + + ros_vector = matrix_numpy_to_msg(deepcopy(numpy_random_vector)) + + for return_vector in (True, False): + numpy_back_converted_vector = matrix_msg_to_numpy(ros_vector, return_vector) + + np.testing.assert_array_equal( + numpy_back_converted_vector, + ( + numpy_random_vector + if return_vector + else numpy_random_vector.reshape(-1, 1) + ), + err_msg="Vector after conversion back to " + + "Numpy is not equal the initial vector " + + f"for case with 'return_vector={return_vector}'", + ) + def test_check_ros_numpy_joint_state_conversion() -> None: numpy_joint_state = lfc_py_types.JointState( @@ -89,6 +104,84 @@ def test_check_ros_numpy_joint_state_conversion() -> None: ) +def test_check_ros_eigen_control_conversion() -> None: + quat = np.random.rand(4) + quat = quat / np.linalg.norm(quat) + + numpy_control = lfc_py_types.Control( + initial_state=lfc_py_types.Sensor( + base_pose=np.concatenate((np.random.rand(3), quat)), + base_twist=np.random.rand(6), + joint_state=lfc_py_types.JointState( + name=["1", "2", "3", "4", "5", "6"], + position=np.random.rand(6), + velocity=np.random.rand(6), + effort=np.random.rand(6), + ), + contacts=[], + ), + feedback_gain=np.random.rand(8, 4), + feedforward=np.random.rand(4), + ) + + ros_control_msg = control_numpy_to_msg(deepcopy(numpy_control)) + back_converted_numpy_control = control_msg_to_numpy(ros_control_msg) + + np.testing.assert_array_equal( + numpy_control.initial_state.base_pose, + back_converted_numpy_control.initial_state.base_pose, + err_msg="Initial state base pose after conversion back to " + + "Numpy is not equal initial values!", + ) + + np.testing.assert_array_equal( + numpy_control.initial_state.base_twist, + back_converted_numpy_control.initial_state.base_twist, + err_msg="Initial state base twist after conversion back to " + + "Numpy is not equal initial values!", + ) + + assert ( + numpy_control.initial_state.joint_state.name + == back_converted_numpy_control.initial_state.joint_state.name + ), "Names after conversion back to Numpy is not equal initial values!" + + np.testing.assert_array_equal( + numpy_control.initial_state.joint_state.position, + back_converted_numpy_control.initial_state.joint_state.position, + err_msg="Joint state position after conversion back to " + + "Numpy is not equal initial values!", + ) + + np.testing.assert_array_equal( + numpy_control.initial_state.joint_state.velocity, + back_converted_numpy_control.initial_state.joint_state.velocity, + err_msg="Joint state velocity after conversion back to " + + "Numpy is not equal initial values!", + ) + + np.testing.assert_array_equal( + numpy_control.initial_state.joint_state.effort, + back_converted_numpy_control.initial_state.joint_state.effort, + err_msg="Joint state effort after conversion back to " + + "Numpy is not equal initial values!", + ) + + np.testing.assert_array_equal( + numpy_control.feedback_gain, + back_converted_numpy_control.feedback_gain, + err_msg="Feedback gains after conversion back to " + + "Numpy is not equal initial values!", + ) + + np.testing.assert_array_equal( + numpy_control.feedforward, + back_converted_numpy_control.feedforward, + err_msg="Feed forward after conversion back to " + + "Numpy is not equal initial values!", + ) + + def test_check_ros_eigen_sensor_conversions() -> None: quat = np.random.rand(4) quat = quat / np.linalg.norm(quat)