Tutorial

This tutorial assumes you have already installed Julia and TORA.jl. See Installation.

Setup

We are going to use TORA.jl as well as other packages to visualize and handle rigid-body mechanisms.

using TORA
using MeshCat
using RigidBodyDynamics

Try to call TORA.greet(). It should simply print "Hello World!"

TORA.greet()

Hello World!

Next, we want to create a 3D viewer using MeshCat.jl

vis = Visualizer()

Let's open the visualizer (in a separate browser tab)

open(vis)

It should look like this:

Loading a Robot

After starting the viewer, we are now ready to load a robot. Let's load a Franka Emika research robot with

robot = TORA.create_robot_franka("panda_arm", vis)

┌ Warning: using Pkg instead of using LazyArtifacts is deprecated
│   caller = ip:0x0
└ @ Core :-1
 Downloading artifact: franka_ros
 Downloading artifact: franka_ros

Afterwards, you should be able to see the robot in the viewer:

Creating a Problem

Having loaded the robot, we are now ready to define the task we want to solve.

TORA.jl uses a Direct Transcription^[1] approach to optimize trajectories. This technique splits the trajectory into $N$ equally-spaced^[2] segments

\[ t_I = t_1 < t_2 < \dots < t_M = t_F,\]

where $t_I$ and $t_F$ are the start and final instants, respectively. This division results in $M = N + 1$ discrete mesh points (a.k.a. knots), for each of which TORA.jl discretizes the states of the system, as well as the control inputs.

To create a new problem, we use the TORA.Problem constructor, which takes three arguments:

1. a TORA.Robot,
2. the number of knots we wish to use for representing the trajectory, and
3. the time step duration between each pair of consecutive knots.

Suppose we want to optimize a motion with a total duration of 2 seconds, and that we want to calculate the control inputs to the system at a frequency of 100 Hz.

const duration = 2.0  # in seconds
const hz = 100

In that case, the time step duration would be

dt = 1/100

0.01

and the total number of knots would be given by

hz * duration + 1

201.0

Therefore, we create the problem by running

problem = TORA.Problem(robot, 201, 1/100)

We can use TORA.show_problem_info to print a summary of relevant information of a TORA.Problem.

TORA.show_problem_info(problem)

Motion duration ...................................... 2.0 seconds
Number of knots ...................................... 201
Number of knots with constrained joint positions ..... 0
Number of knots with constrained joint velocities .... 0
Number of knots with constrained joint torques ....... 0
Number of knots with constrained ee position ......... 0

The summary above shows that the total duration of the motion is 2.0 seconds, just like we wanted.

The summary also shows that there are no constraints defined yet, as we have just now created the problem.

Defining Constraints

The decision variables of the optimization problem at every knot are:

• joint positions,
• joint velocities, and
• joint torques.^[3]

Bounds of the Decision Variables

The bounds of the decision variables need not be defined. They are automatically inferred from the URDF model.

Fixing Values of the Decision Variables

It is possible to fix the values of specific decision variables with:

• TORA.fix_joint_positions!
• TORA.fix_joint_velocities!
• TORA.fix_joint_torques!

Suppose we want to enforce zero joint velocities both at the very start of the motion and at the very end of the motion. We can specify such constraints with

# Constrain initial and final joint velocities to zero
TORA.fix_joint_velocities!(problem, robot, 1, zeros(robot.n_v))
TORA.fix_joint_velocities!(problem, robot, problem.num_knots, zeros(robot.n_v))

Let's have a look at the problem summary output again:

TORA.show_problem_info(problem)

Motion duration ...................................... 2.0 seconds
Number of knots ...................................... 201
Number of knots with constrained joint positions ..... 0
Number of knots with constrained joint velocities .... 2
Number of knots with constrained joint torques ....... 0
Number of knots with constrained ee position ......... 0

The output confirms that there are now two knots for which we have fixed specific joint velocities.

End-effector Constraints

TORA.constrain_ee_position! allows us to enforce specific positions for the end-effector of the robot.

If we want the end-effector of the robot to be located at $[0.5, 0.2, 0.3]$ in knot $k = 33$ we would write

TORA.constrain_ee_position!(problem, 33, [0.5, 0.2, 0.3])

But let's do something more interesting for this tutorial... Let's define a problem where the robot must trace a circular path. For that, we just need to call TORA.constrain_ee_position! multiple times with the right combination of knot and position such that we describe a circle.

let CubicTimeScaling(Tf::Number, t::Number) = 3(t / Tf)^2 - 2(t / Tf)^3
    for k = 1:2:problem.num_knots  # For every other knot
        θ = CubicTimeScaling(problem.num_knots - 1, k - 1) * 2π
        pos = [0.5, 0.2 * cos(θ), 0.8 + 0.2 * sin(θ)]
        TORA.constrain_ee_position!(problem, k, pos)
    end
end

In the snippet above, CubicTimeScaling is a helper function (a cubic polynomial). It allows us to specify a path for the end-effector that accelerates at first, and decelerates near the end. This is a better alternative to tracing the path with constant velocity.

The for loop inside the let block samples every other knot of the trajectory, and computes the position pos of the end-effector at that knot using the angle $\theta$.

Let's print the problem summary once again:

TORA.show_problem_info(problem)

Motion duration ...................................... 2.0 seconds
Number of knots ...................................... 201
Number of knots with constrained joint positions ..... 0
Number of knots with constrained joint velocities .... 2
Number of knots with constrained joint torques ....... 0
Number of knots with constrained ee position ......... 101

The output correctly shows that we now have end-effector position constraints in 151 knots.

Providing an Initial Guess

Trajectory Optimization problems can be very challenging to solve. As such, providing a good initial guess (starting point) for the trajectory helps solvers significantly.

For our circle-tracing task, we are going to define a very simple (but reasonably good) initial guess: a static configuration, zero velocities, and zero torques.

First, let's define the static configuration:

initial_q = [0, 0, 0, -π/2, 0, π, 0]

We can visualize this configuration by running

zero!(robot.state)
set_configuration!(robot.state, initial_q)
set_configuration!(robot.mvis, configuration(robot.state))

This will update the configuration of the robot in the viewer:

We can now define the initial guess for the joint positions with that fixed configuration, repeated for every knot:

initial_qs = repeat(initial_q, 1, problem.num_knots)

7×201 Matrix{Float64}:
  0.0       0.0       0.0       0.0      …   0.0       0.0       0.0
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
 -1.5708   -1.5708   -1.5708   -1.5708      -1.5708   -1.5708   -1.5708
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  3.14159   3.14159   3.14159   3.14159  …   3.14159   3.14159   3.14159
  0.0       0.0       0.0       0.0          0.0       0.0       0.0

For the joint velocities we are going to start with zeroes repeated for every knot:

initial_vs = zeros(robot.n_v, problem.num_knots)

7×201 Matrix{Float64}:
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  …  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0     0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0     0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0     0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0     0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  …  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0     0.0  0.0  0.0  0.0  0.0  0.0  0.0

And the same for the joint torques:

initial_τs = zeros(robot.n_τ, problem.num_knots)

7×201 Matrix{Float64}:
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  …  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0     0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0     0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0     0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0     0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  …  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0     0.0  0.0  0.0  0.0  0.0  0.0  0.0

We can concatenate these matrices into a single one:

initial_guess = [initial_qs; initial_vs; initial_τs]

21×201 Matrix{Float64}:
  0.0       0.0       0.0       0.0      …   0.0       0.0       0.0
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
 -1.5708   -1.5708   -1.5708   -1.5708      -1.5708   -1.5708   -1.5708
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  3.14159   3.14159   3.14159   3.14159  …   3.14159   3.14159   3.14159
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  ⋮                                      ⋱                       ⋮
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  0.0       0.0       0.0       0.0      …   0.0       0.0       0.0
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  0.0       0.0       0.0       0.0          0.0       0.0       0.0
  0.0       0.0       0.0       0.0      …   0.0       0.0       0.0

Notice that the dimensions are 21×201, i.e., the dimension of each knot ($\mathbb{R}^{21}$) times the 201 knots.

We can flatten this matrix into a vector with

initial_guess = vec(initial_guess)

As a very last step, we just need to truncate the last few values that correspond to the control inputs at the last knot. The last knot represents the end of the motion, so we only represent the robot state.

initial_guess = initial_guess[1:end - robot.n_τ]

Solving the Problem

Once we are happy with the problem definition, we just need to call solve and the optimization will start.

cpu_time, x, solver_log = TORA.solve_with_ipopt(problem, robot, initial_guess=initial_guess)

******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
 Ipopt is released as open source code under the Eclipse Public License (EPL).
         For more information visit https://github.com/coin-or/Ipopt
******************************************************************************

This is Ipopt version 3.14.4, running with linear solver MUMPS 5.4.1.

Option "hessian_approximation" is not chosen as "limited-memory", but eval_h returns false.
Exception of type: OPTION_INVALID in file "Interfaces/IpTNLPAdapter.cpp" at line 1218:
 Exception message: eval_h is called but has not been implemented

EXIT: Invalid option encountered.

Showing the Results

When the optimization finishes, we can playback the trajectory on the robot shown in the viewer.

TORA.play_trajectory(vis, problem, robot, x)

You should be able to see this:

We can also plot the positions, velocities, and torques of the obtained trajectory:

TORA.plot_results(problem, robot, x)

Lastly, we can plot the TORA.SolverLog (returned by the solve function) to study the evolution of the feasibility error and of the objective function value (if one has been defined) per iteration.

TORA.plot_log(solver_log)