From 3887f7d2d4562776972fcf28f7aed900cbb9345b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Th=C3=A9o=20Martinez?= Date: Wed, 4 Dec 2024 14:17:19 +0000 Subject: [PATCH 1/2] add yamls and launch file for demo3 --- agimus_demo_03_mpc_dummy_traj/CMakeLists.txt | 32 +++ .../config/controller_manager.yaml | 15 + .../config/joint_state_estimator.yaml | 27 ++ .../config/linear_feedback_controller.yaml | 40 +++ .../config/mpc_params.yaml | 17 ++ .../dependencies.repos | 35 +++ .../launch/bringup.launch.py | 270 ++++++++++++++++++ agimus_demo_03_mpc_dummy_traj/package.xml | 41 +++ 8 files changed, 477 insertions(+) create mode 100644 agimus_demo_03_mpc_dummy_traj/CMakeLists.txt create mode 100644 agimus_demo_03_mpc_dummy_traj/config/controller_manager.yaml create mode 100644 agimus_demo_03_mpc_dummy_traj/config/joint_state_estimator.yaml create mode 100644 agimus_demo_03_mpc_dummy_traj/config/linear_feedback_controller.yaml create mode 100644 agimus_demo_03_mpc_dummy_traj/config/mpc_params.yaml create mode 100644 agimus_demo_03_mpc_dummy_traj/dependencies.repos create mode 100644 agimus_demo_03_mpc_dummy_traj/launch/bringup.launch.py create mode 100644 agimus_demo_03_mpc_dummy_traj/package.xml diff --git a/agimus_demo_03_mpc_dummy_traj/CMakeLists.txt b/agimus_demo_03_mpc_dummy_traj/CMakeLists.txt new file mode 100644 index 0000000..6c8179f --- /dev/null +++ b/agimus_demo_03_mpc_dummy_traj/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.22.1) + +# +# Project definition +# +project(agimus_demo_03_mpc_dummy_traj LANGUAGES CXX) + + +# +# Handle dependencies by reading the package.xml +# +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# +# Unit tests +# +include(CTest) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_auto_find_test_dependencies() + # Integration test of the roscontrol controller with simulation on Talos. + # add_rostest(tests/test_integration.py) +endif() + +# +# Installation +# +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + +ament_auto_package() \ No newline at end of file diff --git a/agimus_demo_03_mpc_dummy_traj/config/controller_manager.yaml b/agimus_demo_03_mpc_dummy_traj/config/controller_manager.yaml new file mode 100644 index 0000000..e7244bf --- /dev/null +++ b/agimus_demo_03_mpc_dummy_traj/config/controller_manager.yaml @@ -0,0 +1,15 @@ +controller_manager: + ros__parameters: + update_rate: 1000 # Hz + + linear_feedback_controller: + type: linear_feedback_controller/LinearFeedbackController + + joint_state_estimator: + type: linear_feedback_controller/JointStateEstimator + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + franka_robot_state_broadcaster: + type: franka_robot_state_broadcaster/FrankaRobotStateBroadcaster \ No newline at end of file diff --git a/agimus_demo_03_mpc_dummy_traj/config/joint_state_estimator.yaml b/agimus_demo_03_mpc_dummy_traj/config/joint_state_estimator.yaml new file mode 100644 index 0000000..bd3eaa8 --- /dev/null +++ b/agimus_demo_03_mpc_dummy_traj/config/joint_state_estimator.yaml @@ -0,0 +1,27 @@ +joint_state_estimator: + ros__parameters: + type: linear_feedback_controller/JointStateEstimator + interfaces: [ + 'fer_joint1/position', + 'fer_joint2/position', + 'fer_joint3/position', + 'fer_joint4/position', + 'fer_joint5/position', + 'fer_joint6/position', + 'fer_joint7/position', + 'fer_joint1/velocity', + 'fer_joint2/velocity', + 'fer_joint3/velocity', + 'fer_joint4/velocity', + 'fer_joint5/velocity', + 'fer_joint6/velocity', + 'fer_joint7/velocity', + 'fer_joint1/effort', + 'fer_joint2/effort', + 'fer_joint3/effort', + 'fer_joint4/effort', + 'fer_joint5/effort', + 'fer_joint6/effort', + 'fer_joint7/effort' + ] + command_prefix: "linear_feedback_controller" diff --git a/agimus_demo_03_mpc_dummy_traj/config/linear_feedback_controller.yaml b/agimus_demo_03_mpc_dummy_traj/config/linear_feedback_controller.yaml new file mode 100644 index 0000000..9877c92 --- /dev/null +++ b/agimus_demo_03_mpc_dummy_traj/config/linear_feedback_controller.yaml @@ -0,0 +1,40 @@ +linear_feedback_controller: + ros__parameters: + type: linear_feedback_controller/LinearFeedbackController + moving_joint_names: [ + 'fer_joint1', + 'fer_joint2', + 'fer_joint3', + 'fer_joint4', + 'fer_joint5', + 'fer_joint6', + 'fer_joint7' + ] + fer_joint1: + p: 60.0 + d: 5.0 + fer_joint2: + p: 60.0 + d: 5.0 + fer_joint3: + p: 60.0 + d: 5.0 + fer_joint4: + p: 60.0 + d: 2.0 + fer_joint5: + p: 20.0 + d: 2.0 + fer_joint6: + p: 10.0 + d: 2.0 + fer_joint7: + p: 5.0 + d: 1.0 + chainable_controller: + command_prefix: '' + reference_prefix: '' + joint_velocity_filter_coefficient: 0.9 + pd_to_lf_transition_duration: 0.1 + remove_gravity_compensation_effort: true + robot_has_free_flyer: false \ No newline at end of file diff --git a/agimus_demo_03_mpc_dummy_traj/config/mpc_params.yaml b/agimus_demo_03_mpc_dummy_traj/config/mpc_params.yaml new file mode 100644 index 0000000..9ad452e --- /dev/null +++ b/agimus_demo_03_mpc_dummy_traj/config/mpc_params.yaml @@ -0,0 +1,17 @@ +ocp: + dt: 0.01 + horizon_size: 20 + armature: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + gripper_weight: 500 + state_weight: 5 + control_weight: 0.001 + max_qp_iter: 100 + max_iter: 10 + use_constraints: True + activate_callback: False + increasing_weights: + max: 100.0 + percent: 0.9 + time_reach_percent: 6 +save_predictions_and_refs: True +rate: 100 diff --git a/agimus_demo_03_mpc_dummy_traj/dependencies.repos b/agimus_demo_03_mpc_dummy_traj/dependencies.repos new file mode 100644 index 0000000..89f5a1e --- /dev/null +++ b/agimus_demo_03_mpc_dummy_traj/dependencies.repos @@ -0,0 +1,35 @@ +repositories: + vcs_repos/linear_feedback_controller_msgs: + type: git + url: https://github.com/loco-3d/linear-feedback-controller-msgs + version: 0.1.3 + + vcs_repos/coal: + type: git + url: https://github.com/coal-library/coal + version: v3.0.0 + + vcs_repos/pinocchio: + type: git + url: https://github.com/stack-of-tasks/pinocchio + version: v3.3.0 + + vcs_repos/colmpc: + type: git + url: https://github.com/agimus-project/colmpc + version: v0.2.0 # TODO: change to new version tag + + vcs_repos/mim_solvers: + type: git + url: https://github.com/machines-in-motion/mim_solvers + version: v0.0.3 + + vcs_repos/franka_ros: + type: git + url: https://github.com/frankaemika/franka_ros + version: develop + + vcs_repos/agimus_controller: + type: git + url: https://github.com/agimus-project/agimus_controller + version: humble-devel diff --git a/agimus_demo_03_mpc_dummy_traj/launch/bringup.launch.py b/agimus_demo_03_mpc_dummy_traj/launch/bringup.launch.py new file mode 100644 index 0000000..3cd0fe8 --- /dev/null +++ b/agimus_demo_03_mpc_dummy_traj/launch/bringup.launch.py @@ -0,0 +1,270 @@ +# Copyright (c) 2024 Franka Robotics GmbH +# +# 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 pathlib import Path +import os +import xacro + +from ament_index_python.packages import get_package_share_directory + +from controller_manager.launch_utils import generate_load_controller_launch_description + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + OpaqueFunction, + ExecuteProcess, + RegisterEventHandler, +) +from launch.event_handlers import OnProcessExit + +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, + LaunchConfiguration, +) +from launch import LaunchContext, LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import AppendEnvironmentVariable +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def get_robot_description(context: LaunchContext, arm_id, load_gripper, franka_hand): + arm_id_str = context.perform_substitution(arm_id) + load_gripper_str = context.perform_substitution(load_gripper) + franka_hand_str = context.perform_substitution(franka_hand) + + franka_xacro_file = os.path.join( + get_package_share_directory("franka_description"), + "robots", + arm_id_str, + arm_id_str + ".urdf.xacro", + ) + + robot_description_config = xacro.process_file( + franka_xacro_file, + mappings={ + "arm_id": arm_id_str, + "hand": load_gripper_str, + "ros2_control": "true", + "gazebo": "true", + "ee_id": franka_hand_str, + "gazebo_effort": "true", + }, + ) + robot_description = {"robot_description": robot_description_config.toxml()} + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[ + robot_description, + ], + ) + + return [robot_state_publisher] + + +def prepare_launch_description(): + # Configure ROS nodes for launch + load_gripper_name = "load_gripper" + franka_hand_name = "franka_hand" + arm_id_name = "arm_id" + package_name = "agimus_demo_03_mpc_dummy_traj" + + load_gripper = LaunchConfiguration(load_gripper_name) + franka_hand = LaunchConfiguration(franka_hand_name) + arm_id = LaunchConfiguration(arm_id_name) + + load_gripper_launch_argument = DeclareLaunchArgument( + load_gripper_name, + default_value="false", + description="true/false for activating the gripper", + ) + franka_hand_launch_argument = DeclareLaunchArgument( + franka_hand_name, + default_value="franka_hand", + description="Default value: franka_hand", + ) + arm_id_launch_argument = DeclareLaunchArgument( + arm_id_name, + default_value="fer", + description="Available values: fr3, fp3 and fer", + ) + + # Get robot description + robot_state_publisher = OpaqueFunction( + function=get_robot_description, args=[arm_id, load_gripper, franka_hand] + ) + + # Gazebo Sim + pkg_ros_gz_sim = get_package_share_directory("ros_gz_sim") + gz_verbose = "" # ' -v 3' + gazebo_empty_world = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ros_gz_sim, "launch", "gz_sim.launch.py") + ), + launch_arguments={ + "gz_args": "empty.sdf -r" + gz_verbose, + }.items(), + ) + + # Spawn + spawn = Node( + package="ros_gz_sim", + executable="create", + arguments=["-topic", "/robot_description"], + output="screen", + ) + + # Visualize in RViz + rviz_file = os.path.join( + get_package_share_directory("franka_description"), + "rviz", + "visualize_franka.rviz", + ) + rviz = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["--display-config", rviz_file, "-f", "world"], + ) + + load_joint_state_broadcaster = ExecuteProcess( + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + "active", + "joint_state_broadcaster", + ], + output="screen", + ) + + joint_state_estimator_params = PathJoinSubstitution( + [ + FindPackageShare(package_name), + "config", + "joint_state_estimator.yaml", + ] + ) + load_joint_state_estimator = generate_load_controller_launch_description( + controller_name="joint_state_estimator", + controller_params_file=joint_state_estimator_params, + ) + + linear_feedback_controller_params = PathJoinSubstitution( + [ + FindPackageShare(package_name), + "config", + "linear_feedback_controller.yaml", + ] + ) + load_linear_feedback_controller = generate_load_controller_launch_description( + controller_name="linear_feedback_controller", + controller_params_file=linear_feedback_controller_params, + ) + + # PD plus + pd_plus_controller_yaml = str( + Path(get_package_share_directory(package_name)) + / "config" + / "pd_plus_controller.yaml" + ) + pd_plus_controller = Node( + package="linear_feedback_controller", + executable="pd_plus_controller", + output="screen", + parameters=[pd_plus_controller_yaml], + ) + + # MPC + mpc_params_yaml = str( + Path(get_package_share_directory(package_name)) / "config" / "mpc_params.yaml" + ) + mpc_controller = Node( + package="agimus_controller_ros", + executable="agimus_controller_node", + name="agimus_controller_node", + output="screen", + parameters=[mpc_params_yaml], + ) + + return LaunchDescription( + [ + load_gripper_launch_argument, + franka_hand_launch_argument, + arm_id_launch_argument, + gazebo_empty_world, + robot_state_publisher, + rviz, + spawn, + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_linear_feedback_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_linear_feedback_controller.entities[-1], + on_exit=[load_joint_state_estimator], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_estimator.entities[-1], + on_exit=[mpc_controller], + ) + ), + Node( + package="joint_state_publisher", + executable="joint_state_publisher", + name="joint_state_publisher", + parameters=[{"source_list": ["joint_states"], "rate": 30}], + ), + Node( + package="agimus_controller_ros", + executable="mpc_input_dummy_publisher", + name="mpc_input_dummy_publisher", + output="screen", + ), + ] + ) + + +def generate_launch_description(): + launch_description = prepare_launch_description() + + set_env_vars_resources = AppendEnvironmentVariable( + "GZ_SIM_RESOURCE_PATH", + os.path.join(get_package_share_directory("franka_description")), + ) + + launch_description.add_action(set_env_vars_resources) + + return launch_description diff --git a/agimus_demo_03_mpc_dummy_traj/package.xml b/agimus_demo_03_mpc_dummy_traj/package.xml new file mode 100644 index 0000000..a62204e --- /dev/null +++ b/agimus_demo_03_mpc_dummy_traj/package.xml @@ -0,0 +1,41 @@ + + + agimus_demo_03_mpc_dummy_traj + 0.0.0 + + agimus_demo_03_mpc_dummy_traj contains the entry points for a simple Panda demo with LFC and + a MPC sending commands while following a dummy trajectory. + + Maximilien Naveau + Maximilien Naveau + + BSD-2 + https://www.agimus-project.eu/ + https://github.com/agimus-project/agimus-demos + https://github.com/agimus-project/agimus-demos/issues + ament_cmake_auto + + + + + ros2launch + + + + + ament_cmake + + + \ No newline at end of file From f25be50cdb5ad9c24db925ad249f2ee68a245f81 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Th=C3=A9o=20Martinez?= Date: Wed, 11 Dec 2024 13:52:16 +0000 Subject: [PATCH 2/2] fix mpc yaml, remove pd plus code in launch file --- .../config/mpc_params.yaml | 49 ++++++++++++------- .../launch/bringup.launch.py | 13 ----- 2 files changed, 32 insertions(+), 30 deletions(-) diff --git a/agimus_demo_03_mpc_dummy_traj/config/mpc_params.yaml b/agimus_demo_03_mpc_dummy_traj/config/mpc_params.yaml index 9ad452e..585a767 100644 --- a/agimus_demo_03_mpc_dummy_traj/config/mpc_params.yaml +++ b/agimus_demo_03_mpc_dummy_traj/config/mpc_params.yaml @@ -1,17 +1,32 @@ -ocp: - dt: 0.01 - horizon_size: 20 - armature: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] - gripper_weight: 500 - state_weight: 5 - control_weight: 0.001 - max_qp_iter: 100 - max_iter: 10 - use_constraints: True - activate_callback: False - increasing_weights: - max: 100.0 - percent: 0.9 - time_reach_percent: 6 -save_predictions_and_refs: True -rate: 100 +agimus_controller_node: + ros__parameters: + ocp: + dt: 0.01 + horizon_size: 20 + armature: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + gripper_weight: 500 + state_weight: 5 + control_weight: 0.001 + max_qp_iter: 100 + max_iter: 10 + use_constraints: True + activate_callback: False + increasing_weights: + max: 100.0 + percent: 0.9 + time_reach_percent: 6 + effector_frame_name: "fer_link8" + moving_joint_names: + [ + "fer_joint1", + "fer_joint2", + "fer_joint3", + "fer_joint4", + "fer_joint5", + "fer_joint6", + "fer_joint7", + ] + save_predictions_and_refs: True + rate: 100 + package_name: "agimus_demo_03_mpc_dummy_traj" + path_to_yaml: "config/mpc_params.yaml" diff --git a/agimus_demo_03_mpc_dummy_traj/launch/bringup.launch.py b/agimus_demo_03_mpc_dummy_traj/launch/bringup.launch.py index 3cd0fe8..7237a47 100644 --- a/agimus_demo_03_mpc_dummy_traj/launch/bringup.launch.py +++ b/agimus_demo_03_mpc_dummy_traj/launch/bringup.launch.py @@ -183,19 +183,6 @@ def prepare_launch_description(): controller_params_file=linear_feedback_controller_params, ) - # PD plus - pd_plus_controller_yaml = str( - Path(get_package_share_directory(package_name)) - / "config" - / "pd_plus_controller.yaml" - ) - pd_plus_controller = Node( - package="linear_feedback_controller", - executable="pd_plus_controller", - output="screen", - parameters=[pd_plus_controller_yaml], - ) - # MPC mpc_params_yaml = str( Path(get_package_share_directory(package_name)) / "config" / "mpc_params.yaml"