From 614d36c58a8468efe360fee8e7122eb8bcb43b13 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 26 Nov 2024 22:05:24 +0000 Subject: [PATCH 1/4] Fix single dimensional vector handling --- .../numpy_conversions.py | 22 ++++++++++++------- tests/test_numpy_conversions.py | 19 ++++++++++++++++ 2 files changed, 33 insertions(+), 8 deletions(-) diff --git a/linear_feedback_controller_msgs_py/numpy_conversions.py b/linear_feedback_controller_msgs_py/numpy_conversions.py index 69e1885..fa67348 100644 --- a/linear_feedback_controller_msgs_py/numpy_conversions.py +++ b/linear_feedback_controller_msgs_py/numpy_conversions.py @@ -180,30 +180,34 @@ 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: @@ -212,7 +216,9 @@ def matrix_msg_to_numpy(msg: Float64MultiArray) -> npt.NDArray[np.float64]: 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..c7d503d 100644 --- a/tests/test_numpy_conversions.py +++ b/tests/test_numpy_conversions.py @@ -50,6 +50,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( From 82e31e8cc8210577b5ce2b5b2462b87618502843 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 26 Nov 2024 22:08:30 +0000 Subject: [PATCH 2/4] Update docstring --- linear_feedback_controller_msgs_py/numpy_conversions.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/linear_feedback_controller_msgs_py/numpy_conversions.py b/linear_feedback_controller_msgs_py/numpy_conversions.py index fa67348..0b71b93 100644 --- a/linear_feedback_controller_msgs_py/numpy_conversions.py +++ b/linear_feedback_controller_msgs_py/numpy_conversions.py @@ -212,6 +212,8 @@ def matrix_msg_to_numpy( 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. From c7040b579c5c2ced9095b1a6afed9027c62547de Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 27 Nov 2024 09:12:14 +0000 Subject: [PATCH 3/4] Add control unit test for numpy --- tests/test_numpy_conversions.py | 81 +++++++++++++++++++++++++++++++-- 1 file changed, 77 insertions(+), 4 deletions(-) diff --git a/tests/test_numpy_conversions.py b/tests/test_numpy_conversions.py index 55c98de..06d62a6 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 @@ -89,6 +85,83 @@ 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.joint_state.name == back_converted_numpy_control.joint_state.name + ), "Names after conversion back to Numpy is not equal initial values!" + + np.testing.assert_array_equal( + numpy_control.joint_state.position, + back_converted_numpy_control.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.joint_state.velocity, + back_converted_numpy_control.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.joint_state.effort, + back_converted_numpy_control.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) From a3fd66dd3a88a5722b7fa021f2cb50541fe0c9f8 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 27 Nov 2024 09:15:23 +0000 Subject: [PATCH 4/4] Fix initial state references in test --- tests/test_numpy_conversions.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/tests/test_numpy_conversions.py b/tests/test_numpy_conversions.py index 11c3cac..7285fe1 100644 --- a/tests/test_numpy_conversions.py +++ b/tests/test_numpy_conversions.py @@ -142,26 +142,27 @@ def test_check_ros_eigen_control_conversion() -> None: ) assert ( - numpy_control.joint_state.name == back_converted_numpy_control.joint_state.name + 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.joint_state.position, - back_converted_numpy_control.joint_state.position, + 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.joint_state.velocity, - back_converted_numpy_control.joint_state.velocity, + 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.joint_state.effort, - back_converted_numpy_control.joint_state.effort, + 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!", )