mirror of
https://github.com/wassname/pyrobolearn.git
synced 2026-09-09 11:31:38 +08:00
update controller and priorities
This commit is contained in:
Brian Delhaisse
@@ -75,18 +75,21 @@ Check the `README.md` file in the `examples` folder.
|
||||
|
||||
```
|
||||
@misc{delhaisse2019pyrobolearn,
|
||||
author = {Delhaisse, Brian and Rozo, Leonel, and Caldwell, Darwin},
|
||||
author = {Delhaisse, Brian and Xin, Songyan and Rozo, Leonel, and Caldwell, Darwin},
|
||||
title = {PyRoboLearn: A Python Framework for Robot Learning Practitioners},
|
||||
howpublished = {\url{https://github.com/robotlearn/pyrobolearn}},
|
||||
year=2019,
|
||||
}
|
||||
```
|
||||
|
||||
If you use a specific learning model, algorithm, robot, controller, and so on, please cite the corresponding paper. The reference(s) can usually be found in the class documentation (at the end), and sometimes in the README file in the corresponding folder.
|
||||
|
||||
|
||||
## Acknowledgements
|
||||
|
||||
Currently, we mainly use the PyBullet simulator.
|
||||
- *PyBullet, a Python module for physcis simulation for games, robotics and machine learning*,
|
||||
- *PyBullet, a Python module for physics simulation for games, robotics and machine learning*,
|
||||
Erwin Coumans and Yunfei Bai, 2016-2019
|
||||
- references for each robot, model, and others can be found in the corresponding class documentation
|
||||
- Locomotion controllers were provided by Songyan Xin (see `pyrobolearn/controllers/locomotion`).
|
||||
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
#!/usr/bin/env python
|
||||
"""Provide the abstract controller's class.
|
||||
"""
|
||||
|
||||
__author__ = ["Brian Delhaisse"]
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__credits__ = ["Brian Delhaisse"]
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Controller(object):
|
||||
r"""Controller (abstract) class.
|
||||
|
||||
Controllers accept as inputs the robot's state (or observations), and outputs the joint actions. Compared to
|
||||
policies (as defined in `pyrobolearn/policies`), they do not possess any parameters to optimize. They are manually
|
||||
coded by the user. They can however use optimization processes inside the controller, such as quadratic
|
||||
programming.
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
pass
|
||||
|
||||
###########
|
||||
# Methods #
|
||||
###########
|
||||
|
||||
def act(self, *args, **kwargs):
|
||||
pass
|
||||
|
||||
#############
|
||||
# Operators #
|
||||
#############
|
||||
|
||||
def __call__(self, *args, **kwargs):
|
||||
return self.act(*args, **kwargs)
|
||||
|
||||
@@ -5,8 +5,8 @@ In this folder, you will find the code for priority "tasks". The "tasks" defined
|
||||
Most of these "tasks" are represented as a constrained optimization problem, where the "task" consists to minimize a certain objective function while respecting certain equality and inequality constraints. This is the reason why they are not called "constraints" to avoid the confusion with the (inequality and equality) constraints defined in the optimization problem. Most of the time, they are formulated as quadratic programming (QP) optimization problem [1]. Priority tasks are divided between kinematic and dynamic tasks, where the former only takes into account position and velocity information, while the latter also include dynamic information (forces and torques applied on the various bodies). The variables that are thus optimized by the optimization problem depends on the type of problem (kinematic or dynamic) we are dealing with. In the case of a kinematic task, the variables are often the joint (or end-effector) positions and/or velocities, while in the dynamic case, the variables are the joint accelerations and the (reaction) forces applied on the robot.
|
||||
|
||||
Priorities can be divided into two categories: soft and hard priorities.
|
||||
* Soft priorities: each objective function is weigthed by an importance weight where higher weights mean that we give more importance to the corresponding objective function. For instance, we might have a humanoid robot with two arms where each arm has to follow a specific trajectory and where we give the same importance to both "tasks".
|
||||
* hard priorities: the most important constrained optimization problem is first solved, and then the next most important one is solved with an additional (optimization) constraint that the solution has to be in the solution space of the previous one. For instance, it is more important for a humanoid robot to maintain its balance than to follow perfectly a trajectory with its end-effector. This way of putting "tasks" on top of each other is known as the stack of tasks in the robotics community [2].
|
||||
* Soft priorities: each objective function is weigthed by an importance weight where higher weights mean that we give more importance to the corresponding objective function. For instance, we might have a humanoid robot with two arms where each arm has to follow a specific trajectory and where we give the same importance to both "tasks". Soft priorities use task augmentation.
|
||||
* hard priorities: the most important constrained optimization problem is first solved, and then the next most important one is solved with an additional (optimization) constraint that the solution has to be in the solution space of the previous one. For instance, it is more important for a humanoid robot to maintain its balance than to follow perfectly a trajectory with its end-effector. This way of putting "tasks" on top of each other is known as the stack of tasks in the robotics community [2]. Hard priorities exploit the null-space of higher priority tasks.
|
||||
|
||||
Soft and hard priorities can be mixed together as done in the following C++ framework [3].
|
||||
|
||||
|
||||
@@ -0,0 +1,66 @@
|
||||
#!/usr/bin/env python
|
||||
"""Provide the various constraints used in QP.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__credits__ = ["Brian Delhaisse", "OpenSoT (Enrico Mingo Hoffman and Alessio Rocchi)", "Songyan Xin"]
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Constraint(object):
|
||||
r"""Constraint (abstract) class.
|
||||
|
||||
Python implementation of Constraints based on the slides of the OpenSoT framework [1].
|
||||
|
||||
References:
|
||||
[1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN"
|
||||
([code](https://opensot.wixsite.com/opensot),
|
||||
[slides](https://docs.google.com/presentation/d/1kwJsAnVi_3ADtqFSTP8wq3JOGLcvDV_ypcEEjPHnCEA),
|
||||
[tutorial video](https://www.youtube.com/watch?v=yFon-ZDdSyg),
|
||||
[old code](https://github.com/songcheng/OpenSoT)), Rocchi et al., 2015
|
||||
"""
|
||||
|
||||
def __init__(self, model):
|
||||
"""
|
||||
Initialize the Constraint.
|
||||
|
||||
Args:
|
||||
model (robot, str): robot model.
|
||||
"""
|
||||
self._model = model
|
||||
|
||||
##############
|
||||
# Properties #
|
||||
##############
|
||||
|
||||
@property
|
||||
def model(self):
|
||||
"""Return the robot model."""
|
||||
return self._model
|
||||
|
||||
###########
|
||||
# Methods #
|
||||
###########
|
||||
|
||||
def compute(self):
|
||||
pass
|
||||
|
||||
#############
|
||||
# Operators #
|
||||
#############
|
||||
|
||||
def __repr__(self):
|
||||
return self.__class__.__name__
|
||||
|
||||
def __str__(self):
|
||||
return self.__class__.__name__
|
||||
|
||||
def __call__(self):
|
||||
return self.compute()
|
||||
@@ -0,0 +1,183 @@
|
||||
#!/usr/bin/env python
|
||||
"""Provide the various tasks (i.e. objective functions) used in QP.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
from pyrobolearn.priorities.constraints.constraint import Constraint
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__credits__ = ["Brian Delhaisse", "OpenSoT (Enrico Mingo Hoffman and Alessio Rocchi)", "Songyan Xin"]
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
# TODO: take into account constraints
|
||||
# TODO: take into account hard priority tasks
|
||||
class Task(object):
|
||||
r"""Task (abstract) class.
|
||||
|
||||
Python implementation of Tasks based on the slides of the OpenSoT framework [1].
|
||||
|
||||
References:
|
||||
[1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN"
|
||||
([code](https://opensot.wixsite.com/opensot),
|
||||
[slides](https://docs.google.com/presentation/d/1kwJsAnVi_3ADtqFSTP8wq3JOGLcvDV_ypcEEjPHnCEA),
|
||||
[tutorial video](https://www.youtube.com/watch?v=yFon-ZDdSyg),
|
||||
[old code](https://github.com/songcheng/OpenSoT)), Rocchi et al., 2015
|
||||
"""
|
||||
|
||||
def __init__(self, tasks=[], model=None, weight=1., constraints=[]):
|
||||
"""
|
||||
Initialize the task.
|
||||
|
||||
Args:
|
||||
tasks (list of list of Task): list of list of tasks, where the list is ordered by hard priorities, and the
|
||||
nested list contains tasks which
|
||||
model (Robot, str): robot model. If str, it needs to be the path to the URDF.
|
||||
constraints (list of Constraint): list of constraints.
|
||||
"""
|
||||
self._tasks = tasks
|
||||
self._model = model
|
||||
self.weight = weight
|
||||
self._constraints = []
|
||||
|
||||
##############
|
||||
# Properties #
|
||||
##############
|
||||
|
||||
@property
|
||||
def tasks(self):
|
||||
"""Return the tasks."""
|
||||
return self._tasks
|
||||
|
||||
@property
|
||||
def level(self):
|
||||
"""Return the level of the tree."""
|
||||
if self._tasks:
|
||||
return len(self._tasks)
|
||||
else:
|
||||
return 1
|
||||
|
||||
@property
|
||||
def model(self):
|
||||
"""Return the robot model."""
|
||||
return self._model
|
||||
|
||||
@property
|
||||
def weight(self):
|
||||
"""Return the relative weight (used for soft priorities)."""
|
||||
return self._weight
|
||||
|
||||
@weight.setter
|
||||
def weight(self, weight):
|
||||
if not isinstance(weight, (int, float)):
|
||||
raise TypeError("Expecting the relative weight to be an int or float, instead got: {}".format(type(weight)))
|
||||
if weight < 0:
|
||||
raise ValueError("Expecting the relative weight to be positive.")
|
||||
self._weight = weight
|
||||
|
||||
@property
|
||||
def constraints(self):
|
||||
"""Return the constraints."""
|
||||
return self._constraints
|
||||
|
||||
###########
|
||||
# Methods #
|
||||
###########
|
||||
|
||||
def _compute(self): # update
|
||||
"""Compute the task.
|
||||
|
||||
Returns:
|
||||
np.array: A matrix used in QP.
|
||||
np.array: b vector used in QP.
|
||||
"""
|
||||
pass
|
||||
|
||||
def compute(self): # update
|
||||
if self.tasks:
|
||||
for hard_task in self.tasks:
|
||||
results = [soft_task.compute() for soft_task in hard_task]
|
||||
As = np.vstack([result[0] for result in results])
|
||||
bs = np.vstack([result[1] for result in results])
|
||||
# TODO: continue for hard priority tasks
|
||||
return As, bs
|
||||
return self._compute()
|
||||
|
||||
#############
|
||||
# Operators #
|
||||
#############
|
||||
|
||||
def __repr__(self):
|
||||
"""Return a string representing the class."""
|
||||
if self.tasks:
|
||||
tasks = []
|
||||
for i, soft_tasks in enumerate(self.tasks):
|
||||
results = []
|
||||
for task in soft_tasks:
|
||||
if task.weight == 1:
|
||||
results.append(str(task))
|
||||
else:
|
||||
results.append(str(task.weight) + ' * ' + str(task))
|
||||
soft_task = ' + '.join(results)
|
||||
tasks.append('Priority {}: '.format(i+1) + soft_task)
|
||||
return '\n'.join(tasks)
|
||||
return self.__class__.__name__
|
||||
|
||||
def __str__(self):
|
||||
"""Return a string describing the class."""
|
||||
return self.__repr__()
|
||||
|
||||
def __call__(self):
|
||||
return self.compute()
|
||||
|
||||
def __add__(self, other): # TODO: check when other has some tasks
|
||||
"""Add a soft priority task."""
|
||||
if not isinstance(other, Task):
|
||||
raise TypeError("Expecting 'other' to be an instance of Task, instead got: {}".format(type(other)))
|
||||
if len(self.tasks) > 0:
|
||||
tasks = list(self.tasks)
|
||||
tasks[-1].append(other)
|
||||
return Task(tasks=tasks)
|
||||
return Task(tasks=[[self, other]])
|
||||
|
||||
def __div__(self, other):
|
||||
"""Add a hard priority task."""
|
||||
if not isinstance(other, Task):
|
||||
raise TypeError("Expecting 'other' to be an instance of Task, instead got: {}".format(type(other)))
|
||||
tasks = list(self.tasks)
|
||||
if other.tasks:
|
||||
# append all the other tasks
|
||||
for task in other.tasks:
|
||||
tasks.append(task)
|
||||
else:
|
||||
tasks.append([other])
|
||||
return Task(tasks=tasks)
|
||||
|
||||
def __lshift__(self, other):
|
||||
"""Insert a constraint (in-place operation)."""
|
||||
if not isinstance(other, Constraint):
|
||||
raise TypeError("Expecting 'other' to be an instance of Constraint, instead got: {}".format(type(other)))
|
||||
self._constraints.append(other)
|
||||
|
||||
def __mul__(self, other):
|
||||
"""Multiply the task by a relative weight."""
|
||||
if not isinstance(other, (int, float)) or other < 0:
|
||||
raise TypeError("Expecting a positive integer or float for the weight.")
|
||||
self.weight = other
|
||||
|
||||
def __rmul__(self, other):
|
||||
self.__mul__(other)
|
||||
|
||||
|
||||
# Tests
|
||||
if __name__ == '__main__':
|
||||
task1 = Task(weight=2)
|
||||
task2 = Task(weight=3)
|
||||
task = Task(tasks=[[task1, task2], [task1]])
|
||||
print(task)
|
||||
Reference in New Issue
Block a user