Flex* - From URDF to Flexible Robots Optimal Control

Here we offer an easy to use library that allows you to do optimal control of a given robot, via urdf. Our tool provide an optimal solution for almost every problem related to a robot that somebody can think of...

You can find some examples of automatically generated reports for minimum energy and maximum final velocity problems (others available here).

Installation Guide

We wrote a simple bash script that set up all the dependencies and needs for you, leaving with a clean installation of the package.

git clone https://github.com/abcamiletto/urdf2optcontrol.git && cd urdf2optcontrol
./install.sh

The needed dependencies are automatically installed with previous command For a custom installation, just the two following commands are needed:

1. casadi

pip install casadi

2. urdf2casadi

git clone https://github.com/abcamiletto/urdf2casadi-light.git
cd urdf2casadi
pip install .

To see if it's working, run of the python files in the example folder.

Example of usage

#!/usr/bin/env python3
from urdf2optcontrol import optimizer
from matplotlib import pyplot as plt 

# URDF options
urdf_path = './urdf/rrbot.urdf'
root = 'link1'
end = 'link3'


# The trajectory in respect we want to minimize the cost function
# If qd isn't given, it will be obtained with differentiation from q
def trajectory_target_(t):
    q = [t] * 2
    return q

# Our cost function
def my_cost_func(q, qd, qdd, ee_pos, u, t):
    return 100*q.T@q + u.T@u/10

# Additional Constraints I may want to set
def my_constraint1(q, q_d, q_dd, ee_pos, u, t):
    return [-30, -30], u, [30, 30]
def my_constraint2(q, q_d, q_dd, ee_pos, u, t):
    return [-15, -15], q_dd, [15, 15]
my_constraints=[my_constraint1, my_constraint2]

# Constraints to be imposed on the final instant of the simulation
# e.g. impose a value of 1 radiant for both the joints
def my_final_constraint1(q, q_d, q_dd, ee_pos, u):
    return [1, 1], q, [1, 1]
my_final_constraints = [my_final_constraint1] 

# Initial Condition in terms of q, qd
in_cond = [0,0] + [0,0]

# Optimization parameters
steps = 40
time_horizon = 1.0    # if not set, it is free (and optimized)

# Load the urdf and calculate the differential equations
optimizer.load_robot(urdf_path, root, end, 
                        get_motor_dynamics=True) # useful only for SEA (default is True)

# Loading the problem conditions
optimizer.load_problem(
    cost_func=my_cost_func,
    control_steps=steps,
    initial_cond=in_cond,
    trajectory_target=trajectory_target_,
    time_horizon=time_horizon,
    constraints=my_constraints, 
    final_constraints=my_final_constraints,
    max_iter=500
    )

# Solving the non linear problem
res = optimizer.solve()

# Print the results!
fig = optimizer.plot_result(show=True)

Things to do:

• Indipendence to number of joints
• Joint limits from URDF
• Reference point different from origin
• Reference Trajectory
• Cost function on final position
• Check performance with SX/MX
• Check sparse Hessian problem
• Fix Latex Mathematical Modeling
• Reconstruct the actual optimal input

Second Round

• Easy UI for algebraic constraints
• Auto Parsing of max_velocity and max_effort
• Friction and Damping modeling
• URDF parsing to get joint stiffness
• Control over trajectory derivative
• Installation Guide
• Code Guide
• Update LaTeX

Third Round

• Test on a multiple arms robot
• Pip package and Auto installation
• SAE modeling

Fourth Round

• SAE with not every joint elastic
• Check damping+friction implementation in urdf2casadi
• Add a parameter to choose whether you want to implement motor dynamics or not
• Fix examples
• Modelling without motor inertias
• Add a method "solve" to optimizer instead of returning on load_problem
• Convert result to numpy array and make sure you can iterate on it
• Return T if we're minimizing it
• Desired trajectory as optional argument
• Insert ee position and q acceleration in cost functions

Fifth Round

• Development of a visualization function for results generating HTML report
• Implement a path tracking with min time optimization
• Customization of report layout
• Link GitHub page for ROS implementation example

To do or not to do?

• Implementation of a minimum time cost function
• Implementation of fixed final constraints
• Direct collocation
• Implement different types of elastic actuators