From 9a0896e9326b3384fd74b8bcac7b2add86fab64f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 11 Nov 2023 22:30:29 +0000 Subject: [PATCH 01/29] Initial commit for cart-pole --- example_XX/CMakeLists.txt | 37 ++++ example_XX/README.md | 5 + .../bringup/config/cart_pole_controllers.yaml | 14 ++ example_XX/bringup/launch/cart_pole.launch.py | 100 ++++++++++ .../description/gazebo/cart_pole.gazebo.xacro | 15 ++ .../description/launch/view_robot.launch.py | 98 ++++++++++ .../ros2_control/cart_pole.ros2_control.xacro | 28 +++ .../description/urdf/cart_pole.urdf.xacro | 22 +++ example_XX/doc/userdoc.rst | 74 ++++++++ example_XX/package.xml | 40 ++++ ros2_control_demo_description/CMakeLists.txt | 5 + .../cart_pole/rviz/cart_pole.rviz | 176 ++++++++++++++++++ .../urdf/cart_pole_description.urdf.xacro | 78 ++++++++ 13 files changed, 692 insertions(+) create mode 100644 example_XX/CMakeLists.txt create mode 100644 example_XX/README.md create mode 100644 example_XX/bringup/config/cart_pole_controllers.yaml create mode 100644 example_XX/bringup/launch/cart_pole.launch.py create mode 100644 example_XX/description/gazebo/cart_pole.gazebo.xacro create mode 100644 example_XX/description/launch/view_robot.launch.py create mode 100644 example_XX/description/ros2_control/cart_pole.ros2_control.xacro create mode 100644 example_XX/description/urdf/cart_pole.urdf.xacro create mode 100644 example_XX/doc/userdoc.rst create mode 100644 example_XX/package.xml create mode 100644 ros2_control_demo_description/cart_pole/rviz/cart_pole.rviz create mode 100644 ros2_control_demo_description/cart_pole/urdf/cart_pole_description.urdf.xacro diff --git a/example_XX/CMakeLists.txt b/example_XX/CMakeLists.txt new file mode 100644 index 000000000..6369cd22d --- /dev/null +++ b/example_XX/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.16) +project(ros2_control_demo_example_xx LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + pluginlib + rclcpp + rclcpp_lifecycle +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +# INSTALL +install( + DIRECTORY description/launch description/ros2_control description/urdf description/gazebo + DESTINATION share/ros2_control_demo_example_xx +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/ros2_control_demo_example_xx +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) +endif() + +## EXPORTS +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/example_XX/README.md b/example_XX/README.md new file mode 100644 index 000000000..50a82a426 --- /dev/null +++ b/example_XX/README.md @@ -0,0 +1,5 @@ +# ros2_control_demo_example_xx + + This example demonstrates a robot with passive joints. + +Find the documentation in [doc/userdoc.rst](doc/userdoc.rst) or on [control.ros.org](https://control.ros.org/master/doc/ros2_control_demos/example_XX/doc/userdoc.html). diff --git a/example_XX/bringup/config/cart_pole_controllers.yaml b/example_XX/bringup/config/cart_pole_controllers.yaml new file mode 100644 index 000000000..baf14bc3d --- /dev/null +++ b/example_XX/bringup/config/cart_pole_controllers.yaml @@ -0,0 +1,14 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + effort_controller: + type: effort_controllers/JointGroupEffortController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +effort_controller: + ros__parameters: + joints: + - slider_to_cart diff --git a/example_XX/bringup/launch/cart_pole.launch.py b/example_XX/bringup/launch/cart_pole.launch.py new file mode 100644 index 000000000..aa3b4e07a --- /dev/null +++ b/example_XX/bringup/launch/cart_pole.launch.py @@ -0,0 +1,100 @@ +# Copyright 2023 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("gazebo_ros"), "launch"), + "/gazebo.launch.py", + ] + ), + launch_arguments={"pause": "true"}.items(), + ) + + ros2_control_demo_example_xx_path = os.path.join( + get_package_share_directory("ros2_control_demo_example_xx") + ) + + xacro_file = os.path.join(ros2_control_demo_example_xx_path, "urdf", "cart_pole.urdf.xacro") + + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + params = {"robot_description": doc.toxml()} + + node_robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[params], + ) + + spawn_entity = Node( + package="gazebo_ros", + executable="spawn_entity.py", + arguments=["-topic", "robot_description", "-entity", "cartpole"], + output="screen", + ) + + load_joint_state_controller = ExecuteProcess( + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + "active", + "joint_state_broadcaster", + ], + output="screen", + ) + + load_joint_trajectory_controller = ExecuteProcess( + cmd=["ros2", "control", "load_controller", "--set-state", "active", "effort_controller"], + output="screen", + ) + + return LaunchDescription( + [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[load_joint_state_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_joint_trajectory_controller], + ) + ), + gazebo, + node_robot_state_publisher, + spawn_entity, + ] + ) diff --git a/example_XX/description/gazebo/cart_pole.gazebo.xacro b/example_XX/description/gazebo/cart_pole.gazebo.xacro new file mode 100644 index 000000000..e2b8e8edb --- /dev/null +++ b/example_XX/description/gazebo/cart_pole.gazebo.xacro @@ -0,0 +1,15 @@ + + + + + + + + + $(find ros2_control_demo_example_xx)/config/cart_pole_controllers.yaml + + +a + + + diff --git a/example_XX/description/launch/view_robot.launch.py b/example_XX/description/launch/view_robot.launch.py new file mode 100644 index 000000000..72a80732c --- /dev/null +++ b/example_XX/description/launch/view_robot.launch.py @@ -0,0 +1,98 @@ +# Copyright 2023 AIT - Austrian Institute of Technology +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description="Description package with robot URDF/xacro files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="cart_pole.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start Rviz2 and Joint State Publisher gui automatically \ + with this launch file.", + ) + ) + + # Initialize Arguments + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + gui = LaunchConfiguration("gui") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_xx"), "urdf", description_file] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "cart_pole/rviz", "cart_pole.rviz"] + ) + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(gui), + ) + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + condition=IfCondition(gui), + ) + + nodes = [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/example_XX/description/ros2_control/cart_pole.ros2_control.xacro b/example_XX/description/ros2_control/cart_pole.ros2_control.xacro new file mode 100644 index 000000000..ec88d1f6b --- /dev/null +++ b/example_XX/description/ros2_control/cart_pole.ros2_control.xacro @@ -0,0 +1,28 @@ + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + -1000 + 1000 + + + + + + + + + + + + + + + diff --git a/example_XX/description/urdf/cart_pole.urdf.xacro b/example_XX/description/urdf/cart_pole.urdf.xacro new file mode 100644 index 000000000..d05f291b3 --- /dev/null +++ b/example_XX/description/urdf/cart_pole.urdf.xacro @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_XX/doc/userdoc.rst b/example_XX/doc/userdoc.rst new file mode 100644 index 000000000..056decf37 --- /dev/null +++ b/example_XX/doc/userdoc.rst @@ -0,0 +1,74 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_9/doc/userdoc.rst + +.. _ros2_control_demos_example_xx_userdoc: + +Example xx: Cart-Pole +================================= + +With *example_xx*, we demonstrate the a robot with passive joints. + +.. note:: + + Follow the installation instructions on :ref:`ros2_control_demos_install` how to install all dependencies, + Gazebo Classic should be automatically installed. + + * If you have installed and compiled this repository locally, you can directly use the commands below. + * If you have installed it via the provided docker image: To run the first two steps of this example (without Gazebo Classic), use the commands as described with :ref:`ros2_control_demos_install`. To run the later steps using Gazebo Classic, execute + + .. code:: + + docker run -it --rm --name ros2_control_demos --net host ros2_control_demos ros2 launch ros2_control_demo_example_9 rrbot_gazebo_classic.launch.py gui:=false + + first. Then on your local machine you can run the Gazebo Classic client with + + .. code-block:: shell + + gzclient + + and/or ``rviz2`` with + + .. code-block:: shell + + rviz2 -d src/ros2_control_demos/example_9/description/rviz/rrbot.rviz + + + For details on the ``gazebo_ros2_control`` plugin, see :ref:`gazebo_ros2_control`. + +Tutorial steps +-------------------------- + +The following example shows a cart with a pendulum arm. This uses the effort command interface for the cart's +degree of freedom on the rail, and the physics of the passive joint of the pendulum is solved correctly. + +.. code-block:: shell + + ros2 launch gazebo_ros2_control_demos pendulum_example_effort.launch.py + ros2 run gazebo_ros2_control_demos example_effort + + +Files used for this demos +------------------------- + +- Launch files: + + + Hardware: `rrbot.launch.py `__ + + Gazebo Classic: `rrbot_gazebo_classic.launch.py `__ + +- Controllers yaml: `rrbot_controllers.yaml `__ +- URDF file: `rrbot.urdf.xacro `__ + + + Description: `rrbot_description.urdf.xacro `__ + + ``ros2_control`` tag: `rrbot.ros2_control.xacro `__ + +- RViz configuration: `rrbot.rviz `__ +- Test nodes goals configuration: + + + `rrbot_forward_position_publisher `__ + +- Hardware interface plugin: `rrbot.cpp `__ + + +Controllers from this demo +-------------------------- +- ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ +- ``Forward Command Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/example_XX/package.xml b/example_XX/package.xml new file mode 100644 index 000000000..cd50f7383 --- /dev/null +++ b/example_XX/package.xml @@ -0,0 +1,40 @@ + + + + ros2_control_demo_example_xx + 0.0.0 + Demo package of `ros2_control` simulation with cart-pole. + + Dr.-Ing. Denis Štogl + Bence Magyar + Christoph Froehlich + + Christoph Froehlich + + Apache-2.0 + + ament_cmake + + pluginlib + rclcpp + rclcpp_lifecycle + + controller_manager + gazebo_ros + gazebo_ros2_control + joint_state_broadcaster + joint_state_publisher_gui + joint_trajectory_controller + robot_state_publisher + ros2_control_demo_description + ros2_controllers_test_nodes + ros2controlcli + rviz2 + xacro + + ament_cmake_gtest + + + ament_cmake + + diff --git a/ros2_control_demo_description/CMakeLists.txt b/ros2_control_demo_description/CMakeLists.txt index b7aedafbf..799601324 100644 --- a/ros2_control_demo_description/CMakeLists.txt +++ b/ros2_control_demo_description/CMakeLists.txt @@ -13,6 +13,11 @@ install( DESTINATION share/${PROJECT_NAME}/diffbot ) +install( + DIRECTORY cart_pole/urdf cart_pole/rviz + DESTINATION share/${PROJECT_NAME}/cart_pole +) + install( DIRECTORY r6bot/meshes r6bot/srdf r6bot/urdf r6bot/rviz DESTINATION share/${PROJECT_NAME}/r6bot diff --git a/ros2_control_demo_description/cart_pole/rviz/cart_pole.rviz b/ros2_control_demo_description/cart_pole/rviz/cart_pole.rviz new file mode 100644 index 000000000..4380ea3d8 --- /dev/null +++ b/ros2_control_demo_description/cart_pole/rviz/cart_pole.rviz @@ -0,0 +1,176 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 555 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + cart: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pendulum: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + slideBar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 8.673279762268066 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3353983461856842 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.060397744178772 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 846 + Hide Left Dock: true + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004b0000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 495 + Y: 82 diff --git a/ros2_control_demo_description/cart_pole/urdf/cart_pole_description.urdf.xacro b/ros2_control_demo_description/cart_pole/urdf/cart_pole_description.urdf.xacro new file mode 100644 index 000000000..bce4c6eb6 --- /dev/null +++ b/ros2_control_demo_description/cart_pole/urdf/cart_pole_description.urdf.xacro @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From e7d871191e3075d5ef26b02160a8da3cc84d8f1e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Nov 2023 18:39:52 +0000 Subject: [PATCH 02/29] Initial version for action client --- example_XX/CMakeLists.txt | 11 +- example_XX/package.xml | 5 +- .../ros2_control_demo_example_xx/__init__.py | 0 .../scripts/trajectory_action_client.py | 189 ++++++++++++++++++ example_XX/swingup.csv | 12 ++ 5 files changed, 215 insertions(+), 2 deletions(-) create mode 100644 example_XX/ros2_control_demo_example_xx/__init__.py create mode 100755 example_XX/scripts/trajectory_action_client.py create mode 100644 example_XX/swingup.csv diff --git a/example_XX/CMakeLists.txt b/example_XX/CMakeLists.txt index 6369cd22d..f6dc296d3 100644 --- a/example_XX/CMakeLists.txt +++ b/example_XX/CMakeLists.txt @@ -9,11 +9,12 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib rclcpp - rclcpp_lifecycle + rclpy ) # find dependencies find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() @@ -28,6 +29,14 @@ install( DESTINATION share/ros2_control_demo_example_xx ) +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) +# Install Python executables +install(PROGRAMS + scripts/trajectory_action_client.py + DESTINATION lib/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) endif() diff --git a/example_XX/package.xml b/example_XX/package.xml index cd50f7383..4d89f56f5 100644 --- a/example_XX/package.xml +++ b/example_XX/package.xml @@ -14,10 +14,13 @@ Apache-2.0 ament_cmake + ament_cmake_python pluginlib rclcpp - rclcpp_lifecycle + rclpy + + python3-pandas controller_manager gazebo_ros diff --git a/example_XX/ros2_control_demo_example_xx/__init__.py b/example_XX/ros2_control_demo_example_xx/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/example_XX/scripts/trajectory_action_client.py b/example_XX/scripts/trajectory_action_client.py new file mode 100755 index 000000000..7eac65ba3 --- /dev/null +++ b/example_XX/scripts/trajectory_action_client.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 +# Copyright 2023 AIT - Austrian Institute of Technology +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pandas as pd +import rclpy +from trajectory_msgs.msg import JointTrajectoryPoint +from rclpy.node import Node +from builtin_interfaces.msg import Duration +import sys +import signal +from rclpy.action import ActionClient +from control_msgs.action import FollowJointTrajectory +from control_msgs.msg import JointTolerance + + +class TrajectoryActionClient(Node): + def __init__(self): + super().__init__("trajectory_action_client") + self.idx = 0 + self.declare_parameter("file_name", "src/ros2_control_demos/example_XX/swingup") + self.declare_parameter("nrJoints", 2) + self.declare_parameter("joint_names", ["slider_to_cart", "cart_to_pendulum"]) + self.declare_parameter("path_tolerance", [100.1, 100.1]) + self.declare_parameter("goal_tolerance", [0.05, 0.05]) + self.declare_parameter("map_joints", [0, 1]) + + self.action_client_ = ActionClient( + self, FollowJointTrajectory, "/trajectory_controllers/follow_joint_trajectory" + ) + + # safe handling of SIGINT/SIGTERM + signal.signal(signal.SIGINT, self.safe_exit) + signal.signal(signal.SIGTERM, self.safe_exit) + + # ---------------------------------------- read Excel-file ------------ + + print("Load Excel-file") + fileName = self.get_parameter("file_name").get_parameter_value().string_value + fileExtension = ".csv" + fileNameWithPath = fileName + fileExtension + Data = pd.read_csv( + fileNameWithPath, + index_col=None, + header=0, + # usecols="A,B,C,D,E,F,G,H", + # , header=None + ) + self.Data_array = Data.values + print("File name = %s" % (fileNameWithPath)) # output file name with logging + + # ------------------------- store data from Excel-file in variables --- + self.nrJoints = self.get_parameter("nrJoints").value + self.map_joints = ( + self.get_parameter("map_joints").get_parameter_value().integer_array_value + ) + + self.Data_array_size = len(self.Data_array[0, :]) + self.trajLen = len(self.Data_array[:, 0]) + print("Length of trajectory: " + str(self.trajLen)) + + print("Data preview:") + print(Data.head(2)) + sys.stdout.flush() + + self.joint_names_ = self.get_parameter("joint_names").value + self.is_goal_rejected = False + self.is_goal_accepted = False + + self.input_equals("\nStart the trajectory by pressing enter", "") + goal_msg = FollowJointTrajectory.Goal() + goal_msg.trajectory.joint_names = self.joint_names_ + goal_msg.trajectory.header.stamp = self.get_clock().now().to_msg() + + # get points from csv data + offset_idx = self.nrJoints + duration_in_seconds = 0 + for i_traj in range(self.trajLen): + traj_point_message = JointTrajectoryPoint() + traj_point_message.positions = [0.0] * self.nrJoints + traj_point_message.velocities = [0.0] * self.nrJoints + duration_in_seconds = duration_in_seconds + self.Data_array[i_traj, 0] + for i_pos in range(self.nrJoints): + traj_point_message.positions[i_pos] = self.Data_array[ + i_traj, 1 + self.map_joints[i_pos] + ] + traj_point_message.velocities[i_pos] = self.Data_array[ + i_traj, 1 + offset_idx + self.map_joints[i_pos] + ] + + traj_point_message.time_from_start = Duration( + sec=int(duration_in_seconds), + nanosec=int((duration_in_seconds - int(duration_in_seconds)) * 1e9), + ) + goal_msg.trajectory.points.append(traj_point_message) + + # set goal and path tolerances for new goal + # TODO: this does not work, see https://github.com/ros-controls/ros2_controllers/pull/716 + for i_pos in range(self.nrJoints): + goal_tolerance = JointTolerance() + goal_tolerance.name = self.joint_names_[i_pos] + goal_tolerance.position = self.get_parameter("goal_tolerance").value[i_pos] + goal_msg.goal_tolerance.append(goal_tolerance) + path_tolerance = JointTolerance() + path_tolerance.name = self.joint_names_[i_pos] + path_tolerance.position = self.get_parameter("path_tolerance").value[i_pos] + goal_msg.path_tolerance.append(path_tolerance) + + self.action_client_.wait_for_server() + self._send_goal_future = self.action_client_.send_goal_async( + goal_msg, self.get_feedback_callback + ) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + self.goal_handle = future.result() + if not self.goal_handle.accepted: + self.get_logger().info("Goal rejected!") + self.is_goal_rejected = True + return + + self.is_goal_accepted = True + self.get_logger().info("Goal accepted!") + + self._get_result_future = self.goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + print(result.error_code) + print(result.error_string) + if result.error_code == result.SUCCESSFUL: + print("Trajectory goal reached!") + + def get_feedback_callback(self, feedback): + + # print(feedback.feedback.desired) + # print(feedback.feedback.actual) + # print(feedback.feedback.error) + # print("Got feedback!") + feedback # eat warnings + + def safe_exit(self, *args): + # stop when the program is terminated from cmdline + if self.is_goal_accepted: + print("stop action") + future = self.goal_handle.cancel_goal_async() + future.add_done_callback(self.goal_canceled_callback) + + rclpy.shutdown() + + def goal_canceled_callback(self, future): + cancel_response = future.result() + if len(cancel_response.goals_canceling) > 0: + self.get_logger().info("Cancelling of goal complete") + else: + self.get_logger().warning("Goal failed to cancel") + + def input_equals(self, message, input_str): + text = "input" + while not text == input_str: + text = input(message) + + +def main(args=None): + rclpy.init(args=args) + + trajectory_action_client = TrajectoryActionClient() + + try: + rclpy.spin(trajectory_action_client) + finally: + trajectory_action_client.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/example_XX/swingup.csv b/example_XX/swingup.csv new file mode 100644 index 000000000..a82af6487 --- /dev/null +++ b/example_XX/swingup.csv @@ -0,0 +1,12 @@ +t,Position,Theta,Velocity,Theta_dot,u +0.0,0.0,0.0,0.0,0.0,5.944526504438802 +0.2,0.14459751392875353,-0.23811242810671707,1.4459751392897984,-2.381124281069673,9.532519320167593 +0.4,0.521046918602387,-0.709008107653924,2.3185189074459993,-2.3278325144017815,2.220517922064043 +0.6000000000000001,0.9406414542980751,-0.8421054477563331,1.877426449510724,0.9968591133777841,-3.8195037612316463 +0.8,1.1351902410469286,-0.14053883862454752,0.06806141797776316,6.018806977939185,-12.382473600336324 +1.0,1.0489101913522823,1.1499581439370654,-0.9308619149242369,6.886162847675596,-3.2357885619765576 +1.2000000000000002,0.9282531011220609,2.1965705762821606,-0.27570898737796495,3.5799614757753173,3.5119422079331595 +1.4000000000000001,0.9146127300819733,2.731011687620789,0.13930527697708425,1.7644496376112258,2.0242518247574086 +1.6,0.9525948077141708,2.9954838533702537,0.24051549934492322,0.88027201988254,0.336747918560546 +1.8,0.9883231788245086,3.1125518544741837,0.11676821175845994,0.2904079911567141,-1.0829257733923188 +2.0,1.0,3.141592653589793,0.0,0.0, From 7b7153c624841c8eef25975511b409bf80f1ce97 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Nov 2023 19:05:09 +0000 Subject: [PATCH 03/29] Add JTC config --- .../bringup/config/cart_pole_controllers.yaml | 25 ++++- example_XX/bringup/config/plotjuggler.xml | 97 +++++++++++++++++++ example_XX/bringup/launch/cart_pole.launch.py | 9 +- .../ros2_control/cart_pole.ros2_control.xacro | 2 - .../scripts/trajectory_action_client.py | 4 +- 5 files changed, 129 insertions(+), 8 deletions(-) create mode 100644 example_XX/bringup/config/plotjuggler.xml diff --git a/example_XX/bringup/config/cart_pole_controllers.yaml b/example_XX/bringup/config/cart_pole_controllers.yaml index baf14bc3d..cabbbbf57 100644 --- a/example_XX/bringup/config/cart_pole_controllers.yaml +++ b/example_XX/bringup/config/cart_pole_controllers.yaml @@ -2,13 +2,32 @@ controller_manager: ros__parameters: update_rate: 100 # Hz - effort_controller: - type: effort_controllers/JointGroupEffortController + trajectory_controllers: + type: joint_trajectory_controller/JointTrajectoryController joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster -effort_controller: +trajectory_controllers: ros__parameters: joints: - slider_to_cart + - cart_to_pendulum + + command_joints: + - slider_to_cart + + command_interfaces: + - effort + + state_interfaces: + - position + - velocity + + gains: + slider_to_cart: + p: 10.0 + i: 0.0 + d: 0.0 + i_clamp: 0.0 + ff_velocity_scale: 0.0 diff --git a/example_XX/bringup/config/plotjuggler.xml b/example_XX/bringup/config/plotjuggler.xml new file mode 100644 index 000000000..24946fd43 --- /dev/null +++ b/example_XX/bringup/config/plotjuggler.xml @@ -0,0 +1,97 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_XX/bringup/launch/cart_pole.launch.py b/example_XX/bringup/launch/cart_pole.launch.py index aa3b4e07a..6a1c00f94 100644 --- a/example_XX/bringup/launch/cart_pole.launch.py +++ b/example_XX/bringup/launch/cart_pole.launch.py @@ -75,7 +75,14 @@ def generate_launch_description(): ) load_joint_trajectory_controller = ExecuteProcess( - cmd=["ros2", "control", "load_controller", "--set-state", "active", "effort_controller"], + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + "active", + "trajectory_controllers", + ], output="screen", ) diff --git a/example_XX/description/ros2_control/cart_pole.ros2_control.xacro b/example_XX/description/ros2_control/cart_pole.ros2_control.xacro index ec88d1f6b..990493a78 100644 --- a/example_XX/description/ros2_control/cart_pole.ros2_control.xacro +++ b/example_XX/description/ros2_control/cart_pole.ros2_control.xacro @@ -14,12 +14,10 @@ - - diff --git a/example_XX/scripts/trajectory_action_client.py b/example_XX/scripts/trajectory_action_client.py index 7eac65ba3..fe8b642a2 100755 --- a/example_XX/scripts/trajectory_action_client.py +++ b/example_XX/scripts/trajectory_action_client.py @@ -17,7 +17,7 @@ import rclpy from trajectory_msgs.msg import JointTrajectoryPoint from rclpy.node import Node -from builtin_interfaces.msg import Duration +from builtin_interfaces.msg import Duration, Time import sys import signal from rclpy.action import ActionClient @@ -81,7 +81,7 @@ def __init__(self): self.input_equals("\nStart the trajectory by pressing enter", "") goal_msg = FollowJointTrajectory.Goal() goal_msg.trajectory.joint_names = self.joint_names_ - goal_msg.trajectory.header.stamp = self.get_clock().now().to_msg() + goal_msg.trajectory.header.stamp = Time(sec=0, nanosec=0) # start now # get points from csv data offset_idx = self.nrJoints From 47c61d3d168af145d5e335ff2d51a82d5907c5c9 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Nov 2023 21:02:38 +0000 Subject: [PATCH 04/29] Switch to velocity interface --- .../bringup/config/cart_pole_controllers.yaml | 6 +++--- .../ros2_control/cart_pole.ros2_control.xacro | 2 +- .../urdf/cart_pole_description.urdf.xacro | 18 +++++++++--------- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/example_XX/bringup/config/cart_pole_controllers.yaml b/example_XX/bringup/config/cart_pole_controllers.yaml index cabbbbf57..efcb8c08f 100644 --- a/example_XX/bringup/config/cart_pole_controllers.yaml +++ b/example_XX/bringup/config/cart_pole_controllers.yaml @@ -18,7 +18,7 @@ trajectory_controllers: - slider_to_cart command_interfaces: - - effort + - velocity state_interfaces: - position @@ -26,8 +26,8 @@ trajectory_controllers: gains: slider_to_cart: - p: 10.0 + p: 1.0 i: 0.0 d: 0.0 i_clamp: 0.0 - ff_velocity_scale: 0.0 + ff_velocity_scale: 1.0 diff --git a/example_XX/description/ros2_control/cart_pole.ros2_control.xacro b/example_XX/description/ros2_control/cart_pole.ros2_control.xacro index 990493a78..e5a70bba7 100644 --- a/example_XX/description/ros2_control/cart_pole.ros2_control.xacro +++ b/example_XX/description/ros2_control/cart_pole.ros2_control.xacro @@ -8,7 +8,7 @@ gazebo_ros2_control/GazeboSystem - + -1000 1000 diff --git a/ros2_control_demo_description/cart_pole/urdf/cart_pole_description.urdf.xacro b/ros2_control_demo_description/cart_pole/urdf/cart_pole_description.urdf.xacro index bce4c6eb6..572634be4 100644 --- a/ros2_control_demo_description/cart_pole/urdf/cart_pole_description.urdf.xacro +++ b/ros2_control_demo_description/cart_pole/urdf/cart_pole_description.urdf.xacro @@ -8,19 +8,19 @@ - + - + - + - + @@ -58,20 +58,20 @@ - + - + - - + + - + From 84833beef2d564f789b7952f8c7fcdecb6bdc89e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Nov 2023 21:02:57 +0000 Subject: [PATCH 05/29] Add example trajectory and plotjuggler file --- example_XX/bringup/config/plotjuggler.xml | 37 +++++------ .../scripts/trajectory_action_client.py | 3 +- example_XX/swingup.csv | 62 +++++++++++++++---- 3 files changed, 67 insertions(+), 35 deletions(-) diff --git a/example_XX/bringup/config/plotjuggler.xml b/example_XX/bringup/config/plotjuggler.xml index 24946fd43..0bba80c46 100644 --- a/example_XX/bringup/config/plotjuggler.xml +++ b/example_XX/bringup/config/plotjuggler.xml @@ -1,49 +1,42 @@ - + - - + + - + - + - + - - - - - - - - - + + - + - + - - + + - + - - + + diff --git a/example_XX/scripts/trajectory_action_client.py b/example_XX/scripts/trajectory_action_client.py index fe8b642a2..80f9f3daf 100755 --- a/example_XX/scripts/trajectory_action_client.py +++ b/example_XX/scripts/trajectory_action_client.py @@ -85,12 +85,11 @@ def __init__(self): # get points from csv data offset_idx = self.nrJoints - duration_in_seconds = 0 for i_traj in range(self.trajLen): traj_point_message = JointTrajectoryPoint() traj_point_message.positions = [0.0] * self.nrJoints traj_point_message.velocities = [0.0] * self.nrJoints - duration_in_seconds = duration_in_seconds + self.Data_array[i_traj, 0] + duration_in_seconds = self.Data_array[i_traj, 0] for i_pos in range(self.nrJoints): traj_point_message.positions[i_pos] = self.Data_array[ i_traj, 1 + self.map_joints[i_pos] diff --git a/example_XX/swingup.csv b/example_XX/swingup.csv index a82af6487..badcf814d 100644 --- a/example_XX/swingup.csv +++ b/example_XX/swingup.csv @@ -1,12 +1,52 @@ t,Position,Theta,Velocity,Theta_dot,u -0.0,0.0,0.0,0.0,0.0,5.944526504438802 -0.2,0.14459751392875353,-0.23811242810671707,1.4459751392897984,-2.381124281069673,9.532519320167593 -0.4,0.521046918602387,-0.709008107653924,2.3185189074459993,-2.3278325144017815,2.220517922064043 -0.6000000000000001,0.9406414542980751,-0.8421054477563331,1.877426449510724,0.9968591133777841,-3.8195037612316463 -0.8,1.1351902410469286,-0.14053883862454752,0.06806141797776316,6.018806977939185,-12.382473600336324 -1.0,1.0489101913522823,1.1499581439370654,-0.9308619149242369,6.886162847675596,-3.2357885619765576 -1.2000000000000002,0.9282531011220609,2.1965705762821606,-0.27570898737796495,3.5799614757753173,3.5119422079331595 -1.4000000000000001,0.9146127300819733,2.731011687620789,0.13930527697708425,1.7644496376112258,2.0242518247574086 -1.6,0.9525948077141708,2.9954838533702537,0.24051549934492322,0.88027201988254,0.336747918560546 -1.8,0.9883231788245086,3.1125518544741837,0.11676821175845994,0.2904079911567141,-1.0829257733923188 -2.0,1.0,3.141592653589793,0.0,0.0, +0.0,0.0,0.0,0.0,0.0,6.097474709142868 +0.16,0.017784322052586694,-0.024372717096010185,0.22230402623912646,-0.3046589643734476,22.721090524117656 +0.32,0.08930870964978355,-0.1138356328028402,0.6717508187258355,-0.813627481961929,36.46404755020618 +0.48,0.23803666102819018,-0.26891410820766487,1.1873485735042484,-1.1248534605983778,33.740526565911225 +0.64,0.45536759942845817,-0.42404380675785014,1.5292881564991005,-0.8142677712789355,16.78675650394237 +0.8,0.7024407880033816,-0.4758964158494348,1.559126700687441,0.1661101576341313,-5.5182969161082935 +0.96,0.9265395286537074,-0.3394190255438927,1.24210755744163,1.5398572211851482,-28.516591321245755 +1.12,1.077112892051486,0.008100896077192329,0.6400594850306018,2.8041417990783652,-45.652046081377364 +1.28,1.129749081104865,0.4832422007879571,0.017892878136635756,3.1351245098061087,-40.595285462851756 +1.44,1.103063187195788,0.903104422310626,-0.3514665520000908,2.113153259227521,-19.89875151668065 +1.6,1.0376501593194252,1.0919942151672302,-0.4661962964545211,0.24796915148001641,-5.034104338276215 +1.76,0.9703771794892315,0.9643135329679791,-0.37471595142289293,-1.843977678970664,8.489428253729846 +1.92,0.9432166632878712,0.5069412132808199,0.03520949890589886,-3.873176317118637,32.75422599143835 +2.08,0.9988462477782032,-0.20851861265808067,0.6601603072232629,-5.070071507117591,45.73400244926797 +2.24,1.1343769595245616,-0.9640226446469277,1.0339735896062308,-4.37372889274328,20.315394301772095 +2.4,1.3017137579809162,-1.4960962289225495,1.0577363910982092,-2.277190910702503,-0.6961906471860003 +2.56,1.4674774328349172,-1.672714488318252,1.0143095445768084,0.06946266825624338,-2.074662650802777 +2.72,1.624016691873637,-1.4736321900445886,0.9424311934071969,2.4190660601645764,-4.136070015941636 +2.88,1.7447856728434579,-0.8950667439292147,0.5671810687155692,4.813002016276971,-28.326909013380156 +3.04,1.7752447935496858,0.022628833694412764,-0.18644205988772372,6.658192704019665,-57.17510995513326 +3.2,1.7084555339291951,1.0577782309591741,-0.6484236853684118,6.281174761790038,-25.695069701339612 +3.36,1.6120077079850008,1.8972807250518577,-0.5571741389340112,4.2126064143666415,8.77737615801712 +3.52,1.5388643198212606,2.4330422129562757,-0.35711821311273334,2.484412184439077,13.077758292436448 +3.68,1.4921528996828162,2.7457058877273623,-0.22677453861781494,1.4238837501991999,8.169900647362958 +3.84,1.4609411787074162,2.9240290296887426,-0.16337197357467662,0.8051555243180454,4.011651187338257 +4.0,1.4367681219019066,3.024409788364219,-0.13879123649419123,0.449603959125404,1.6199461595834075 +4.16,1.4149845534697665,3.080188209079528,-0.1335033689075575,0.24762629981595785,0.39855404741159034 +4.32,1.3933512745273975,3.110713697963135,-0.13691261787205505,0.1339423112291204,-0.17458692088261402 +4.48,1.3709076170208503,3.1270628288284086,-0.14363310095978535,0.0704218245867997,-0.4058349851654826 +4.64,1.3473386642770866,3.1355078190293697,-0.15097880833726285,0.03514055292521545,-0.45926858657453895 +4.8,1.3226511057187522,3.1395705754442362,-0.1576156736419154,0.0156439022606188,-0.4201414478638008 +4.96,1.2970111212672155,3.1412155236862227,-0.1628841320022929,0.0049179507642097605,-0.3332665511814336 +5.12,1.2706636559484608,3.141532229459051,-0.16645918448214,-0.0009591286038534624,-0.22117006161875458 +5.28,1.243892624917586,3.1411224829547697,-0.1681787034037929,-0.004162702699663808,-0.0957799886116619 +5.44,1.2170010891928777,3.140317700871882,-0.16796549315506462,-0.005897073336433171,0.036443416667160076 +5.6000000000000005,1.1903011692024625,3.139300034989957,-0.16578350672513575,-0.006823750187629958,0.172553311608966 +5.76,1.1641094285579852,3.1381704707450617,-0.1616132513308424,-0.007295802873559883,0.3110957883324216 +5.92,1.1387446751636605,3.136987177045821,-0.15544616609822323,-0.0074953683669494574,0.45084122317122005 +6.08,1.1145263363610733,3.135787002262449,-0.14728306893409832,-0.007506816425203588,0.5910481216921446 +6.24,1.0917732962634237,3.134598869973761,-0.13712993228653006,-0.007344837183393141,0.7312617184349128 +6.4,1.070802843950517,3.1334540632358707,-0.1250007216248058,-0.006965247040235522,0.8704170367023458 +6.5600000000000005,1.051928519489209,3.1323956656530583,-0.1109283341415509,-0.006264722744920327,1.0065316773763464 +6.72,1.0354558306010522,3.131489384912816,-0.09498027696041811,-0.005063786508109729,1.1362322900255448 +6.88,1.0216739750046688,3.1308371725716015,-0.07729291799436658,-0.0030888677570687613,1.2526297927699215 +7.04,1.0108397581716568,3.130592996167731,-0.058134792418294355,3.6662708688836547e-05,1.3434450170110768 +7.2,1.0031457019245928,3.130974069859624,-0.0380409106700071,0.0047267584399700415,1.3834856977528813 +7.36,0.9986559717155432,3.1322451038482484,-0.018080716943114406,0.011161166417833797,1.3249195434474328 +7.5200000000000005,0.9971786375495771,3.1346168813118553,-0.00038596013146047654,0.018486051877256528,1.0743291327721014 +7.68,0.9980116197023657,3.1379132366352147,0.010798237041317046,0.022718389664735748,0.45150287098627506 +7.84,0.9994377393809001,3.140661680698516,0.0070282589403627745,0.011637161126526126,-0.8930570601088956 +8.0,1.0,3.141592653589793,0.0,0.0, From 1aa65dcb6699be3622f042507cd105c28733ad72 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 14 Nov 2023 13:42:02 +0000 Subject: [PATCH 06/29] Fix stuck action client --- example_XX/bringup/config/plotjuggler.xml | 43 ++++++++++--------- .../scripts/trajectory_action_client.py | 5 +++ 2 files changed, 27 insertions(+), 21 deletions(-) diff --git a/example_XX/bringup/config/plotjuggler.xml b/example_XX/bringup/config/plotjuggler.xml index 0bba80c46..941e96670 100644 --- a/example_XX/bringup/config/plotjuggler.xml +++ b/example_XX/bringup/config/plotjuggler.xml @@ -1,42 +1,43 @@ - - + + - - + + - - + + - - + + - - + + - - + + + - + - - + + - - + + - - + + - - + + diff --git a/example_XX/scripts/trajectory_action_client.py b/example_XX/scripts/trajectory_action_client.py index 80f9f3daf..22e3a9761 100755 --- a/example_XX/scripts/trajectory_action_client.py +++ b/example_XX/scripts/trajectory_action_client.py @@ -75,6 +75,10 @@ def __init__(self): sys.stdout.flush() self.joint_names_ = self.get_parameter("joint_names").value + + self.send_goal() + + def send_goal(self): self.is_goal_rejected = False self.is_goal_accepted = False @@ -141,6 +145,7 @@ def get_result_callback(self, future): print(result.error_string) if result.error_code == result.SUCCESSFUL: print("Trajectory goal reached!") + self.send_goal() def get_feedback_callback(self, feedback): From 8dd4e0b3423cc46564ea882d6d307c59dd179a85 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 14 Nov 2023 13:42:23 +0000 Subject: [PATCH 07/29] swingup with higher update rate --- example_XX/swingup.csv | 132 +++++++++++++++++++++++++---------------- 1 file changed, 81 insertions(+), 51 deletions(-) diff --git a/example_XX/swingup.csv b/example_XX/swingup.csv index badcf814d..25385448e 100644 --- a/example_XX/swingup.csv +++ b/example_XX/swingup.csv @@ -1,52 +1,82 @@ t,Position,Theta,Velocity,Theta_dot,u -0.0,0.0,0.0,0.0,0.0,6.097474709142868 -0.16,0.017784322052586694,-0.024372717096010185,0.22230402623912646,-0.3046589643734476,22.721090524117656 -0.32,0.08930870964978355,-0.1138356328028402,0.6717508187258355,-0.813627481961929,36.46404755020618 -0.48,0.23803666102819018,-0.26891410820766487,1.1873485735042484,-1.1248534605983778,33.740526565911225 -0.64,0.45536759942845817,-0.42404380675785014,1.5292881564991005,-0.8142677712789355,16.78675650394237 -0.8,0.7024407880033816,-0.4758964158494348,1.559126700687441,0.1661101576341313,-5.5182969161082935 -0.96,0.9265395286537074,-0.3394190255438927,1.24210755744163,1.5398572211851482,-28.516591321245755 -1.12,1.077112892051486,0.008100896077192329,0.6400594850306018,2.8041417990783652,-45.652046081377364 -1.28,1.129749081104865,0.4832422007879571,0.017892878136635756,3.1351245098061087,-40.595285462851756 -1.44,1.103063187195788,0.903104422310626,-0.3514665520000908,2.113153259227521,-19.89875151668065 -1.6,1.0376501593194252,1.0919942151672302,-0.4661962964545211,0.24796915148001641,-5.034104338276215 -1.76,0.9703771794892315,0.9643135329679791,-0.37471595142289293,-1.843977678970664,8.489428253729846 -1.92,0.9432166632878712,0.5069412132808199,0.03520949890589886,-3.873176317118637,32.75422599143835 -2.08,0.9988462477782032,-0.20851861265808067,0.6601603072232629,-5.070071507117591,45.73400244926797 -2.24,1.1343769595245616,-0.9640226446469277,1.0339735896062308,-4.37372889274328,20.315394301772095 -2.4,1.3017137579809162,-1.4960962289225495,1.0577363910982092,-2.277190910702503,-0.6961906471860003 -2.56,1.4674774328349172,-1.672714488318252,1.0143095445768084,0.06946266825624338,-2.074662650802777 -2.72,1.624016691873637,-1.4736321900445886,0.9424311934071969,2.4190660601645764,-4.136070015941636 -2.88,1.7447856728434579,-0.8950667439292147,0.5671810687155692,4.813002016276971,-28.326909013380156 -3.04,1.7752447935496858,0.022628833694412764,-0.18644205988772372,6.658192704019665,-57.17510995513326 -3.2,1.7084555339291951,1.0577782309591741,-0.6484236853684118,6.281174761790038,-25.695069701339612 -3.36,1.6120077079850008,1.8972807250518577,-0.5571741389340112,4.2126064143666415,8.77737615801712 -3.52,1.5388643198212606,2.4330422129562757,-0.35711821311273334,2.484412184439077,13.077758292436448 -3.68,1.4921528996828162,2.7457058877273623,-0.22677453861781494,1.4238837501991999,8.169900647362958 -3.84,1.4609411787074162,2.9240290296887426,-0.16337197357467662,0.8051555243180454,4.011651187338257 -4.0,1.4367681219019066,3.024409788364219,-0.13879123649419123,0.449603959125404,1.6199461595834075 -4.16,1.4149845534697665,3.080188209079528,-0.1335033689075575,0.24762629981595785,0.39855404741159034 -4.32,1.3933512745273975,3.110713697963135,-0.13691261787205505,0.1339423112291204,-0.17458692088261402 -4.48,1.3709076170208503,3.1270628288284086,-0.14363310095978535,0.0704218245867997,-0.4058349851654826 -4.64,1.3473386642770866,3.1355078190293697,-0.15097880833726285,0.03514055292521545,-0.45926858657453895 -4.8,1.3226511057187522,3.1395705754442362,-0.1576156736419154,0.0156439022606188,-0.4201414478638008 -4.96,1.2970111212672155,3.1412155236862227,-0.1628841320022929,0.0049179507642097605,-0.3332665511814336 -5.12,1.2706636559484608,3.141532229459051,-0.16645918448214,-0.0009591286038534624,-0.22117006161875458 -5.28,1.243892624917586,3.1411224829547697,-0.1681787034037929,-0.004162702699663808,-0.0957799886116619 -5.44,1.2170010891928777,3.140317700871882,-0.16796549315506462,-0.005897073336433171,0.036443416667160076 -5.6000000000000005,1.1903011692024625,3.139300034989957,-0.16578350672513575,-0.006823750187629958,0.172553311608966 -5.76,1.1641094285579852,3.1381704707450617,-0.1616132513308424,-0.007295802873559883,0.3110957883324216 -5.92,1.1387446751636605,3.136987177045821,-0.15544616609822323,-0.0074953683669494574,0.45084122317122005 -6.08,1.1145263363610733,3.135787002262449,-0.14728306893409832,-0.007506816425203588,0.5910481216921446 -6.24,1.0917732962634237,3.134598869973761,-0.13712993228653006,-0.007344837183393141,0.7312617184349128 -6.4,1.070802843950517,3.1334540632358707,-0.1250007216248058,-0.006965247040235522,0.8704170367023458 -6.5600000000000005,1.051928519489209,3.1323956656530583,-0.1109283341415509,-0.006264722744920327,1.0065316773763464 -6.72,1.0354558306010522,3.131489384912816,-0.09498027696041811,-0.005063786508109729,1.1362322900255448 -6.88,1.0216739750046688,3.1308371725716015,-0.07729291799436658,-0.0030888677570687613,1.2526297927699215 -7.04,1.0108397581716568,3.130592996167731,-0.058134792418294355,3.6662708688836547e-05,1.3434450170110768 -7.2,1.0031457019245928,3.130974069859624,-0.0380409106700071,0.0047267584399700415,1.3834856977528813 -7.36,0.9986559717155432,3.1322451038482484,-0.018080716943114406,0.011161166417833797,1.3249195434474328 -7.5200000000000005,0.9971786375495771,3.1346168813118553,-0.00038596013146047654,0.018486051877256528,1.0743291327721014 -7.68,0.9980116197023657,3.1379132366352147,0.010798237041317046,0.022718389664735748,0.45150287098627506 -7.84,0.9994377393809001,3.140661680698516,0.0070282589403627745,0.011637161126526126,-0.8930570601088956 -8.0,1.0,3.141592653589793,0.0,0.0, +0.0,0.0,0.0,0.0,0.0,-4.403090628950663 +0.1,-0.004539967043690484,0.006567578479378633,-0.09079934055534572,0.13135157053477234,-14.336821359062391 +0.2,-0.02273162951555217,0.03194798245423959,-0.2730339088818896,0.37625650896244817,-23.537839246957386 +0.30000000000000004,-0.06237033141829319,0.08378203483900226,-0.5197401291729447,0.6604245387327948,-28.37164839967838 +0.4,-0.12743339135962653,0.16034667293525184,-0.7815210696537127,0.8708682231921904,-27.921389547369944 +0.5,-0.21681180430277192,0.2489875275102259,-1.006047189209185,0.9019488683072834,-22.290934617044996 +0.6000000000000001,-0.32463740293892646,0.3286600279313409,-1.150464783513884,0.6915011401150791,-12.81309376357328 +0.7000000000000001,-0.44165775305843236,0.3752673021198155,-1.189942218876299,0.24064434365443615,-1.3938443679687373 +0.8,-0.5569596689653602,0.36746852409796266,-1.1160960992622515,-0.3966199040914958,10.519948439174561 +0.9,-0.6593092560098988,0.2908594034147643,-0.9308956416285196,-1.1355625095724688,22.05596982534712 +1.0,-0.7382951197342537,0.14093191712530337,-0.6488216328585825,-1.8629872162166896,31.831857636290803 +1.1,-0.7862236622468436,-0.07311825992593296,-0.309749217393208,-2.4180163248080597,37.059867232838386 +1.2000000000000002,-0.8007770109134821,-0.32450520027195306,0.01868224406043229,-2.609722482112431,35.08043406690576 +1.3,-0.7862253565125626,-0.5710823532465932,0.2723508439579582,-2.3218205773801213,26.804813555458352 +1.4000000000000001,-0.7512191507287564,-0.7671684546002095,0.42777327171815815,-1.5999014496921895,16.564881403350938 +1.5,-0.7049963409616377,-0.8769743892029062,0.49668292362422084,-0.5962172423618473,7.598605523796512 +1.6,-0.6556233670012954,-0.8796930078411156,0.49077655558262123,0.541844869597657,-0.34691255993324266 +1.7000000000000002,-0.6110697874219091,-0.766887078972639,0.40029503600510635,1.7142737077718935,-9.474243507408476 +1.8,-0.5808844940097705,-0.5390974608346483,0.20341083223766757,2.841518654987698,-20.899475261237065 +1.9000000000000001,-0.5753514043865737,-0.20789092143287294,-0.09274903977373222,3.78261213304796,-31.49901237035887 +2.0,-0.600393699792452,0.1940395326584534,-0.40809686834383957,4.2559969487788205,-33.85784628755254 +2.1,-0.6523154482210297,0.6070148792619021,-0.6303381002276928,4.003509983289554,-24.891126073211257 +2.2,-0.7199571003574637,0.9615792677054635,-0.7224949425009457,3.087777785582106,-11.962333014509005 +2.3000000000000003,-0.7926659739059346,1.2063410746721273,-0.7316825284685285,1.807458353751302,-2.910554830768037 +2.4000000000000004,-0.8646270105389016,1.316601749759348,-0.707538204190815,0.3977551479931468,1.377986372466846 +2.5,-0.9328767906047193,1.2844667772652634,-0.6574573971255426,-1.040454597874811,4.451562937509129 +2.6,-0.9930192560378676,1.1088720510421795,-0.5453919115374358,-2.4714399265864584,10.765150128486487 +2.7,-1.0359009909507688,0.791850259305105,-0.31224278672059025,-3.8689959081551586,23.079176368164696 +2.8000000000000003,-1.048458213906512,0.34281562960372813,0.061098327605725374,-5.1116966858725394,37.970113577020015 +2.9000000000000004,-1.0224317658636146,-0.2019830983533615,0.4594306332522275,-5.784277873269352,42.46034674057756 +3.0,-0.9643427874037644,-0.7642027011895978,0.7023489359447768,-5.4601141834552855,29.098083414702003 +3.1,-0.8912252953923374,-1.253111273198937,0.760000904283799,-4.318057256731427,11.136459667165761 +3.2,-0.816272142441897,-1.6131403870313399,0.7390621547250231,-2.8825250199165176,1.04116771201548 +3.3000000000000003,-0.7431456690807253,-1.8291324832819604,0.7234673124984259,-1.4373169050963417,-0.7404236295181752 +3.4000000000000004,-0.6699266705120884,-1.9020904820658,0.7409126588742506,-0.02184307058019866,1.6328964816747367 +3.5,-0.5938588318163679,-1.8331093227664423,0.7804441150400883,1.4014662565672134,4.072150416815463 +3.6,-0.5151346195186446,-1.6203044782806464,0.7940401309144063,2.8546306331488247,2.487650167355039 +3.7,-0.4406292790635794,-1.2610484869672982,0.6960666781869317,4.330489193118279,-7.403687168080474 +3.8000000000000003,-0.38566190487363916,-0.7546242484715866,0.4032808056119048,5.797995576796658,-26.472778776856114 +3.9000000000000004,-0.36687719952345366,-0.11726284410665795,-0.027586698608200285,6.949232510501926,-42.83021953133303 +4.0,-0.3848814472739218,0.5834904701397841,-0.3324982564011413,7.065833774426743,-35.176640975036676 +4.1000000000000005,-0.419611136232232,1.238697576092158,-0.3620955227650963,6.038308344621699,-11.101164209020258 +4.2,-0.4497700609787529,1.7716489703706968,-0.24108297216532457,4.620719540946969,6.276339787711619 +4.3,-0.4670725782997097,2.1710748876333694,-0.10496737425381406,3.367798804306375,12.159214419255632 +4.4,-0.47214735080000275,2.4596369217925766,0.003471924247955181,2.4034418788786116,11.680325808588183 +4.5,-0.4679242108394951,2.664690304547718,0.08099087496219745,1.6976257762241465,9.215871244426518 +4.6000000000000005,-0.4571747855498045,2.8091873241307956,0.13399763083160815,1.1923146154373576,6.702795523989609 +4.7,-0.4419991164099382,2.910539734380356,0.16951575196572025,0.8347335895538702,4.694235416135336 +4.800000000000001,-0.42387241659706953,2.981449265175817,0.19301824429165348,0.5834570263553359,3.2207407178313825 +4.9,-0.4038049741105546,3.03100656018601,0.20833060543864731,0.4076888738485182,2.173088426570403 +5.0,-0.38248778512486953,3.0656476690881758,0.2180131742750621,0.285133304194785,1.4325250543101793 +5.1000000000000005,-0.360399678477496,3.089898618380898,0.22374895867240974,0.19988568165965356,0.9044350457382715 +5.2,-0.33788038815261767,3.106928013625275,0.22663684782514898,0.14070222322788958,0.5206236241384486 +5.300000000000001,-0.31517892609246145,3.118947098524287,0.22739239337798184,0.0996794747523499,0.23385787443045758 +5.4,-0.2924852580107236,3.127495245508811,0.22648096825677555,0.07128346493813084,0.012100692547606477 +5.5,-0.26995098429246867,3.1336418881862222,0.22420450610831566,0.051649388610092335,-0.16636135038498537 +5.6000000000000005,-0.24770279800679662,3.1381285827675836,0.2207592196051269,0.03808450301713759,-0.3158069781958893 +5.7,-0.2258512127040312,3.1414685771137782,0.2162724864501604,0.028715383906750436,-0.44608114589606834 +5.800000000000001,-0.20449622810607834,3.144016349630042,0.2108272055088984,0.02224006641852563,-0.5634123058911273 +5.9,-0.18373098963712964,3.1460160453467125,0.20447756387007812,0.017753847914881892,-0.6722671608074265 +6.0,-0.16364415286078593,3.1476350836179177,0.19725917165679793,0.014626917509220615,-0.7752976341281789 +6.1000000000000005,-0.14432138425620708,3.14898739355341,0.18919620043478094,0.01241928120062738,-0.8744605630321567 +6.2,-0.12584629541118617,3.150149361918534,0.180305576465633,0.010820086101863566,-0.9708156509497355 +6.300000000000001,-0.10830097350094355,3.151170675302601,0.1706008617392238,0.009606181579471195,-1.0650056432737174 +6.4,-0.09176618435398666,3.1520815936632856,0.1600949211999151,0.008612185634218807,-1.1571700999515204 +6.5,-0.07632135957162563,3.1528975926242766,0.14880157444731051,0.007707793585604551,-1.2473354113632387 +6.6000000000000005,-0.062044398054685014,3.153622036316146,0.13673765589150372,0.006781080251780027,-1.3349989148566008 +6.7,-0.049011139409104,3.1542475108985264,0.12392751702012586,0.005728411395822755,-1.4190733589248965 +6.800000000000001,-0.03729438788705122,3.1547562113215895,0.1104075134209294,0.00444559706543621,-1.4979567832024028 +6.9,-0.02696241744590261,3.1551195484072423,0.09623189540204458,0.00282114464763199,-1.5691710122418345 +7.0,-0.01807668856791361,3.155297335549895,0.08148268215773521,0.0007345982054157112,-1.6288900116684375 +7.1000000000000005,-0.010688303024193473,3.155237168512657,0.06628502871666732,-0.00193793895018388,-1.671263545154879 +7.2,-0.00483261232052018,3.1548748133920435,0.05082878535679939,-0.00530916346208096,-1.6878079841940998 +7.300000000000001,-0.0005210057875410092,3.1541371789997927,0.03540334530278396,-0.009443524382931733,-1.6652950518089904 +7.4,0.0022716632927734233,3.1529507289202603,0.02045003630350484,-0.014285477207716259,-1.5843239485610294 +7.5,0.003626192846594814,3.15126001307949,0.006640554772922894,-0.01952883960769178,-1.4155766846390554 +7.6000000000000005,0.0037080572787918076,3.1490643970503758,-0.005003266128983026,-0.02438348097459484,-1.115456884614229 +7.7,0.002811628969080266,3.146486831633828,-0.012925300065247817,-0.02716782735635895,-0.6179817299092234 +7.800000000000001,0.0014275117593718896,3.143897490642365,-0.014757044128919735,-0.02461899247289809,0.1749954296373198 +7.9,0.0003448297747109859,3.142129597304204,-0.006896595564298339,-0.010738874290320376,1.4108476709784925 +8.0,0.0,3.141592653589793,0.0,0.0, From a23e7b3bef001895d839c99113b318bc1ac38c02 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 14 Nov 2023 16:44:43 +0000 Subject: [PATCH 08/29] Add initial commit for lqr plugin --- example_XX/CMakeLists.txt | 40 ++++- .../bringup/config/cart_pole_controllers.yaml | 2 + .../cartpole_lqr_trajectory_plugin.cpp | 149 ++++++++++++++++++ ...pole_lqr_trajectory_plugin_parameters.yaml | 37 +++++ .../cartpole_lqr_trajectory_plugin.hpp | 95 +++++++++++ .../visibility_control.h | 49 ++++++ example_XX/package.xml | 2 + example_XX/plugins.xml | 6 + 8 files changed, 378 insertions(+), 2 deletions(-) create mode 100644 example_XX/controller/cartpole_lqr_trajectory_plugin.cpp create mode 100644 example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml create mode 100644 example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp create mode 100644 example_XX/controller/include/ros2_control_demo_example_xx/visibility_control.h create mode 100644 example_XX/plugins.xml diff --git a/example_XX/CMakeLists.txt b/example_XX/CMakeLists.txt index f6dc296d3..86a0fe49a 100644 --- a/example_XX/CMakeLists.txt +++ b/example_XX/CMakeLists.txt @@ -8,6 +8,10 @@ endif() # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib + generate_parameter_library + joint_trajectory_controller_plugins + control_toolbox + trajectory_msgs rclcpp rclpy ) @@ -19,14 +23,40 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(cartpole_lqr_trajectory_plugin_parameters + controller/cartpole_lqr_trajectory_plugin_parameters.yaml +) +add_library(${PROJECT_NAME} SHARED controller/cartpole_lqr_trajectory_plugin.cpp) +add_library(ros2_control_demo_example_xx::cartpole_lqr_trajectory_plugin ALIAS ${PROJECT_NAME}) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) +ament_target_dependencies(${PROJECT_NAME} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(${PROJECT_NAME} PUBLIC + cartpole_lqr_trajectory_plugin_parameters +) +pluginlib_export_plugin_description_file(joint_trajectory_controller_plugins plugins.xml) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_EXAMPLE_XX_BUILDING_LIBRARY") + # INSTALL install( DIRECTORY description/launch description/ros2_control description/urdf description/gazebo - DESTINATION share/ros2_control_demo_example_xx + DESTINATION share/${PROJECT_NAME} ) install( DIRECTORY bringup/launch bringup/config - DESTINATION share/ros2_control_demo_example_xx + DESTINATION share/${PROJECT_NAME} +) +install( + TARGETS ${PROJECT_NAME} cartpole_lqr_trajectory_plugin_parameters + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) # Install Python modules @@ -42,5 +72,11 @@ if(BUILD_TESTING) endif() ## EXPORTS +ament_export_libraries( + cartpole_lqr_trajectory_plugin +) +ament_export_targets( + export_${PROJECT_NAME} +) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/example_XX/bringup/config/cart_pole_controllers.yaml b/example_XX/bringup/config/cart_pole_controllers.yaml index efcb8c08f..aed093435 100644 --- a/example_XX/bringup/config/cart_pole_controllers.yaml +++ b/example_XX/bringup/config/cart_pole_controllers.yaml @@ -24,6 +24,8 @@ trajectory_controllers: - position - velocity + controller_plugin: ros2_control_demo_example_xx::CartpoleLqrTrajectoryPlugin + gains: slider_to_cart: p: 1.0 diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp new file mode 100644 index 000000000..bdb708d08 --- /dev/null +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -0,0 +1,149 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp" + +namespace ros2_control_demo_example_xx +{ + +bool CartpoleLqrTrajectoryPlugin::initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector map_cmd_to_joints) +{ + node_ = node; + map_cmd_to_joints_ = map_cmd_to_joints; + + try + { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(node_); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return false; + } + + return true; +} + +bool CartpoleLqrTrajectoryPlugin::configure() +{ + try + { + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what()); + return false; + } + + // parse read-only params + num_cmd_joints_ = params_.command_joints.size(); + if (num_cmd_joints_ == 0) + { + RCLCPP_ERROR(node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] No command joints specified."); + return false; + } + if (num_cmd_joints_ != map_cmd_to_joints_.size()) + { + RCLCPP_ERROR( + node_->get_logger(), + "[CartpoleLqrTrajectoryPlugin] map_cmd_to_joints has to be of size num_cmd_joints."); + return false; + } + pids_.resize(num_cmd_joints_); + ff_velocity_scale_.resize(num_cmd_joints_); + + return true; +}; + +bool CartpoleLqrTrajectoryPlugin::activate() +{ + params_ = param_listener_->get_params(); + updateGains(); + return true; +}; + +bool CartpoleLqrTrajectoryPlugin::updateGainsRT() +{ + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + updateGains(); + } + + return true; +} + +void CartpoleLqrTrajectoryPlugin::updateGains() +{ + for (size_t i = 0; i < num_cmd_joints_; ++i) + { + RCLCPP_DEBUG( + node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] params_.command_joints %lu : %s", i, + params_.command_joints[i].c_str()); + + const auto & gains = params_.gains.command_joints_map.at(params_.command_joints[i]); + pids_[i] = std::make_shared( + gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + ff_velocity_scale_[i] = gains.ff_velocity_scale; + + RCLCPP_DEBUG(node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] gains.p: %f", gains.p); + RCLCPP_DEBUG( + node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] ff_velocity_scale_: %f", + ff_velocity_scale_[i]); + } + + RCLCPP_INFO( + node_->get_logger(), + "[CartpoleLqrTrajectoryPlugin] Loaded PID gains from ROS parameters for %lu joint(s).", + num_cmd_joints_); +} + +void CartpoleLqrTrajectoryPlugin::computeCommands( + std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint /*current*/, + const trajectory_msgs::msg::JointTrajectoryPoint error, + const trajectory_msgs::msg::JointTrajectoryPoint desired, + const rclcpp::Duration & /*duration_since_start*/, const rclcpp::Duration & period) +{ + // Update PIDs + for (auto i = 0ul; i < num_cmd_joints_; ++i) + { + tmp_command[map_cmd_to_joints_[i]] = + (desired.velocities[map_cmd_to_joints_[i]] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + error.positions[map_cmd_to_joints_[i]], error.velocities[map_cmd_to_joints_[i]], + (uint64_t)period.nanoseconds()); + } +} + +void CartpoleLqrTrajectoryPlugin::reset() +{ + for (const auto & pid : pids_) + { + if (pid) + { + pid->reset(); + } + } +} + +} // namespace ros2_control_demo_example_xx + +#include + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_example_xx::CartpoleLqrTrajectoryPlugin, + joint_trajectory_controller_plugins::TrajectoryControllerBase) diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml b/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml new file mode 100644 index 000000000..49475c627 --- /dev/null +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml @@ -0,0 +1,37 @@ +ros2_control_demo_example_xx: + command_joints: { + type: string_array, + default_value: [], + description: "Names of joints used by the controller plugin", + read_only: true, + validation: { + size_gt<>: [0], + } + } + gains: + __map_command_joints: + p: { + type: double, + default_value: 0.0, + description: "Proportional gain for PID" + } + i: { + type: double, + default_value: 0.0, + description: "Integral gain for PID" + } + d: { + type: double, + default_value: 0.0, + description: "Derivative gain for PID" + } + i_clamp: { + type: double, + default_value: 0.0, + description: "Integral clamp. Symmetrical in both positive and negative direction." + } + ff_velocity_scale: { + type: double, + default_value: 0.0, + description: "Feed-forward scaling of velocity." + } diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp new file mode 100644 index 000000000..dcbfa642c --- /dev/null +++ b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp @@ -0,0 +1,95 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_XX__CARTPOLE_LQR_TRAJECTORY_PLUGIN_HPP_ +#define ROS2_CONTROL_DEMO_EXAMPLE_XX__CARTPOLE_LQR_TRAJECTORY_PLUGIN_HPP_ + +#include +#include +#include + +#include "control_toolbox/pid.hpp" + +#include "cartpole_lqr_trajectory_plugin_parameters.hpp" // auto-generated by generate_parameter_library +#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp" + +using PidPtr = std::shared_ptr; + +namespace ros2_control_demo_example_xx +{ + +class CartpoleLqrTrajectoryPlugin +: public joint_trajectory_controller_plugins::TrajectoryControllerBase +{ +public: + bool initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::vector map_cmd_to_joints) override; + + bool configure() override; + + bool activate() override; + + bool computeControlLawNonRT_impl( + const std::shared_ptr & /*trajectory*/) override + { + // nothing to do + return true; + } + + bool computeControlLawRT_impl( + const std::shared_ptr & /*trajectory*/) override + { + // nothing to do + return true; + } + + bool updateGainsRT() override; + + void computeCommands( + std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, + const trajectory_msgs::msg::JointTrajectoryPoint error, + const trajectory_msgs::msg::JointTrajectoryPoint desired, + const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) override; + + void reset() override; + + void start() override + { + // nothing to do + } + +protected: + /** + * @brief parse PID gains from parameter struct + */ + void updateGains(); + + // number of command joints + size_t num_cmd_joints_; + // map from joints in the message to command joints + std::vector map_cmd_to_joints_; + // PID controllers + std::vector pids_; + // Feed-forward velocity weight factor when calculating closed loop pid adapter's command + std::vector ff_velocity_scale_; + + // Parameters from ROS for ros2_control_demo_example_xx + std::shared_ptr param_listener_; + Params params_; +}; + +} // namespace ros2_control_demo_example_xx + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_XX__CARTPOLE_LQR_TRAJECTORY_PLUGIN_HPP_ diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/visibility_control.h b/example_XX/controller/include/ros2_control_demo_example_xx/visibility_control.h new file mode 100644 index 000000000..3fdddedc9 --- /dev/null +++ b/example_XX/controller/include/ros2_control_demo_example_xx/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_XX__VISIBILITY_CONTROL_H_ +#define ROS2_CONTROL_DEMO_EXAMPLE_XX__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_EXPORT __attribute__((dllexport)) +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_IMPORT __attribute__((dllimport)) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_EXPORT __declspec(dllexport) +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_IMPORT __declspec(dllimport) +#endif +#ifdef ROS2_CONTROL_DEMO_EXAMPLE_XX_BUILDING_LIBRARY +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_XX_EXPORT +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_XX_IMPORT +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_PUBLIC_TYPE ROS2_CONTROL_DEMO_EXAMPLE_XX_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_LOCAL +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_EXPORT __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_IMPORT +#if __GNUC__ >= 4 +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_PUBLIC __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_LOCAL __attribute__((visibility("hidden"))) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_LOCAL +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_XX_PUBLIC_TYPE +#endif + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_XX__VISIBILITY_CONTROL_H_ diff --git a/example_XX/package.xml b/example_XX/package.xml index 4d89f56f5..b0ef34072 100644 --- a/example_XX/package.xml +++ b/example_XX/package.xml @@ -19,6 +19,8 @@ pluginlib rclcpp rclpy + generate_parameter_library + joint_trajectory_controller_plugins python3-pandas diff --git a/example_XX/plugins.xml b/example_XX/plugins.xml new file mode 100644 index 000000000..50432fd68 --- /dev/null +++ b/example_XX/plugins.xml @@ -0,0 +1,6 @@ + + + A trajectory controller consisting of a LQR for the cart pole example. + + From f9c63ea3640da770110ed02dd6d39572630d5682 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 14 Nov 2023 22:32:48 +0000 Subject: [PATCH 09/29] Initial implementation of LQR --- example_XX/CMakeLists.txt | 1 - example_XX/bringup/launch/cart_pole.launch.py | 2 +- .../cartpole_lqr_trajectory_plugin.cpp | 183 ++++++++++++++---- ...pole_lqr_trajectory_plugin_parameters.yaml | 48 ++--- .../cartpole_lqr_trajectory_plugin.hpp | 36 ++-- example_XX/package.xml | 1 + 6 files changed, 184 insertions(+), 87 deletions(-) diff --git a/example_XX/CMakeLists.txt b/example_XX/CMakeLists.txt index 86a0fe49a..9b5088d56 100644 --- a/example_XX/CMakeLists.txt +++ b/example_XX/CMakeLists.txt @@ -10,7 +10,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib generate_parameter_library joint_trajectory_controller_plugins - control_toolbox trajectory_msgs rclcpp rclpy diff --git a/example_XX/bringup/launch/cart_pole.launch.py b/example_XX/bringup/launch/cart_pole.launch.py index 6a1c00f94..6248abadb 100644 --- a/example_XX/bringup/launch/cart_pole.launch.py +++ b/example_XX/bringup/launch/cart_pole.launch.py @@ -35,7 +35,7 @@ def generate_launch_description(): "/gazebo.launch.py", ] ), - launch_arguments={"pause": "true"}.items(), + launch_arguments={"pause": "false"}.items(), ) ros2_control_demo_example_xx_path = os.path.join( diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index bdb708d08..dadef579d 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp" namespace ros2_control_demo_example_xx @@ -22,6 +24,17 @@ bool CartpoleLqrTrajectoryPlugin::initialize( { node_ = node; map_cmd_to_joints_ = map_cmd_to_joints; + if (1 != map_cmd_to_joints_.size()) + { + RCLCPP_ERROR( + node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] map_cmd_to_joints has to have size 1."); + return false; + } + for (const auto & map_cmd_to_joint : map_cmd_to_joints_) + { + RCLCPP_INFO( + node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] map_cmd_to_joint: %lu", map_cmd_to_joint); + } try { @@ -63,8 +76,8 @@ bool CartpoleLqrTrajectoryPlugin::configure() "[CartpoleLqrTrajectoryPlugin] map_cmd_to_joints has to be of size num_cmd_joints."); return false; } - pids_.resize(num_cmd_joints_); - ff_velocity_scale_.resize(num_cmd_joints_); + + reset(); return true; }; @@ -76,68 +89,158 @@ bool CartpoleLqrTrajectoryPlugin::activate() return true; }; -bool CartpoleLqrTrajectoryPlugin::updateGainsRT() +bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( + const std::shared_ptr & trajectory) { - if (param_listener_->is_old(params_)) + // parameters + auto Q = Eigen::Matrix::Identity(); + auto R = Eigen::Matrix::Identity(); + auto N = Eigen::Matrix::Zero(); + + Eigen::Matrix Ks; + Eigen::Matrix Ps; + + calcLQR_steady(Eigen::Vector2d::Zero(), Eigen::Vector2d::Zero(), Q, R, N, Ks, Ps); + + Eigen::Matrix K; + Eigen::Matrix Phi; + Eigen::Matrix Gamma; + Eigen::Matrix C; + C.setIdentity(); + Eigen::Matrix P(Ps), P_new; + // TODO(christophfroehlich) use a buffer to switch from RT thread to new gain + K_vec_.clear(); + time_vec_.clear(); + + // iterate Riccati equation + for (const auto & point : trajectory->points) { - params_ = param_listener_->get_params(); - updateGains(); + // TODO(christophfroehlich) get x_DDot + // TODO(christophfroehlich) create map to get the correct joint position + get_linear_system_matrices(point.positions[1], 0.0, dt, Phi, Gamma); + K = -(R + Gamma.transpose() * P * Gamma).inverse() * (N + Gamma.transpose() * P * Phi); + P_new = (Q + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; + P = 0.5 * (P_new + P_new.transpose()); + K_vec_.push_back(K); + time_vec_.push_back(point.time_from_start); + } + // could be more efficient using reverse iterator + std::reverse(K_vec_.begin(), K_vec_.end()); + + for (const auto & mat : K_vec_) + { + std::cout << "K: " << mat << std::endl; } return true; } -void CartpoleLqrTrajectoryPlugin::updateGains() +void CartpoleLqrTrajectoryPlugin::calcLQR_steady( + Eigen::Vector2d q, Eigen::Vector2d qDot, Eigen::Matrix Q, + Eigen::Matrix R, Eigen::Matrix N, Eigen::Matrix & Ks, + Eigen::Matrix & Ps) +{ + Eigen::Matrix Phi; + Eigen::Matrix Gamma; + Eigen::Matrix C; + C.setIdentity(); + + double theta = q[1]; + + get_linear_system_matrices(theta, 0.0, dt, Phi, Gamma); + + // solve steady-state Riccati equation iteratively + Eigen::Matrix P, P_new; + Eigen::Matrix K; + P.setIdentity(); + + P = P * 1e5; // initial value of P + int ct = 0; + for (ct = 1; ct < 1e4; ct++) + { + K = -(R + Gamma.transpose() * P * Gamma).inverse() * (N + Gamma.transpose() * P * Phi); + P_new = (Q + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; + if ((P_new - P).norm() < 1e-3) + { // abort criterium + break; + } + P = 0.5 * (P_new + P_new.transpose()); + } + Ks = K; + Ps = P; +} + +bool CartpoleLqrTrajectoryPlugin::updateGainsRT() { - for (size_t i = 0; i < num_cmd_joints_; ++i) + if (param_listener_->is_old(params_)) { - RCLCPP_DEBUG( - node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] params_.command_joints %lu : %s", i, - params_.command_joints[i].c_str()); - - const auto & gains = params_.gains.command_joints_map.at(params_.command_joints[i]); - pids_[i] = std::make_shared( - gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); - ff_velocity_scale_[i] = gains.ff_velocity_scale; - - RCLCPP_DEBUG(node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] gains.p: %f", gains.p); - RCLCPP_DEBUG( - node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] ff_velocity_scale_: %f", - ff_velocity_scale_[i]); + params_ = param_listener_->get_params(); + updateGains(); } - RCLCPP_INFO( - node_->get_logger(), - "[CartpoleLqrTrajectoryPlugin] Loaded PID gains from ROS parameters for %lu joint(s).", - num_cmd_joints_); + return true; } +void CartpoleLqrTrajectoryPlugin::updateGains() {} + void CartpoleLqrTrajectoryPlugin::computeCommands( std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint /*current*/, const trajectory_msgs::msg::JointTrajectoryPoint error, const trajectory_msgs::msg::JointTrajectoryPoint desired, - const rclcpp::Duration & /*duration_since_start*/, const rclcpp::Duration & period) + const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) { - // Update PIDs - for (auto i = 0ul; i < num_cmd_joints_; ++i) + if (K_vec_.size() > 0) { - tmp_command[map_cmd_to_joints_[i]] = - (desired.velocities[map_cmd_to_joints_[i]] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - error.positions[map_cmd_to_joints_[i]], error.velocities[map_cmd_to_joints_[i]], - (uint64_t)period.nanoseconds()); + // TODO(christophfroehlich) create map to get the correct joint from error + Eigen::Vector2d e{error.positions[1], error.velocities[1]}; + // TODO(christophfroehlich) interpolate K from time + auto it = std::upper_bound(time_vec_.begin(), time_vec_.end(), duration_since_start); + int idx; + if (it == time_vec_.end()) + { + idx = time_vec_.size() - 1; + } + else + { + idx = std::distance(time_vec_.begin(), it); + } + // integrate acceleration from state feedback to velocity + u += period.seconds() * (K_vec_.at(idx).dot(e)); + // set system input as desired velocity + integrated LQR control output + tmp_command.at(map_cmd_to_joints_.at(0)) = desired.velocities.at(map_cmd_to_joints_.at(0)) + u; } } void CartpoleLqrTrajectoryPlugin::reset() { - for (const auto & pid : pids_) - { - if (pid) - { - pid->reset(); - } - } + K_vec_.clear(); + u = 0; +} + +void CartpoleLqrTrajectoryPlugin::get_linear_system_matrices( + const double theta, const double x_ddot, const double dt, Eigen::Matrix & Phi, + Eigen::Matrix & Gamma) +{ + // system parameters + double lS = params_.system.lS; + double mS = params_.system.mS; + double g = params_.system.g; + double IzzS = params_.system.IzzS; + + // Jacobians of continuous-time dynamics + Eigen::Matrix J_x; + J_x(0, 0) = 0; + J_x(0, 1) = 1; + J_x(1, 0) = -0.2e1 * lS * mS * (g * cos(theta) - sin(theta) * x_ddot) / (lS * lS * mS + 4 * IzzS); + J_x(1, 1) = 0; + + Eigen::Matrix J_u(2); + J_u(0, 0) = 0; + J_u(1, 0) = -0.2e1 * lS * mS * cos(theta) / (lS * lS * mS + 4 * IzzS); + + // discrete-time linearized system matrices + Phi = Eigen::Matrix::Identity() + dt * J_x; + Gamma = dt * J_u; } } // namespace ros2_control_demo_example_xx diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml b/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml index 49475c627..2b6d2adde 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml @@ -8,30 +8,24 @@ ros2_control_demo_example_xx: size_gt<>: [0], } } - gains: - __map_command_joints: - p: { - type: double, - default_value: 0.0, - description: "Proportional gain for PID" - } - i: { - type: double, - default_value: 0.0, - description: "Integral gain for PID" - } - d: { - type: double, - default_value: 0.0, - description: "Derivative gain for PID" - } - i_clamp: { - type: double, - default_value: 0.0, - description: "Integral clamp. Symmetrical in both positive and negative direction." - } - ff_velocity_scale: { - type: double, - default_value: 0.0, - description: "Feed-forward scaling of velocity." - } + system: + lS: { + type: double, + default_value: 1.0, + description: "Length of the pole" + } + mS: { + type: double, + default_value: 1.1, + description: "mass of the pole" + } + IzzS: { + type: double, + default_value: 0.09, + description: "moment of inertia of the pendulum about its center of mass" + } + g: { + type: double, + default_value: 9.81, + description: "gravitational constant" + } diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp index dcbfa642c..f3119ee9a 100644 --- a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp +++ b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp @@ -15,17 +15,14 @@ #ifndef ROS2_CONTROL_DEMO_EXAMPLE_XX__CARTPOLE_LQR_TRAJECTORY_PLUGIN_HPP_ #define ROS2_CONTROL_DEMO_EXAMPLE_XX__CARTPOLE_LQR_TRAJECTORY_PLUGIN_HPP_ +#include #include #include #include -#include "control_toolbox/pid.hpp" - #include "cartpole_lqr_trajectory_plugin_parameters.hpp" // auto-generated by generate_parameter_library #include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp" -using PidPtr = std::shared_ptr; - namespace ros2_control_demo_example_xx { @@ -42,11 +39,7 @@ class CartpoleLqrTrajectoryPlugin bool activate() override; bool computeControlLawNonRT_impl( - const std::shared_ptr & /*trajectory*/) override - { - // nothing to do - return true; - } + const std::shared_ptr & /*trajectory*/) override; bool computeControlLawRT_impl( const std::shared_ptr & /*trajectory*/) override @@ -65,29 +58,36 @@ class CartpoleLqrTrajectoryPlugin void reset() override; - void start() override - { - // nothing to do - } + void start() override {} protected: /** - * @brief parse PID gains from parameter struct + * @brief parse gains from parameter struct */ void updateGains(); + void get_linear_system_matrices( + const double theta, const double x_ddot, const double dt, Eigen::Matrix & A, + Eigen::Matrix & B); + void calcLQR_steady( + Eigen::Vector2d q, Eigen::Vector2d qDot, Eigen::Matrix Q, + Eigen::Matrix R, Eigen::Matrix N, Eigen::Matrix & Ks, + Eigen::Matrix & Ps); + std::vector> K_vec_; + std::vector time_vec_; + double u; + // number of command joints size_t num_cmd_joints_; // map from joints in the message to command joints std::vector map_cmd_to_joints_; - // PID controllers - std::vector pids_; - // Feed-forward velocity weight factor when calculating closed loop pid adapter's command - std::vector ff_velocity_scale_; // Parameters from ROS for ros2_control_demo_example_xx std::shared_ptr param_listener_; Params params_; + // TODO(christophfroehlich): do we need the controller update rate here, or the trajectory + // sampling? + double dt = 0.01; }; } // namespace ros2_control_demo_example_xx diff --git a/example_XX/package.xml b/example_XX/package.xml index b0ef34072..a48b5b5a8 100644 --- a/example_XX/package.xml +++ b/example_XX/package.xml @@ -21,6 +21,7 @@ rclpy generate_parameter_library joint_trajectory_controller_plugins + eigen python3-pandas From 94d48af2857a68562e52374facd998bd5a6c84a2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Nov 2023 17:03:43 +0000 Subject: [PATCH 10/29] Add sidestep --- example_XX/sidestep.csv | 82 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 82 insertions(+) create mode 100644 example_XX/sidestep.csv diff --git a/example_XX/sidestep.csv b/example_XX/sidestep.csv new file mode 100644 index 000000000..cbe42d2f7 --- /dev/null +++ b/example_XX/sidestep.csv @@ -0,0 +1,82 @@ +t,Position,Theta,Velocity,Theta_dot,u +0.0,0.0,0.0,0.0,0.0,11.668695509317981 +0.025,0.0005249982868349666,-0.0007856124258703744,0.04199986319398341,-0.06284899342082974,22.874429527610104 +0.05,0.002255178687858652,-0.0033674601335596803,0.0964145688879113,-0.14369882319431368,21.930011266123948 +0.07500000000000001,0.005315119314648953,-0.0079082576545442,0.14838068125531348,-0.219564978484449,20.951532311416635 +0.1,0.009642189740324375,-0.0142725666341639,0.19778495279872116,-0.2895797398851283,19.94372911865718 +0.125,0.015171112504901071,-0.02230418497901912,0.24452886836741514,-0.3529497277032949,18.911946298370474 +0.15000000000000002,0.02183435070454961,-0.03182812929072389,0.2885301876044683,-0.4089658172330901,17.861799246710994 +0.17500000000000002,0.029562528295831295,-0.0426528515567987,0.32972401969806686,-0.4570119640528962,16.799055085823905 +0.2,0.03828488002765833,-0.05457267310749891,0.3680641188480954,-0.4965737600031202,15.729897916881407 +0.225,0.047929732366117514,-0.06737041990249446,0.40352406822864073,-0.5272459835965259,14.660583646795388 +0.25,0.05842500561557665,-0.08082022051411232,0.4360977917280854,-0.5487380653328955,13.597294953873602 +0.275,0.06969872762758651,-0.09469042279313516,0.4657999692327073,-0.5608781169889384,12.5463174499735 +0.30000000000000004,0.08167955355911681,-0.10874658655638617,0.4926661052897107,-0.5636149840711318,11.513728005039958 +0.325,0.09429727653478268,-0.1227544919734844,0.5167517327635623,-0.5570174492967327,10.50516883206907 +0.35000000000000003,0.10748331539011449,-0.13648310388494866,0.5381313756629855,-0.541271503620413,9.52605565612203 +0.375,0.12117116956622538,-0.14970743874853132,0.5568969584258865,-0.5166752854661943,8.581082259470591 +0.4,0.13529683094957928,-0.16221128306162697,0.5731559522424253,-0.4836322595814595,7.6746386182231605 +0.42500000000000004,0.14979914859969146,-0.1737897254910946,0.5870294597665479,-0.4426431347759503,6.81043862347416 +0.45,0.1646201388362714,-0.18425146526551597,0.5986497591598482,-0.3942960471777597,5.991493032486073 +0.47500000000000003,0.17970523717136225,-0.19342087147838435,0.6081581076474197,-0.33925644985171005,5.220343358359275 +0.5,0.19500349723493648,-0.20113978702754004,0.6157026974385196,-0.2782567940807448,4.498898116148843 +0.525,0.21046773975318198,-0.20726907357653673,0.6214367040211193,-0.2120861298389916,3.828577648359694 +0.55,0.22605465955289059,-0.21168990603476945,0.6255168799555653,-0.14158046681962702,3.2104970301060676 +0.5750000000000001,0.24172489976831896,-0.21430483053869864,0.6281023372786997,-0.06761349349470999,2.6453045177047416 +0.6000000000000001,0.25744309799165593,-0.21503859580655277,0.6293535205882569,0.008912272066381545,2.1333690127512974 +0.625,0.2731779082988229,-0.21383876770756985,0.6294313039851105,0.08707397585225056,1.674580493827355 +0.65,0.28890200699913215,-0.21067614259971942,0.6284965920396305,0.16593603277578367,1.2688807525689019 +0.675,0.30459208832703083,-0.2055449715713566,0.6267099141922619,0.24455764949324482,0.9156814752802812 +0.7000000000000001,0.3202288510129543,-0.19846299826205152,0.6242311006816131,0.32200021525116046,0.6145120118048036 +0.7250000000000001,0.33579696950379717,-0.18947130030144413,0.621218378585818,0.3973356215974337,0.36393822900272865 +0.75,0.3512850446028227,-0.17863392380021123,0.6178276293362255,0.4696544985011989,0.1628650279201731 +0.775,0.3666855353996621,-0.16603730903933642,0.6142116344109173,0.5380746823687896,0.009349399260925631 +0.8,0.3819946600310658,-0.1517894834116517,0.6105183361013808,0.6017513678459884,-0.09887379448297638 +0.8250000000000001,0.3972122524747707,-0.13601899722346925,0.6068890593950121,0.6598875272086043,-0.16478707570376366 +0.8500000000000001,0.41234157084512935,-0.1188735915676659,0.6034564102336734,0.7117449252556582,-0.19180530485021954 +0.875,0.4273890518586057,-0.10051858931005407,0.6003420708444415,0.7566552553532805,-0.18401292266717156 +0.9,0.44236400447468166,-0.08113500219772471,0.5976541384416397,0.7940317136330605,-0.14621151140730718 +0.925,0.4572782394994751,-0.06091735855070153,0.5954846635418278,0.8233797781287878,-0.08367292015737192 +0.9500000000000001,0.4721456403330127,-0.04007127530502692,0.5939074031411812,0.8443068815251776,-0.002240099454505675 +0.9750000000000001,0.48698167934474695,-0.01881080438328371,0.592975717797555,0.8565307922142901,0.09179270728981989 +1.0,0.501802888605128,0.0026444033147508083,0.5927210230329271,0.8598858236284694,0.19185280757215076 +1.0250000000000001,0.5166263014813472,0.024072055376475936,0.5931520070646146,0.854326341309538,0.29133187984739645 +1.05,0.5314688797538006,0.04525023433759509,0.594254254731659,0.8399279755799913,0.38343062658971844 +1.075,0.5463469404211817,0.06596040928489993,0.5959905986588216,0.8168860202043917,0.46165655710985837 +1.1,0.5612756036989813,0.08599036804962876,0.5983024635651413,0.7855106809739117,0.5199030641704605 +1.125,0.5762682771152859,0.10513700805547752,0.6011114097392273,0.7462205194939925,0.5523326501108841 +1.1500000000000001,0.591336181991782,0.12320894155561601,0.6043209803804601,0.6995341605170873,0.5535973265139249 +1.175,0.6064879361537685,0.14002886596335648,0.6078193525784567,0.6460597921021509,0.5191993689948812 +1.2000000000000002,0.6217292027432357,0.15543566305388096,0.6114819745789212,0.5864839751398052,0.4451086474048233 +1.225,0.6370624007005793,0.16928621939145655,0.6151738620085673,0.521560531866245,0.3279177536361516 +1.25,0.652486471010042,0.1814569690751375,0.6187517627484449,0.4520994428282295,0.16468405840680875 +1.2750000000000001,0.6679966966193562,0.19184515971244467,0.6220662859966991,0.37895580815634583,-0.046799079109961075 +1.3,0.683584572788048,0.2003698484028877,0.62496380749864,0.3030192870790997,-0.30829583375105857 +1.3250000000000002,0.6992377174310365,0.20697264773771995,0.6272877639404412,0.22520465970748524,-0.6211747843771158 +1.35,0.7149398065977296,0.21161824937617132,0.6288793693950105,0.14644347136862232,-0.9866916744035031 +1.375,0.7306705303827283,0.21429473754604703,0.6295785334048886,0.06767558222143097,-1.4054224679482592 +1.4000000000000001,0.7464055697784847,0.21501369595696238,0.6292246182556104,-0.010158909348200646,-1.8778839096742028 +1.425,0.7621165862519903,0.21381012259861346,0.6276566996248617,-0.08612695931971259,-2.4042885201155584 +1.4500000000000002,0.7777712192434109,0.21074215974790772,0.6247139396887706,-0.15931006873674444,-2.984709126501733 +1.475,0.7933330965206831,0.20589063017828066,0.6202362424930075,-0.22881229683341078,-3.618736963048404 +1.5,0.8087618630550444,0.199358367925156,0.6140650802559019,-0.29376868341656526,-4.305774593430852 +1.5250000000000001,0.8240132309705702,0.19126933572920943,0.6060443529861627,-0.3533538922591561,-5.04483401862314 +1.55,0.8390390584731728,0.18176751364473517,0.596021847222048,-0.40679187449878634,-5.834376476504156 +1.5750000000000002,0.8537874699709103,0.17101553840586398,0.5838510725969508,-0.45336614461091024,-6.672292023165344 +1.6,0.8682030189129212,0.15919309164768797,0.5693928427639164,-0.4924295960431725,-7.556245048881028 +1.625,0.8822268981174934,0.14649503432378086,0.552517493601866,-0.5234149898693883,-8.482887429268468 +1.6500000000000001,0.8957972075505788,0.13312928212802366,0.5331072610449538,-0.5458451857911936,-9.448555095433012 +1.675,0.9088492774451692,0.1193144404714833,0.5110583305222833,-0.5593421467320456,-10.448833271121988 +1.7000000000000002,0.9213160440699738,0.10527722467852432,0.48628299946210013,-0.563635116704683,-11.478836624116797 +1.725,0.9331284735402927,0.09124969996383876,0.4587113581634168,-0.558566860470148,-12.533215363462125 +1.75,0.9442160255022357,0.07746638622641001,0.4282927987920271,-0.5440982385241578,-13.60614101035722 +1.7750000000000001,0.954507149306268,0.0641612754083064,0.39499710553055334,-0.5203106269241236,-14.691490899536946 +1.8,0.9639297989764113,0.05156482069784136,0.358814868080901,-0.4874057499130838,-15.783038904596799 +1.8250000000000002,0.9724119517108009,0.03990095933234734,0.3197573506702638,-0.445703159326435,-16.87454294982393 +1.85,0.9798821180545898,0.029384223749353903,0.27785595683284986,-0.39563568731303833,-17.959849366139903 +1.875,0.9862698384417842,0.020216982304025584,0.23316167414270306,-0.3377436283132264,-19.032706664602173 +1.9000000000000001,0.9915061588801355,0.012586848637039374,0.18574396092540627,-0.27266706504566773,-20.08736174424844 +1.925,0.9955240740875725,0.006664299697547782,0.1356892556695488,-0.20113685011366053,-21.11827585794378 +1.9500000000000002,0.9982589376065562,0.002600519666894806,0.08309982584914587,-0.12396555233857313,-22.12017662288569 +1.975,0.9996488427163353,0.0005254751354907722,0.02809258293318419,-0.042038010173750404,-23.0881141893242 +2.0,1.0,0.0,0.0,0.0, From 9b41bed46eb598d519ff6b6ae6cfa0adbfb91d54 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Nov 2023 17:08:55 +0000 Subject: [PATCH 11/29] Add LQR gains to parameters and increase number states --- .../bringup/config/cart_pole_controllers.yaml | 24 ++++ example_XX/bringup/config/plotjuggler.xml | 28 +++-- .../cartpole_lqr_trajectory_plugin.cpp | 117 +++++++++++------- ...pole_lqr_trajectory_plugin_parameters.yaml | 20 ++- .../cartpole_lqr_trajectory_plugin.hpp | 15 ++- 5 files changed, 140 insertions(+), 64 deletions(-) diff --git a/example_XX/bringup/config/cart_pole_controllers.yaml b/example_XX/bringup/config/cart_pole_controllers.yaml index aed093435..3ace28331 100644 --- a/example_XX/bringup/config/cart_pole_controllers.yaml +++ b/example_XX/bringup/config/cart_pole_controllers.yaml @@ -26,6 +26,28 @@ trajectory_controllers: controller_plugin: ros2_control_demo_example_xx::CartpoleLqrTrajectoryPlugin + gains: + R: 1.0 + Q: [1.0, 1.0, 1.0, 1.0] + cart_to_pendulum: + angle_wraparound: true + +trajectory_controllers_pid: + ros__parameters: + joints: + - slider_to_cart + - cart_to_pendulum + + command_joints: + - slider_to_cart + + command_interfaces: + - velocity + + state_interfaces: + - position + - velocity + gains: slider_to_cart: p: 1.0 @@ -33,3 +55,5 @@ trajectory_controllers: d: 0.0 i_clamp: 0.0 ff_velocity_scale: 1.0 + cart_to_pendulum: + angle_wraparound: true diff --git a/example_XX/bringup/config/plotjuggler.xml b/example_XX/bringup/config/plotjuggler.xml index 941e96670..1a8dad5f0 100644 --- a/example_XX/bringup/config/plotjuggler.xml +++ b/example_XX/bringup/config/plotjuggler.xml @@ -3,41 +3,45 @@ - - + + - - + + + - - + + - + + - + - - + + + - - + + + diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index dadef579d..b2bf4aca6 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -89,35 +89,55 @@ bool CartpoleLqrTrajectoryPlugin::activate() return true; }; +Eigen::Vector get_state_from_point( + trajectory_msgs::msg::JointTrajectoryPoint point) +{ + // TODO(anyone) create map to get the correct joint order + return Eigen::Vector{ + point.positions[1], point.velocities[1], point.positions[0], point.velocities[0]}; +} + bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( const std::shared_ptr & trajectory) { // parameters - auto Q = Eigen::Matrix::Identity(); - auto R = Eigen::Matrix::Identity(); - auto N = Eigen::Matrix::Zero(); - - Eigen::Matrix Ks; - Eigen::Matrix Ps; - - calcLQR_steady(Eigen::Vector2d::Zero(), Eigen::Vector2d::Zero(), Q, R, N, Ks, Ps); - - Eigen::Matrix K; - Eigen::Matrix Phi; - Eigen::Matrix Gamma; - Eigen::Matrix C; + Eigen::Vector Q_diag{params_.gains.Q.data()}; + Eigen::Matrix Q(Q_diag.asDiagonal()); + Eigen::Matrix R{params_.gains.R}; + std::cout << "Q: " << std::endl << Q << std::endl; + std::cout << "R: " << std::endl << R << std::endl; + auto N = Eigen::Matrix::Zero(); + + Eigen::Matrix Ks; + Eigen::Matrix Ps; + auto x_R = get_state_from_point(trajectory->points.back()); + Eigen::Vector u_R{0}; + calcLQR_steady(x_R, u_R, Q, R, N, Ks, Ps); + std::cout << "Ks: " << Ks << std::endl; + std::cout << "Ps: " << Ps << std::endl; + + Eigen::Matrix K; + Eigen::Matrix Phi; + Eigen::Matrix Gamma; + Eigen::Matrix C; C.setIdentity(); - Eigen::Matrix P(Ps), P_new; + Eigen::Matrix P(Ps), P_new; // TODO(christophfroehlich) use a buffer to switch from RT thread to new gain K_vec_.clear(); time_vec_.clear(); + double x_dot_old = 0; + Eigen::Vector x; + Eigen::Vector u; // iterate Riccati equation for (const auto & point : trajectory->points) { - // TODO(christophfroehlich) get x_DDot - // TODO(christophfroehlich) create map to get the correct joint position - get_linear_system_matrices(point.positions[1], 0.0, dt, Phi, Gamma); + x = get_state_from_point(point); + // get x_ddot for trajectory + u[0] = (x(3) - x_dot_old) / dt; + x_dot_old = x(3); + + get_linear_system_matrices(x, u, dt, Phi, Gamma); K = -(R + Gamma.transpose() * P * Gamma).inverse() * (N + Gamma.transpose() * P * Phi); P_new = (Q + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; P = 0.5 * (P_new + P_new.transpose()); @@ -129,29 +149,28 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( for (const auto & mat : K_vec_) { - std::cout << "K: " << mat << std::endl; + std::cout << "K: " << std::endl << mat << std::endl; } return true; } void CartpoleLqrTrajectoryPlugin::calcLQR_steady( - Eigen::Vector2d q, Eigen::Vector2d qDot, Eigen::Matrix Q, - Eigen::Matrix R, Eigen::Matrix N, Eigen::Matrix & Ks, - Eigen::Matrix & Ps) + Eigen::Vector x, Eigen::Vector u, + Eigen::Matrix Q, Eigen::Matrix R, + Eigen::Matrix N, Eigen::Matrix & Ks, + Eigen::Matrix & Ps) { - Eigen::Matrix Phi; - Eigen::Matrix Gamma; - Eigen::Matrix C; + Eigen::Matrix Phi; + Eigen::Matrix Gamma; + Eigen::Matrix C; C.setIdentity(); - double theta = q[1]; - - get_linear_system_matrices(theta, 0.0, dt, Phi, Gamma); + get_linear_system_matrices(x, u, dt, Phi, Gamma); // solve steady-state Riccati equation iteratively - Eigen::Matrix P, P_new; - Eigen::Matrix K; + Eigen::Matrix P, P_new; + Eigen::Matrix K; P.setIdentity(); P = P * 1e5; // initial value of P @@ -160,8 +179,8 @@ void CartpoleLqrTrajectoryPlugin::calcLQR_steady( { K = -(R + Gamma.transpose() * P * Gamma).inverse() * (N + Gamma.transpose() * P * Phi); P_new = (Q + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; - if ((P_new - P).norm() < 1e-3) - { // abort criterium + if ((P_new - P).norm() < 1e-3) // abort criterium + { break; } P = 0.5 * (P_new + P_new.transpose()); @@ -191,8 +210,7 @@ void CartpoleLqrTrajectoryPlugin::computeCommands( { if (K_vec_.size() > 0) { - // TODO(christophfroehlich) create map to get the correct joint from error - Eigen::Vector2d e{error.positions[1], error.velocities[1]}; + auto e = get_state_from_point(error); // TODO(christophfroehlich) interpolate K from time auto it = std::upper_bound(time_vec_.begin(), time_vec_.end(), duration_since_start); int idx; @@ -204,8 +222,17 @@ void CartpoleLqrTrajectoryPlugin::computeCommands( { idx = std::distance(time_vec_.begin(), it); } - // integrate acceleration from state feedback to velocity - u += period.seconds() * (K_vec_.at(idx).dot(e)); + // integrate acceleration from state feedback to velocity. consider sign of e! + // u += period.seconds() * (K_vec_.at(idx).dot(-e)); + // RCLCPP_INFO_STREAM_THROTTLE( + // node_->get_logger(), *node_->get_clock(), 500, "K:" << K_vec_.at(idx)); + // RCLCPP_INFO_STREAM_THROTTLE( + // node_->get_logger(), *node_->get_clock(), 500, "e:" << -e); + // RCLCPP_INFO_STREAM_THROTTLE( + // node_->get_logger(), *node_->get_clock(), 500, "a:" << dt*K_vec_.at(idx).dot(-e)); + u += dt * (K_vec_.at(idx).dot(-e)); + // RCLCPP_INFO_THROTTLE( + // node_->get_logger(), *node_->get_clock(), 500, "u: %f", u); // set system input as desired velocity + integrated LQR control output tmp_command.at(map_cmd_to_joints_.at(0)) = desired.velocities.at(map_cmd_to_joints_.at(0)) + u; } @@ -218,9 +245,13 @@ void CartpoleLqrTrajectoryPlugin::reset() } void CartpoleLqrTrajectoryPlugin::get_linear_system_matrices( - const double theta, const double x_ddot, const double dt, Eigen::Matrix & Phi, - Eigen::Matrix & Gamma) + Eigen::Vector x, Eigen::Vector u, const double dt, + Eigen::Matrix & Phi, Eigen::Matrix & Gamma) { + // states and input + double theta = x[0]; + double x_ddot = u[0]; + // system parameters double lS = params_.system.lS; double mS = params_.system.mS; @@ -228,18 +259,18 @@ void CartpoleLqrTrajectoryPlugin::get_linear_system_matrices( double IzzS = params_.system.IzzS; // Jacobians of continuous-time dynamics - Eigen::Matrix J_x; - J_x(0, 0) = 0; + Eigen::Matrix J_x = + Eigen::Matrix::Zero(); J_x(0, 1) = 1; J_x(1, 0) = -0.2e1 * lS * mS * (g * cos(theta) - sin(theta) * x_ddot) / (lS * lS * mS + 4 * IzzS); - J_x(1, 1) = 0; + J_x(2, 3) = 1; - Eigen::Matrix J_u(2); - J_u(0, 0) = 0; + Eigen::Matrix J_u = Eigen::Matrix::Zero(); J_u(1, 0) = -0.2e1 * lS * mS * cos(theta) / (lS * lS * mS + 4 * IzzS); + J_u(3, 0) = 1; // discrete-time linearized system matrices - Phi = Eigen::Matrix::Identity() + dt * J_x; + Phi = Eigen::Matrix::Identity() + dt * J_x; Gamma = dt * J_u; } diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml b/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml index 2b6d2adde..09b204c11 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml @@ -16,16 +16,30 @@ ros2_control_demo_example_xx: } mS: { type: double, - default_value: 1.1, + default_value: 1.100, description: "mass of the pole" } IzzS: { type: double, - default_value: 0.09, + default_value: 0.092, description: "moment of inertia of the pendulum about its center of mass" } g: { type: double, - default_value: 9.81, + default_value: 9.80, description: "gravitational constant" } + gains: + R: { + type: double, + default_value: 1.0, + description: "Cost on the control input" + } + Q: { + type: double_array, + default_value: [1.0, 1.0, 1.0, 1.0], + description: "Cost on the state, diagonal of matrix", + validation: { + fixed_size<>: [4], + } + } diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp index f3119ee9a..7fb653e1b 100644 --- a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp +++ b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp @@ -66,14 +66,17 @@ class CartpoleLqrTrajectoryPlugin */ void updateGains(); +#define NUM_STATES 4 void get_linear_system_matrices( - const double theta, const double x_ddot, const double dt, Eigen::Matrix & A, - Eigen::Matrix & B); + Eigen::Vector x, Eigen::Vector u, const double dt, + Eigen::Matrix & Phi, + Eigen::Matrix & Gamma); void calcLQR_steady( - Eigen::Vector2d q, Eigen::Vector2d qDot, Eigen::Matrix Q, - Eigen::Matrix R, Eigen::Matrix N, Eigen::Matrix & Ks, - Eigen::Matrix & Ps); - std::vector> K_vec_; + Eigen::Vector q, Eigen::Vector u, + Eigen::Matrix Q, Eigen::Matrix R, + Eigen::Matrix N, Eigen::Matrix & Ks, + Eigen::Matrix & Ps); + std::vector> K_vec_; std::vector time_vec_; double u; From d1938aa27cf00d2dc2a7aa8aeb1d05c4fe91f9cf Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 17 Nov 2023 18:52:23 +0000 Subject: [PATCH 12/29] Fix LQR calculation --- example_XX/CMakeLists.txt | 1 + .../bringup/config/cart_pole_controllers.yaml | 2 +- .../cartpole_lqr_trajectory_plugin.cpp | 130 ++++++++++-------- ...pole_lqr_trajectory_plugin_parameters.yaml | 5 + .../cartpole_lqr_trajectory_plugin.hpp | 6 +- example_XX/package.xml | 1 + 6 files changed, 87 insertions(+), 58 deletions(-) diff --git a/example_XX/CMakeLists.txt b/example_XX/CMakeLists.txt index 9b5088d56..74d603fa1 100644 --- a/example_XX/CMakeLists.txt +++ b/example_XX/CMakeLists.txt @@ -7,6 +7,7 @@ endif() # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS + backward_ros pluginlib generate_parameter_library joint_trajectory_controller_plugins diff --git a/example_XX/bringup/config/cart_pole_controllers.yaml b/example_XX/bringup/config/cart_pole_controllers.yaml index 3ace28331..7d351555e 100644 --- a/example_XX/bringup/config/cart_pole_controllers.yaml +++ b/example_XX/bringup/config/cart_pole_controllers.yaml @@ -27,7 +27,7 @@ trajectory_controllers: controller_plugin: ros2_control_demo_example_xx::CartpoleLqrTrajectoryPlugin gains: - R: 1.0 + R: 0.1 Q: [1.0, 1.0, 1.0, 1.0] cart_to_pendulum: angle_wraparound: true diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index b2bf4aca6..98b6a2304 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -80,19 +80,20 @@ bool CartpoleLqrTrajectoryPlugin::configure() reset(); return true; -}; +} bool CartpoleLqrTrajectoryPlugin::activate() { params_ = param_listener_->get_params(); updateGains(); return true; -}; +} Eigen::Vector get_state_from_point( trajectory_msgs::msg::JointTrajectoryPoint point) { // TODO(anyone) create map to get the correct joint order + // theta, theta_dot, x, x_dot return Eigen::Vector{ point.positions[1], point.velocities[1], point.positions[0], point.velocities[0]}; } @@ -101,62 +102,81 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( const std::shared_ptr & trajectory) { // parameters - Eigen::Vector Q_diag{params_.gains.Q.data()}; - Eigen::Matrix Q(Q_diag.asDiagonal()); - Eigen::Matrix R{params_.gains.R}; - std::cout << "Q: " << std::endl << Q << std::endl; - std::cout << "R: " << std::endl << R << std::endl; auto N = Eigen::Matrix::Zero(); Eigen::Matrix Ks; Eigen::Matrix Ps; auto x_R = get_state_from_point(trajectory->points.back()); Eigen::Vector u_R{0}; - calcLQR_steady(x_R, u_R, Q, R, N, Ks, Ps); - std::cout << "Ks: " << Ks << std::endl; - std::cout << "Ps: " << Ps << std::endl; + calcLQR_steady(x_R, u_R, 0.1, Q_, R_, N, Ks, Ps); + // std::cout << "Ks: " << Ks << std::endl; + // std::cout << "Ps: " << Ps << std::endl; - Eigen::Matrix K; - Eigen::Matrix Phi; - Eigen::Matrix Gamma; - Eigen::Matrix C; - C.setIdentity(); - Eigen::Matrix P(Ps), P_new; - // TODO(christophfroehlich) use a buffer to switch from RT thread to new gain - K_vec_.clear(); - time_vec_.clear(); - double x_dot_old = 0; - Eigen::Vector x; - Eigen::Vector u; - - // iterate Riccati equation - for (const auto & point : trajectory->points) + if (trajectory->points.size() == 1) { - x = get_state_from_point(point); - // get x_ddot for trajectory - u[0] = (x(3) - x_dot_old) / dt; - x_dot_old = x(3); - - get_linear_system_matrices(x, u, dt, Phi, Gamma); - K = -(R + Gamma.transpose() * P * Gamma).inverse() * (N + Gamma.transpose() * P * Phi); - P_new = (Q + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; - P = 0.5 * (P_new + P_new.transpose()); - K_vec_.push_back(K); - time_vec_.push_back(point.time_from_start); + // use steady-state gain + K_vec_.push_back(Ks); + time_vec_.push_back(rclcpp::Duration::from_seconds(0.0)); } - // could be more efficient using reverse iterator - std::reverse(K_vec_.begin(), K_vec_.end()); - - for (const auto & mat : K_vec_) + else { - std::cout << "K: " << std::endl << mat << std::endl; + Eigen::Matrix K; + Eigen::Matrix Phi; + Eigen::Matrix Gamma; + Eigen::Matrix C; + C.setIdentity(); + Eigen::Matrix P(Ps), P_new; + // TODO(christophfroehlich) use a buffer to switch from RT thread to new gain + K_vec_.clear(); + time_vec_.clear(); + Eigen::Vector x_p1; + Eigen::Vector x; + Eigen::Vector u{0}; + auto get_time = [](const trajectory_msgs::msg::JointTrajectoryPoint & point) + { return point.time_from_start.sec + point.time_from_start.nanosec * 1e-9; }; + double dt_traj; + + // iterate Riccati equation, backwards in time + for (size_t i = trajectory->points.size(); i > 0; i--) + { + const trajectory_msgs::msg::JointTrajectoryPoint point = trajectory->points.at(i - 1); + x = get_state_from_point(point); + // system input acceleration, derivative of velocity + // we don't have acceleration in trajectory, so we have to calculate it + if (i == trajectory->points.size()) + { + dt_traj = 0.1; + u[0] = 0.0; + x_p1 = x; + } + else + { + dt_traj = get_time(trajectory->points.at(i)) - get_time(point); + x_p1 = get_state_from_point(trajectory->points.at(i)); + u[0] = (x_p1(3) - x(3)) / dt_traj; + } + get_linear_system_matrices(x, u, dt_traj, Phi, Gamma); + + K = -(R_ + Gamma.transpose() * P * Gamma).inverse() * (N + Gamma.transpose() * P * Phi); + P_new = (Q_ + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; + P = 0.5 * (P_new + P_new.transpose()); + + K_vec_.push_back(K); + time_vec_.push_back(point.time_from_start); + } + // could be more efficient using reverse iterator in computeCommand + std::reverse(K_vec_.begin(), K_vec_.end()); } + // for (const auto & mat : K_vec_) { + // std::cout << "K: " << std::endl << mat << std::endl; + // } + return true; } void CartpoleLqrTrajectoryPlugin::calcLQR_steady( - Eigen::Vector x, Eigen::Vector u, + Eigen::Vector x, Eigen::Vector u, double dt, Eigen::Matrix Q, Eigen::Matrix R, Eigen::Matrix N, Eigen::Matrix & Ks, Eigen::Matrix & Ps) @@ -179,8 +199,8 @@ void CartpoleLqrTrajectoryPlugin::calcLQR_steady( { K = -(R + Gamma.transpose() * P * Gamma).inverse() * (N + Gamma.transpose() * P * Phi); P_new = (Q + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; - if ((P_new - P).norm() < 1e-3) // abort criterium - { + if ((P_new - P).norm() < 1e-3) + { // abort criterium break; } P = 0.5 * (P_new + P_new.transpose()); @@ -200,7 +220,16 @@ bool CartpoleLqrTrajectoryPlugin::updateGainsRT() return true; } -void CartpoleLqrTrajectoryPlugin::updateGains() {} +void CartpoleLqrTrajectoryPlugin::updateGains() +{ + Eigen::Vector Q_diag{params_.gains.Q.data()}; + Q_ = Eigen::Matrix{Q_diag.asDiagonal()}; + R_ = Eigen::Matrix{params_.gains.R}; + dt_ = params_.dt; + std::cout << "Q: " << std::endl << Q_ << std::endl; + std::cout << "R: " << std::endl << R_ << std::endl; + std::cout << "dt: " << std::endl << dt_ << std::endl; +} void CartpoleLqrTrajectoryPlugin::computeCommands( std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint /*current*/, @@ -223,16 +252,7 @@ void CartpoleLqrTrajectoryPlugin::computeCommands( idx = std::distance(time_vec_.begin(), it); } // integrate acceleration from state feedback to velocity. consider sign of e! - // u += period.seconds() * (K_vec_.at(idx).dot(-e)); - // RCLCPP_INFO_STREAM_THROTTLE( - // node_->get_logger(), *node_->get_clock(), 500, "K:" << K_vec_.at(idx)); - // RCLCPP_INFO_STREAM_THROTTLE( - // node_->get_logger(), *node_->get_clock(), 500, "e:" << -e); - // RCLCPP_INFO_STREAM_THROTTLE( - // node_->get_logger(), *node_->get_clock(), 500, "a:" << dt*K_vec_.at(idx).dot(-e)); - u += dt * (K_vec_.at(idx).dot(-e)); - // RCLCPP_INFO_THROTTLE( - // node_->get_logger(), *node_->get_clock(), 500, "u: %f", u); + u += period.seconds() * (K_vec_.at(idx).dot(-e)); // set system input as desired velocity + integrated LQR control output tmp_command.at(map_cmd_to_joints_.at(0)) = desired.velocities.at(map_cmd_to_joints_.at(0)) + u; } diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml b/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml index 09b204c11..dbd2710c6 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml @@ -8,6 +8,11 @@ ros2_control_demo_example_xx: size_gt<>: [0], } } + dt: { + type: double, + default_value: 0.01, + description: "control loop update interval" + } system: lS: { type: double, diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp index 7fb653e1b..fc5043711 100644 --- a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp +++ b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp @@ -72,7 +72,7 @@ class CartpoleLqrTrajectoryPlugin Eigen::Matrix & Phi, Eigen::Matrix & Gamma); void calcLQR_steady( - Eigen::Vector q, Eigen::Vector u, + Eigen::Vector q, Eigen::Vector u, double dt, Eigen::Matrix Q, Eigen::Matrix R, Eigen::Matrix N, Eigen::Matrix & Ks, Eigen::Matrix & Ps); @@ -90,7 +90,9 @@ class CartpoleLqrTrajectoryPlugin Params params_; // TODO(christophfroehlich): do we need the controller update rate here, or the trajectory // sampling? - double dt = 0.01; + double dt_ = 0.01; + Eigen::Matrix Q_; + Eigen::Matrix R_; }; } // namespace ros2_control_demo_example_xx diff --git a/example_XX/package.xml b/example_XX/package.xml index a48b5b5a8..ae4ce6aaf 100644 --- a/example_XX/package.xml +++ b/example_XX/package.xml @@ -16,6 +16,7 @@ ament_cmake ament_cmake_python + backward_ros pluginlib rclcpp rclpy From 6df3c22c14bd59ff062a25c0128d9c97fda45ed2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 19 Nov 2023 21:57:33 +0000 Subject: [PATCH 13/29] Implement computeControlLawRT_impl --- .../cartpole_lqr_trajectory_plugin.cpp | 38 ++++++++++++++++--- .../cartpole_lqr_trajectory_plugin.hpp | 12 +++--- 2 files changed, 37 insertions(+), 13 deletions(-) diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index 98b6a2304..9a52ae7f2 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -85,7 +85,7 @@ bool CartpoleLqrTrajectoryPlugin::configure() bool CartpoleLqrTrajectoryPlugin::activate() { params_ = param_listener_->get_params(); - updateGains(); + parseGains(); return true; } @@ -112,6 +112,10 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( // std::cout << "Ks: " << Ks << std::endl; // std::cout << "Ps: " << Ps << std::endl; + // TODO(christophfroehlich) use a buffer to switch from RT thread to new gain + K_vec_.clear(); + time_vec_.clear(); + if (trajectory->points.size() == 1) { // use steady-state gain @@ -126,9 +130,6 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( Eigen::Matrix C; C.setIdentity(); Eigen::Matrix P(Ps), P_new; - // TODO(christophfroehlich) use a buffer to switch from RT thread to new gain - K_vec_.clear(); - time_vec_.clear(); Eigen::Vector x_p1; Eigen::Vector x; Eigen::Vector u{0}; @@ -175,6 +176,31 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( return true; } +bool CartpoleLqrTrajectoryPlugin::computeControlLawRT_impl( + const std::shared_ptr & trajectory) +{ + // parameters + auto N = Eigen::Matrix::Zero(); + + Eigen::Matrix Ks; + Eigen::Matrix Ps; + auto x_R = get_state_from_point(trajectory->points.back()); + Eigen::Vector u_R{0}; + calcLQR_steady(x_R, u_R, 0.1, Q_, R_, N, Ks, Ps); + // std::cout << "Ks: " << Ks << std::endl; + // std::cout << "Ps: " << Ps << std::endl; + + // TODO(christophfroehlich) use a buffer to switch from RT thread to new gain + K_vec_.clear(); + time_vec_.clear(); + + // use steady-state gain + K_vec_.push_back(Ks); + time_vec_.push_back(rclcpp::Duration::from_seconds(0.0)); + + return true; +} + void CartpoleLqrTrajectoryPlugin::calcLQR_steady( Eigen::Vector x, Eigen::Vector u, double dt, Eigen::Matrix Q, Eigen::Matrix R, @@ -214,13 +240,13 @@ bool CartpoleLqrTrajectoryPlugin::updateGainsRT() if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); - updateGains(); + parseGains(); } return true; } -void CartpoleLqrTrajectoryPlugin::updateGains() +void CartpoleLqrTrajectoryPlugin::parseGains() { Eigen::Vector Q_diag{params_.gains.Q.data()}; Q_ = Eigen::Matrix{Q_diag.asDiagonal()}; diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp index fc5043711..95020545a 100644 --- a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp +++ b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp @@ -38,15 +38,13 @@ class CartpoleLqrTrajectoryPlugin bool activate() override; + // computes the control law for a given trajectory, for non-RT thread bool computeControlLawNonRT_impl( - const std::shared_ptr & /*trajectory*/) override; + const std::shared_ptr & trajectory) override; + // fast version of computeControlLawNonRT_impl for a single point only, for RT thread bool computeControlLawRT_impl( - const std::shared_ptr & /*trajectory*/) override - { - // nothing to do - return true; - } + const std::shared_ptr & trajectory) override; bool updateGainsRT() override; @@ -64,7 +62,7 @@ class CartpoleLqrTrajectoryPlugin /** * @brief parse gains from parameter struct */ - void updateGains(); + void parseGains(); #define NUM_STATES 4 void get_linear_system_matrices( From ca44ed795f8e52fd52f4e756ae3e858bab5cddf2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 19 Nov 2023 21:58:06 +0000 Subject: [PATCH 14/29] Tune controllers --- example_XX/bringup/config/cart_pole_controllers.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/example_XX/bringup/config/cart_pole_controllers.yaml b/example_XX/bringup/config/cart_pole_controllers.yaml index 7d351555e..a8be02d2a 100644 --- a/example_XX/bringup/config/cart_pole_controllers.yaml +++ b/example_XX/bringup/config/cart_pole_controllers.yaml @@ -28,7 +28,7 @@ trajectory_controllers: gains: R: 0.1 - Q: [1.0, 1.0, 1.0, 1.0] + Q: [5.0, 5.0, 0.1, 1.0] cart_to_pendulum: angle_wraparound: true From 631f355a686f94d31693dabf3636a1b8b11958f6 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 20 Nov 2023 08:56:15 +0000 Subject: [PATCH 15/29] Add minimal doc --- example_XX/doc/userdoc.rst | 40 ++++++++++++++++++-------------------- 1 file changed, 19 insertions(+), 21 deletions(-) diff --git a/example_XX/doc/userdoc.rst b/example_XX/doc/userdoc.rst index 056decf37..880c8cdf8 100644 --- a/example_XX/doc/userdoc.rst +++ b/example_XX/doc/userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_9/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_XX/doc/userdoc.rst .. _ros2_control_demos_example_xx_userdoc: @@ -17,7 +17,7 @@ With *example_xx*, we demonstrate the a robot with passive joints. .. code:: - docker run -it --rm --name ros2_control_demos --net host ros2_control_demos ros2 launch ros2_control_demo_example_9 rrbot_gazebo_classic.launch.py gui:=false + docker run -it --rm --name ros2_control_demos --net host ros2_control_demos ros2 launch ros2_control_demo_example_xx cart_pole.launch.py gui:=false first. Then on your local machine you can run the Gazebo Classic client with @@ -29,7 +29,7 @@ With *example_xx*, we demonstrate the a robot with passive joints. .. code-block:: shell - rviz2 -d src/ros2_control_demos/example_9/description/rviz/rrbot.rviz + rviz2 -d src/ros2_control_demos/ros2_control_demo_description/cart_pole/rviz/cart_pole.rviz For details on the ``gazebo_ros2_control`` plugin, see :ref:`gazebo_ros2_control`. @@ -37,13 +37,12 @@ With *example_xx*, we demonstrate the a robot with passive joints. Tutorial steps -------------------------- -The following example shows a cart with a pendulum arm. This uses the effort command interface for the cart's -degree of freedom on the rail, and the physics of the passive joint of the pendulum is solved correctly. +The following example shows a cart with a pendulum arm. .. code-block:: shell - ros2 launch gazebo_ros2_control_demos pendulum_example_effort.launch.py - ros2 run gazebo_ros2_control_demos example_effort + ros2 launch ros2_control_demo_example_xx cart_pole.launch.py + ros2 run ros2_control_demo_example_xx trajectory_action_client.py Files used for this demos @@ -51,24 +50,23 @@ Files used for this demos - Launch files: - + Hardware: `rrbot.launch.py `__ - + Gazebo Classic: `rrbot_gazebo_classic.launch.py `__ + + View robot: `view_robot.launch.py `__ + + Gazebo Classic: `cart_pole.launch.py `__ -- Controllers yaml: `rrbot_controllers.yaml `__ -- URDF file: `rrbot.urdf.xacro `__ +- Controllers yaml: `cart_pole_controllers.yaml `__ +- URDF file: - + Description: `rrbot_description.urdf.xacro `__ - + ``ros2_control`` tag: `rrbot.ros2_control.xacro `__ + + Main File: `cart_pole.urdf.xacro `__ + + Description: `cart_pole_description.urdf.xacro `__ + + ``ros2_control`` tag: `cart_pole.ros2_control.xacro `__ + + ``gazebo`` tag: `cart_pole.gazebo.xacro `__ -- RViz configuration: `rrbot.rviz `__ -- Test nodes goals configuration: - - + `rrbot_forward_position_publisher `__ - -- Hardware interface plugin: `rrbot.cpp `__ +- RViz configuration: `cart_pole.rviz `__ +- Action client: `trajectory_action_client.py `__ Controllers from this demo -------------------------- -- ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ -- ``Forward Command Controller`` (`ros2_controllers repository `__): `doc `__ +* ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ + +* ``Joint Trajectory Controller`` (`ros2_controllers repository `__): `doc `__ From 0ba88f9d1cc42d6784f598620e9fb5935da5e87d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 20 Nov 2023 10:43:08 +0000 Subject: [PATCH 16/29] Use class for LQR gain storage --- .../cartpole_lqr_trajectory_plugin.cpp | 79 ++++++++++++------- .../cartpole_lqr_trajectory_plugin.hpp | 27 ++++++- 2 files changed, 73 insertions(+), 33 deletions(-) diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index 9a52ae7f2..0d1d2850b 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -19,6 +19,23 @@ namespace ros2_control_demo_example_xx { +Eigen::Matrix TrajectoryLQR::get_feedback_gain( + const rclcpp::Duration & duration_since_start) +{ + // TODO(christophfroehlich) interpolate K from time + auto it = std::upper_bound(time_vec_.begin(), time_vec_.end(), duration_since_start); + int idx; + if (it == time_vec_.end()) + { + idx = time_vec_.size() - 1; + } + else + { + idx = std::distance(time_vec_.begin(), it); + } + return K_vec_.at(idx); +} + bool CartpoleLqrTrajectoryPlugin::initialize( rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector map_cmd_to_joints) { @@ -47,6 +64,8 @@ bool CartpoleLqrTrajectoryPlugin::initialize( return false; } + trajectory_lqr_ptr_ = std::make_shared(); + return true; } @@ -101,6 +120,8 @@ Eigen::Vector get_state_from_point( bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( const std::shared_ptr & trajectory) { + auto start_time = this->node_->now(); + // parameters auto N = Eigen::Matrix::Zero(); @@ -113,14 +134,13 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( // std::cout << "Ps: " << Ps << std::endl; // TODO(christophfroehlich) use a buffer to switch from RT thread to new gain - K_vec_.clear(); - time_vec_.clear(); + trajectory_lqr_ptr_->reset(); if (trajectory->points.size() == 1) { // use steady-state gain - K_vec_.push_back(Ks); - time_vec_.push_back(rclcpp::Duration::from_seconds(0.0)); + trajectory_lqr_ptr_->K_vec_.push_back(Ks); + trajectory_lqr_ptr_->time_vec_.push_back(rclcpp::Duration::from_seconds(0.0)); } else { @@ -146,7 +166,7 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( // we don't have acceleration in trajectory, so we have to calculate it if (i == trajectory->points.size()) { - dt_traj = 0.1; + dt_traj = dt_; u[0] = 0.0; x_p1 = x; } @@ -162,13 +182,25 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( P_new = (Q_ + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; P = 0.5 * (P_new + P_new.transpose()); - K_vec_.push_back(K); - time_vec_.push_back(point.time_from_start); + trajectory_lqr_ptr_->K_vec_.push_back(K); + trajectory_lqr_ptr_->time_vec_.push_back(point.time_from_start); + + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production + // code wait to check if RT buffer works well + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + // END: This part here is for exemplary purposes - Please do not copy to your production code } - // could be more efficient using reverse iterator in computeCommand - std::reverse(K_vec_.begin(), K_vec_.end()); + // would be more efficient using reverse iterator in get_feedback_gain + std::reverse(trajectory_lqr_ptr_->K_vec_.begin(), trajectory_lqr_ptr_->K_vec_.end()); + std::reverse(trajectory_lqr_ptr_->time_vec_.begin(), trajectory_lqr_ptr_->time_vec_.end()); } + auto end_time = this->node_->now(); + auto duration = end_time - start_time; + RCLCPP_INFO( + node_->get_logger(), + "[CartpoleLqrTrajectoryPlugin] computed control law for %lu points in %es.", + trajectory->points.size(), duration.seconds()); // for (const auto & mat : K_vec_) { // std::cout << "K: " << std::endl << mat << std::endl; // } @@ -191,12 +223,11 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawRT_impl( // std::cout << "Ps: " << Ps << std::endl; // TODO(christophfroehlich) use a buffer to switch from RT thread to new gain - K_vec_.clear(); - time_vec_.clear(); + trajectory_lqr_ptr_->reset(); // use steady-state gain - K_vec_.push_back(Ks); - time_vec_.push_back(rclcpp::Duration::from_seconds(0.0)); + trajectory_lqr_ptr_->K_vec_.push_back(Ks); + trajectory_lqr_ptr_->time_vec_.push_back(rclcpp::Duration::from_seconds(0.0)); return true; } @@ -263,31 +294,21 @@ void CartpoleLqrTrajectoryPlugin::computeCommands( const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) { - if (K_vec_.size() > 0) + if (!trajectory_lqr_ptr_->is_empty()) { auto e = get_state_from_point(error); - // TODO(christophfroehlich) interpolate K from time - auto it = std::upper_bound(time_vec_.begin(), time_vec_.end(), duration_since_start); - int idx; - if (it == time_vec_.end()) - { - idx = time_vec_.size() - 1; - } - else - { - idx = std::distance(time_vec_.begin(), it); - } + auto K = trajectory_lqr_ptr_->get_feedback_gain(duration_since_start); // integrate acceleration from state feedback to velocity. consider sign of e! - u += period.seconds() * (K_vec_.at(idx).dot(-e)); + u_ += period.seconds() * (K.dot(-e)); // set system input as desired velocity + integrated LQR control output - tmp_command.at(map_cmd_to_joints_.at(0)) = desired.velocities.at(map_cmd_to_joints_.at(0)) + u; + tmp_command.at(map_cmd_to_joints_.at(0)) = desired.velocities.at(map_cmd_to_joints_.at(0)) + u_; } } void CartpoleLqrTrajectoryPlugin::reset() { - K_vec_.clear(); - u = 0; + trajectory_lqr_ptr_->reset(); + u_ = 0; } void CartpoleLqrTrajectoryPlugin::get_linear_system_matrices( diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp index 95020545a..27337daec 100644 --- a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp +++ b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp @@ -23,9 +23,29 @@ #include "cartpole_lqr_trajectory_plugin_parameters.hpp" // auto-generated by generate_parameter_library #include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp" +// shortcut +#define NUM_STATES 4 namespace ros2_control_demo_example_xx { +class TrajectoryLQR +{ +public: + std::vector> K_vec_; + std::vector time_vec_; + + Eigen::Matrix get_feedback_gain( + const rclcpp::Duration & duration_since_start); + + void reset() + { + K_vec_.clear(); + time_vec_.clear(); + }; + + bool is_empty() { return K_vec_.empty(); } +}; + class CartpoleLqrTrajectoryPlugin : public joint_trajectory_controller_plugins::TrajectoryControllerBase { @@ -64,7 +84,6 @@ class CartpoleLqrTrajectoryPlugin */ void parseGains(); -#define NUM_STATES 4 void get_linear_system_matrices( Eigen::Vector x, Eigen::Vector u, const double dt, Eigen::Matrix & Phi, @@ -74,9 +93,7 @@ class CartpoleLqrTrajectoryPlugin Eigen::Matrix Q, Eigen::Matrix R, Eigen::Matrix N, Eigen::Matrix & Ks, Eigen::Matrix & Ps); - std::vector> K_vec_; - std::vector time_vec_; - double u; + double u_; // number of command joints size_t num_cmd_joints_; @@ -91,6 +108,8 @@ class CartpoleLqrTrajectoryPlugin double dt_ = 0.01; Eigen::Matrix Q_; Eigen::Matrix R_; + + std::shared_ptr trajectory_lqr_ptr_; }; } // namespace ros2_control_demo_example_xx From 319ce1481aaed51f6f9570943dec5f968d01d708 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 20 Nov 2023 10:43:26 +0000 Subject: [PATCH 17/29] Add more trajectory examples --- example_XX/sidestep.csv | 82 ------------------------------- example_XX/sidestep_low_left.csv | 22 +++++++++ example_XX/sidestep_low_right.csv | 22 +++++++++ example_XX/sidestep_up_left.csv | 22 +++++++++ example_XX/sidestep_up_right.csv | 22 +++++++++ example_XX/swingdown.csv | 82 +++++++++++++++++++++++++++++++ 6 files changed, 170 insertions(+), 82 deletions(-) delete mode 100644 example_XX/sidestep.csv create mode 100644 example_XX/sidestep_low_left.csv create mode 100644 example_XX/sidestep_low_right.csv create mode 100644 example_XX/sidestep_up_left.csv create mode 100644 example_XX/sidestep_up_right.csv create mode 100644 example_XX/swingdown.csv diff --git a/example_XX/sidestep.csv b/example_XX/sidestep.csv deleted file mode 100644 index cbe42d2f7..000000000 --- a/example_XX/sidestep.csv +++ /dev/null @@ -1,82 +0,0 @@ -t,Position,Theta,Velocity,Theta_dot,u -0.0,0.0,0.0,0.0,0.0,11.668695509317981 -0.025,0.0005249982868349666,-0.0007856124258703744,0.04199986319398341,-0.06284899342082974,22.874429527610104 -0.05,0.002255178687858652,-0.0033674601335596803,0.0964145688879113,-0.14369882319431368,21.930011266123948 -0.07500000000000001,0.005315119314648953,-0.0079082576545442,0.14838068125531348,-0.219564978484449,20.951532311416635 -0.1,0.009642189740324375,-0.0142725666341639,0.19778495279872116,-0.2895797398851283,19.94372911865718 -0.125,0.015171112504901071,-0.02230418497901912,0.24452886836741514,-0.3529497277032949,18.911946298370474 -0.15000000000000002,0.02183435070454961,-0.03182812929072389,0.2885301876044683,-0.4089658172330901,17.861799246710994 -0.17500000000000002,0.029562528295831295,-0.0426528515567987,0.32972401969806686,-0.4570119640528962,16.799055085823905 -0.2,0.03828488002765833,-0.05457267310749891,0.3680641188480954,-0.4965737600031202,15.729897916881407 -0.225,0.047929732366117514,-0.06737041990249446,0.40352406822864073,-0.5272459835965259,14.660583646795388 -0.25,0.05842500561557665,-0.08082022051411232,0.4360977917280854,-0.5487380653328955,13.597294953873602 -0.275,0.06969872762758651,-0.09469042279313516,0.4657999692327073,-0.5608781169889384,12.5463174499735 -0.30000000000000004,0.08167955355911681,-0.10874658655638617,0.4926661052897107,-0.5636149840711318,11.513728005039958 -0.325,0.09429727653478268,-0.1227544919734844,0.5167517327635623,-0.5570174492967327,10.50516883206907 -0.35000000000000003,0.10748331539011449,-0.13648310388494866,0.5381313756629855,-0.541271503620413,9.52605565612203 -0.375,0.12117116956622538,-0.14970743874853132,0.5568969584258865,-0.5166752854661943,8.581082259470591 -0.4,0.13529683094957928,-0.16221128306162697,0.5731559522424253,-0.4836322595814595,7.6746386182231605 -0.42500000000000004,0.14979914859969146,-0.1737897254910946,0.5870294597665479,-0.4426431347759503,6.81043862347416 -0.45,0.1646201388362714,-0.18425146526551597,0.5986497591598482,-0.3942960471777597,5.991493032486073 -0.47500000000000003,0.17970523717136225,-0.19342087147838435,0.6081581076474197,-0.33925644985171005,5.220343358359275 -0.5,0.19500349723493648,-0.20113978702754004,0.6157026974385196,-0.2782567940807448,4.498898116148843 -0.525,0.21046773975318198,-0.20726907357653673,0.6214367040211193,-0.2120861298389916,3.828577648359694 -0.55,0.22605465955289059,-0.21168990603476945,0.6255168799555653,-0.14158046681962702,3.2104970301060676 -0.5750000000000001,0.24172489976831896,-0.21430483053869864,0.6281023372786997,-0.06761349349470999,2.6453045177047416 -0.6000000000000001,0.25744309799165593,-0.21503859580655277,0.6293535205882569,0.008912272066381545,2.1333690127512974 -0.625,0.2731779082988229,-0.21383876770756985,0.6294313039851105,0.08707397585225056,1.674580493827355 -0.65,0.28890200699913215,-0.21067614259971942,0.6284965920396305,0.16593603277578367,1.2688807525689019 -0.675,0.30459208832703083,-0.2055449715713566,0.6267099141922619,0.24455764949324482,0.9156814752802812 -0.7000000000000001,0.3202288510129543,-0.19846299826205152,0.6242311006816131,0.32200021525116046,0.6145120118048036 -0.7250000000000001,0.33579696950379717,-0.18947130030144413,0.621218378585818,0.3973356215974337,0.36393822900272865 -0.75,0.3512850446028227,-0.17863392380021123,0.6178276293362255,0.4696544985011989,0.1628650279201731 -0.775,0.3666855353996621,-0.16603730903933642,0.6142116344109173,0.5380746823687896,0.009349399260925631 -0.8,0.3819946600310658,-0.1517894834116517,0.6105183361013808,0.6017513678459884,-0.09887379448297638 -0.8250000000000001,0.3972122524747707,-0.13601899722346925,0.6068890593950121,0.6598875272086043,-0.16478707570376366 -0.8500000000000001,0.41234157084512935,-0.1188735915676659,0.6034564102336734,0.7117449252556582,-0.19180530485021954 -0.875,0.4273890518586057,-0.10051858931005407,0.6003420708444415,0.7566552553532805,-0.18401292266717156 -0.9,0.44236400447468166,-0.08113500219772471,0.5976541384416397,0.7940317136330605,-0.14621151140730718 -0.925,0.4572782394994751,-0.06091735855070153,0.5954846635418278,0.8233797781287878,-0.08367292015737192 -0.9500000000000001,0.4721456403330127,-0.04007127530502692,0.5939074031411812,0.8443068815251776,-0.002240099454505675 -0.9750000000000001,0.48698167934474695,-0.01881080438328371,0.592975717797555,0.8565307922142901,0.09179270728981989 -1.0,0.501802888605128,0.0026444033147508083,0.5927210230329271,0.8598858236284694,0.19185280757215076 -1.0250000000000001,0.5166263014813472,0.024072055376475936,0.5931520070646146,0.854326341309538,0.29133187984739645 -1.05,0.5314688797538006,0.04525023433759509,0.594254254731659,0.8399279755799913,0.38343062658971844 -1.075,0.5463469404211817,0.06596040928489993,0.5959905986588216,0.8168860202043917,0.46165655710985837 -1.1,0.5612756036989813,0.08599036804962876,0.5983024635651413,0.7855106809739117,0.5199030641704605 -1.125,0.5762682771152859,0.10513700805547752,0.6011114097392273,0.7462205194939925,0.5523326501108841 -1.1500000000000001,0.591336181991782,0.12320894155561601,0.6043209803804601,0.6995341605170873,0.5535973265139249 -1.175,0.6064879361537685,0.14002886596335648,0.6078193525784567,0.6460597921021509,0.5191993689948812 -1.2000000000000002,0.6217292027432357,0.15543566305388096,0.6114819745789212,0.5864839751398052,0.4451086474048233 -1.225,0.6370624007005793,0.16928621939145655,0.6151738620085673,0.521560531866245,0.3279177536361516 -1.25,0.652486471010042,0.1814569690751375,0.6187517627484449,0.4520994428282295,0.16468405840680875 -1.2750000000000001,0.6679966966193562,0.19184515971244467,0.6220662859966991,0.37895580815634583,-0.046799079109961075 -1.3,0.683584572788048,0.2003698484028877,0.62496380749864,0.3030192870790997,-0.30829583375105857 -1.3250000000000002,0.6992377174310365,0.20697264773771995,0.6272877639404412,0.22520465970748524,-0.6211747843771158 -1.35,0.7149398065977296,0.21161824937617132,0.6288793693950105,0.14644347136862232,-0.9866916744035031 -1.375,0.7306705303827283,0.21429473754604703,0.6295785334048886,0.06767558222143097,-1.4054224679482592 -1.4000000000000001,0.7464055697784847,0.21501369595696238,0.6292246182556104,-0.010158909348200646,-1.8778839096742028 -1.425,0.7621165862519903,0.21381012259861346,0.6276566996248617,-0.08612695931971259,-2.4042885201155584 -1.4500000000000002,0.7777712192434109,0.21074215974790772,0.6247139396887706,-0.15931006873674444,-2.984709126501733 -1.475,0.7933330965206831,0.20589063017828066,0.6202362424930075,-0.22881229683341078,-3.618736963048404 -1.5,0.8087618630550444,0.199358367925156,0.6140650802559019,-0.29376868341656526,-4.305774593430852 -1.5250000000000001,0.8240132309705702,0.19126933572920943,0.6060443529861627,-0.3533538922591561,-5.04483401862314 -1.55,0.8390390584731728,0.18176751364473517,0.596021847222048,-0.40679187449878634,-5.834376476504156 -1.5750000000000002,0.8537874699709103,0.17101553840586398,0.5838510725969508,-0.45336614461091024,-6.672292023165344 -1.6,0.8682030189129212,0.15919309164768797,0.5693928427639164,-0.4924295960431725,-7.556245048881028 -1.625,0.8822268981174934,0.14649503432378086,0.552517493601866,-0.5234149898693883,-8.482887429268468 -1.6500000000000001,0.8957972075505788,0.13312928212802366,0.5331072610449538,-0.5458451857911936,-9.448555095433012 -1.675,0.9088492774451692,0.1193144404714833,0.5110583305222833,-0.5593421467320456,-10.448833271121988 -1.7000000000000002,0.9213160440699738,0.10527722467852432,0.48628299946210013,-0.563635116704683,-11.478836624116797 -1.725,0.9331284735402927,0.09124969996383876,0.4587113581634168,-0.558566860470148,-12.533215363462125 -1.75,0.9442160255022357,0.07746638622641001,0.4282927987920271,-0.5440982385241578,-13.60614101035722 -1.7750000000000001,0.954507149306268,0.0641612754083064,0.39499710553055334,-0.5203106269241236,-14.691490899536946 -1.8,0.9639297989764113,0.05156482069784136,0.358814868080901,-0.4874057499130838,-15.783038904596799 -1.8250000000000002,0.9724119517108009,0.03990095933234734,0.3197573506702638,-0.445703159326435,-16.87454294982393 -1.85,0.9798821180545898,0.029384223749353903,0.27785595683284986,-0.39563568731303833,-17.959849366139903 -1.875,0.9862698384417842,0.020216982304025584,0.23316167414270306,-0.3377436283132264,-19.032706664602173 -1.9000000000000001,0.9915061588801355,0.012586848637039374,0.18574396092540627,-0.27266706504566773,-20.08736174424844 -1.925,0.9955240740875725,0.006664299697547782,0.1356892556695488,-0.20113685011366053,-21.11827585794378 -1.9500000000000002,0.9982589376065562,0.002600519666894806,0.08309982584914587,-0.12396555233857313,-22.12017662288569 -1.975,0.9996488427163353,0.0005254751354907722,0.02809258293318419,-0.042038010173750404,-23.0881141893242 -2.0,1.0,0.0,0.0,0.0, diff --git a/example_XX/sidestep_low_left.csv b/example_XX/sidestep_low_left.csv new file mode 100644 index 000000000..8d8331a62 --- /dev/null +++ b/example_XX/sidestep_low_left.csv @@ -0,0 +1,22 @@ +t,Position,Theta,Velocity,Theta_dot,u +0.0,1.0,0.0,0.0,0.0,-13.582842304726647 +0.1,0.9908400049720579,0.01325045947812364,-0.1831999005715079,0.26500918950732566,-24.22806648751422 +0.2,0.9623566453157602,0.052565166873090374,-0.38646729255444856,0.52128495839201,-18.259059275845477 +0.30000000000000004,0.9166427183024465,0.10931412918041443,-0.5278112477118159,0.6136942877544568,-12.409036061429301 +0.4,0.8597160223395942,0.16673016926182568,-0.6107226715452487,0.5346265138737955,-7.2450579701905 +0.5,0.7969366091968367,0.20897819835507048,-0.6448655913099005,0.3103340679911038,-3.168999707927978 +0.6000000000000001,0.7325144382934369,0.22411292945016167,-0.6435778267581107,-0.007639446089287271,-0.35800513557509617 +0.7000000000000001,0.6692669802334312,0.2060214565712224,-0.6213713344420202,-0.3541900114895065,1.1702208961271794 +0.8,0.6085709733682355,0.15526386244161725,-0.5925488028619008,-0.6609618711025866,1.496266501741761 +0.9,0.550440893312543,0.0789076728029028,-0.5700527982519421,-0.8661619216717187,0.8424467884987269 +1.0,0.49377839635442694,-0.010725728068270426,-0.5631971409103624,-0.9265060957516191,-0.3493165885348479 +1.1,0.4368766314951924,-0.09849539661859454,-0.5748381562743512,-0.8288872752548936,-1.4650534397548274 +1.2000000000000002,0.37812064688200187,-0.1696677891950051,-0.6002815359894682,-0.5945605762733314,-1.916160952165578 +1.3,0.3166562921794397,-0.21302135655203985,-0.6290055580617843,-0.27251077086738457,-1.3121255256793616 +1.4000000000000001,0.252828781822614,-0.22300268299049306,-0.6475446490747488,0.07288424209831199,0.532195156617898 +1.5,0.1883792282949881,-0.20069615558897821,-0.6414464214777829,0.3732463059319788,3.6797330199316685 +1.6,0.12649024436409825,-0.15374203695777106,-0.5963332571400103,0.5658360666921706,8.087921167936358 +1.7000000000000002,0.07169626725907002,-0.09523451300126391,-0.49954628496054043,0.6043144124380037,13.532466373327605 +1.8,0.02957881217220647,-0.04150615853439743,-0.3428028167767392,0.4702526768993162,19.563572248350802 +1.9000000000000001,0.006219335666416475,-0.008996762346338082,-0.12438671333905998,0.1799352468618647,25.597486939648455 +2.0,0.0,0.0,0.0,0.0, diff --git a/example_XX/sidestep_low_right.csv b/example_XX/sidestep_low_right.csv new file mode 100644 index 000000000..4eeea1b14 --- /dev/null +++ b/example_XX/sidestep_low_right.csv @@ -0,0 +1,22 @@ +t,Position,Theta,Velocity,Theta_dot,u +0.0,0.0,0.0,0.0,0.0,13.582834681774651 +0.1,0.009159998805574011,-0.013250464940438968,0.1831999761137826,-0.2650092987687595,24.22808970251364 +0.2,0.037643367135360706,-0.052565184108354666,0.3864673904819508,-0.5212850845895562,18.259040901977798 +0.30000000000000004,0.0833573037937571,-0.10931415711133492,0.5278113426859683,-0.6136943754700693,12.409054285177184 +0.4,0.14028401155960885,-0.1667302070099039,0.610722812631055,-0.5346266225012926,7.245049840839648 +0.5,0.20306343560767343,-0.20897823937905882,0.6448656683302373,-0.31033402488180983,3.168995317107675 +0.6000000000000001,0.26748560733101473,-0.2241129537452494,0.6435777661366053,0.0076397375579876315,0.35798161654959254 +0.7000000000000001,0.33073305334271935,-0.20602144259604266,0.6213711540975038,0.3541904854261542,-1.1702220386857307 +0.8,0.3914290444317056,-0.15526380693065628,0.5925486676822251,0.6609622278815681,-1.4962566182800145 +0.9,0.44955911646814123,-0.0789075946714122,0.5700527730464776,0.866162017303333,-0.8424351388021747 +1.0,0.5062216168861734,0.010725801254260484,0.56319723531419,0.9265059012101524,0.34932827353212964 +1.1,0.5631233967221619,0.0984954378668856,0.5748383614055668,0.8288868310423773,1.4650636813635405 +1.2000000000000002,0.6218793975421866,0.16966779078740932,0.600281654994933,0.5945602273681059,1.9161327770081475 +1.3,0.6833437534882811,0.2130213395128202,0.6290054639269657,0.2725107471400949,1.3121099393996063 +1.4000000000000001,0.7471712506067291,0.22300267025047876,0.6475444784420132,-0.0728841323869364,-0.5321952059388131 +1.5,0.8116207944340279,0.20069614387838913,0.6414463981039753,-0.37324639505485,-3.6797024941986103 +1.6,0.8735097810616919,0.15374201036630308,0.5963333344492998,-0.5658362751868663,-8.087930632335542 +1.7000000000000002,0.9283037569698974,0.09523448102646237,0.49954618371479753,-0.6043143116099807,-13.532493179839031 +1.8,0.9704211992933057,0.041506142345594264,0.34280266275336,-0.4702524620073983,-19.563555969370874 +1.9000000000000001,0.993780666216168,0.008996759623795886,0.12438667570388585,-0.1799351924285638,-25.597479194421577 +2.0,1.0,0.0,0.0,0.0, diff --git a/example_XX/sidestep_up_left.csv b/example_XX/sidestep_up_left.csv new file mode 100644 index 000000000..99a328a2e --- /dev/null +++ b/example_XX/sidestep_up_left.csv @@ -0,0 +1,22 @@ +t,Position,Theta,Velocity,Theta_dot,u +0.0,1.0,3.141592653589793,0.0,0.0,25.727222325497788 +0.1,1.0131967205717793,3.1621392896044482,0.26393441147946906,0.41093272034183115,28.427563725305465 +0.2,1.0446805712312321,3.214300376886786,0.365742601709609,0.6322890253049573,-8.053814755431427 +0.30000000000000004,1.0727725120916252,3.272368210966063,0.19609621549825854,0.5290676562805826,-28.277225491338164 +0.4,1.0770437032803744,3.313641489926836,-0.11067239172328348,0.296397922934826,-37.2429050694341 +0.5,1.0483117721579391,3.3304205767736565,-0.4639662307254031,0.039183814001593376,-38.56887738441252 +0.6000000000000001,0.9848857328264592,3.322766846328476,-0.8045545559042157,-0.19225842290519513,-34.91622016919236 +0.7000000000000001,0.889904067170022,3.294287399420048,-1.095078757224509,-0.3773305152633704,-28.127853041475003 +0.8,0.7695175554342428,3.2499629988004664,-1.3126514774910374,-0.5091574971282213,-19.431738056537547 +0.9,0.6316923907716203,3.195201167299268,-1.4438518157612081,-0.5860791328957171,-9.653766676708562 +1.0,0.4854110980005052,3.1355008802567093,-1.4817740396609596,-0.6079266079554391,0.6061625205688302 +1.1,0.34009387893961884,3.076368133612908,-1.4245703415568365,-0.5747283249205931,10.843605473722643 +1.2000000000000002,0.20511158058826864,3.0233087198590716,-1.2750756254699807,-0.48645995015617455,20.54133620246714 +1.3,0.08928842899531193,2.9818131438692546,-1.041387406389295,-0.3434515696402237,29.060110809727608 +1.4000000000000001,0.0002895324189204924,2.9572385490827298,-0.7385905251385729,-0.14804032609025766,35.49723335244904 +1.5,-0.05624415925942309,2.9543834874427968,-0.3920833084282454,0.0909390932916079,38.501628227761735 +1.6,-0.07800178917435524,2.9763453328069662,-0.04306928987045941,0.34829781399180226,36.05018372967955 +1.7000000000000002,-0.06797701410719119,3.021995398100074,0.2435647912137226,0.5647034918703048,25.256542703251032 +1.8,-0.03733141300472916,3.0812263540516276,0.36934723083551474,0.6199156271607722,2.2162626076042717 +1.9000000000000001,-0.00943202573156447,3.126907394498823,0.18864051462780426,0.2937051817831894,-38.59189016444331 +2.0,0.0,3.141592653589793,0.0,0.0, diff --git a/example_XX/sidestep_up_right.csv b/example_XX/sidestep_up_right.csv new file mode 100644 index 000000000..5d1141234 --- /dev/null +++ b/example_XX/sidestep_up_right.csv @@ -0,0 +1,22 @@ +t,Position,Theta,Velocity,Theta_dot,u +0.0,0.0,3.141592653589793,0.0,0.0,-25.726955400977594 +0.1,-0.01319663724708581,3.121046147293422,-0.2639327449088351,-0.41093012600776374,-28.427488665402784 +0.2,-0.044680372107853535,3.0688852603510104,-0.3657419523065087,-0.632287612840499,8.053527713728787 +0.30000000000000004,-0.07277236862408736,3.010817411663038,-0.196097978018168,-0.529069360918943,28.277010882465976 +0.4,-0.0770438392399718,2.96954382136828,0.11066856570048476,-0.2964024449761723,37.242690145633944 +0.5,-0.04831225726168709,2.9527643154940555,0.46396307386521624,-0.03918767250831095,38.56923147155754 +0.6000000000000001,0.015113614853642935,2.9604178390552467,0.8045543684413661,0.19225814373213537,34.91648579551417 +0.7000000000000001,0.11009537913580322,2.9888973880774365,1.0950809172018487,0.3773328367116665,28.128080947670394 +0.8,0.23048215206842299,3.033222058802621,1.3126545414504873,0.5091605777920026,19.43170385221482 +0.9,0.3683075879105022,3.087984136246791,1.4438541753910483,0.5860809710914082,9.653659501849429 +1.0,0.5145890612883659,3.147684527713766,1.4817752921661829,0.6079268582481018,-0.6062826320262397 +1.1,0.6599064111804592,3.20681731664309,1.424571705675741,0.5747289203383725,-10.843463499966113 +1.2000000000000002,0.7948888929565243,3.2598768763037,1.2750779298456454,0.48646227287383714,-20.541287057644332 +1.3,0.9107122693287941,3.3013727077557307,1.0413895975997745,0.3434543561667906,-29.060188518636096 +1.4000000000000001,0.9997112471290509,3.325947424022019,0.738589958405365,0.14803996915898412,-35.497733451904544 +1.5,1.056244695189266,3.328802215276431,0.392079002798928,-0.0909441440707547,-38.50190903689841 +1.6,1.0780018809576026,3.3068398558692222,0.04306471256781089,-0.34830304407344753,-36.0499636811846 +1.7000000000000002,1.0679767835716063,3.2611894467772236,-0.2435666602877448,-0.5647051377665463,-25.256202053248504 +1.8,1.0373311671436036,3.2019585502833987,-0.36934566827230775,-0.6199127921099595,-2.2158907192831525 +1.9000000000000001,1.0094319418642066,3.1562777821354597,-0.18863883731563338,-0.2937025708487912,38.59154699727519 +2.0,1.0,3.141592653589793,0.0,0.0, diff --git a/example_XX/swingdown.csv b/example_XX/swingdown.csv new file mode 100644 index 000000000..6b5f26200 --- /dev/null +++ b/example_XX/swingdown.csv @@ -0,0 +1,82 @@ +t,Position,Theta,Velocity,Theta_dot,u +0.0,0.0,3.141592653589793,0.0,0.0,0.8980980968135133 +0.1,0.0004896649596369584,3.142355123974358,0.009793298292100449,0.0152494077253126,1.1112087627417346 +0.2,0.0017406019392048767,3.144419461859018,0.015225441299257883,0.026037349967879896,-0.016433486596925884 +0.30000000000000004,0.00309302502961027,3.1470736357868807,0.011823020508849955,0.027046128589377036,-0.7418827832236262 +0.4,0.0038288989777238436,3.1496050234652375,0.002894458453421439,0.0235816249777562,-1.1977295735281952 +0.5,0.0035044866082561293,3.151709377795235,-0.009382705842775728,0.018505461622190713,-1.4739075795762786 +0.6000000000000001,0.0018563489883896482,3.1532979761192617,-0.023580046554553886,0.013266504858342142,-1.6301627551386686 +0.7000000000000001,-0.0012605088396836816,3.1543877424717164,-0.03875711000691232,0.00852882219075312,-1.705734216468739 +0.8,-0.005913047380817745,3.1550409027900645,-0.05429366081576932,0.004534384176206157,-1.727359578534122 +0.9,-0.012116989137624275,3.1553327620981415,-0.06978517432036145,0.0013028019853370582,-1.7134357779474039 +1.0,-0.019854659484664437,3.1553354279300496,-0.08496823262044174,-0.0012494853471709457,-1.6755566520404839 +1.1,-0.029086446707295543,3.155110982634697,-0.09966751183217953,-0.0032394205598901492,-1.6211867168887624 +1.2000000000000002,-0.039758298488405314,3.154709296374018,-0.11376952379001466,-0.004794304653672454,-1.5563886020403435 +1.3,-0.05180674396814163,3.1541678273683758,-0.12719938580471063,-0.006035075459183664,-1.4836836730853995 +1.4000000000000001,-0.06516195688578291,3.153512575893647,-0.1399048725481111,-0.007069954035391924,-1.4056822752759301 +1.5,-0.07974987995218227,3.1527587493147924,-0.15185358877986915,-0.008006577541703256,-1.3241573747967887 +1.6,-0.09549360413019388,3.151910938231885,-0.16302089478036874,-0.008949644116443715,-1.239072919871877 +1.7000000000000002,-0.11231418190166902,3.1509625518837137,-0.17339066064914113,-0.010018082846986095,-1.1521853376524365 +1.8,-0.13013126455848215,3.1498938193565764,-0.18295099248708876,-0.011356567695756135,-1.0623869547505147 +1.9000000000000001,-0.14886324188853667,3.148668552138284,-0.1916885541140103,-0.013148776670092305,-0.970028601866152 +2.0,-0.16842712621290823,3.1472287968245434,-0.19958913237338086,-0.01564632960471758,-0.8738091043550451 +2.1,-0.18873822791484507,3.145486487855202,-0.20663290166536805,-0.0191998497821099,-0.7726844481549661 +2.2,-0.20970954461530003,3.1433109465648155,-0.21279343234374345,-0.02431097602562016,-0.6645577494530157 +2.3000000000000003,-0.23125067378786315,3.140510613529957,-0.2180291511075292,-0.03169568467153932,-0.5457869124900124 +2.4000000000000004,-0.2532658008723289,3.1368069222597876,-0.22227339058179277,-0.04237814073186093,-0.4110241453814053 +2.5,-0.27565101842136386,3.131795928340245,-0.22543096039889687,-0.05784173765898385,-0.2541964711091684 +2.6,-0.29829046041415674,3.1248926445432876,-0.2273578794569579,-0.08022393828016433,-0.06331427179159357 +2.7,-0.32104942639483347,3.1152524462905986,-0.22782144015656883,-0.11258002677362543,0.17943100986374397 +2.8000000000000003,-0.3437639617660014,3.1016584475636426,-0.22646926726682917,-0.15929994776550016,0.49732748174430363 +2.9000000000000004,-0.3662255822480557,3.0823599064696037,-0.22276314237429423,-0.2266708741152824,0.9285033360059032 +3.0,-0.3881572255335267,3.054843720074165,-0.2158697233351155,-0.32365285379348985,1.5272494632304612 +3.1,-0.40917611061048137,3.015512704251639,-0.20450797820396557,-0.46296746265702476,2.3720547468996873 +3.2,-0.42873766724857854,2.9592360199383907,-0.18672315455796396,-0.66256622360793,3.567184971676484 +3.3000000000000003,-0.44605179163447584,2.8787321812440676,-0.15955933316004306,-0.9475105502785446,5.236895895390032 +3.4000000000000004,-0.459962036054507,2.7637491312010822,-0.11864555524056082,-1.3521504505811954,7.466954481647186 +3.5,-0.4687889865580481,2.600040655057765,-0.057893454830299465,-1.9220190722848844,10.109641625627033 +3.6,-0.4701862704605304,2.3682818061164386,0.029947776780646175,-2.7131579065416784,12.252034157871782 +3.7,-0.4612120095236938,2.043615687579791,0.14953744195611735,-3.7801644641908188,11.182905333347946 +3.8000000000000003,-0.43929357192739477,1.5985507117653028,0.2888313099698903,-5.12113505209843,1.728062852065051 +3.9000000000000004,-0.4059929982589307,1.0178437316877835,0.37718016339927235,-6.493004549451259,-19.53626204512096 +4.0,-0.3744608756518423,0.3344651197287577,0.25346228874239496,-7.174567689728002,-40.89726206649438 +4.1000000000000005,-0.36817218951644054,-0.3553951846456218,-0.1276885660343651,-6.62263839776081,-38.891673658665475 +4.2,-0.40116226775695646,-0.9508720544211346,-0.5321129987759737,-5.286898997752897,-19.316043092299775 +4.3,-0.46531120892520844,-1.4055192694285705,-0.7508658245892018,-3.8060453023955882,-3.182993515840792 +4.4,-0.5427499072240677,-1.712669777288166,-0.797908141387946,-2.3369648547988207,3.3096980647061787 +4.5,-0.6209993037443283,-1.8742489685072985,-0.7670797890172317,-0.8946189695850393,2.981333342191629 +4.6000000000000005,-0.69595053292025,-1.8929123978038993,-0.7319447945011752,0.5213503836530551,0.1391127194208197 +4.7,-0.7688851595897345,-1.769642186561791,-0.7267477388884998,1.9440538411889423,-1.2561271326200802 +4.800000000000001,-0.8427156659797289,-1.50249728866807,-0.7498623889113916,3.398844116680922,3.0478002124550216 +4.9,-0.9180013713586771,-1.0935134576946153,-0.7558517186676181,4.7808325027843965,16.494180625702253 +5.0,-0.9878784538699831,-0.5698255168418146,-0.6416899315586427,5.692926314272013,34.94033493317614 +5.1000000000000005,-1.036433511867334,-0.0027744421832299297,-0.32941122838858855,5.648095178904186,42.80431219739947 +5.2,-1.0487893583739194,0.5151194333624814,0.08229429825694297,4.709782332019062,33.13323732383973 +5.300000000000001,-1.0241223445274774,0.9197483202017762,0.41104597867194353,3.382795404764735,18.310527048229503 +5.4,-0.9738115514595095,1.1873081193311825,0.5951698826872703,1.9684005778213873,8.27951170062056 +5.5,-0.9101005632475544,1.3123447736587521,0.6790498815517286,0.5323325087280888,3.627011420900077 +5.6000000000000005,-0.8402534619489789,1.2938708562888341,0.7178921444201042,-0.9018108561259277,0.6623630866536132 +5.7,-0.7676822472094255,1.134624119559335,0.7335321503709314,-2.283123878465559,-5.033332590621164 +5.800000000000001,-0.6958992617757562,0.8470056889708181,0.7021275583024962,-3.469244733306682,-15.944791400573198 +5.9,-0.6324694783638981,0.4645532119784668,0.5664681099347891,-4.179804806540196,-28.723101707152214 +6.0,-0.5890805367592223,0.0474081389755057,0.3013107221589057,-4.163096653523947,-34.232625603103436 +6.1000000000000005,-0.5750581791966085,-0.3350662341218295,-0.020863570906553962,-3.48639080842075,-28.304151106089527 +6.2,-0.5904512405617786,-0.6321303247279811,-0.28699765639688407,-2.4548910037000153,-16.851520094457275 +6.300000000000001,-0.6270208963641315,-0.8199410647664278,-0.44439545965004823,-1.3013237970662628,-6.24091495273785 +6.4,-0.6743618031219135,-0.8916209889802971,-0.5024226755057242,-0.13227468721162922,2.1919224864127247 +6.5,-0.7235714649504932,-0.8496416997712712,-0.48177056106594895,0.9718604713928619,10.309687597992536 +6.6000000000000005,-0.7668619338222116,-0.7063186803631503,-0.3840388163684142,1.8945999167681746,19.94029826420947 +6.7,-0.7957491297917574,-0.4877168951765774,-0.1937051030225677,2.4774357869632575,30.13028948482256 +6.800000000000001,-0.8008354561635319,-0.23422919292778654,0.0919785755871028,2.5923182580116046,36.748468953948255 +6.9,-0.7745846357679811,0.008006638866464105,0.43303783232395227,2.252398377875861,36.24163682942152 +7.0,-0.7149900207306187,0.20138758875389037,0.7588544684232612,1.61522061987069,29.2106086051237 +7.1000000000000005,-0.626557021699009,0.3255922215871434,1.0098055122089014,0.8688720367926253,18.70223340190598 +7.2,-0.518243100543454,0.3768171945732728,1.1564729109022718,0.15562742293034776,7.001344992384389 +7.300000000000001,-0.4009245089892408,0.36344195757848335,1.1898989201821193,-0.4231321628260471,-4.84948894416588 +7.4,-0.2858597501443424,0.3026891723922275,1.1113962567160176,-0.7919235408990056,-15.844350099666507 +7.5,-0.18356357612394775,0.2174388101297234,0.934527223692065,-0.9130837043511333,-24.2800202025846 +7.6000000000000005,-0.10228970355598846,0.13136249285357085,0.6909502276673065,-0.8084426411720554,-28.311773261349494 +7.7,-0.04632658313154299,0.06297569771286457,0.4283121808213341,-0.5592932616422452,-26.988222586113274 +7.800000000000001,-0.014946660411973017,0.021109318186221818,0.19928627356987313,-0.278034328890364,-20.588667670906553 +7.9,-0.002491173368576613,0.003603800877532851,0.04982346729808403,-0.07207601728338339,-10.253041957485154 +8.0,0.0,0.0,0.0,0.0, From 037c03afdb9f747a6fe3cb7c6c72ca002e8e69a3 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 20 Nov 2023 11:07:58 +0000 Subject: [PATCH 18/29] Use RT buffer for gains --- .../cartpole_lqr_trajectory_plugin.cpp | 40 +++++++++++-------- .../cartpole_lqr_trajectory_plugin.hpp | 5 ++- 2 files changed, 26 insertions(+), 19 deletions(-) diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index 0d1d2850b..a2363b343 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -64,7 +64,7 @@ bool CartpoleLqrTrajectoryPlugin::initialize( return false; } - trajectory_lqr_ptr_ = std::make_shared(); + trajectory_active_lqr_ptr_ = std::make_shared(); return true; } @@ -133,14 +133,13 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( // std::cout << "Ks: " << Ks << std::endl; // std::cout << "Ps: " << Ps << std::endl; - // TODO(christophfroehlich) use a buffer to switch from RT thread to new gain - trajectory_lqr_ptr_->reset(); + auto new_trajectory_gains = std::make_shared(); if (trajectory->points.size() == 1) { // use steady-state gain - trajectory_lqr_ptr_->K_vec_.push_back(Ks); - trajectory_lqr_ptr_->time_vec_.push_back(rclcpp::Duration::from_seconds(0.0)); + new_trajectory_gains->K_vec_.push_back(Ks); + new_trajectory_gains->time_vec_.push_back(rclcpp::Duration::from_seconds(0.0)); } else { @@ -182,19 +181,20 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( P_new = (Q_ + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; P = 0.5 * (P_new + P_new.transpose()); - trajectory_lqr_ptr_->K_vec_.push_back(K); - trajectory_lqr_ptr_->time_vec_.push_back(point.time_from_start); + new_trajectory_gains->K_vec_.push_back(K); + new_trajectory_gains->time_vec_.push_back(point.time_from_start); // BEGIN: This part here is for exemplary purposes - Please do not copy to your production // code wait to check if RT buffer works well std::this_thread::sleep_for(std::chrono::milliseconds(10)); // END: This part here is for exemplary purposes - Please do not copy to your production code } - // would be more efficient using reverse iterator in get_feedback_gain - std::reverse(trajectory_lqr_ptr_->K_vec_.begin(), trajectory_lqr_ptr_->K_vec_.end()); - std::reverse(trajectory_lqr_ptr_->time_vec_.begin(), trajectory_lqr_ptr_->time_vec_.end()); + // TODO(anyone) would be more efficient using reverse iterator in get_feedback_gain + std::reverse(new_trajectory_gains->K_vec_.begin(), new_trajectory_gains->K_vec_.end()); + std::reverse(new_trajectory_gains->time_vec_.begin(), new_trajectory_gains->time_vec_.end()); } + trajectory_next_lqr_ptr_.writeFromNonRT(new_trajectory_gains); auto end_time = this->node_->now(); auto duration = end_time - start_time; RCLCPP_INFO( @@ -222,13 +222,13 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawRT_impl( // std::cout << "Ks: " << Ks << std::endl; // std::cout << "Ps: " << Ps << std::endl; - // TODO(christophfroehlich) use a buffer to switch from RT thread to new gain - trajectory_lqr_ptr_->reset(); + auto new_trajectory_gains = std::make_shared(); // use steady-state gain - trajectory_lqr_ptr_->K_vec_.push_back(Ks); - trajectory_lqr_ptr_->time_vec_.push_back(rclcpp::Duration::from_seconds(0.0)); + new_trajectory_gains->K_vec_.push_back(Ks); + new_trajectory_gains->time_vec_.push_back(rclcpp::Duration::from_seconds(0.0)); + trajectory_next_lqr_ptr_.writeFromNonRT(new_trajectory_gains); return true; } @@ -294,10 +294,10 @@ void CartpoleLqrTrajectoryPlugin::computeCommands( const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) { - if (!trajectory_lqr_ptr_->is_empty()) + if (!trajectory_active_lqr_ptr_->is_empty()) { auto e = get_state_from_point(error); - auto K = trajectory_lqr_ptr_->get_feedback_gain(duration_since_start); + auto K = trajectory_active_lqr_ptr_->get_feedback_gain(duration_since_start); // integrate acceleration from state feedback to velocity. consider sign of e! u_ += period.seconds() * (K.dot(-e)); // set system input as desired velocity + integrated LQR control output @@ -307,10 +307,16 @@ void CartpoleLqrTrajectoryPlugin::computeCommands( void CartpoleLqrTrajectoryPlugin::reset() { - trajectory_lqr_ptr_->reset(); + trajectory_active_lqr_ptr_->reset(); u_ = 0; } +void CartpoleLqrTrajectoryPlugin::start() +{ + // switch storage to new gains + trajectory_active_lqr_ptr_ = *trajectory_next_lqr_ptr_.readFromRT(); +} + void CartpoleLqrTrajectoryPlugin::get_linear_system_matrices( Eigen::Vector x, Eigen::Vector u, const double dt, Eigen::Matrix & Phi, Eigen::Matrix & Gamma) diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp index 27337daec..79fa8d5a1 100644 --- a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp +++ b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp @@ -76,7 +76,7 @@ class CartpoleLqrTrajectoryPlugin void reset() override; - void start() override {} + void start() override; protected: /** @@ -109,7 +109,8 @@ class CartpoleLqrTrajectoryPlugin Eigen::Matrix Q_; Eigen::Matrix R_; - std::shared_ptr trajectory_lqr_ptr_; + std::shared_ptr trajectory_active_lqr_ptr_; + realtime_tools::RealtimeBuffer> trajectory_next_lqr_ptr_; }; } // namespace ros2_control_demo_example_xx From 2c7148f9307134c52b97fdb09b4dd3c4e8b77347 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 20 Nov 2023 11:57:58 +0000 Subject: [PATCH 19/29] Parse dt from update_rate of controller-node --- example_XX/bringup/config/cart_pole_controllers.yaml | 2 ++ .../controller/cartpole_lqr_trajectory_plugin.cpp | 12 ++++++++---- .../cartpole_lqr_trajectory_plugin_parameters.yaml | 5 ----- .../cartpole_lqr_trajectory_plugin.hpp | 4 ++-- 4 files changed, 12 insertions(+), 11 deletions(-) diff --git a/example_XX/bringup/config/cart_pole_controllers.yaml b/example_XX/bringup/config/cart_pole_controllers.yaml index a8be02d2a..50cc3cde4 100644 --- a/example_XX/bringup/config/cart_pole_controllers.yaml +++ b/example_XX/bringup/config/cart_pole_controllers.yaml @@ -10,6 +10,8 @@ controller_manager: trajectory_controllers: ros__parameters: + update_rate: 100 # Hz + joints: - slider_to_cart - cart_to_pendulum diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index a2363b343..1ce669128 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -282,10 +282,14 @@ void CartpoleLqrTrajectoryPlugin::parseGains() Eigen::Vector Q_diag{params_.gains.Q.data()}; Q_ = Eigen::Matrix{Q_diag.asDiagonal()}; R_ = Eigen::Matrix{params_.gains.R}; - dt_ = params_.dt; - std::cout << "Q: " << std::endl << Q_ << std::endl; - std::cout << "R: " << std::endl << R_ << std::endl; - std::cout << "dt: " << std::endl << dt_ << std::endl; + // this is part of controller_interface definition, and not defined in GPL params_ + // TODO(christophfroehlich): change API once + // https://github.com/ros-controls/ros2_control/pull/1141 is merged + dt_ = 1. / node_->get_parameter("update_rate").as_int(); + + RCLCPP_INFO_STREAM(node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] Q: " << std::endl << Q_); + RCLCPP_INFO_STREAM(node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] R: " << std::endl << R_); + RCLCPP_INFO_STREAM(node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] dt: " << std::endl << dt_); } void CartpoleLqrTrajectoryPlugin::computeCommands( diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml b/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml index dbd2710c6..09b204c11 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin_parameters.yaml @@ -8,11 +8,6 @@ ros2_control_demo_example_xx: size_gt<>: [0], } } - dt: { - type: double, - default_value: 0.01, - description: "control loop update interval" - } system: lS: { type: double, diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp index 79fa8d5a1..15e26ad7a 100644 --- a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp +++ b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp @@ -103,12 +103,12 @@ class CartpoleLqrTrajectoryPlugin // Parameters from ROS for ros2_control_demo_example_xx std::shared_ptr param_listener_; Params params_; - // TODO(christophfroehlich): do we need the controller update rate here, or the trajectory - // sampling? + // controller update rate double dt_ = 0.01; Eigen::Matrix Q_; Eigen::Matrix R_; + // storage for LQR gains std::shared_ptr trajectory_active_lqr_ptr_; realtime_tools::RealtimeBuffer> trajectory_next_lqr_ptr_; }; From a36bf9d9a8f80f9a990a5411f24153d4ba925b6c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 22 Nov 2023 08:42:36 +0000 Subject: [PATCH 20/29] Set sample time for gains constant --- .../cartpole_lqr_trajectory_plugin.cpp | 26 ++++++++-------- .../cartpole_lqr_trajectory_plugin.hpp | 31 ++++++++++++++++++- 2 files changed, 43 insertions(+), 14 deletions(-) diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index 1ce669128..9a68d06f3 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -130,8 +130,6 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( auto x_R = get_state_from_point(trajectory->points.back()); Eigen::Vector u_R{0}; calcLQR_steady(x_R, u_R, 0.1, Q_, R_, N, Ks, Ps); - // std::cout << "Ks: " << Ks << std::endl; - // std::cout << "Ps: " << Ps << std::endl; auto new_trajectory_gains = std::make_shared(); @@ -152,8 +150,6 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( Eigen::Vector x_p1; Eigen::Vector x; Eigen::Vector u{0}; - auto get_time = [](const trajectory_msgs::msg::JointTrajectoryPoint & point) - { return point.time_from_start.sec + point.time_from_start.nanosec * 1e-9; }; double dt_traj; // iterate Riccati equation, backwards in time @@ -165,13 +161,14 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( // we don't have acceleration in trajectory, so we have to calculate it if (i == trajectory->points.size()) { - dt_traj = dt_; + dt_traj = dt_traj_; u[0] = 0.0; x_p1 = x; } else { - dt_traj = get_time(trajectory->points.at(i)) - get_time(point); + dt_traj = + get_time(trajectory->points.at(i).time_from_start) - get_time(point.time_from_start); x_p1 = get_state_from_point(trajectory->points.at(i)); u[0] = (x_p1(3) - x(3)) / dt_traj; } @@ -195,15 +192,15 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( } trajectory_next_lqr_ptr_.writeFromNonRT(new_trajectory_gains); + auto end_time = this->node_->now(); auto duration = end_time - start_time; RCLCPP_INFO( node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] computed control law for %lu points in %es.", trajectory->points.size(), duration.seconds()); - // for (const auto & mat : K_vec_) { - // std::cout << "K: " << std::endl << mat << std::endl; - // } + + new_trajectory_gains->print(); return true; } @@ -218,17 +215,17 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawRT_impl( Eigen::Matrix Ps; auto x_R = get_state_from_point(trajectory->points.back()); Eigen::Vector u_R{0}; - calcLQR_steady(x_R, u_R, 0.1, Q_, R_, N, Ks, Ps); - // std::cout << "Ks: " << Ks << std::endl; - // std::cout << "Ps: " << Ps << std::endl; + calcLQR_steady(x_R, u_R, dt_traj_, Q_, R_, N, Ks, Ps); auto new_trajectory_gains = std::make_shared(); // use steady-state gain new_trajectory_gains->K_vec_.push_back(Ks); new_trajectory_gains->time_vec_.push_back(rclcpp::Duration::from_seconds(0.0)); - trajectory_next_lqr_ptr_.writeFromNonRT(new_trajectory_gains); + + new_trajectory_gains->print(); + return true; } @@ -264,6 +261,9 @@ void CartpoleLqrTrajectoryPlugin::calcLQR_steady( } Ks = K; Ps = P; + + // std::cout << "Ks: " << Ks << std::endl; + // std::cout << "Ps: " << Ps << std::endl; } bool CartpoleLqrTrajectoryPlugin::updateGainsRT() diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp index 15e26ad7a..3eb5bd78c 100644 --- a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp +++ b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp @@ -28,22 +28,48 @@ namespace ros2_control_demo_example_xx { +double get_time(const builtin_interfaces::msg::Duration & time_from_start) +{ + return time_from_start.sec + time_from_start.nanosec * 1e-9; +}; + class TrajectoryLQR { public: std::vector> K_vec_; std::vector time_vec_; + /** + * @brief sample the feedback gain at the given time + */ Eigen::Matrix get_feedback_gain( const rclcpp::Duration & duration_since_start); + /** + * @brief resets internal storage + */ void reset() { K_vec_.clear(); time_vec_.clear(); }; + /** + * @brief returns true if the trajectory is empty + */ bool is_empty() { return K_vec_.empty(); } + + /** + * @brief print the gains on std::cout + */ + void print() + { + for (size_t i = 0; i < K_vec_.size(); ++i) + { + std::cout << "at t: " << get_time(time_vec_.at(i)) << "s K: " << std::endl + << K_vec_.at(i) << std::endl; + } + } }; class CartpoleLqrTrajectoryPlugin @@ -103,8 +129,11 @@ class CartpoleLqrTrajectoryPlugin // Parameters from ROS for ros2_control_demo_example_xx std::shared_ptr param_listener_; Params params_; - // controller update rate + // sampling time of the controller double dt_ = 0.01; + // feedback gains were tuned with trajectory sampling of 0.1s + // TODO(christophfroehlich): change this to make it independent of the trajectory sampling + double dt_traj_ = 0.1; Eigen::Matrix Q_; Eigen::Matrix R_; From c2ef378db98ff6a9691824404f18b7351f8bf7eb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 22 Nov 2023 08:52:03 +0000 Subject: [PATCH 21/29] No need for explicit update_rate any more --- example_XX/bringup/config/cart_pole_controllers.yaml | 1 - example_XX/controller/cartpole_lqr_trajectory_plugin.cpp | 3 +-- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/example_XX/bringup/config/cart_pole_controllers.yaml b/example_XX/bringup/config/cart_pole_controllers.yaml index 50cc3cde4..bea3f85f4 100644 --- a/example_XX/bringup/config/cart_pole_controllers.yaml +++ b/example_XX/bringup/config/cart_pole_controllers.yaml @@ -10,7 +10,6 @@ controller_manager: trajectory_controllers: ros__parameters: - update_rate: 100 # Hz joints: - slider_to_cart diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index 9a68d06f3..a572d817d 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -283,8 +283,7 @@ void CartpoleLqrTrajectoryPlugin::parseGains() Q_ = Eigen::Matrix{Q_diag.asDiagonal()}; R_ = Eigen::Matrix{params_.gains.R}; // this is part of controller_interface definition, and not defined in GPL params_ - // TODO(christophfroehlich): change API once - // https://github.com/ros-controls/ros2_control/pull/1141 is merged + // if not set explicitly, it will be the cm_update rate dt_ = 1. / node_->get_parameter("update_rate").as_int(); RCLCPP_INFO_STREAM(node_->get_logger(), "[CartpoleLqrTrajectoryPlugin] Q: " << std::endl << Q_); From 87bbad79fdf58949bbe051c2dffc4869231d0853 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 5 Dec 2023 10:03:10 +0000 Subject: [PATCH 22/29] Fix computation of LQR gains --- .../cartpole_lqr_trajectory_plugin.cpp | 67 +++++++++++-------- .../cartpole_lqr_trajectory_plugin.hpp | 5 +- 2 files changed, 42 insertions(+), 30 deletions(-) diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index a572d817d..485414aa1 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -129,7 +129,7 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( Eigen::Matrix Ps; auto x_R = get_state_from_point(trajectory->points.back()); Eigen::Vector u_R{0}; - calcLQR_steady(x_R, u_R, 0.1, Q_, R_, N, Ks, Ps); + calcLQR_steady(x_R, u_R, dt_, Q_, R_, N, Ks, Ps); auto new_trajectory_gains = std::make_shared(); @@ -137,7 +137,7 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( { // use steady-state gain new_trajectory_gains->K_vec_.push_back(Ks); - new_trajectory_gains->time_vec_.push_back(rclcpp::Duration::from_seconds(0.0)); + new_trajectory_gains->time_vec_.push_back(rclcpp::Duration::from_seconds(dt_)); } else { @@ -145,50 +145,63 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( Eigen::Matrix Phi; Eigen::Matrix Gamma; Eigen::Matrix C; - C.setIdentity(); + C.setIdentity(); // we can measure every state Eigen::Matrix P(Ps), P_new; Eigen::Vector x_p1; Eigen::Vector x; Eigen::Vector u{0}; - double dt_traj; + + // reserve space for feedback gains for every point in trajectory + // this is a design choice to save memory of the gains, theoretically we should + // save the gains at the dt-grid. + new_trajectory_gains->K_vec_.resize(trajectory->points.size() - 1); + new_trajectory_gains->time_vec_.resize(trajectory->points.size() - 1); + + double t_traj = get_time(trajectory->points.back().time_from_start); + auto idx_traj = trajectory->points.size(); // iterate Riccati equation, backwards in time - for (size_t i = trajectory->points.size(); i > 0; i--) + while (t_traj > 0.0) { - const trajectory_msgs::msg::JointTrajectoryPoint point = trajectory->points.at(i - 1); + // TODO(christophfroehlich) interpolate from time or use trajectory class? + const trajectory_msgs::msg::JointTrajectoryPoint point = trajectory->points.at(idx_traj - 1); x = get_state_from_point(point); // system input acceleration, derivative of velocity // we don't have acceleration in trajectory, so we have to calculate it - if (i == trajectory->points.size()) - { - dt_traj = dt_traj_; - u[0] = 0.0; - x_p1 = x; - } - else + if (idx_traj < trajectory->points.size()) { - dt_traj = - get_time(trajectory->points.at(i).time_from_start) - get_time(point.time_from_start); - x_p1 = get_state_from_point(trajectory->points.at(i)); + double dt_traj = get_time(trajectory->points.at(idx_traj).time_from_start) - + get_time(point.time_from_start); + x_p1 = get_state_from_point(trajectory->points.at(idx_traj)); u[0] = (x_p1(3) - x(3)) / dt_traj; } - get_linear_system_matrices(x, u, dt_traj, Phi, Gamma); + get_linear_system_matrices(x, u, dt_, Phi, Gamma); K = -(R_ + Gamma.transpose() * P * Gamma).inverse() * (N + Gamma.transpose() * P * Phi); P_new = (Q_ + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; P = 0.5 * (P_new + P_new.transpose()); - new_trajectory_gains->K_vec_.push_back(K); - new_trajectory_gains->time_vec_.push_back(point.time_from_start); - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production - // code wait to check if RT buffer works well - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + // code + // DEBUG: wait to check if RT buffer works well + std::this_thread::sleep_for(std::chrono::milliseconds(1)); // END: This part here is for exemplary purposes - Please do not copy to your production code + + t_traj -= dt_; + if (t_traj < get_time(trajectory->points.at(idx_traj - 1).time_from_start)) + { + idx_traj--; + if (idx_traj == 0lu) + { + // we reached the start of the trajectory + break; + } + // save feedback gain for this point + new_trajectory_gains->K_vec_.at(idx_traj - 1) = K; + new_trajectory_gains->time_vec_.at(idx_traj - 1) = + trajectory->points.at(idx_traj).time_from_start; + } } - // TODO(anyone) would be more efficient using reverse iterator in get_feedback_gain - std::reverse(new_trajectory_gains->K_vec_.begin(), new_trajectory_gains->K_vec_.end()); - std::reverse(new_trajectory_gains->time_vec_.begin(), new_trajectory_gains->time_vec_.end()); } trajectory_next_lqr_ptr_.writeFromNonRT(new_trajectory_gains); @@ -215,13 +228,13 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawRT_impl( Eigen::Matrix Ps; auto x_R = get_state_from_point(trajectory->points.back()); Eigen::Vector u_R{0}; - calcLQR_steady(x_R, u_R, dt_traj_, Q_, R_, N, Ks, Ps); + calcLQR_steady(x_R, u_R, dt_, Q_, R_, N, Ks, Ps); auto new_trajectory_gains = std::make_shared(); // use steady-state gain new_trajectory_gains->K_vec_.push_back(Ks); - new_trajectory_gains->time_vec_.push_back(rclcpp::Duration::from_seconds(0.0)); + new_trajectory_gains->time_vec_.push_back(rclcpp::Duration::from_seconds(dt_)); trajectory_next_lqr_ptr_.writeFromNonRT(new_trajectory_gains); new_trajectory_gains->print(); diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp index 3eb5bd78c..b6705a224 100644 --- a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp +++ b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp @@ -131,10 +131,9 @@ class CartpoleLqrTrajectoryPlugin Params params_; // sampling time of the controller double dt_ = 0.01; - // feedback gains were tuned with trajectory sampling of 0.1s - // TODO(christophfroehlich): change this to make it independent of the trajectory sampling - double dt_traj_ = 0.1; + // LQR cost function parameter for states Eigen::Matrix Q_; + // LQR cost function parameter for input Eigen::Matrix R_; // storage for LQR gains From b6935a960833cd7f534f3c2bdd9b8e9059143ce9 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 10 Dec 2023 18:15:12 +0000 Subject: [PATCH 23/29] Correctly load pid_alternative --- example_XX/bringup/config/cart_pole_controllers.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/example_XX/bringup/config/cart_pole_controllers.yaml b/example_XX/bringup/config/cart_pole_controllers.yaml index bea3f85f4..d90a012c0 100644 --- a/example_XX/bringup/config/cart_pole_controllers.yaml +++ b/example_XX/bringup/config/cart_pole_controllers.yaml @@ -5,6 +5,9 @@ controller_manager: trajectory_controllers: type: joint_trajectory_controller/JointTrajectoryController + trajectory_controllers_pid: + type: joint_trajectory_controller/JointTrajectoryController + joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster From 19fe6924f70113e9f6f0b1c66b98f06cbd24a1ee Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 10 Dec 2023 18:15:46 +0000 Subject: [PATCH 24/29] Throw warning if trajectory points is wrong --- example_XX/controller/cartpole_lqr_trajectory_plugin.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index 485414aa1..b340a3d9c 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -113,6 +113,14 @@ Eigen::Vector get_state_from_point( { // TODO(anyone) create map to get the correct joint order // theta, theta_dot, x, x_dot + if (point.positions.size() != 2) + { + throw std::runtime_error("JointTrajectoryPoint.positions does not have the correct size."); + } + if (point.velocities.size() != 2) + { + throw std::runtime_error("JointTrajectoryPoint.velocities does not have the correct size."); + } return Eigen::Vector{ point.positions[1], point.velocities[1], point.positions[0], point.velocities[0]}; } From 3fd5582194faf497446bde67ee6a81aeb4be0942 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 14 Dec 2023 12:28:15 +0000 Subject: [PATCH 25/29] Rename method --- example_XX/controller/cartpole_lqr_trajectory_plugin.cpp | 8 ++++---- .../cartpole_lqr_trajectory_plugin.hpp | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index b340a3d9c..429e5a0f3 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -165,7 +165,7 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( new_trajectory_gains->K_vec_.resize(trajectory->points.size() - 1); new_trajectory_gains->time_vec_.resize(trajectory->points.size() - 1); - double t_traj = get_time(trajectory->points.back().time_from_start); + double t_traj = get_time_from_duration(trajectory->points.back().time_from_start); auto idx_traj = trajectory->points.size(); // iterate Riccati equation, backwards in time @@ -178,8 +178,8 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( // we don't have acceleration in trajectory, so we have to calculate it if (idx_traj < trajectory->points.size()) { - double dt_traj = get_time(trajectory->points.at(idx_traj).time_from_start) - - get_time(point.time_from_start); + double dt_traj = get_time_from_duration(trajectory->points.at(idx_traj).time_from_start) - + get_time_from_duration(point.time_from_start); x_p1 = get_state_from_point(trajectory->points.at(idx_traj)); u[0] = (x_p1(3) - x(3)) / dt_traj; } @@ -196,7 +196,7 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( // END: This part here is for exemplary purposes - Please do not copy to your production code t_traj -= dt_; - if (t_traj < get_time(trajectory->points.at(idx_traj - 1).time_from_start)) + if (t_traj < get_time_from_duration(trajectory->points.at(idx_traj - 1).time_from_start)) { idx_traj--; if (idx_traj == 0lu) diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp index b6705a224..5dc405060 100644 --- a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp +++ b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp @@ -24,11 +24,11 @@ #include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp" // shortcut -#define NUM_STATES 4 namespace ros2_control_demo_example_xx { +#define NUM_STATES 4 -double get_time(const builtin_interfaces::msg::Duration & time_from_start) +double get_time_from_duration(const builtin_interfaces::msg::Duration & time_from_start) { return time_from_start.sec + time_from_start.nanosec * 1e-9; }; @@ -66,7 +66,7 @@ class TrajectoryLQR { for (size_t i = 0; i < K_vec_.size(); ++i) { - std::cout << "at t: " << get_time(time_vec_.at(i)) << "s K: " << std::endl + std::cout << "at t: " << get_time_from_duration(time_vec_.at(i)) << "s K: " << std::endl << K_vec_.at(i) << std::endl; } } From 8f1bc1839aa196d726d66e3048e508436e903eb2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 14 Dec 2023 12:39:18 +0000 Subject: [PATCH 26/29] Make make_symmetric method --- example_XX/controller/cartpole_lqr_trajectory_plugin.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index 429e5a0f3..cea87cf1f 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -18,6 +18,11 @@ namespace ros2_control_demo_example_xx { +// Make P symmetric +auto make_symmetric(Eigen::Matrix & P) +{ + return 0.5 * (P + P.transpose()); +} Eigen::Matrix TrajectoryLQR::get_feedback_gain( const rclcpp::Duration & duration_since_start) @@ -278,7 +283,7 @@ void CartpoleLqrTrajectoryPlugin::calcLQR_steady( { // abort criterium break; } - P = 0.5 * (P_new + P_new.transpose()); + P = make_symmetric(P_new); } Ks = K; Ps = P; From 1af9f2ac80dd66a95d6173098f060c3ae20bd818 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 14 Dec 2023 13:13:23 +0000 Subject: [PATCH 27/29] Create riccati step method --- .../cartpole_lqr_trajectory_plugin.cpp | 37 +++++++++++-------- .../cartpole_lqr_trajectory_plugin.hpp | 15 ++++++++ 2 files changed, 37 insertions(+), 15 deletions(-) diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index cea87cf1f..3a38abf18 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -18,11 +18,8 @@ namespace ros2_control_demo_example_xx { -// Make P symmetric -auto make_symmetric(Eigen::Matrix & P) -{ - return 0.5 * (P + P.transpose()); -} + +/* ----- TrajectoryLQR ----- */ Eigen::Matrix TrajectoryLQR::get_feedback_gain( const rclcpp::Duration & duration_since_start) @@ -41,6 +38,8 @@ Eigen::Matrix TrajectoryLQR::get_feedback_gain( return K_vec_.at(idx); } +/* ----- CartpoleLqrTrajectoryPlugin ----- */ + bool CartpoleLqrTrajectoryPlugin::initialize( rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector map_cmd_to_joints) { @@ -189,10 +188,8 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( u[0] = (x_p1(3) - x(3)) / dt_traj; } get_linear_system_matrices(x, u, dt_, Phi, Gamma); - - K = -(R_ + Gamma.transpose() * P * Gamma).inverse() * (N + Gamma.transpose() * P * Phi); - P_new = (Q_ + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; - P = 0.5 * (P_new + P_new.transpose()); + riccati_step(K, P_new, P, Phi, Gamma, Q_, R_, N); + P = make_symmetric(P_new); // BEGIN: This part here is for exemplary purposes - Please do not copy to your production // code @@ -271,14 +268,11 @@ void CartpoleLqrTrajectoryPlugin::calcLQR_steady( // solve steady-state Riccati equation iteratively Eigen::Matrix P, P_new; Eigen::Matrix K; - P.setIdentity(); + P = P.setIdentity() * 1e5; // initial value of P - P = P * 1e5; // initial value of P - int ct = 0; - for (ct = 1; ct < 1e4; ct++) + for (int ct = 0; ct < 1e4; ++ct) { - K = -(R + Gamma.transpose() * P * Gamma).inverse() * (N + Gamma.transpose() * P * Phi); - P_new = (Q + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; + riccati_step(K, P_new, P, Phi, Gamma, Q, R, N); if ((P_new - P).norm() < 1e-3) { // abort criterium break; @@ -346,6 +340,19 @@ void CartpoleLqrTrajectoryPlugin::start() trajectory_active_lqr_ptr_ = *trajectory_next_lqr_ptr_.readFromRT(); } +/* ----- local functions ----- */ +void CartpoleLqrTrajectoryPlugin::riccati_step( + Eigen::Matrix & K, Eigen::Matrix & P_new, + const Eigen::Matrix & P, + const Eigen::Matrix & Phi, + const Eigen::Matrix & Gamma, + const Eigen::Matrix & Q, const Eigen::Matrix & R, + const Eigen::Matrix & N) +{ + K = -(R + Gamma.transpose() * P * Gamma).inverse() * (N + Gamma.transpose() * P * Phi); + P_new = (Q + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; +} + void CartpoleLqrTrajectoryPlugin::get_linear_system_matrices( Eigen::Vector x, Eigen::Vector u, const double dt, Eigen::Matrix & Phi, Eigen::Matrix & Gamma) diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp index 5dc405060..2b54e9012 100644 --- a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp +++ b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp @@ -33,6 +33,12 @@ double get_time_from_duration(const builtin_interfaces::msg::Duration & time_fro return time_from_start.sec + time_from_start.nanosec * 1e-9; }; +// Make P symmetric +auto make_symmetric(Eigen::Matrix & P) +{ + return 0.5 * (P + P.transpose()); +} + class TrajectoryLQR { public: @@ -119,6 +125,15 @@ class CartpoleLqrTrajectoryPlugin Eigen::Matrix Q, Eigen::Matrix R, Eigen::Matrix N, Eigen::Matrix & Ks, Eigen::Matrix & Ps); + void riccati_step( + Eigen::Matrix & K, Eigen::Matrix & P_new, + const Eigen::Matrix & P, + const Eigen::Matrix & Phi, + const Eigen::Matrix & Gamma, + const Eigen::Matrix & Q, const Eigen::Matrix & R, + const Eigen::Matrix & N); + + // integrator state for system input/LQR output double u_; // number of command joints From de7a42988a385bb6aa9a26f0be74ad38010a0064 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 14 Dec 2023 13:16:16 +0000 Subject: [PATCH 28/29] Reorder methods --- .../cartpole_lqr_trajectory_plugin.cpp | 71 ++++++++++--------- 1 file changed, 36 insertions(+), 35 deletions(-) diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index 3a38abf18..c06b29183 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -252,40 +252,6 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawRT_impl( return true; } -void CartpoleLqrTrajectoryPlugin::calcLQR_steady( - Eigen::Vector x, Eigen::Vector u, double dt, - Eigen::Matrix Q, Eigen::Matrix R, - Eigen::Matrix N, Eigen::Matrix & Ks, - Eigen::Matrix & Ps) -{ - Eigen::Matrix Phi; - Eigen::Matrix Gamma; - Eigen::Matrix C; - C.setIdentity(); - - get_linear_system_matrices(x, u, dt, Phi, Gamma); - - // solve steady-state Riccati equation iteratively - Eigen::Matrix P, P_new; - Eigen::Matrix K; - P = P.setIdentity() * 1e5; // initial value of P - - for (int ct = 0; ct < 1e4; ++ct) - { - riccati_step(K, P_new, P, Phi, Gamma, Q, R, N); - if ((P_new - P).norm() < 1e-3) - { // abort criterium - break; - } - P = make_symmetric(P_new); - } - Ks = K; - Ps = P; - - // std::cout << "Ks: " << Ks << std::endl; - // std::cout << "Ps: " << Ps << std::endl; -} - bool CartpoleLqrTrajectoryPlugin::updateGainsRT() { if (param_listener_->is_old(params_)) @@ -340,7 +306,42 @@ void CartpoleLqrTrajectoryPlugin::start() trajectory_active_lqr_ptr_ = *trajectory_next_lqr_ptr_.readFromRT(); } -/* ----- local functions ----- */ +/* ----- private functions ----- */ + +void CartpoleLqrTrajectoryPlugin::calcLQR_steady( + Eigen::Vector x, Eigen::Vector u, double dt, + Eigen::Matrix Q, Eigen::Matrix R, + Eigen::Matrix N, Eigen::Matrix & Ks, + Eigen::Matrix & Ps) +{ + Eigen::Matrix Phi; + Eigen::Matrix Gamma; + Eigen::Matrix C; + C.setIdentity(); + + get_linear_system_matrices(x, u, dt, Phi, Gamma); + + // solve steady-state Riccati equation iteratively + Eigen::Matrix P, P_new; + Eigen::Matrix K; + P = P.setIdentity() * 1e5; // initial value of P + + for (int ct = 0; ct < 1e4; ++ct) + { + riccati_step(K, P_new, P, Phi, Gamma, Q, R, N); + if ((P_new - P).norm() < 1e-3) + { // abort criterium + break; + } + P = make_symmetric(P_new); + } + Ks = K; + Ps = P; + + // std::cout << "Ks: " << Ks << std::endl; + // std::cout << "Ps: " << Ps << std::endl; +} + void CartpoleLqrTrajectoryPlugin::riccati_step( Eigen::Matrix & K, Eigen::Matrix & P_new, const Eigen::Matrix & P, From 430e9559ac14776342bdf79efa999cb0620d550e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 14 Dec 2023 13:28:00 +0000 Subject: [PATCH 29/29] Use typedefs and make methods const --- .../cartpole_lqr_trajectory_plugin.cpp | 81 ++++++++----------- .../cartpole_lqr_trajectory_plugin.hpp | 44 +++++----- 2 files changed, 55 insertions(+), 70 deletions(-) diff --git a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp index c06b29183..39debc64a 100644 --- a/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp +++ b/example_XX/controller/cartpole_lqr_trajectory_plugin.cpp @@ -21,8 +21,7 @@ namespace ros2_control_demo_example_xx /* ----- TrajectoryLQR ----- */ -Eigen::Matrix TrajectoryLQR::get_feedback_gain( - const rclcpp::Duration & duration_since_start) +MatrixUX TrajectoryLQR::get_feedback_gain(const rclcpp::Duration & duration_since_start) const { // TODO(christophfroehlich) interpolate K from time auto it = std::upper_bound(time_vec_.begin(), time_vec_.end(), duration_since_start); @@ -112,8 +111,7 @@ bool CartpoleLqrTrajectoryPlugin::activate() return true; } -Eigen::Vector get_state_from_point( - trajectory_msgs::msg::JointTrajectoryPoint point) +VectorX get_state_from_point(trajectory_msgs::msg::JointTrajectoryPoint point) { // TODO(anyone) create map to get the correct joint order // theta, theta_dot, x, x_dot @@ -125,8 +123,7 @@ Eigen::Vector get_state_from_point( { throw std::runtime_error("JointTrajectoryPoint.velocities does not have the correct size."); } - return Eigen::Vector{ - point.positions[1], point.velocities[1], point.positions[0], point.velocities[0]}; + return VectorX{point.positions[1], point.velocities[1], point.positions[0], point.velocities[0]}; } bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( @@ -135,12 +132,12 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( auto start_time = this->node_->now(); // parameters - auto N = Eigen::Matrix::Zero(); + auto N = MatrixUX::Zero(); - Eigen::Matrix Ks; - Eigen::Matrix Ps; + MatrixUX Ks; + MatrixXX Ps; auto x_R = get_state_from_point(trajectory->points.back()); - Eigen::Vector u_R{0}; + VectorU u_R{0}; calcLQR_steady(x_R, u_R, dt_, Q_, R_, N, Ks, Ps); auto new_trajectory_gains = std::make_shared(); @@ -153,15 +150,15 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawNonRT_impl( } else { - Eigen::Matrix K; - Eigen::Matrix Phi; - Eigen::Matrix Gamma; - Eigen::Matrix C; + MatrixUX K; + MatrixXX Phi; + MatrixXU Gamma; + MatrixXX C; C.setIdentity(); // we can measure every state - Eigen::Matrix P(Ps), P_new; - Eigen::Vector x_p1; - Eigen::Vector x; - Eigen::Vector u{0}; + MatrixXX P(Ps), P_new; + VectorX x_p1; + VectorX x; + VectorU u{0}; // reserve space for feedback gains for every point in trajectory // this is a design choice to save memory of the gains, theoretically we should @@ -232,12 +229,12 @@ bool CartpoleLqrTrajectoryPlugin::computeControlLawRT_impl( const std::shared_ptr & trajectory) { // parameters - auto N = Eigen::Matrix::Zero(); + auto N = MatrixUX::Zero(); - Eigen::Matrix Ks; - Eigen::Matrix Ps; + MatrixUX Ks; + MatrixXX Ps; auto x_R = get_state_from_point(trajectory->points.back()); - Eigen::Vector u_R{0}; + VectorU u_R{0}; calcLQR_steady(x_R, u_R, dt_, Q_, R_, N, Ks, Ps); auto new_trajectory_gains = std::make_shared(); @@ -265,9 +262,9 @@ bool CartpoleLqrTrajectoryPlugin::updateGainsRT() void CartpoleLqrTrajectoryPlugin::parseGains() { - Eigen::Vector Q_diag{params_.gains.Q.data()}; - Q_ = Eigen::Matrix{Q_diag.asDiagonal()}; - R_ = Eigen::Matrix{params_.gains.R}; + VectorX Q_diag{params_.gains.Q.data()}; + Q_ = MatrixXX{Q_diag.asDiagonal()}; + R_ = MatrixUU{params_.gains.R}; // this is part of controller_interface definition, and not defined in GPL params_ // if not set explicitly, it will be the cm_update rate dt_ = 1. / node_->get_parameter("update_rate").as_int(); @@ -309,21 +306,19 @@ void CartpoleLqrTrajectoryPlugin::start() /* ----- private functions ----- */ void CartpoleLqrTrajectoryPlugin::calcLQR_steady( - Eigen::Vector x, Eigen::Vector u, double dt, - Eigen::Matrix Q, Eigen::Matrix R, - Eigen::Matrix N, Eigen::Matrix & Ks, - Eigen::Matrix & Ps) + const VectorX x, const VectorU u, const double dt, const MatrixXX Q, const MatrixUU R, + const MatrixUX N, MatrixUX & Ks, MatrixXX & Ps) const { - Eigen::Matrix Phi; - Eigen::Matrix Gamma; - Eigen::Matrix C; + MatrixXX Phi; + MatrixXU Gamma; + MatrixXX C; C.setIdentity(); get_linear_system_matrices(x, u, dt, Phi, Gamma); // solve steady-state Riccati equation iteratively - Eigen::Matrix P, P_new; - Eigen::Matrix K; + MatrixXX P, P_new; + MatrixUX K; P = P.setIdentity() * 1e5; // initial value of P for (int ct = 0; ct < 1e4; ++ct) @@ -343,20 +338,15 @@ void CartpoleLqrTrajectoryPlugin::calcLQR_steady( } void CartpoleLqrTrajectoryPlugin::riccati_step( - Eigen::Matrix & K, Eigen::Matrix & P_new, - const Eigen::Matrix & P, - const Eigen::Matrix & Phi, - const Eigen::Matrix & Gamma, - const Eigen::Matrix & Q, const Eigen::Matrix & R, - const Eigen::Matrix & N) + MatrixUX & K, MatrixXX & P_new, const MatrixXX & P, const MatrixXX & Phi, const MatrixXU & Gamma, + const MatrixXX & Q, const MatrixUU & R, const MatrixUX & N) const { K = -(R + Gamma.transpose() * P * Gamma).inverse() * (N + Gamma.transpose() * P * Phi); P_new = (Q + Phi.transpose() * P * Phi) + (N + Gamma.transpose() * P * Phi).transpose() * K; } void CartpoleLqrTrajectoryPlugin::get_linear_system_matrices( - Eigen::Vector x, Eigen::Vector u, const double dt, - Eigen::Matrix & Phi, Eigen::Matrix & Gamma) + const VectorX x, const VectorU u, const double dt, MatrixXX & Phi, MatrixXU & Gamma) const { // states and input double theta = x[0]; @@ -369,18 +359,17 @@ void CartpoleLqrTrajectoryPlugin::get_linear_system_matrices( double IzzS = params_.system.IzzS; // Jacobians of continuous-time dynamics - Eigen::Matrix J_x = - Eigen::Matrix::Zero(); + MatrixXX J_x = MatrixXX::Zero(); J_x(0, 1) = 1; J_x(1, 0) = -0.2e1 * lS * mS * (g * cos(theta) - sin(theta) * x_ddot) / (lS * lS * mS + 4 * IzzS); J_x(2, 3) = 1; - Eigen::Matrix J_u = Eigen::Matrix::Zero(); + MatrixXU J_u = MatrixXU::Zero(); J_u(1, 0) = -0.2e1 * lS * mS * cos(theta) / (lS * lS * mS + 4 * IzzS); J_u(3, 0) = 1; // discrete-time linearized system matrices - Phi = Eigen::Matrix::Identity() + dt * J_x; + Phi = MatrixXX::Identity() + dt * J_x; Gamma = dt * J_u; } diff --git a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp index 2b54e9012..8e9ef809a 100644 --- a/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp +++ b/example_XX/controller/include/ros2_control_demo_example_xx/cartpole_lqr_trajectory_plugin.hpp @@ -26,7 +26,15 @@ // shortcut namespace ros2_control_demo_example_xx { +// shortcut #define NUM_STATES 4 +#define NUM_INPUTS 1 +typedef Eigen::Matrix MatrixUX; +typedef Eigen::Matrix MatrixXU; +typedef Eigen::Matrix MatrixXX; +typedef Eigen::Matrix MatrixUU; +typedef Eigen::Vector VectorX; +typedef Eigen::Vector VectorU; double get_time_from_duration(const builtin_interfaces::msg::Duration & time_from_start) { @@ -34,22 +42,18 @@ double get_time_from_duration(const builtin_interfaces::msg::Duration & time_fro }; // Make P symmetric -auto make_symmetric(Eigen::Matrix & P) -{ - return 0.5 * (P + P.transpose()); -} +auto make_symmetric(MatrixXX & P) { return 0.5 * (P + P.transpose()); } class TrajectoryLQR { public: - std::vector> K_vec_; + std::vector K_vec_; std::vector time_vec_; /** * @brief sample the feedback gain at the given time */ - Eigen::Matrix get_feedback_gain( - const rclcpp::Duration & duration_since_start); + MatrixUX get_feedback_gain(const rclcpp::Duration & duration_since_start) const; /** * @brief resets internal storage @@ -63,12 +67,12 @@ class TrajectoryLQR /** * @brief returns true if the trajectory is empty */ - bool is_empty() { return K_vec_.empty(); } + bool is_empty() const { return K_vec_.empty(); } /** * @brief print the gains on std::cout */ - void print() + void print() const { for (size_t i = 0; i < K_vec_.size(); ++i) { @@ -117,21 +121,13 @@ class CartpoleLqrTrajectoryPlugin void parseGains(); void get_linear_system_matrices( - Eigen::Vector x, Eigen::Vector u, const double dt, - Eigen::Matrix & Phi, - Eigen::Matrix & Gamma); + const VectorX x, const VectorU u, const double dt, MatrixXX & Phi, MatrixXU & Gamma) const; void calcLQR_steady( - Eigen::Vector q, Eigen::Vector u, double dt, - Eigen::Matrix Q, Eigen::Matrix R, - Eigen::Matrix N, Eigen::Matrix & Ks, - Eigen::Matrix & Ps); + const VectorX q, const VectorU u, const double dt, const MatrixXX Q, const MatrixUU R, + const MatrixUX N, MatrixUX & Ks, MatrixXX & Ps) const; void riccati_step( - Eigen::Matrix & K, Eigen::Matrix & P_new, - const Eigen::Matrix & P, - const Eigen::Matrix & Phi, - const Eigen::Matrix & Gamma, - const Eigen::Matrix & Q, const Eigen::Matrix & R, - const Eigen::Matrix & N); + MatrixUX & K, MatrixXX & P_new, const MatrixXX & P, const MatrixXX & Phi, + const MatrixXU & Gamma, const MatrixXX & Q, const MatrixUU & R, const MatrixUX & N) const; // integrator state for system input/LQR output double u_; @@ -147,9 +143,9 @@ class CartpoleLqrTrajectoryPlugin // sampling time of the controller double dt_ = 0.01; // LQR cost function parameter for states - Eigen::Matrix Q_; + MatrixXX Q_; // LQR cost function parameter for input - Eigen::Matrix R_; + MatrixUU R_; // storage for LQR gains std::shared_ptr trajectory_active_lqr_ptr_;