echandler5956f / galileo

A light-weight and extensible C++ library for Pseudospectral Collocation of Switched Systems

Galileo

A light-weight and extensible C++ library for Pseudospectral Collocation of Switched Systems using CasADi and Pinocchio.

Galileo_Sequence.mp4

Galileo.Results.mp4

See the extended abstract for details!

Named after the famous scientist who posed one variation of the Brachistochrone problem, Galileo is an efficient optimal control framework that uses Gauss-Legendre Pseudospectral Collocation to solve the switched systems problem for legged robots.

Features:

✔️ Intuitive and efficient formulation of variables, cost and constraints using CasADi.

✔️ Solver interface enables using the high-performance solvers Ipopt and SNOPT.

✔️ pinocchio makes custom robot integration as simple as switching the URDF.

✔️ ROS/catkin integration (optional).

✔️ Light-weight framework makes it easy to use and extend.

Install • Run • Visualize • Develop • Contribute • Publications • Credits

Install

The following Linux installation instructions are provided for your convenience:

From source with CMake

1. Install Galileo's mandatory dependencies:

   • CasADi
   • pinocchio
   • Eigen
   • Boost

2. Install Galileo's optional dependencies

   • SNOPT
   • HSL
   • OpenMP
   • gnuplot

CasADi Source Install

First, gather the dependencies listed here: https://github.com/casadi/casadi/wiki/InstallationLinux

Then, clone the repo and make a build folder with

git clone https://github.com/casadi/casadi.git && cd casadi && mkdir build && cd build

Now, run

cmake -DCMAKE_BUILD_TYPE=Release -DWITH_BUILD_IPOPT=ON -DWITH_IPOPT=ON -DWITH_BUILD_METIS=ON -DWITH_MUMPS=ON -DWITH_BUILD_MUMPS=ON  ..

If you have an HSL license (highly recommended, as these solvers tend to speed up convergence by ~2x for our problems, and academics can acquire one for free!), you should manually build the ThirdParty-HSL interface and then add the following CasADi flag:

 -DWITH_HSL=ON

If you want to compile CasADi with OpenMP (recomended), you can add

-DWITH_OPENMP=ON

Similarly, you can add the optional SNOPT interface with

-DWITH_SNOPT=ON

Now, build CasADi from source:

make
sudo make install

Pinocchio Source Install

First, gather the dependencies listed here: https://stack-of-tasks.github.io/pinocchio/download.html

Clone the repo and its submodules:

git clone --recursive https://github.com/stack-of-tasks/pinocchio

Checkout the Pinocchio3 preview branch

cd pinocchio && git checkout pinocchio3-preview

Make a build directory

mkdir build && cd build

Run cmake with CasADi support

cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local -DBUILD_WITH_CASADI_SUPPORT=ON ..

If you are not building the Python interface, you can add the following flag:

-DBUILD_PYTHON_INTERFACE=OFF

and then build and install with

make -j4
sudo make install

Gnuplot Optional Install

Gnuplot is our plotting library of choice. Installation is very straightforward from the package manager with:

sudo apt-get update
sudo apt-get install gnuplot

Galileo Source Install

Once you have the required dependencies for Galileo, clone the repo

git clone https://github.com/echandler5956f/Galileo.git

create a build directory

cd Galileo && mkdir build && cd build

and finally, build and install Galileo with

 cmake -DCMAKE_BUILD_TYPE=Release .. && make && sudo make install

Optionally, you can add the -DBUILD_WITH_OPENMP=ON flag to enable using OpenMP for parallel evaluation of the constraint maps (highly recommended). Note that you must have enabled the OpenMP interface when installing CasADi for this to work.

To uninstall the library, simply run

sudo make uninstall

from within the build directory.

Run

We created a test-installs folder with some simple scripts to test your CasADi + pinocchio install. Be sure to take a look if you are running into trouble. To test the actual library, please refer to the examples folder. If you have already built the repo from source, you can test it by running

build/examples/huron_test

or

build/examples/go1_test

in the main repo directory.

Visualize

The solutions output by Galileo can be easily visualized using our ROS interface.

To run Galileo and visualize a solution using Rviz, run

roslaunch galileo_ros go1_galileo_ros.launch

and in another terminal

roslaunch galileo_ros go1_galileo_ros_rviz.launch

We also support connection with https://github.com/YifuYuan/legged_control. Follow the installation instructions listed, and launch a Galileo ROS legged robot example such as

roslaunch galileo_ros go1_galileo_ros.launch

and then launch the legged control nodes as instructed on the repo's readme. The control schematic for the combined Galileo + perceptive legged_control framework is as follows:

This approach has been used to deploy Galileo in Gazebo and on the real Unitree Go1 hardware.

Documentation

Doxygen

Run

doxygen

in the main directory, and then open the index.html file in the docs folder. If you are using WSL like me, you can run

cd docs/html && explorer.exe index.html

to view the doxygen output.

Develop

Right now, our main development goal is to get the framework to solve general trajectory optimization problems with fixed contact sequences in an MPC context. This is essentially the same problem that OCS2 and Crocoddyl solve. Our hope is that using Pseudospectral Collocation will yield faster convergence, and that the Casadi + Pinocchio pipeline will result in smaller, more digestible code, which is easier to expand upon than the bulky frameworks mentioned prior.

Contribute

We are open to contributions from the community! Please feel free to submit a pull request or post an issue if you have any suggestions. This framework is still in its most nascent phase, and will take time to mature. Be patient, and hopefully something good will come from this work.

Publications

Coming soon (ICRA 2025?).

Credits

Written by

• Ethan Chandler
• Akshay Jaitly

With contributions from

• Yuen Lam Leung
• Hushmand Esmaeili
• Ibrahim Salman Al-Tameemi
• Duc Doan
• Zhun Cheng

galileo's Issues

Update dependencies

Add OpenMP to install dependencies (what is best practice for optional dependencies?)

Add non-horizontal surface support in legged implementation

For basic functionality, we assumed the contact surfaces were horizontal. This made it easier to plan footstep trajectories.

The desired z-height could easily be determined as a function of time, and the trajectory could be decided a-priori.

Now, we want surface normals that are possibly parallel to the world ground plane. This will be necessary when planning on messy or oddly oriented surfaces.

LeggedRobotStates is in the galileo::opt namespace, but it should be in galileo::legged

Discuss how to improve Environment Surfaces moving forward.

Implement VelocityConstraintBuilder

Finish FrictionConeConstraintBuider

TrajectoryGeneration Class

Do we actually need the TrajectoryGeneration class? If so, where does it fit in? Right now, targets can be specified via terminal cost (Phi), and state initial conditions are already handled in TrajectoryOpt.

Add a collision constraint with the ground

Legs have a tendancy to go through the ground in swing. Add a constraint such that it does not do that.

This is going to be hard to do without knowing the surfaces that the swing goes over/through.

Automatically add floating joint

Right now the user has to manually add floating joints to the urdf. We can easily use Pinocchio's buildFromUrdf function to choose the root joint type.

Retrieve interpolated inputs

Add a method for retrieving barycentric Lagrange interpolated inputs. Should be nearly identical to the state interpolation

ProblemData/ConstraintBuilder are very bulky for simple problems

We should spend some time later down the road making these two classes easier to setup. A factory pattern might be useful here.

NaN in traj_test jac

Is my setup correct? Dynamics are identical to python/huron_centroidal_v4.py. PseudospectralSegment and TrajOpt seem to work without bugs for rosenbrock.

TODO: Fix asap

Generalize FrictionConeConstraintBuilder to 6DOF end effectors

First Order Approximation --
[ 1 0 -mu 0 0 0]
[ 0 1 -mu 0 0 0]
[-1 0 -mu 0 0 0] * AugmentedRotation * Wrench<= 0
[ 0 -1 -mu 0 0 0]
[ 0 0 -1 0 0 0]

Later enhancement

Add a Constructor to LeggedProblemData

It must take in data like surfaces and distribute them to member data like FrictionConeConstraint and VelocityConstraint.

Quaternion to Euler Angles

For our LeggedControl interface, it may be useful to be able to convert from quaternions to Euler angles, since the WBC desired state seems to be expressed with ZYX angles.

Looking into:
https://www.ncbi.nlm.nih.gov/pmc/articles/PMC9648712/
https://stackoverflow.com/questions/1031005/is-there-an-algorithm-for-converting-quaternion-rotations-to-euler-angle-rotatio
https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

for closed-form solutions.

Add decision-variables only constraints

TODO in PseudospectralSegment

Casadi has specialized tools for evaluating lbx < x < ubx which can be faster than lbg < g < ubg.

Create Atlas example

Create an example using Atlas from https://github.com/RobotLocomotion/models/tree/master

Validate that general (G) constraints are working

Find a good constraint to test with on the sample problem. Perhaps the legged system will be a good demo. I have a suspicion that they aren't quite right yet.

Reorder state decision variable data

Put state decision variable constraints in correct order w.r.t. time

Contact is not valid

examples/model_building_test returns with

terminate called after throwing an instance of 'std::runtime_error' what(): 'Contact is not valid!' Aborted

There seems to be an issue somewhere in addPhase

Create Fdif: The difference operator

Background

$$\dot{x} = F(x, u)$$ $$x_{2} = F_{\text{int}}(x_{1}, dx)$$

Where $F_{\text{int}}$ is the function which integrates $x_{1}$ by the deviant of $x$ (which is simply $dx$) over one unit time.

User must define a function $F_{\text{dif}}$ such that:

$$dx = F_{\text{dif}}(x_{1}, x_{2})$$

where $dx$ (the deviant of x), is the tangent vector that must be integrated one unit of time to go from $x_{1}$ to $x_{2}$.

This function should then be used for initial guess generation, such that the user can pass in guesses for $x$, which are integral expressions of the decision variables (deviants of $x$).

Add templates for the Problem Data or Problem Setup in order to make the problem customizable

Some form of the Problem Setup / Problem Data should be templatized. When setting up legged robot problems specifically, this will help form the constraints by storing things like the contact sequence, swing leg trajectory, environment surfaces, etcetera.

Galileo Legged Solver Interface

Currently the solver interface is ROS specific. Making a general legged solver interface will be cleaner, and allow for multithreading when accessing solutions for MPC purposes.

Create a DecisionDataBuilder

This structure would be analogous to the ConstraintBuilder class, but would deal with initial guesses and variable bounds.

Create a ROS implementation of Galileo

xxxProblemData generally needs two types of `model` and `data`

xxxProblemData for the legged constraints needs a symbolic model/data and a numerical model/data- the former to access the symbolic FK, and the latter to access the numeric functions (necessary for Baumgarte corrector to get the reference we are tracking).

Return constraint violations for plotting

In traj opt, collect all the constraint functions and evaluate them at the solution for plotting. Even better, add a callback function to evaluate them at each iteration.

Remove quaternion algebra from LeggedBody

It looks like quaternion operations were added to LeggedBody in commit b5027ff . This isn't ideal, we should look at removing it.

While it makes sense that it's needed for fint, maybe we do an anonymous function or move it into galileo/math

Getting the solution is painfully slow

The solution retrieval process should be much, much faster. It should also be a part of a separate class from traj_opt, so that we can run the optimizer and retrieve solutions in parallel.

There are a lot of obvious ways to speed up the queries, most notably reducing the amount of copying.

Transform initial guess for x to initial guess for dx

Transform initial guess for x to initial guess for dx. Use $f_{\text{diff}}$, the inverse of $f_{\text{int}}$.

How should we deal with doubled computation in the constraint builder class?

We have "CreateBounds" and "CreateFunction" functions to create

b_min < g(x) < b_max

but often the computation of the bounds and the function are dependent on each other. In the instance of the contact constraint the number of constraints (size of b and g) is dependent on the current mode.

How should we implement a constraint builder where there is shared computation?

I propose we make the functions CreateApplyAt, CreateBounds and CreateFunction protected and empty virtual functions, and make BuildConstraint virtual.

That way, when extending the ConstraintBuilder, you can elect to implement the "constraint building" in BuildConstraint itself.

Build MPC interface

Warm starting, and testing with SNOPT

Galileo Branch Archives Convention

When deleting branches, we will use the following convention to ensure that the history of the branch is preserved if we ever want to come back to it, but removed from active view.

To archive and delete a branch using tags:

# Create a tag named similarly to the branch but with the 'archive/' prefix
# Point it to the latest commit of the branch
git tag archive/<branchname> <branchname>

# Delete the branch locally
git branch -D <branchname>

# Delete the local reference of remote branch ("forget" the remote branch)
git branch -d -r origin/<branchname>

# Push local tags to remote
git push --tags

# Delete the remote branch
git push -d origin <branchname>
# or, similarly, 'git push origin :<branchname>'

To view an archived/deleted branch, check 'tags':

In order to restore an archived branch:

git checkout -b <branchname> archive/<branchname>

Credit: https://stackoverflow.com/questions/1307114/how-can-i-archive-git-branches

Barycentric interpolation and gnuplot plotting

Integrate the barycentric interpolation strategy from examples/casadi_demo2.cpp into traj_opt and add an option for solution plotting.

Legged Robot States indexing is inconvenient

Right now, items such as nh, ndh, nq, etc are inconvenient for use in indexing the state. They are appropriate for obtaining the size of certain state elements (nh=6 is the size of the momentum vector, for instance), but there should be a separate mechanism for getting the first index of a certain state element.

For example: Right now, to get the first three elements of qj (the joint position variables), we must do state_vector(si->nh + si->ndh + si->nqb, si->nh + si->ndh + si->nqb + 3), where state_vector is some structure holding the full state, and si is an instance of the LeggedRobotStates class.

More intuitively, we would like to do something like state_vector(qj_index, qj_index + 3).

Clean up access modifiers and documentation

opt/ is a bit messy right now; inconsistent use of private, and some documentation is missing. Also consider splitting declaration and definition into .hpp and .hxx files like pinocchio does.

Dealing with multiphase dynamics

Figuring out how to handle multiphase dynamics has been bothering me for some time. The original approach used in huron_centroidal_v4 is no longer valid because it reduces generality, and masking the input does not work because it would necessitate either a rule to prune decision variables that get masked from the decision vector, or it would require us to keep those extra decision variables in the problem, which MIT Cheetah Software discovered would dramatically increase solving time.

We should have a dynamics function for each phase, which is included in the problem data. This makes it completely general to multi-phase systems which do not have such trivial changes in dynamics.

Figure out Contact Constraint Logic

See python/traj_opt/centroidal_traj_opt.py in solve_problem.

ProblemData usage is ambiguous

My understanding is that we have an abstract ConstraintBuilder which is templatized by some <ProblemData> type.

Then, the user implements concrete ConstraintBuilder such as FrictionConeConstraintBuilder, which are also templatized by some <ProblemData> type (but we know ahead of time that that type will be a user-defined FrictionConeProblemData).

Then, the FrictionConeConstraintBuilder overrides the virtual ConstraintBuilder methods, and creates the ingredients to produce a ConstraintData.

Up to here, I think I understand.

What I do not understand is how we templatize TrajectoryOpt with some < ProblemData > type. I am assuming the intent is so that we can run the concrete xxxConstraintBuilder's from within TrajectoryOpt, so that the user does not have to. This would also be convenient for updating the ConstraintData. I imagine we have something like this pseudocode:

class LeggedRobotProblemData 
{
    GeneralProblemData;
    std::vector<ProblemDatas*> problem_data_vec = {FrictionConeProblemData, ContactProblemData}
}

traj_opt(LeggedRobotProblemData, std::vector<ConstraintBuilder> = {FrictionConeConstraintBuilder, ContactConstraintBuilder})

    in traj_opt:
    Gvec = {}
    i = 0
    for (auto &builder : builders)
    {
        builder->build_constraints(problem_data_vec[i]);
        Gvec.insert(builder->get_g());
        ++i
    }

However, there is not yet a base class which xxxSpecificProblemData's are derived from, so we cannot index them from a container within TrajectoryOpt. And if we do add a base class, other things begin to not make sense. Also, if we pass in a vector of * ConstraintBuilders, how do we know what type of ProblemData they are associated with? The more I play around with it, the more problems I run into.

How do we rectify this? At the end of the day, all we need is a vector of ConstraintData and some metadata about the current phase, but the current approach seems somewhat bloated and overcomplicated.

Map for Initial Guess of dxc

Replace the for loops with Casadi maps to match the standard proposed for the initial guess of dxk.

Make gnuplot optional

Update main CMakeLists.txt and add macros before plotting to check if it exists

Fix "Warning: slow font initialization"

When using the GNUPlotInterface (see galileo/tools) a strange bug occurs. Sometimes, the figures plot perfectly fine, but other times, we get "Warning: slow font initialization" and the legend disappears from the plot. This only seems to happen for the first plot- any subsequent ones work fine.

Set up initial guess generator

Function which takes in time and outputs a decision-variable-at-one-point sized vector (ndx + nu). Use inverse of Fint to transform guesses in x to the actual decision variables space (deviants of x)

Symbol Lookup Error

At runtime on df7e0e5

./examples/traj_test: symbol lookup error: ./examples/traj_test: undefined symbol: _ZN7galileo6legged7contact11ContactMode18createModeDynamicsEN9pinocchio8ModelTplIdLi0ENS3_25JointCollectionDefaultTplEEESt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt10shared_ptrINS1_11EndEffectorEESt4lessISD_ESaISt4pairIKSD_SG_EEESE_INS_3opt17LeggedRobotStatesEE

Is something linked improperly?? What is the issue?

Initial Guess for dxc

The transformation of $xc$ to $dxc$ is slightly less trivial. While $x_k = F_{\text{int}}(x_0, dx_k)$, for $xc_k$, we have $xc_k = F_{\text{int}}(x_k, dxc_k)$ which is equivalent to $xc_k = F_{\text{int}}(F_{\text{int}}(x_0, dx_k), dxc_k)$.

Thus, $dxc_k = F_{\text{dif}}(F_{\text{int}}(x_0, dx_k), xc_k))$.

GRF Definition

Not clear where GRF is supposed to be defined in FrictionConeConstraintBuilder.h

Multi-Phase Initial Guesses

The current initial guess procedure is not quite right for multi-phase optimization, because x0_init (from PseudospectralSegment) could be symbolic, since for phase $i &gt; 0$, x0_init represents the final state of the previous phase, which is symbolic. On the other hand, x0_init is a Casadi numeric for phase $i = 0$, because it is the initial condition.

This discrepancy is a problem because we cannot have symbolic variables in the initial guess.

One way to rectify this is to make the initial guesses based on the global time, rather than the phase-local time. This would require each phase class to hold the initial condition, but I worry this may get messy in an MPC context.

JIT Compilation/Codegen for Casadi Functions

JIT compiling takes unreasonably long for functions from Pinocchio. Look into the Casadi functions for generating c code for the SX functions and cache it between CMake changes.

Time varying general and decision constraints

Validate in examples/simple_test.cpp

Purpose of LeggedBody and inclusion of Data in xxxConstraintBuilder

What is the intended usage of the LeggedBody class? Is it just a wrapper for the Pinocchio model?

On a similar note, where are we storing Pinocchio data? I suggest we include it as a component inside of LeggedBody (we need data to acquire the forward kinematics). I also propose that we store LeggedBody inside of LeggedRobotProblemData, and disseminate copies of the shared_ptrs to xxxProblemData inside of a LeggedRobotProblemData constructor (the latter notion being an idea proposed by Akshay).