Issue in OSQP interface

OpTaS

OpTaS is an OPtimization-based TAsk Specification library for trajectory optimization and model predictive control.

Code: https://github.com/cmower/optas
Documentation: https://cmower.github.io/optas/
PyPI: https://pypi.org/project/pyoptas/
Issues: https://github.com/cmower/optas/issues
ICRA 2023 paper:
- (arXiv) https://arxiv.org/abs/2301.13512
- (ieee) https://ieeexplore.ieee.org/document/10161272
Video: https://youtu.be/gCMNOenFngU
Presentation: 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.

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.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

$ 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 .
- if you want to run the examples use: $ pip install .[example]
- if you want to run the tests use: $ pip install .[test]

Build documentation

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

Run tests

$ cd /path/to/optas
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{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.

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).

	def create_cylinder_actor(start, end, radius):

	s = cs.np.array(start)
	e = cs.np.array(end)
	a = e - s
	l = cs.np.linalg.norm(a)
	p = 0.5*a
	y = a / l

	temp = cs.np.random.random(3)
	temp /= cs.np.linalg.norm(temp)

	x = cs.np.cross(y, temp)
	z = cs.np.cross(x, y)
	T = cs.np.eye(4)
	T[:3, 0] = x
	T[:3, 1] = y
	T[:3, 2] = z
	T[:3, 3] = p

	cylinder = vtkCylinderSource()
	cylinder.SetRadius(radius)
	cylinder.SetHeight(l)
	cylinder.SetResolution(20)

	mapper = vtk.vtkPolyDataMapper()
	mapper.SetInputConnection(cylinder.GetOutputPort())

	actor = vtk.vtkActor()
	actor.SetMapper(mapper)

	transform = vtk.vtkTransform()
	transform.SetMatrix(T.flatten().tolist())
	actor.SetUserTransform(transform)

	return actor

	def test_mul_correct_output(self):
	for _ in range(NUM_RANDOM):
	quat0 = self._random_quaternion()
	quat1 = self._random_quaternion()
	quat_optas = quat0 * quat1

	q0 = quat0.getquat().toarray().flatten()
	q1 = quat1.getquat().toarray().flatten()
	quat_lib = (Rot.from_quat(q1) * Rot.from_quat(q0)).as_quat()

	assert isclose(quat_optas.getquat().toarray().flatten(), quat_lib)

	def __mul__(self, quat):
	"""Quaternion multiplication"""
	assert isinstance(quat, Quaternion), "unsupported type"
	x0, y0, z0, w0 = self.split()
	x1, y1, z1, w1 = quat.split()
	return Quaternion(
	x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
	-x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
	x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0,
	-x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0,
	)

	@arrayify_args
	@listify_output
	def get_global_link_quaternion(self, link, q):
	"""Get a quaternion in the global frame."""

	# Setup
	assert link in self._urdf.link_map.keys(), "given link does not appear in URDF"
	root = self._urdf.get_root()
	quat = Quaternion(0.0, 0.0, 0.0, 1.0)
	if link == root:
	return quat.getquat()

	# Iterate over joints in chain
	for joint_name in self._urdf.get_chain(root, link, links=False):
	joint = self._urdf.joint_map[joint_name]
	xyz, rpy = self.get_joint_origin(joint)

	if joint.type == "fixed":
	quat = Quaternion.fromrpy(rpy) * quat
	continue

	joint_index = self._get_actuated_joint_index(joint.name)
	qi = q[joint_index]

	quat = Quaternion.fromrpy(rpy) * quat
	if joint.type in {"revolute", "continuous"}:
	quat = Quaternion.fromangvec(qi, self.get_joint_axis(joint)) * quat

	elif joint.type == "prismatic":
	pass

	else:
	raise JointTypeNotSupported(joint.type)

	return quat.getquat()

	@arrayify_args
	@listify_output
	def get_link_quaternion(self, link, q, base_link):
	"""Get the quaternion defined in a given base frame."""
	quat_L_W = Quaternion(self.get_global_link_quaternion(link, q))
	quat_B_W = Quaternion(self.get_global_link_quaternion(base_link, q))
	return (quat_L_W * quat_B_W.inv()).getquat()

	@arrayify_args
	@listify_output
	def get_link_transform(self, link, q, base_link):
	"""Get the link transform in a given base frame."""
	T_L_W = self.get_global_link_transform(link, q)
	T_B_W = self.get_global_link_transform(base_link, q)
	return T_L_W @ invt(T_B_W)

	def line(start, end, rgb, alpha, linewidth):
	points = vtk.vtkPoints()
	points.InsertNextPoint(*start)
	points.InsertNextPoint(*end)

	line = vtk.vtkLine()
	line.GetPointIds().SetId(0, 0)
	line.GetPointIds().SetId(0, 1)

	lines = vtk.vtkCellArray()
	lines.InsertNextCell(line)

	polydata = vtk.vtkPolyData()
	polydata.SetPoints(points)
	polydata.SetLines(lines)

	mapper = vtk.vtkPolyDataMapper()
	mapper.SetInputData(polydata)

	actor = vtk.vtkActor()
	actor.SetMapper(mapper)

	actor.GetProperty().SetLineWidth(linewidth)

	if isinstance(rgb, list):
	assert len(rgb) == 3, f"rgb is incorrect length, got {len(rgb)} expected 3"
	actor.GetProperty().SetColor(*rgb)

	if alpha < 1.0:
	assert alpha >= 0.0, "the scalar alpha must be in the range [0, 1]"
	actor.GetProperty().SetOpacity(alpha)

	return actor

	if urdf_link.visual is None:
	continue

	material = urdf_link.visual.material
	rgb = None
	if isinstance(material.name, str) and material.name in material_names:
	rgba = get_material_rgba(urdf_link.visual.material.name)
	rgb = rgba[:3]

	if isinstance(geometry, Mesh):
	if geometry.filename.lower().endswith(".stl"):
	filename = geometry.filename

	if not os.path.exists(filename):
	relpath = robot_model.get_urdf_dirname()
	filename = os.path.join(relpath, filename)

	scale = None
	if geometry.scale is not None:
	scale = geometry.scale

	actors.append(
	stl(
	filename,
	scale=scale,
	rgb=rgb,
	alpha=alpha,
	position=position,
	orientation=orientation,
	euler_seq="xyz",
	euler_degrees=True,
	)
	)

	def _integr(self, m, n):
	"""Returns an integration function where m is the state dimension, and n is the number of trajectory points."""
	xd = cs.SX.sym("xd", m)
	x0 = cs.SX.sym("x0", m)
	x1 = cs.SX.sym("x1", m)
	dt = cs.SX.sym("dt")
	integr = cs.Function("integr", [x0, x1, xd, dt], [x0 + dt * xd - x1])
	return integr.map(n)

	def integrate_model_states(self, name, time_deriv, dt=None):
	"""Integrates the model states over time.

	Syntax
	------

	builder.integrate_model_states(name, time_deriv, dt=None)

	Parameters
	----------

	name (string)
	Name of the model.

	time_deriv (int)
	The time-deriviative required (i.e. position is 0, velocity is 1, etc.).

	dt (None, float, or array-like: casadi.SX, casadi.DM, or list or numpy.ndarray)
	Integration time step.

	"""

	if self.optimize_time and dt is not None:
	raise ValueError("dt is given but user specified optimize_time as True")
	if not self.optimize_time and dt is None:
	raise ValueError("dt is not given")

	model = self.get_model(name)
	xd = self.get_model_states(name, time_deriv)
	x = self.get_model_states(name, time_deriv - 1)
	n = x.shape[1]
	if self.derivs_align:
	xd = xd[:, :-1]

	if self.optimize_time:
	dt = self.get_dt()[:n]
	else:
	dt = cs.vec(dt)
	assert dt.shape[0] in {
	1,
	n - 1,
	}, f"dt should be scalar or have {n-1} elements"
	if dt.shape[0] == 1:
	dt = dt * cs.DM.ones(n - 1)
	dt = cs.vec(dt).T # ensure dt is 1-by-(n-1) array

	if isinstance(model, RobotModel):
	integr = self._integr(model.num_opt_joints, n - 1)
	else:
	integr = self._integr(model.dim, n - 1)
	name = f"__integrate_model_states_{name}_{time_deriv}__"
	self.add_equality_constraint(name, integr(x[:, :-1], x[:, 1:], xd, dt))

	# def test_example():
	# path = examples_path / "example.py"
	# module = import_module(path)

	def get_global_link_rpy(self, link, q):
	"""Get the Roll-Pitch-Yaw angles in the global frame."""
	return Quaternion.fromvec(self.get_global_link_quaternion(link, q)).getrpy()

cmower / optas Goto Github PK

optas's Introduction

OpTaS

Example

Support

Install

Via pip

From source

Build documentation

Run tests

Known Issues

Citation

Contributing

Acknowledgement

optas's People

Contributors

Stargazers

Watchers

Forkers

optas's Issues

License

Documentation

Examples

Recommend Projects

Recommend Topics

Recommend Org