LQR local planner

Description

This package can be used as controller plugin for ros2 navigation stack

For this moment you can use

at this file to build trajectory using robot kinematic model. More you can find follow this link. This is good article about that

Iterative LQR

is about and how it works.

Also You can use

with robot model for following the given trajectory using kinematic and dynamic models. So you implement your own robot model( more read this)

Write and Load new plugin

Plugins

All classes:

• Trajectory Following (
  LQR
  )
• Trajectory Creator (
  ILQR
  )
• Robot model (
  DiffNoSpeedModel
  )

you can load dynamically. Read more about pluginlib

About PluginLib

You can load using pluginlib. Here an example of loading robot model for differential drive without controll of his speed:

Write your plugin

Write one of the plugins from section of plugins and create new library in CMakeLists.txt

Choose your plugin type and write a new

file with your plugin and put it in CMakeLists.txt like code bellow

LQR

We using discrete time for infinite space to follow the given trajectory. We minimizing the given error: difference between our state and state in which we want to be after some time

auto new_speed = - K * error;

How find

matrix:

Where

- solution of Riccati equation

LQR plugins

For now I have 2

plugins:

• LQR ( without openACC optimization. Class written using
  Eigen
  )
• LQR_ACC ( with openacc optimization. Class written using
  boost::numeric::ublas::matrix<double>
  )

Boost speed about 40-50 times ( Without using `Eigen::initParallel ). With using openmp as backend of Eigen boost is about 7.5-8.5 times. In some cases over 15 times.

Preparation step (build:

# source /opt/ros...
cmake -B cmake-build-debug;
cmake --build cmake-build-debug -j 4

You can compare it using (

):

Configuration file

He placed in plugin directory

Architecture

Ros Plugin loading steps

See graph online on Mermaid

Following Node Classes

Trajectory Node Classes

Just go to Doxygen Docs at this file