OpTaS: An optimization-based task specification library for trajectory optimization and model predictive control.
OTHER License
OpTaS is an OPtimization-based TAsk Specification library for trajectory optimization and model predictive control.
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.
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
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.
The following operating systems and python versions are officially supported:
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.
Make sure pip
is up-to-date by running $ python -m pip install --upgrade pip
.
$ pip install pyoptas
Alternatively, you can also install OpTaS using:
$ python -m pip install 'optas @ git+https://github.com/cmower/optas.git'
$ git clone --recursive [email protected]:cmower/optas.git
(if you do not want to build the documentation then the --recursive
flag is not necessary)$ cd optas
$ pip install .
$ pip install .[example]
$ pip install .[test]
$ cd /path/to/optas/doc
$ sudo apt install doxygen graphviz
$ python gen_mainpage.py
$ doxygen
html/index.html
latex/refman.pdf
$ cd /path/to/optas
$ 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
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.If you use OpTaS in your work, please consider including the following citation.
@inproceedings{mower23optas,
author={Mower, Christopher E. and Moura, João and Behabadi, Nazanin Zamani and Vijayakumar, Sethu and Vercauteren, Tom and Bergeles, Christos},
booktitle={2023 IEEE International Conference on Robotics and Automation (ICRA)},
title={OpTaS: An Optimization-based Task Specification Library for Trajectory Optimization and Model Predictive Control},
year={2023},
volume={},
number={},
pages={9118-9124},
doi={10.1109/ICRA48891.2023.10161272}
}
The preprint can be found on arXiv.
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.
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).