pyoptas 1.0.2

An optimization-based task specification library for task and motion planning (TAMP), trajectory optimization, and model predictive control.

OpTaS

OpTaS is an OPtimization-based TAsk Specification library for trajectory optimization and model predictive control. Please see our paper accepted at ICRA 2023, and also our video (link below).

In the past, OpTaS supported ROS from an internal module. This functionality, with additional updates, has now been moved to a dedicated repository: optas_ros.

Example

In this example we implement an optimization-based IK problem. The problem computes an optimal joint configuration $q^*\in\mathbb{R}^n$ given by

$$ q^* = \underset{q}{\text{arg}\min}~||q - q_N||^2\quad\text{subject to}\quad p(q) = p_g, q^-\leq q \leq q^+ $$

where

• $q\in\mathbb{R}^n$ is the joint configuration for an $n$-dof robot (in our example, we use the KUKA LWR in the above figure with $n=7$),
• $q_N\in\mathbb{R}^n$ is a nominal joint configuration,
• $||\cdot||$ is the Euclidean norm,
• $p: \mathbb{R}^n\rightarrow\mathbb{R}^3$ computes the end-effector position via the forward kinematics,
• $p_g\in\mathbb{R}^3$ is a goal position, and
• $q^-, q^+\in\mathbb{R}^n$ is the lower and upper joint position limits respectively.

The example problem has a quadratic cost function with nonlinear constraints. We use the nominal configuration $q_N$ as the initial seed for the problem.

The following example script showcases some of the main features of OpTaS: creating a robot model, building an optimization problem, passing the problem to a solver, computing an optimal solution, and visualizing the robot in a given configuration.

import os
import pathlib

import optas

# Specify URDF filename
cwd = pathlib.Path(__file__).parent.resolve()  # path to current working directory
urdf_filename = os.path.join(
    cwd, "robots", "kuka_lwr", "kuka_lwr.urdf"
)  # KUKA LWR, 7-DoF

# Setup robot model
robot = optas.RobotModel(urdf_filename=urdf_filename)
name = robot.get_name()

# Setup optimization builder
T = 1
builder = optas.OptimizationBuilder(T, robots=robot)

# Setup parameters
qn = builder.add_parameter("q_nominal", robot.ndof)
pg = builder.add_parameter("p_goal", 3)

# Constraint: end goal
q = builder.get_model_state(name, 0)
end_effector_name = "end_effector_ball"
p = robot.get_global_link_position(end_effector_name, q)
builder.add_equality_constraint("end_goal", p, pg)

# Cost: nominal configuration
builder.add_cost_term("nominal", optas.sumsqr(q - qn))

# Constraint: joint position limits
builder.enforce_model_limits(name)  # joint limits extracted from URDF

# Build optimization problem
optimization = builder.build()

# Interface optimization problem with a solver
solver = optas.CasADiSolver(optimization).setup("ipopt")
# solver = optas.ScipyMinimizeSolver(optimization).setup("SLSQP")

# Specify a nominal configuration
q_nominal = optas.deg2rad([0, 45, 0, -90, 0, -45, 0])

# Get end-effector position in nominal configuration
p_nominal = robot.get_global_link_position(end_effector_name, q_nominal)

# Specify a goal end-effector position
p_goal = p_nominal + optas.DM([0.0, 0.3, -0.2])

# Reset solver parameters
solver.reset_parameters({"q_nominal": q_nominal, "p_goal": p_goal})

# Reset initial seed
solver.reset_initial_seed({f"{name}/q": q_nominal})

# Compute a solution
solution = solver.solve()
q_solution = solution[f"{name}/q"]

# Visualize the robot
vis = optas.Visualizer(quit_after_delay=2.0)

# Draw goal position and start visualizer
vis.sphere(0.05, rgb=[0, 1, 0], position=p_goal.toarray().flatten().tolist())
# vis.robot(robot, q=q_nominal,display_link_names=True,show_links=True)   # nominal
vis.robot(robot, q=q_solution, display_link_names=True, show_links=True)  # solution

vis.start()

Run the example script example.py. Other examples, including dual-arm planning, Model Predictive Control, Trajectory Optimization, etc can be found in the example/ directory.

Support

The following operating systems and python versions are officially supported:

• Ubuntu 20.04 and 22.04
  • Python 3.7, 3.8, 3.9
• Windows
  • Python 3.7, 3.8, 3.9
• Mac OS
  • Python 3.9

Note that OpTaS makes use of dataclasses that was introduced in Python 3.7, and so Python versions from 3.6 and lower are not supported on any operating system. Other operating systems or higher Python versions will likely work. If you experience problems, please submit an issue.

Install

Make sure pip is up-to-date by running $ python -m pip install --upgrade pip.

Via pip

$ pip install pyoptas

Alternatively, you can also install OpTaS using:

$ python -m pip install 'optas @ git+https://github.com/cmower/optas.git'

From source

1. $ git clone [e-mail]:cmower/optas.git
2. $ cd optas
3. $ pip install .
4. (optional) If you want to run the examples/tests: $ pip install -r requirements_all.txt

Build documentation

1. $ cd /path/to/optas/doc
2. $ sudo apt install doxygen graphviz
3. $ python gen_mainpage.py
4. $ doxygen
5. Open the documentation in either HTML or PDF:
   • html/index.html
   • latex/refman.pdf

Run tests

1. $ cd /path/to/optas
2. Each test can be run as follows
   • $ pytest tests/test_builder.py
   • $ pytest tests/test_examples.py
   • $ pytest tests/test_models.py
   • $ pytest tests/test_optas_utils.py
   • $ pytest tests/test_optimization.py
   • $ pytest tests/test_solver.py
   • $ pytest tests/test_spatialmath.py
   • $ pytest tests/test_sx_container.py

Known Issues

• Loading robot models from xacro files is supported, however there can be issues if you are running this in a ROS agnositic environment. If you do not have ROS installed, then the xacro file should not contain ROS-specific features. For further details see here.
• If NumPy ver 1.24 is installed, an AttributeError error is thrown when you try to solve an unconstrained problem with the OSQP interface. A temporary workaround is to add a constraint, e.g. x >= -1e9 where x is a decision variable. See details on the issue here and pull request here.

Citation

If you use OpTaS in your work, please consider including the following citation.

@inproceedings{Mower2023,
  author={Mower, Christopher E. and Moura, João and Zamani Behabadi, Nazanin and Vijayakumar, Sethu and Vercauteren, Tom and Bergeles, Christos},
  booktitle={2023 International Conference on Robotics and Automation (ICRA)},
  title={OpTaS: An Optimization-based Task Specification Library for Trajectory Optimization and Model Predictive Control},
  year={2023},
  url = {https://github.com/cmower/optas},
}

The preprint can be found on arXiv.

Contributing

We welcome contributions from the community. If you come across any issues or inacuracies in the documentation, please submit an issue. If you would like to contribute any new features, please fork the repository, and submit a pull request.

Acknowledgement

This research received funding from the European Union’s Horizon 2020 research and innovation program under grant agreement No. 101016985 (FAROS). Further, this work was supported by core funding from the Wellcome/EPSRC [WT203148/Z/16/Z; NS/A000049/1]. T. Vercauteren is supported by a Medtronic / RAEng Research Chair [RCSRF1819\7\34], and C. Bergeles by an ERC Starting Grant [714562]. This work has received funding from the European Union’s Horizon 2020 research and innovation programme under grant agreement No 101017008, Enhancing Healthcare with Assistive Robotic Mobile Manipulation (HARMONY).