Feature/cartesian mpc controller

README.md

@@ -1,26 +1,91 @@
-This repository illustrates the use of Croccodyl and pinocchio on a mobile base, here a PAL Tiago robot.
+This repository illustrates the use of **Crocoddyl** and **Pinocchio** for optimal control on mobile manipulators, demonstrated on a PAL Robotics Tiago robot.
 The version considered here is a differential mobile base equipped with a torso and a seven DoFs arm.
 
-Dependencies
---------------
+## Packages descriptions
+### tiago_lfc_bringup
+Provides environment setup to launch Tiago in Gazebo simulation. Also includes useful launch files for controller management.
+
+## tiago_simple_mpc
+MPC implementation example using Crocoddyl for Tiago control.
+
+Two nodes are available once simulation is running:
+- `test_ocp_reaching.py` - Visualizes OCP solutions offline (uses meshcat)
+![OPC Demo](tiago_simple_mpc/media/cartesian_OCP2.gif)
+- `cartesian_target_mpc_node.py` - Runs MPC in closed-loop with Gazebo
+
+## Installation
+
+Create workspace and clone repository
+
+```bash
+mkdir -p tiago_ws/src
+cd tiago_ws/src
+git clone git@github.com:Gepetto/gep_tiago_control_stack.git
+vcs import . < tiago_control_stack/tiago_lfc_bringup/dependencies/lfc_dependencies.repos
+vcs import . < tiago_control_stack/tiago_lfc_bringup/dependencies/tiago_dependencies.repos
+cd ..
+rosdep update
+rosdep install --from-paths src --ignore-src -r -y
+
+# install crocoddyl
+export MAKEFLAGS="-j2"
+colcon build --packages-up-to crocoddyl --event-handlers console_direct+ --cmake-args -DBUILD_PYTHON_BINDINGS=ON
+
+# install linear feedback controller
+colcon build --symlink-install \
+    --cmake-args -DBUILD_TESTING=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_DISABLE_FIND_PACKAGE_Doxygen=ON \
+    --packages-select tiago_lfc \
+                      linear_feedback_controller_msgs \
+                      linear_feedback_controller
+
+# install all other packages
+colcon build --symlink-install
+```
+
+## Usage
+
+### Start Tiago, simulated in Gazebo Harmonic and add lfc controllers
+
+You can use simple launch file to set up environment.
+Useful launch files can be found under directory tiago_lfc/launch
+
+```bash
+# launch tiago in gazebo
+ros2 launch tiago_lfc tiago_gazebo.launch.py world_name:=house # or empty
+# Load lfc controllers
+ros2 launch tiago_lfc switch_to_lfc_controllers.launch.py
+```
+
+> [!note]
+> 3 controllers are packaged together and are in chained mode.
+> hardware -> Joint-state-estimator -> Linear-feedback-controller -> Passthrough-controller -> hardware
+
+### Use high level launch file
+
+Concreate application can be found in launch folder at the root of the package.
+
+|**Launch file**                  | **Description**                                                                                     |
+|---------------------------------|---------------------------------------------------------------------------------------------------|
+|`pd_plus_controller.launch.py`   |Use pd plus exemple found in lfc package.
+
+
+
+## Dependencies
 
 gep_tiago_control_stack is based on the Tiago harmonic project developed by
 Juan Carlos Manzanares Serrano, Francisco Martín Rico, and Juan Sebastían Cely Gutiérrez.
 We are very thankful to them for providing this open source simulation based on Gazebo Harmonic.
 
 gep_tiago_control_stack is also using heavily ros2_control, and we also thanks the ros2_control community for their effort.
 
-License
----------
+## License
 
 This software is distributed under the terms of both the MIT license and the Apache License (Version 2.0).
 
 See LICENSE-APACHE and LICENSE-MIT for details.
 
-Contributors
----------------
+## Contributors
 Clément Pène
 
-Funding Agency
---------------
+## Funding Agency
 Défi-Clef Robotique Centrée sur l'humain

odometry_gz_hw_interface/include/odometry_gz_hw_interface/odometry_hardware.hpp

@@ -37,7 +37,7 @@ class OdometryHardware : public gz_ros2_control::GazeboSimSystemInterface {
  private:
   void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
 
-  std::string topic_name_ = "/mobile_base_controller/odom";
+  std::string topic_name_ = "/odom_mpc";
 
   rclcpp::Node::SharedPtr nh_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;

odometry_gz_hw_interface/package.xml

@@ -4,8 +4,8 @@
   <version>0.0.1</version>
   <description>Custom gz_ros2_control GazeboSimSystemInterface reading an Odometry topic.</description>
 
-  <maintainer email="you@example.com">Your Name</maintainer>
-  <license>Apache-2.0</license>
+  <maintainer email="pene;clement@gmail.com">Clément Pène</maintainer>
+  <license>MIT OR Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

odometry_hardware_interface/package.xml

@@ -3,9 +3,9 @@
 <package format="3">
   <name>odometry_hardware_interface</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="pene.clement@gmail.com">cpene</maintainer>
-  <license>TODO: License declaration</license>
+  <description>Custom hardware interface reading an Odometry topic.</description>
+  <maintainer email="pene.clement@gmail.com">Clément Pène</maintainer>
+  <license>MIT OR Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

tiago_agimus_mpc/package.xml

@@ -3,11 +3,14 @@
 <package format="3">
   <name>tiago_agimus_mpc</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Agimus MPC integration for Tiago robot</description>
   <maintainer email="pene.clement@gmail.com">cpene</maintainer>
   <license>TODO: License declaration</license>
 
   <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>agimus_controller_ros</depend>
+  <depend>tiago_lfc_bringup</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

tiago_agimus_mpc/setup.py

@@ -1,27 +1,34 @@
 from setuptools import find_packages, setup
+from glob import glob
+import os
 
-package_name = "tiago_agimus_mpc"
+package_name = 'tiago_agimus_mpc'
 
 setup(
     name=package_name,
-    version="0.0.0",
-    packages=find_packages(exclude=["test"]),
+    version='0.1.0',
+    packages=find_packages(exclude=['test']),
     data_files=[
-        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
-        ("share/" + package_name, ["package.xml"]),
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        # Launch files
+        (os.path.join('share', package_name, 'launch'), 
+            glob('launch/*.launch.py')),
+        # Config files
+        (os.path.join('share', package_name, 'config'), 
+            glob('config/*.yaml')),
     ],
-    install_requires=["setuptools"],
+    install_requires=['setuptools'],
     zip_safe=True,
-    maintainer="cpene",
-    maintainer_email="pene.clement@gmail.com",
-    description="TODO: Package description",
-    license="TODO: License declaration",
-    extras_require={
-        "test": [
-            "pytest",
-        ],
-    },
+    maintainer='Clement Pene',
+    maintainer_email='pene.clement@gmail.com',
+    description='Agimus MPC integration for Tiago',
+    license='MIT OR Apache-2.0',
+    tests_require=['pytest'],
     entry_points={
-        "console_scripts": [],
+        'console_scripts': [
+            'test_agimus_reaching = tiago_agimus_mpc.nodes.test_agimus_reaching:main',
+        ],
     },
 )

tiago_lfc_bringup/config/fixed/dummy_controllers.yaml

@@ -0,0 +1,23 @@
+---
+passthrough_controller:
+  ros__parameters:
+    type: linear_feedback_controller/PassthroughController
+    command_interfaces: [
+      torso_lift_joint/effort,
+      arm_1_joint/effort,
+      arm_2_joint/effort,
+      arm_3_joint/effort,
+      arm_4_joint/effort,
+      arm_5_joint/effort,
+      arm_6_joint/effort,
+      arm_7_joint/effort]
+    reference_interfaces: [
+      torso_lift_joint/effort,
+      arm_1_joint/effort,
+      arm_2_joint/effort,
+      arm_3_joint/effort,
+      arm_4_joint/effort,
+      arm_5_joint/effort,
+      arm_6_joint/effort,
+      arm_7_joint/effort
+    ]

tiago_lfc_bringup/config/fixed/joint_state_estimator_params.yaml

@@ -0,0 +1,61 @@
+---
+joint_state_estimator:
+  ros__parameters:
+    type: linear_feedback_controller/JointStateEstimator
+    state_interfaces: [
+      torso_lift_joint/position,
+      arm_1_joint/position,
+      arm_2_joint/position,
+      arm_3_joint/position,
+      arm_4_joint/position,
+      arm_5_joint/position,
+      arm_6_joint/position,
+      arm_7_joint/position,
+
+      torso_lift_joint/velocity,
+      arm_1_joint/velocity,
+      arm_2_joint/velocity,
+      arm_3_joint/velocity,
+      arm_4_joint/velocity,
+      arm_5_joint/velocity,
+      arm_6_joint/velocity,
+      arm_7_joint/velocity,
+
+      torso_lift_joint/effort,
+      arm_1_joint/effort,
+      arm_2_joint/effort,
+      arm_3_joint/effort,
+      arm_4_joint/effort,
+      arm_5_joint/effort,
+      arm_6_joint/effort,
+      arm_7_joint/effort
+      ]
+    command_interfaces: [
+
+      linear_feedback_controller/torso_lift_joint/position,
+      linear_feedback_controller/arm_1_joint/position,
+      linear_feedback_controller/arm_2_joint/position,
+      linear_feedback_controller/arm_3_joint/position,
+      linear_feedback_controller/arm_4_joint/position,
+      linear_feedback_controller/arm_5_joint/position,
+      linear_feedback_controller/arm_6_joint/position,
+      linear_feedback_controller/arm_7_joint/position,
+
+      linear_feedback_controller/torso_lift_joint/velocity,
+      linear_feedback_controller/arm_1_joint/velocity,
+      linear_feedback_controller/arm_2_joint/velocity,
+      linear_feedback_controller/arm_3_joint/velocity,
+      linear_feedback_controller/arm_4_joint/velocity,
+      linear_feedback_controller/arm_5_joint/velocity,
+      linear_feedback_controller/arm_6_joint/velocity,
+      linear_feedback_controller/arm_7_joint/velocity,
+
+      linear_feedback_controller/torso_lift_joint/effort,
+      linear_feedback_controller/arm_1_joint/effort,
+      linear_feedback_controller/arm_2_joint/effort,
+      linear_feedback_controller/arm_3_joint/effort,
+      linear_feedback_controller/arm_4_joint/effort,
+      linear_feedback_controller/arm_5_joint/effort,
+      linear_feedback_controller/arm_6_joint/effort,
+      linear_feedback_controller/arm_7_joint/effort
+      ]

tiago_lfc_bringup/config/fixed/linear_feedback_controller_params.yaml

@@ -0,0 +1,51 @@
+linear_feedback_controller:
+  ros__parameters:
+    type: linear_feedback_controller/LinearFeedbackController
+    chainable_controller:
+      command_interfaces:
+        - passthrough_controller/torso_lift_joint/effort
+        - passthrough_controller/arm_1_joint/effort
+        - passthrough_controller/arm_2_joint/effort
+        - passthrough_controller/arm_3_joint/effort
+        - passthrough_controller/arm_4_joint/effort
+        - passthrough_controller/arm_5_joint/effort
+        - passthrough_controller/arm_6_joint/effort
+        - passthrough_controller/arm_7_joint/effort
+    reference_prefix: linear_feedback_controller
+    moving_joint_names:
+      - torso_lift_joint
+      - arm_1_joint
+      - arm_2_joint
+      - arm_3_joint
+      - arm_4_joint
+      - arm_5_joint
+      - arm_6_joint
+      - arm_7_joint
+    torso_lift_joint:
+      p: 50.0
+      d: 0.5
+    arm_1_joint:
+      p: 50.0
+      d: 0.5
+    arm_2_joint:
+      p: 50.0
+      d: 0.5
+    arm_3_joint:
+      p: 50.0
+      d: 0.5
+    arm_4_joint:
+      p: 50.0
+      d: 0.5
+    arm_5_joint:
+      p: 10.0
+      d: 0.3
+    arm_6_joint:
+      p: 5.0
+      d: 0.3
+    arm_7_joint:
+      p: 5.0
+      d: 0.3
+    robot_has_free_flyer: false
+    remove_gravity_compensation_effort: false
+    joint_velocity_filter_coefficient: 0.9
+    pd_to_lf_transition_duration: 0.5

tiago_lfc_bringup/config/dummy_controllers.yaml → tiago_lfc_bringup/config/free_flyer/dummy_controllers.yaml

@@ -4,7 +4,7 @@ passthrough_controller:
     type: linear_feedback_controller/PassthroughController
     command_interfaces: [
       wheel_left_joint/effort,
-      # wheel_right_joint/effort,
+      wheel_right_joint/effort,
       # torso_lift_joint/effort,
       arm_1_joint/effort,
       arm_2_joint/effort,
@@ -15,7 +15,7 @@ passthrough_controller:
       arm_7_joint/effort]
     reference_interfaces: [
       wheel_left_joint/effort,
-      # wheel_right_joint/effort,
+      wheel_right_joint/effort,
       # torso_lift_joint/effort,
       arm_1_joint/effort,
       arm_2_joint/effort,