Feature/vel to pos refactoring

.github/workflows/lint_build_test.yaml

@@ -58,7 +58,7 @@ jobs:
           fi
           echo "Processing Level $i dependencies..."
           for f in $REPOS_FILES; do
-            vcs import . < "$f"
+            vcs import . < "$f" --skip-existing
           done
           PREVIOUS_HASH="$CURRENT_HASH"
         done

README.md

@@ -80,7 +80,7 @@ Converts **velocity references** into **position commands** for joint hardware.
     * Configurable deceleration ramp (`braking_deceleration`) for smooth stopping.
 * **Joint synchronization:**
     * Joints in the same synchronization group are kept aligned via P-control on position offsets.
-    * **Synced braking:** When synced joints stop together, the faster-stopping joint reduces its braking to maintain the position difference with the weaker joint (controlled by `kp_braking_sync`). Both joints transition to STOPPED only when all group partners have finished braking.
+    * Braking is independent per joint — each decelerates at `braking_deceleration` regardless of sync group membership.
 * **URDF position limit clamping:**
     * Position commands are clamped to URDF joint limits for revolute/prismatic joints. Continuous joints are unclamped.
 * **E-Stop support:**
@@ -90,7 +90,7 @@ Converts **velocity references** into **position commands** for joint hardware.
 * **Debug joint state publishers:**
     * Optionally publishes incoming velocity references and outgoing position commands as `sensor_msgs/JointState` (dynamically togglable).
 * **Dynamic parameter reconfiguration:**
-    * `kp`, `kd`, `kp_sync`, `kp_braking_sync`, and `braking_deceleration` can be changed at runtime via ROS parameter callbacks.
+    * `kp`, `kd`, `kp_sync`, and `braking_deceleration` can be changed at runtime via ROS parameter callbacks.
 
 ### Control Law
 
@@ -109,7 +109,6 @@ pos_cmd = desired_pos[i] + vel_p - vel_d + sync_correction
 | **kp**                          | `double`       | `1.0`                    | Proportional gain for velocity tracking.                                                                                                        |
 | **kd**                          | `double`       | `0.1`                    | Derivative gain for acceleration damping.                                                                                                       |
 | **kp_sync**                     | `double`       | `1.0`                    | Proportional gain for synchronization of synced joints during movement.                                                                         |
-| **kp_braking_sync**             | `double`       | `1.0`                    | Proportional gain for synced braking correction. Controls how aggressively the faster joint slows its braking to maintain the position offset.  |
 | **stopping_velocity_threshold** | `double`       | `0.005`                  | Velocity threshold below which a joint is considered stopped.                                                                                   |
 | **braking_deceleration**        | `double`       | `5.0`                    | Deceleration in rad/s² used to smoothly bring joints to a stop when velocity command becomes zero.                                              |
 | **synchronous_groups**          | `string_array` | `[]`                     | Group names per joint for synchronization (empty = no sync).                                                                                    |

dependencies.repos

@@ -11,3 +11,7 @@ repositories:
     type: git
     url: https://github.com/Beam-and-Spyrosoft/rtest.git
     version: jazzy
+  hector_gamepad_manager:
+    type: git
+    url: https://github.com/tu-darmstadt-ros-pkg/hector_gamepad_manager.git
+    version: main

dynamics_mock_hardware/CMakeLists.txt

@@ -0,0 +1,67 @@
+cmake_minimum_required(VERSION 3.16)
+project(dynamics_mock_hardware LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS hardware_interface pluginlib pinocchio rclcpp
+                                 rclcpp_lifecycle)
+
+find_package(ament_cmake REQUIRED)
+foreach(dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${dependency} REQUIRED)
+endforeach()
+
+add_library(${PROJECT_NAME} SHARED src/dynamics_mock_hardware.cpp)
+
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+
+target_include_directories(
+  ${PROJECT_NAME} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+                         $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+
+target_link_libraries(${PROJECT_NAME} PUBLIC pinocchio::pinocchio)
+
+ament_target_dependencies(${PROJECT_NAME} PUBLIC
+                          ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+target_compile_definitions(
+  ${PROJECT_NAME} PUBLIC PINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR
+                         PINOCCHIO_URDFDOM_USE_STD_SHARED_PTR)
+
+pluginlib_export_plugin_description_file(hardware_interface
+                                         dynamics_mock_hardware_plugin.xml)
+
+install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
+
+install(DIRECTORY config launch rviz DESTINATION share/${PROJECT_NAME})
+
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  INCLUDES
+  DESTINATION include)
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(lifecycle_msgs REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gmock(test_dynamics_mock_hardware
+                  test/test_dynamics_mock_hardware.cpp)
+  target_link_libraries(test_dynamics_mock_hardware ${PROJECT_NAME})
+  ament_target_dependencies(test_dynamics_mock_hardware hardware_interface
+                            lifecycle_msgs rclcpp rclcpp_lifecycle)
+  target_compile_definitions(
+    test_dynamics_mock_hardware PRIVATE PINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR
+                                        PINOCCHIO_URDFDOM_USE_STD_SHARED_PTR)
+endif()
+
+ament_package()

dynamics_mock_hardware/README.md

@@ -0,0 +1,38 @@
+# dynamics_mock_hardware
+
+Mock hardware interface that supports position, velocity, effort (torque) control and uses pinocchio to simulate the robots behavior.
+
+- [dynamics_mock_hardware](#dynamics_mock_hardware)
+
+
+## `dynamics_mock_hardware`
+
+### Subscribed Topics
+
+| Topic | Type | Description |
+| --- | --- | --- |
+|  |  |  |
+
+### Published Topics
+
+| Topic | Type | Description |
+| --- | --- | --- |
+|  |  |  |
+
+### Services
+
+| Service | Type | Description |
+| --- | --- | --- |
+|  |  |  |
+
+### Actions
+
+| Action | Type | Description |
+| --- | --- | --- |
+|  |  |  |
+
+### Parameters
+
+| Parameter | Type | Description |
+| --- | --- | --- |
+|  |  |  |