<p align="center">
<img src="https://raw.githubusercontent.com/cmower/optas/master/doc/logo.png" width="60" align="right">
</p>
# OpTaS
[](https://github.com/psf/black)
[](https://github.com/cmower/optas/actions/workflows/black.yaml)
[](https://github.com/cmower/optas/actions/workflows/pytest.yaml)
[](https://github.com/cmower/optas/actions/workflows/documentation.yaml)
OpTaS is an OPtimization-based TAsk Specification library for trajectory optimization and model predictive control.
- Code: [https://github.com/cmower/optas](https://github.com/cmower/optas)
- Documentation: [https://cmower.github.io/optas/](https://cmower.github.io/optas/)
- PyPI: [https://pypi.org/project/pyoptas/](https://pypi.org/project/pyoptas/)
- Issues: [https://github.com/cmower/optas/issues](https://github.com/cmower/optas/issues)
- ICRA 2023 paper:
- (arXiv) [https://arxiv.org/abs/2301.13512](https://arxiv.org/abs/2301.13512)
- (ieee) [https://ieeexplore.ieee.org/document/10161272](https://ieeexplore.ieee.org/document/10161272)
- Video: [https://youtu.be/gCMNOenFngU](https://youtu.be/gCMNOenFngU)
- Presentation: [https://vimeo.com/824802366](https://vimeo.com/824802366)
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](https://github.com/cmower/optas_ros).
# Example
<p align="center">
<img src="https://raw.githubusercontent.com/cmower/optas/master/doc/image/kuka_example.png" width="61.803398875%">
</p>
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.
```python
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](example/example.py).
Other examples, including dual-arm planning, Model Predictive Control, Trajectory Optimization, etc can be found in the [example/](example) directory.
# Support
The following operating systems and python versions are [officially supported](https://github.com/cmower/optas/blob/master/.github/workflows/pytest.yaml):
* Ubuntu 20.04 and 22.04
* Python 3.7, 3.8, 3.9
* Windows
* Python 3.8, 3.9
* Mac OS
* Python 3.9
Note that OpTaS makes use of [dataclasses](https://docs.python.org/3/library/dataclasses.html) that was [introduced in Python 3.7](https://peps.python.org/pep-0557/), 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](https://github.com/cmower/optas/issues/new/choose).
# 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 --recursive git@github.com:cmower/optas.git` (if you do not want to build the documentation then the `--recursive` flag is not necessary)
2. `$ cd optas`
4. `$ pip install .`
- if you want to run the examples use: `$ pip install .[example]`
- if you want to run the tests use: `$ pip install .[test]`
### Build documentation
1. `$ cd /path/to/optas/doc`
2. `$ sudo apt install doxygen graphviz`
3. `$ python gen_mainpage.py`
3. `$ doxygen`
4. 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](https://github.com/cmower/optas/issues/78).
- 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](https://github.com/osqp/osqp-python/issues/104) and pull request [here](https://github.com/osqp/osqp-python/pull/105).
# Citation
If you use OpTaS in your work, please consider including the following citation.
```bibtex
@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](https://arxiv.org/abs/2301.13512).
# Contributing
We welcome contributions from the community.
If you come across any issues or inacuracies in the documentation, please [submit an issue](https://github.com/cmower/optas/issues/new/choose).
If you would like to contribute any new features, please [fork the repository](https://github.com/cmower/optas/fork), 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](https://h2020faros.eu/)).
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](https://harmony-eu.org/)).
<p align="center">
<img src="https://raw.githubusercontent.com/cmower/optas/master/doc/image/eu.png" width="180" align="left">
</p>
Raw data
{
"_id": null,
"home_page": "https://github.com/cmower/optas",
"name": "pyoptas",
"maintainer": "",
"docs_url": null,
"requires_python": ">=3.7",
"maintainer_email": "",
"keywords": "",
"author": "Christopher E. Mower",
"author_email": "\"Christopher E. Mower\" <christopher.mower@kcl.ac.uk>",
"download_url": "https://files.pythonhosted.org/packages/91/23/a1b11d7451aa5392b35f331af2e3959990b58ecf6ec465205376b72b03e7/pyoptas-1.0.7.tar.gz",
"platform": null,
"description": "<p align=\"center\">\n <img src=\"https://raw.githubusercontent.com/cmower/optas/master/doc/logo.png\" width=\"60\" align=\"right\">\n</p>\n\n# OpTaS\n\n[](https://github.com/psf/black)\n[](https://github.com/cmower/optas/actions/workflows/black.yaml)\n[](https://github.com/cmower/optas/actions/workflows/pytest.yaml)\n[](https://github.com/cmower/optas/actions/workflows/documentation.yaml)\n\n\nOpTaS is an OPtimization-based TAsk Specification library for trajectory optimization and model predictive control.\n\n- Code: [https://github.com/cmower/optas](https://github.com/cmower/optas)\n- Documentation: [https://cmower.github.io/optas/](https://cmower.github.io/optas/)\n- PyPI: [https://pypi.org/project/pyoptas/](https://pypi.org/project/pyoptas/)\n- Issues: [https://github.com/cmower/optas/issues](https://github.com/cmower/optas/issues)\n- ICRA 2023 paper:\n - (arXiv) [https://arxiv.org/abs/2301.13512](https://arxiv.org/abs/2301.13512)\n - (ieee) [https://ieeexplore.ieee.org/document/10161272](https://ieeexplore.ieee.org/document/10161272)\n- Video: [https://youtu.be/gCMNOenFngU](https://youtu.be/gCMNOenFngU)\n- Presentation: [https://vimeo.com/824802366](https://vimeo.com/824802366)\n\nIn the past, OpTaS supported ROS from an internal module. This functionality, with additional updates, has now been moved to a dedicated repository: [optas_ros](https://github.com/cmower/optas_ros).\n\n# Example\n\n<p align=\"center\">\n <img src=\"https://raw.githubusercontent.com/cmower/optas/master/doc/image/kuka_example.png\" width=\"61.803398875%\">\n</p>\n\n\nIn this example we implement an optimization-based IK problem.\nThe problem computes an optimal joint configuration $q^*\\in\\mathbb{R}^n$ given by\n\n$$\nq^* = \\underset{q}{\\text{arg}\\min}~\\|\\|q - q_N\\|\\|^2\\quad\\text{subject to}\\quad p(q) = p_g, q^-\\leq q \\leq q^+\n$$\n\nwhere\n* $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$),\n* $q_N\\in\\mathbb{R}^n$ is a nominal joint configuration,\n* $\\|\\|\\cdot\\|\\|$ is the Euclidean norm,\n* $p: \\mathbb{R}^n\\rightarrow\\mathbb{R}^3$ computes the end-effector position via the forward kinematics,\n* $p_g\\in\\mathbb{R}^3$ is a goal position, and\n* $q^-, q^+\\in\\mathbb{R}^n$ is the lower and upper joint position limits respectively.\n\nThe example problem has a quadratic cost function with nonlinear constraints.\nWe use the nominal configuration $q_N$ as the initial seed for the problem.\n\nThe following example script showcases some of the main features of OpTaS:\ncreating a robot model,\nbuilding an optimization problem,\npassing the problem to a solver,\ncomputing an optimal solution, and\nvisualizing the robot in a given configuration.\n\n```python\nimport os\nimport pathlib\n\nimport optas\n\n# Specify URDF filename\ncwd = pathlib.Path(__file__).parent.resolve() # path to current working directory\nurdf_filename = os.path.join(\n cwd, \"robots\", \"kuka_lwr\", \"kuka_lwr.urdf\"\n) # KUKA LWR, 7-DoF\n\n# Setup robot model\nrobot = optas.RobotModel(urdf_filename=urdf_filename)\nname = robot.get_name()\n\n# Setup optimization builder\nT = 1\nbuilder = optas.OptimizationBuilder(T, robots=robot)\n\n# Setup parameters\nqn = builder.add_parameter(\"q_nominal\", robot.ndof)\npg = builder.add_parameter(\"p_goal\", 3)\n\n# Constraint: end goal\nq = builder.get_model_state(name, 0)\nend_effector_name = \"end_effector_ball\"\np = robot.get_global_link_position(end_effector_name, q)\nbuilder.add_equality_constraint(\"end_goal\", p, pg)\n\n# Cost: nominal configuration\nbuilder.add_cost_term(\"nominal\", optas.sumsqr(q - qn))\n\n# Constraint: joint position limits\nbuilder.enforce_model_limits(name) # joint limits extracted from URDF\n\n# Build optimization problem\noptimization = builder.build()\n\n# Interface optimization problem with a solver\nsolver = optas.CasADiSolver(optimization).setup(\"ipopt\")\n# solver = optas.ScipyMinimizeSolver(optimization).setup(\"SLSQP\")\n\n# Specify a nominal configuration\nq_nominal = optas.deg2rad([0, 45, 0, -90, 0, -45, 0])\n\n# Get end-effector position in nominal configuration\np_nominal = robot.get_global_link_position(end_effector_name, q_nominal)\n\n# Specify a goal end-effector position\np_goal = p_nominal + optas.DM([0.0, 0.3, -0.2])\n\n# Reset solver parameters\nsolver.reset_parameters({\"q_nominal\": q_nominal, \"p_goal\": p_goal})\n\n# Reset initial seed\nsolver.reset_initial_seed({f\"{name}/q\": q_nominal})\n\n# Compute a solution\nsolution = solver.solve()\nq_solution = solution[f\"{name}/q\"]\n\n# Visualize the robot\nvis = optas.Visualizer(quit_after_delay=2.0)\n\n# Draw goal position and start visualizer\nvis.sphere(0.05, rgb=[0, 1, 0], position=p_goal.toarray().flatten().tolist())\n# vis.robot(robot, q=q_nominal,display_link_names=True,show_links=True) # nominal\nvis.robot(robot, q=q_solution, display_link_names=True, show_links=True) # solution\n\nvis.start()\n```\n\nRun the example script [example.py](example/example.py).\nOther examples, including dual-arm planning, Model Predictive Control, Trajectory Optimization, etc can be found in the [example/](example) directory.\n\n# Support\n\nThe following operating systems and python versions are [officially supported](https://github.com/cmower/optas/blob/master/.github/workflows/pytest.yaml):\n\n* Ubuntu 20.04 and 22.04\n * Python 3.7, 3.8, 3.9\n* Windows\n * Python 3.8, 3.9\n* Mac OS\n * Python 3.9\n\nNote that OpTaS makes use of [dataclasses](https://docs.python.org/3/library/dataclasses.html) that was [introduced in Python 3.7](https://peps.python.org/pep-0557/), and so Python versions from 3.6 and lower are not supported on any operating system.\nOther operating systems or higher Python versions will likely work.\nIf you experience problems, please [submit an issue](https://github.com/cmower/optas/issues/new/choose).\n\n# Install\n\nMake sure `pip` is up-to-date by running `$ python -m pip install --upgrade pip`.\n\n## Via pip\n\n```\n$ pip install pyoptas\n```\n\nAlternatively, you can also install OpTaS using:\n\n```\n$ python -m pip install 'optas @ git+https://github.com/cmower/optas.git'\n```\n\n## From source\n1. `$ git clone --recursive git@github.com:cmower/optas.git` (if you do not want to build the documentation then the `--recursive` flag is not necessary)\n2. `$ cd optas`\n4. `$ pip install .`\n - if you want to run the examples use: `$ pip install .[example]`\n - if you want to run the tests use: `$ pip install .[test]`\n\n### Build documentation\n\n1. `$ cd /path/to/optas/doc`\n2. `$ sudo apt install doxygen graphviz`\n3. `$ python gen_mainpage.py`\n3. `$ doxygen`\n4. Open the documentation in either HTML or PDF:\n - `html/index.html`\n - `latex/refman.pdf`\n\n### Run tests\n\n1. `$ cd /path/to/optas`\n2. Each test can be run as follows\n - `$ pytest tests/test_builder.py`\n - `$ pytest tests/test_examples.py`\n - `$ pytest tests/test_models.py`\n - `$ pytest tests/test_optas_utils.py`\n - `$ pytest tests/test_optimization.py`\n - `$ pytest tests/test_solver.py`\n - `$ pytest tests/test_spatialmath.py`\n - `$ pytest tests/test_sx_container.py`\n\n# Known Issues\n\n- 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](https://github.com/cmower/optas/issues/78).\n- 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](https://github.com/osqp/osqp-python/issues/104) and pull request [here](https://github.com/osqp/osqp-python/pull/105).\n\n# Citation\n\nIf you use OpTaS in your work, please consider including the following citation.\n\n```bibtex\n@inproceedings{mower23optas,\n author={Mower, Christopher E. and Moura, Jo\u00e3o and Behabadi, Nazanin Zamani and Vijayakumar, Sethu and Vercauteren, Tom and Bergeles, Christos},\n booktitle={2023 IEEE International Conference on Robotics and Automation (ICRA)},\n title={OpTaS: An Optimization-based Task Specification Library for Trajectory Optimization and Model Predictive Control},\n year={2023},\n volume={},\n number={},\n pages={9118-9124},\n doi={10.1109/ICRA48891.2023.10161272}\n}\n```\n\nThe preprint can be found on [arXiv](https://arxiv.org/abs/2301.13512).\n\n# Contributing\n\nWe welcome contributions from the community.\nIf you come across any issues or inacuracies in the documentation, please [submit an issue](https://github.com/cmower/optas/issues/new/choose).\nIf you would like to contribute any new features, please [fork the repository](https://github.com/cmower/optas/fork), and submit a pull request.\n\n# Acknowledgement\n\nThis research received funding from the European Union\u2019s Horizon 2020 research and innovation program under grant agreement No. 101016985 ([FAROS](https://h2020faros.eu/)).\nFurther, this work was supported by core funding from the Wellcome/EPSRC [WT203148/Z/16/Z; NS/A000049/1].\nT. Vercauteren is supported by a Medtronic / RAEng Research Chair [RCSRF1819\\7\\34], and C. Bergeles by an ERC Starting Grant [714562].\nThis work has received funding from the European Union\u2019s Horizon 2020 research and innovation programme under grant agreement No 101017008, Enhancing Healthcare with Assistive Robotic Mobile Manipulation ([HARMONY](https://harmony-eu.org/)).\n\n<p align=\"center\">\n <img src=\"https://raw.githubusercontent.com/cmower/optas/master/doc/image/eu.png\" width=\"180\" align=\"left\">\n</p>\n",
"bugtrack_url": null,
"license": "Apache License, Version 2.0",
"summary": "An optimization-based task specification library for task and motion planning (TAMP), trajectory optimization, and model predictive control.",
"version": "1.0.7",
"project_urls": {
"Bug Tracker": "https://github.com/cmower/optas/issues",
"Documentation": "https://cmower.github.io/optas/",
"Homepage": "https://cmower.github.io/optas/",
"Source": "https://github.com/cmower/optas"
},
"split_keywords": [],
"urls": [
{
"comment_text": "",
"digests": {
"blake2b_256": "f8b6d0b5ac6a63acf3a672970c7acf1f9560c5ca0c2e424930beff5fe687890f",
"md5": "47cb6cb17068ebd74af2d6413de5dbe0",
"sha256": "04e386d99a28c249fc86ffd31febe7e3291a06f64d31a0a1af954b4def1ccddf"
},
"downloads": -1,
"filename": "pyoptas-1.0.7-py3-none-any.whl",
"has_sig": false,
"md5_digest": "47cb6cb17068ebd74af2d6413de5dbe0",
"packagetype": "bdist_wheel",
"python_version": "py3",
"requires_python": ">=3.7",
"size": 46632,
"upload_time": "2023-08-04T09:53:01",
"upload_time_iso_8601": "2023-08-04T09:53:01.994353Z",
"url": "https://files.pythonhosted.org/packages/f8/b6/d0b5ac6a63acf3a672970c7acf1f9560c5ca0c2e424930beff5fe687890f/pyoptas-1.0.7-py3-none-any.whl",
"yanked": false,
"yanked_reason": null
},
{
"comment_text": "",
"digests": {
"blake2b_256": "9123a1b11d7451aa5392b35f331af2e3959990b58ecf6ec465205376b72b03e7",
"md5": "998965152ac848291de474fbedec4e82",
"sha256": "b078e71c36c6ee4383a852767965c6cb13f05c70a8f06e2eaa16f1dcbea896de"
},
"downloads": -1,
"filename": "pyoptas-1.0.7.tar.gz",
"has_sig": false,
"md5_digest": "998965152ac848291de474fbedec4e82",
"packagetype": "sdist",
"python_version": "source",
"requires_python": ">=3.7",
"size": 59930,
"upload_time": "2023-08-04T09:53:08",
"upload_time_iso_8601": "2023-08-04T09:53:08.547396Z",
"url": "https://files.pythonhosted.org/packages/91/23/a1b11d7451aa5392b35f331af2e3959990b58ecf6ec465205376b72b03e7/pyoptas-1.0.7.tar.gz",
"yanked": false,
"yanked_reason": null
}
],
"upload_time": "2023-08-04 09:53:08",
"github": true,
"gitlab": false,
"bitbucket": false,
"codeberg": false,
"github_user": "cmower",
"github_project": "optas",
"travis_ci": false,
"coveralls": false,
"github_actions": true,
"lcname": "pyoptas"
}
JSON
