From 8968bd1bbf19146b7835b081c34bbd69bef87271 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Tue, 26 Mar 2019 17:26:49 +0100 Subject: [PATCH] update states, actions, tasks, simulators --- pyrobolearn/actions/action.py | 30 +- pyrobolearn/models/nn/neat_model.py | 4 +- pyrobolearn/simulators/bullet_ros.py | 10 +- pyrobolearn/simulators/flex.py | 4 +- pyrobolearn/simulators/gazebo-ros.py | 4 +- pyrobolearn/simulators/gazebo.py | 4 +- pyrobolearn/simulators/isaac.py | 52 ++ pyrobolearn/simulators/mujoco.py | 4 +- pyrobolearn/simulators/opensim.py | 4 +- pyrobolearn/simulators/ros.py | 25 +- pyrobolearn/simulators/ros_rbdl.py | 4 +- pyrobolearn/simulators/simureal.py | 62 -- pyrobolearn/states/__init__.py | 7 + pyrobolearn/states/generators/__init__.py | 3 + .../states/generators/state_generator.py | 609 ++++++++++++++++++ pyrobolearn/states/processors/__init__.py | 3 + .../{ => processors}/state_processor.py | 0 .../states/robot_states/sensor_states.py | 45 +- pyrobolearn/states/state.py | 31 +- pyrobolearn/states/state_generator.py | 180 ------ pyrobolearn/states/time_states.py | 29 +- pyrobolearn/tasks/__init__.py | 3 + pyrobolearn/tasks/distillation.py | 115 ++++ pyrobolearn/tasks/imitation.py | 2 + pyrobolearn/tasks/misc.py | 4 +- pyrobolearn/tasks/reinforcement.py | 2 +- pyrobolearn/tasks/task.py | 2 + .../terminal_conditions/terminal_condition.py | 2 +- 28 files changed, 959 insertions(+), 285 deletions(-) create mode 100644 pyrobolearn/simulators/isaac.py delete mode 100644 pyrobolearn/simulators/simureal.py create mode 100644 pyrobolearn/states/generators/__init__.py create mode 100644 pyrobolearn/states/generators/state_generator.py create mode 100644 pyrobolearn/states/processors/__init__.py rename pyrobolearn/states/{ => processors}/state_processor.py (100%) delete mode 100644 pyrobolearn/states/state_generator.py create mode 100644 pyrobolearn/tasks/distillation.py diff --git a/pyrobolearn/actions/action.py b/pyrobolearn/actions/action.py index 399871a..3c632e5 100644 --- a/pyrobolearn/actions/action.py +++ b/pyrobolearn/actions/action.py @@ -157,6 +157,8 @@ class Action(object): if not isinstance(data, np.ndarray): if isinstance(data, (list, tuple)): data = np.array(data) + if len(data) == 1 and self._data.shape != data.shape: # TODO: check this line + data = data[0] elif isinstance(data, (int, float, np.integer)): # np.integer is for Py3.5 data = data * np.ones(self._data.shape) else: @@ -226,6 +228,7 @@ class Action(object): else: raise TypeError("Expecting a Torch tensor, numpy array, a list/tuple of int/float, or an int/float for" " 'data'") + if self._torch_data.shape != data.shape: raise ValueError("The given data does not have the same shape as previously.") @@ -320,24 +323,45 @@ class Action(object): Return the shape of each action. Some actions, such as camera actions have more than 1 dimension. """ # if self.has_actions(): - return [d.shape for d in self.data] + return [data.shape for data in self.data] # return [self.data.shape] + @property + def merged_shape(self): + """ + Return the shape of each merged action. + """ + return [data.shape for data in self.merged_data] + @property def size(self): """ Return the size of each action. """ # if self.has_actions(): - return [d.size for d in self.data] + return [data.size for data in self.data] # return [len(self.data)] + @property + def merged_size(self): + """ + Return the size of each merged action. + """ + return [data.size for data in self.merged_data] + @property def dimension(self): """ Return the dimension (length of shape) of each action. """ - return [len(d.shape) for d in self.data] + return [len(data.shape) for data in self.data] + + @property + def merged_dimension(self): + """ + Return the dimension (length of shape) of each merged state. + """ + return [len(data.shape) for data in self.merged_data] @property def num_dimensions(self): diff --git a/pyrobolearn/models/nn/neat_model.py b/pyrobolearn/models/nn/neat_model.py index c50666b..74f8490 100644 --- a/pyrobolearn/models/nn/neat_model.py +++ b/pyrobolearn/models/nn/neat_model.py @@ -342,9 +342,9 @@ class NEATModel(object): # Model): # set new network self.model = self.set_network(self.genome, self.config) - def predict(self, x=None): + def predict(self, x=None, to_numpy=True): """Predict the output of the model given the input.""" - return self.model.activate(x) + return np.array(self.model.activate(x)) def save(self, filename): """ diff --git a/pyrobolearn/simulators/bullet_ros.py b/pyrobolearn/simulators/bullet_ros.py index 15ca7fc..4cddd92 100644 --- a/pyrobolearn/simulators/bullet_ros.py +++ b/pyrobolearn/simulators/bullet_ros.py @@ -22,9 +22,13 @@ References: [4] PEP8: https://www.python.org/dev/peps/pep-0008/ """ -from simulator import Simulator -from bullet import Bullet -from ros import ROS +# TODO + +import rospy + +from pyrobolearn.simulators.simulator import Simulator +# from pyrobolearn.simulators.bullet import Bullet +# from pyrobolearn.simulators.ros import ROS __author__ = "Brian Delhaisse" diff --git a/pyrobolearn/simulators/flex.py b/pyrobolearn/simulators/flex.py index 1f91cdb..b8797ae 100644 --- a/pyrobolearn/simulators/flex.py +++ b/pyrobolearn/simulators/flex.py @@ -17,7 +17,9 @@ References: https://sites.google.com/view/accelerated-gpu-simulation/home """ -from simulator import Simulator +# TODO: see Isaac instead... + +from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" diff --git a/pyrobolearn/simulators/gazebo-ros.py b/pyrobolearn/simulators/gazebo-ros.py index a49e998..1317ff8 100644 --- a/pyrobolearn/simulators/gazebo-ros.py +++ b/pyrobolearn/simulators/gazebo-ros.py @@ -14,6 +14,8 @@ References: [3] RBDL: https://rbdl.bitbucket.io/ """ +# TODO: this is not finished + import numpy as np import subprocess, os, signal, sys, time @@ -37,7 +39,7 @@ from gazebo_ros import gazebo_interface import tf.transformations as tft # import PRL -from ros_rbdl import ROS_RBDL +from pyrobolearn.simulators.ros_rbdl import ROS_RBDL __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" diff --git a/pyrobolearn/simulators/gazebo.py b/pyrobolearn/simulators/gazebo.py index e66d422..252a2c4 100644 --- a/pyrobolearn/simulators/gazebo.py +++ b/pyrobolearn/simulators/gazebo.py @@ -15,7 +15,9 @@ References: [1] Gazebo: http://gazebosim.org/ """ -from simulator import Simulator +# TODO + +from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" diff --git a/pyrobolearn/simulators/isaac.py b/pyrobolearn/simulators/isaac.py new file mode 100644 index 0000000..81db76a --- /dev/null +++ b/pyrobolearn/simulators/isaac.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python +"""Define the Isaac SDK simulator API. + +This is the main interface that communicates with the Isaac SDK simulator [1]. By defining this interface, it allows to +decouple the PyRoboLearn framework from the simulator. + +The signature of each method defined here are inspired by [2] but in accordance with the PEP8 style guide [3]. +Parts of the documentation for the methods have been copied-pasted from [2] for completeness purposes. + +Dependencies in PRL: +* `pyrobolearn.simulators.simulator.Simulator` + +References: + [1] Isaac SDK: https://developer.nvidia.com/isaac-sdk + [2] PyBullet Quickstart Guide: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA + [3] PEP8: https://www.python.org/dev/peps/pep-0008/ +""" + +# TODO: waiting for its release at the end of March + +import time +import numpy as np + +from pyrobolearn.simulators.simulator import Simulator + + +__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 Isaac(Simulator): + r"""Isaac simulator. + + "Isaac Sim is a virtual robotics laboratory, a high-fidelity 3D world simulator, that accelerates the research, + design and development of robots by reducing both cost and risk. Developers can quickly and easily train and test + their robots created with the Isaac SDK, in detailed, highly realistic scenarios resulting robots that can safely + operate and cooperate with humans." [1] + + References: + [1] https://developer.nvidia.com/isaac-sdk + [2] https://www.nvidia.com/en-au/deep-learning-ai/industries/robotics/ + [3] "GPU-Accelerated Robotic Simulation for Distributed Reinforcement Learning", Liang et al., 2018 + """ + + def __init__(self, render=True, **kwargs): + super(Isaac, self).__init__() diff --git a/pyrobolearn/simulators/mujoco.py b/pyrobolearn/simulators/mujoco.py index 448f79b..46a73d0 100644 --- a/pyrobolearn/simulators/mujoco.py +++ b/pyrobolearn/simulators/mujoco.py @@ -16,7 +16,9 @@ References: [3] DeepMind Control Suite: https://github.com/deepmind/dm_control/tree/master/dm_control/mujoco """ -from simulator import Simulator +# TODO + +from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" diff --git a/pyrobolearn/simulators/opensim.py b/pyrobolearn/simulators/opensim.py index 20f2c9b..2c742d2 100644 --- a/pyrobolearn/simulators/opensim.py +++ b/pyrobolearn/simulators/opensim.py @@ -16,7 +16,9 @@ References: [3] OpenSim Reinforcement Learning: https://github.com/stanfordnmbl/osim-rl """ -from simulator import Simulator +# TODO + +from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" diff --git a/pyrobolearn/simulators/ros.py b/pyrobolearn/simulators/ros.py index a38c031..e64d696 100644 --- a/pyrobolearn/simulators/ros.py +++ b/pyrobolearn/simulators/ros.py @@ -17,8 +17,11 @@ References: [3] PEP8: https://www.python.org/dev/peps/pep-0008/ """ +# TODO + import rospy -from simulator import Simulator + +from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -51,13 +54,13 @@ class ROS(Simulator): super(ROS, self).__init__() self.models = [] - def load_urdf(self, filename, position=None, orientation=None): - # load URDF: get ros services and ros topics - model = ROSModel(filename) - - # create id and add model to the list of models - idx = len(self.models) - self.models.append(model) - - # return id - return idx + # def load_urdf(self, filename, position=None, orientation=None): + # # load URDF: get ros services and ros topics + # model = ROSModel(filename) + # + # # create id and add model to the list of models + # idx = len(self.models) + # self.models.append(model) + # + # # return id + # return idx diff --git a/pyrobolearn/simulators/ros_rbdl.py b/pyrobolearn/simulators/ros_rbdl.py index e4720a0..ed93c90 100644 --- a/pyrobolearn/simulators/ros_rbdl.py +++ b/pyrobolearn/simulators/ros_rbdl.py @@ -16,10 +16,12 @@ References: [2] RBDL: https://rbdl.bitbucket.io/ """ +# TODO + import rospy import rbdl -from simulator import Simulator +from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" diff --git a/pyrobolearn/simulators/simureal.py b/pyrobolearn/simulators/simureal.py deleted file mode 100644 index 9412a52..0000000 --- a/pyrobolearn/simulators/simureal.py +++ /dev/null @@ -1,62 +0,0 @@ -# This file defines an interface which is used by the robot classes. -# This falls under the "Adapter" design pattern, where we add an abstraction -# layer, by providing a common interface to different simulators and real robots. -# -# The UML diagram is depicted below: -# -# simuRealInterface -----------<> robot / gym-env -# -----^----- -# | | -# ros_rbdl pybullet -# | -# ros_gazebo -# -# where the robot and gym-env classes only interact with children from env_interface. -# -# --- Example --- -# env_gazebo = ros_gazebo() -# robot = Robot(env_gazebo, 'path_to_urdf') -# print(robot.getJointStates()) # will check the joint state in gazebo. -# robot.drawCoM() # will draw a small sphere at the CoM in the gazebo simulator. -# -# env_bullet = pybullet() -# robot.change_env(env_bullet) # change env and reload the urdf in the given env. -# print(robot.getJointStates()) # will check the joint state in pybullet. -# robot.drawCoM() # will draw a small sphere at the CoM in the pybullet simulator. -# -# env_ros = ros_rbdl() # assuming the real robot can send and recv msgs via -# robot.change_env(env_ros) # rostopics/rosservices, you can interact with it. -# print(robot.getJointStates()) # will check the joint state via ros. -# robot.drawCoM() # return error as we can't draw in the real world. -# --------------- -# -# You can thus interact with different simulators or the real robots. -# Simulators: pybullet, pygazebo, ros-gazebo -# -# Warning: the name might change in the future. - - -from abc import ABCMeta, abstractmethod - - -class SimuRealInterface(object): - """Simulation-Reality Interface. - This abstract class must be inherited by any simulators, or real interfaces. - """ - __metaclass__ = ABCMeta - - def __init__(self): - pass - - @abstractmethod - def stepSimulation(self): - raise NotImplementedError("Step simulation is not implemented.") - - @abstractmethod - def render(self): - raise NotImplementedError() - - @abstractmethod - def loadURDF(self, filename, position, orientation): - raise NotImplementedError() - diff --git a/pyrobolearn/states/__init__.py b/pyrobolearn/states/__init__.py index 597c6d3..1f0d34c 100644 --- a/pyrobolearn/states/__init__.py +++ b/pyrobolearn/states/__init__.py @@ -16,3 +16,10 @@ from .robot_states import * # import gym states from .gym_states import * + + +# # import state generators +# from .generators import * +# +# # import state processors +# from .processors import * diff --git a/pyrobolearn/states/generators/__init__.py b/pyrobolearn/states/generators/__init__.py new file mode 100644 index 0000000..998a861 --- /dev/null +++ b/pyrobolearn/states/generators/__init__.py @@ -0,0 +1,3 @@ + +# import state generators +from state_generator import * diff --git a/pyrobolearn/states/generators/state_generator.py b/pyrobolearn/states/generators/state_generator.py new file mode 100644 index 0000000..405550c --- /dev/null +++ b/pyrobolearn/states/generators/state_generator.py @@ -0,0 +1,609 @@ +#!/usr/bin/env python +"""Define various initial state generators. + +The initial state generator generates the initial state which is returned by the environment when calling +`env.reset()`. Note that the state can be generated in a deterministic manner or randomly based on a distribution. + +Dependencies: + - `pyrobolearn.states` + +See Also: + - `pyrobolearn.envs` +""" + +import queue +import numpy as np + +from pyrobolearn.states import State + +__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 StateGenerator(object): + r"""Initial State Generator + + Initialize the state which will be given as the first state by the environment when calling ``env.reset()``. + If for instance, the state consists of joint positions and velocities, we can generate them from a distribution + that is given or learned from data. + + The `state generator` is tightly coupled with a `state` object. + + Sometimes a mapping between different states is necessary. For instance, the state generator might generate + human joint states that need first to be mapped to robot joint states in order to initialize the robot. + In this example, a kinematic mapping which is modeled mathematically or learned need to be provided additionally. + This is particularly significant as robot data are lacking, while human data is pretty abundant. + The mapping function has to return a `state` object. + """ + + def __init__(self, state): + """Initialize the state generator. + + Args: + state (State): state instance. + """ + self.state = state + + @property + def state(self): + """Return the state instance.""" + return self._state + + @state.setter + def state(self, state): + """Set the state.""" + if not isinstance(state, State): + raise TypeError("Expecting the given state to be an instance of `State`, instead got: " + "{}".format(type(state))) + self._state = state + + def generate(self, set_data=True): + """Generate the state. + + Args: + set_data (bool): If True, it will set the generated data to the state. + + Returns: + (list of) np.array: state data + """ + raise NotImplementedError + + def __repr__(self): + return self.__class__.__name__ + + def __str__(self): + return self.__class__.__name__ + + def __call__(self, set_data=True): + return self.generate(set_data=set_data) + + +class FixedStateGenerator(StateGenerator): + r"""Fixed Initial State Generator + + This generator returns the same initial state each time it is called. + """ + + def __init__(self, state): + """Initialize the fixed state generator. + + Args: + state (State): state instance. + """ + super(FixedStateGenerator, self).__init__(state) + self.initial_data = self.state.data + + def generate(self, set_data=True): + """Generate the state. + + Args: + set_data (bool): If True, it will set the generated data to the state. + + Returns: + (list of) np.array: state data + """ + if set_data: + self.state.data = self.initial_data + return self.initial_data + + +class QueueStateGenerator(StateGenerator): + r"""Abstract Queue state generator + """ + + def __init__(self, state, queue): + super(QueueStateGenerator, self).__init__(state) + self.queue = queue + self.initial_data = self.state.data + + @property + def queue(self): + """Return the queue.""" + return self._queue + + @queue.setter + def queue(self, q): + if not isinstance(q, queue.Queue): + raise TypeError("Expecting the given queue to be an instance of `queue.Queue`, instead got: " + "{}".format(type(q))) + self._queue = queue + + def put(self, item, block=False, timeout=None): + """Put an item into the queue. + + If optional args 'block' is true and 'timeout' is None (the default), block if necessary until a free slot + is available. If 'timeout' is a non-negative number, it blocks at most 'timeout' seconds and raises + the Full exception if no free slot was available within that time. + Otherwise ('block' is false), put an item on the queue if a free slot is immediately available, else raise + the Full exception ('timeout' is ignored in that case). + """ + if not self.queue.full(): + self.queue.put(item, block=block, timeout=timeout) + + # alias + add = put + + def get(self, block=False, timeout=None): + """Remove and return an item from the queue. + + If optional args 'block' is true and 'timeout' is None (the default), block if necessary until an item is + available. If 'timeout' is a non-negative number, it blocks at most 'timeout' seconds and raises + the Empty exception if no item was available within that time. + Otherwise ('block' is false), return an item if one is immediately available, else raise the Empty exception + ('timeout' is ignored in that case). + """ + if self.queue.empty(): + return self.initial_data + return self.queue.get(block=block, timeout=timeout) + + # alias + pop = get + + def empty(self): + """Return True if the queue is empty, False otherwise (not reliable!).""" + return self.queue.empty() + + def full(self): + """Return True if the queue is full, False otherwise (not reliable!).""" + return self.queue.full() + + def qsize(self): + """Return the approximate size of the queue (not reliable!).""" + return self.queue.qsize() + + def generate(self, set_data=True): + """Generate the state. + + Args: + set_data (bool): If True, it will set the generated data to the state. + + Returns: + (list of) np.array: state data + """ + data = self.get() + if isinstance(data, State): + data = data.data + if set_data: + self.state.data = data + return data + + def __len__(self): + """Return the size of the queue.""" + return self.qsize() + + +class FIFOQueueStateGenerator(QueueStateGenerator): + r"""FIFO Queue Initial State Generator + + Generate the initial state from a FIFO queue. If the queue is empty returns the default initial state. + The queue is filled by the user during training. + """ + + def __init__(self, state, maxsize=0): + """Initialize the FIFO queue state generator. + + Args: + state (State): state instance. + maxsize (int): maximum size of the queue. If :attr:`maxsize` is <= 0, the queue size is infinite. + """ + q = queue.Queue(maxsize) + super(FIFOQueueStateGenerator, self).__init__(state, queue=q) + + +class LIFOQueueStateGenerator(QueueStateGenerator): + r"""LIFO Queue Initial State Generator + + Generate the initial state from a LIFO queue. If the queue is empty returns the default initial state. + The queue is filled by the user during training. + """ + + def __init__(self, state, maxsize=0): + """Initialize the LIFO queue state generator. + + Args: + state (State): state instance. + maxsize (int): maximum size of the queue. If :attr:`maxsize` is <= 0, the queue size is infinite. + """ + q = queue.LifoQueue(maxsize) + super(LIFOQueueStateGenerator, self).__init__(state, queue=q) + + +class PriorityQueueStateGenerator(QueueStateGenerator): + r"""Priority Queue Initial State Generator + + Generate the initial state from a priority queue filled by the user. If empty, it returns the default initial + state. The queue can be filled for instance with states that have high/low uncertainty, or high/low rewards. + + The queue has a limited capacity, and can be used to include states from which the agent/policy performed + poorly during the training. + """ + + def __init__(self, state, maxsize=0, ascending=True): + """Initialize the priority queue state generator. + + Args: + state (State): state instance. + maxsize (int): maximum size of the queue. If :attr:`maxsize` is <= 0, the queue size is infinite. + ascending (bool): if True, the item with the lowest priority will be the first one to be retrieved. + """ + q = queue.PriorityQueue(maxsize) + super(PriorityQueueStateGenerator, self).__init__(state, queue=q) + self.ascending = ascending + + def get(self, block=False, timeout=None): + """Remove and return an item from the queue. + + If optional args 'block' is true and 'timeout' is None (the default), block if necessary until an item is + available. If 'timeout' is a non-negative number, it blocks at most 'timeout' seconds and raises + the Empty exception if no item was available within that time. + Otherwise ('block' is false), return an item if one is immediately available, else raise the Empty exception + ('timeout' is ignored in that case). + """ + if self.queue.empty(): + return self.initial_data + item = self.queue.get(block=block, timeout=timeout) + return item[1] + + def put(self, item, block=False, timeout=None): + """Put an item into the queue. + + If optional args 'block' is true and 'timeout' is None (the default), block if necessary until a free slot + is available. If 'timeout' is a non-negative number, it blocks at most 'timeout' seconds and raises + the Full exception if no free slot was available within that time. + Otherwise ('block' is false), put an item on the queue if a free slot is immediately available, else raise + the Full exception ('timeout' is ignored in that case). + """ + if not self.queue.full(): + if not isinstance(item, tuple) or len(item) != 2: + raise TypeError("Expecting the item to be a tuple of length 2 with (priority number, data), instead " + "got: {}".format(item)) + if not self.ascending: + item = (-item[0], item[1]) + self.queue.put(item, block=block, timeout=timeout) + + # aliases + add = put + pop = get + + +class StateDistributionGenerator(StateGenerator): + r"""Initial State Distribution Generator + + The initial states :math:`s` are generated by a probability distribution :math:`p(s)`, that is :math:`s \sim p(s)`. + The probability distribution can be learned using generative models. + """ + + def __init__(self, state, seed=None): + """Initialize the state distribution generator. + + Args: + state (State): state instance. + seed (None, int): random seed. + """ + super(StateDistributionGenerator, self).__init__(state) + self.seed = seed + + @property + def seed(self): + """Return the random seed.""" + return self._seed + + @seed.setter + def seed(self, seed): + """Set the random seed + + Args: + seed (int): random seed + """ + if seed is not None: + np.random.seed(seed) + + +class UniformStateGenerator(StateDistributionGenerator): + r"""Uniform Initial State Generator + + The initial states are generated by a uniform distribution. If no upper/lower limits are specified, the limits + will be set to be the range of the states. + """ + + def __init__(self, state, low=None, high=None): + """Initialize the state distribution generator. + + Args: + state (State): state instance. + low (None, float, np.array, list of np.array): lower bound + high (None, float, np.array, list of np.array): upper bound + """ + super(UniformStateGenerator, self).__init__(state) + self.low = low + self.high = high + + @property + def low(self): + """Return the lower bound.""" + return self._low + + @low.setter + def low(self, low): + """Set the lower bound.""" + if low is None: + low = [-np.infty] * len(self.state) + elif isinstance(low, (int, float)): + low = [low] * len(self.state) + elif isinstance(low, (list, tuple)): + if len(low) != len(self.state): + raise ValueError("The lower bound doesn't have the same size as the number of states; len(low) = {} " + "and len(state) = {}".format(len(low), len(self.state))) + else: + raise TypeError + self._low = low + + @property + def high(self): + """Return the upper bound.""" + return self._high + + @high.setter + def high(self, high): + """Set the higher bound.""" + if high is None: + high = [-np.infty] * len(self.state) + elif isinstance(high, (int, float)): + high = [high] * len(self.state) + elif isinstance(high, (list, tuple)): + if len(high) != len(self.state): + raise ValueError("The higher bound doesn't have the same size as the number of states; len(high) = {} " + "and len(state) = {}".format(len(high), len(self.state))) + else: + raise TypeError + self._high = high + + def generate(self, set_data=True): + """Generate the state. + + Args: + set_data (bool): If True, it will set the generated data to the state. + + Returns: + (list of) np.array: state data + """ + spaces = self.state.space + data = [space.sample() for space in spaces] + for idx, datum, low, high in np.clip(zip(data, self.low, self.high)): + data[idx] = np.clip(datum, low, high) + if set_data: + self.state.data = data + return data + + +class NormalStateGenerator(StateDistributionGenerator): + r"""Normal Initial State Generator + + The initial states are generated by a normal distribution, where the mean and standard deviation are specified. + The states are then truncated / clipped to be inside their corresponding range. + """ + + def __init__(self, state, means=0, scales=1.): + """ + Initialize the Normal state generator. + + Args: + state (State): state instance. + means: + scales: + """ + super(NormalStateGenerator, self).__init__(state) + + def generate(self, set_data=True): + pass + + +class GenerativeStateGenerator(StateGenerator): + r"""Generative Initial State Generator + + This uses a generative model that has been trained to learn a distribution to generate the initial states. + """ + + def __init__(self, state, model): + """ + Initialize the Generative initial state generator. + + Args: + state (State): state instance. + model (Model): generative model instance. + """ + super(GenerativeStateGenerator, self).__init__(state) + self.model = model + + +class VAEStateGenerator(GenerativeStateGenerator): + r"""Variational Autoencoder (VAE) Initial State Generator + + This uses the decoder a pretrained VAE to generate initial states. + """ + + def __init__(self, state, model): + super(VAEStateGenerator, self).__init__(state, model) + + def generate(self, set_data=True): + """Generate the state. + + Args: + set_data (bool): If True, it will set the generated data to the state. + + Returns: + (list of) np.array: state data + """ + pass + + +class GANStateGenerator(GenerativeStateGenerator): + r"""Generative Adversarial Network (GAN) Initial State Generator + + This uses the generator of a trained GAN model to generate similar states. + """ + + def __init__(self, state, model, distribution=None, mapping=None): + """ + Initialize the GAN initial state generator. + + Args: + states: states that need to be generated + model: GAN or generator of GAN + distribution: distribution over the noise vector + mapping: + """ + # checking and setting the model + if isinstance(model, GAN): + self.generator = model.get_generator() + elif isinstance(model, Generator): + self.generator = model + else: + raise TypeError("The `model` parameter should be an instance of GAN or Generator.") + + # checking and setting the distribution + if distribution is None: + # create normal distribution with dimension of the generator input + pass + else: + if not isinstance(distribution, Distribution): + raise TypeError("The given `distribution` is not an instance of Distribution.") + + self.distribution = distribution + + # setting mapping + self.mapping = mapping + + super(GANStateGenerator, self).__init__(state, model) + + def generate(self, set_data=True): + """Generate the state. + + Args: + set_data (bool): If True, it will set the generated data to the state. + + Returns: + (list of) np.array: state data + """ + noise_vector = self.distribution.sample() + states = self.generator(noise_vector) + if self.mapping is not None: + return self.mapping(states) + return states + + +class GMMStateGenerator(GenerativeStateGenerator): + r"""Gaussian Mixture Model Initial State Generator + + This uses a pretrained GMM to generate the states. + """ + + def __init__(self, state, model): + super(GMMStateGenerator, self).__init__(state, model) + + def generate(self, set_data=True): + """Generate the state. + + Args: + set_data (bool): If True, it will set the generated data to the state. + + Returns: + (list of) np.array: state data + """ + pass + + +class UncertaintyStateGenerator(StateGenerator): + r"""State generator that exploits the uncertainty of initial states. + """ + pass + + +class BOStateGenerator(UncertaintyStateGenerator): + r"""State generator based on Bayesian Optimization. + + We use Bayesian Optimization to generate the initial states. + """ + pass + + +class AEBOStateGenerator(GenerativeStateGenerator): + r"""AutoEncoder (AE) - Bayesian Optimization (BO) Initial State Generator + + Using a pretrained AE on plausible states, and keeping the decoder allows us to explore in the lower dimensional + state space using BO (GP). The BO will provide the reduced state vector based on the uncertainty / objective + fct value. Then, the outputted vector can be fed to the decoder which will return the corresponding high- + dimensional state. + + In addition, we fix a certain capacity to the kernel matrix of the GP underlying the BO. If when inserting a + new (low-dimensional) state, the capacity is exceeded, the oldest state is removed from the kernel to allow + the incoming state. + + If the states have a certain range, we use the encoder part to get the corresponding low-dimensional state limits. + The exploration will then be carried out in the hyperrectangle formed by these 2 reduced state vector limits. + """ + + def __init__(self, state, model, kernel_capacity=100): + """ + Initialize the autoencoder + bayesian optimization initial state generator. + + Args: + state (State): state instance. + model (Model): autoencoder model instance. + kernel_capacity (int): + """ + super(AEBOStateGenerator, self).__init__(state, model) + + def generate(self, set_data=True): + """Generate the state. + + Args: + set_data (bool): If True, it will set the generated data to the state. + + Returns: + (list of) np.array: state data + """ + pass + + +# # Tests +# if __name__ == '__main__': +# from pyrobolearn.states import AbsoluteTimeState, CumulativeTimeState +# +# s = AbsoluteTimeState() + CumulativeTimeState() +# s = CumulativeTimeState() +# s.data = [2.] +# data = s.data +# print("Initial state: {}".format(data)) +# +# for _ in range(3): +# s() +# s.data = data +# print(s.data) +# print(data) diff --git a/pyrobolearn/states/processors/__init__.py b/pyrobolearn/states/processors/__init__.py new file mode 100644 index 0000000..16f142d --- /dev/null +++ b/pyrobolearn/states/processors/__init__.py @@ -0,0 +1,3 @@ + +# import state processors +from .state_processor import * diff --git a/pyrobolearn/states/state_processor.py b/pyrobolearn/states/processors/state_processor.py similarity index 100% rename from pyrobolearn/states/state_processor.py rename to pyrobolearn/states/processors/state_processor.py diff --git a/pyrobolearn/states/robot_states/sensor_states.py b/pyrobolearn/states/robot_states/sensor_states.py index fa50e3a..1f947c6 100644 --- a/pyrobolearn/states/robot_states/sensor_states.py +++ b/pyrobolearn/states/robot_states/sensor_states.py @@ -5,9 +5,11 @@ This includes notably the camera, contact, IMU, force/torque sensors and others. """ from abc import ABCMeta +import collections +import numpy as np from pyrobolearn.states.robot_states.robot_states import RobotState - +from pyrobolearn.robots.legged_robot import LeggedRobot __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -46,20 +48,51 @@ class ContactState(SensorState): """ def __init__(self, robot, contacts=None): + """Initialize the contact state. + + Args: + robot (Robot): robot instance. + contacts (int, list of int, ContactSensor, list of ContactSensor, None): link id(s) or contact sensor(s). + If None, it will check if the robot has some contact sensors. If there are no contact sensors, it + will check the contact with all the links. + """ super(ContactState, self).__init__(robot) + self.contacts = contacts + # read the data + self._read() def _read(self): - pass + contacts = [self.robot.simulator.get_contact_points(body1=self.robot.id, link1_id=link_id) + for link_id in self.contacts] + contacts = np.array([int(len(contact) > 0) for contact in contacts]) + self.data = contacts class FeetContactState(ContactState): r"""Feet Contact State - Return the contact states between + Return the contact states between the foot of the robot and an object in the world (including the floor). """ def __init__(self, robot, contacts=None): - super(FeetContactState, self).__init__(robot, contacts) + # check if the robot has feet + if not isinstance(robot, LeggedRobot): + raise TypeError("Expecting the robot to be an instance of `LeggedRobot`, instead got: " + "{}".format(type(robot))) + if len(robot.feet) == 0: + raise ValueError("The given robot has no feet; please set the `feet` attribute in the robot.") - def _read(self): - pass + # check if the contact sensors or link ids are valid + if contacts is None: + feet = robot.feet + feet_ids = [] + for foot in feet: + if isinstance(foot, int): + feet_ids.append(foot) + elif isinstance(foot, collections.Iterable): + for f in foot: + feet_ids.append(f) + else: + raise TypeError("Expecting the list of feet ids to be a list of integers.") + contacts = feet_ids + super(FeetContactState, self).__init__(robot, contacts) diff --git a/pyrobolearn/states/state.py b/pyrobolearn/states/state.py index 0939c17..bccec75 100644 --- a/pyrobolearn/states/state.py +++ b/pyrobolearn/states/state.py @@ -201,11 +201,15 @@ class State(object): if not isinstance(data, np.ndarray): if isinstance(data, (list, tuple)): data = np.array(data) + if len(data) == 1 and self._data.shape != data.shape: # TODO: check this line + data = data[0] elif isinstance(data, (int, float)): data = data * np.ones(self._data.shape) else: raise TypeError("Expecting a numpy array, a list/tuple of int/float, or an int/float for 'data'") if self._data is not None and self._data.shape != data.shape: + print(data.shape) + print(self._data.shape) raise ValueError("The given data does not have the same shape as previously.") # clip the value using the space @@ -362,21 +366,42 @@ class State(object): """ Return the shape of each state. Some states, such as camera states have more than 1 dimension. """ - return [d.shape for d in self.data] + return [data.shape for data in self.data] + + @property + def merged_shape(self): + """ + Return the shape of each merged state. + """ + return [data.shape for data in self.merged_data] @property def size(self): """ Return the size of each state. """ - return [d.size for d in self.data] + return [data.size for data in self.data] + + @property + def merged_size(self): + """ + Return the size of each merged state. + """ + return [data.size for data in self.merged_data] @property def dimension(self): """ Return the dimension (length of shape) of each state. """ - return [len(d.shape) for d in self.data] + return [len(data.shape) for data in self.data] + + @property + def merged_dimension(self): + """ + Return the dimension (length of shape) of each merged state. + """ + return [len(data.shape) for data in self.merged_data] @property def num_dimensions(self): diff --git a/pyrobolearn/states/state_generator.py b/pyrobolearn/states/state_generator.py deleted file mode 100644 index 4b3ff7b..0000000 --- a/pyrobolearn/states/state_generator.py +++ /dev/null @@ -1,180 +0,0 @@ - -from abc import ABCMeta, abstractmethod - - -class InitialStateGenerator(object): - r"""Initial State Generator - - Initialize the state which will be given as the first state by the environment when calling ``env.reset()``. - If for instance, the state consists of joint positions and velocities, we can generate them from a distribution - that is given or learned from data. - - The `state generator` is tightly coupled with a `state` object. - - Sometimes a mapping between different states is necessary. For instance, the state generator might generate - human joint states that need first to be mapped to robot joint states in order to initialize the robot. - In this example, a kinematic mapping which is modeled mathematically or learned need to be provided additionally. - This is particularly significant as robot data are lacking, while human data is pretty abundant. - The mapping function has to return a `state` object. - """ - __metaclass__ = ABCMeta - - def __init__(self): - pass - - @abstractmethod - def generate(self): - pass - - -class FixedInitialStateGenerator(InitialStateGenerator): - r"""Fixed Initial State Generator - - This generator returns the same initial state each time it is called. - """ - def __init__(self): - super(FixedInitialStateGenerator, self).__init__() - - -class FIFOQueueInitialStateGenerator(InitialStateGenerator): - r"""FIFO Queue Initial State Generator - - Generate the initial state from a FIFO queue. If the queue is empty returns the default initial state. - The queue is filled by the user during training. - """ - def __init__(self): - super(FIFOQueueInitialStateGenerator, self).__init__() - - -class PriorityQueueInitialStateGenerator(InitialStateGenerator): - r"""Priority Queue Initial State Generator - - Generate the initial state from a priority queue filled by the user. If empty, it returns the default initial - state. The queue can be filled for instance with states that have high/low uncertainty, or high/low rewards. - - The queue has a limited capacity, and can be used to include states from which the agent/policy performed - poorly during the training. - """ - def __init__(self): - pass - - -class DistributedInitialStateGenerator(): - r"""Distributed Initial State Generator - - The initial states :math:`s` are generated by a probability distribution :math:`p(s)`, that is :math:`s \sim p(s)`. - The probability distribution can be learned using generative models. - """ - def __init__(self): - pass - - -class UniformInitialStateGenerator(): - r"""Uniform Initial State Generator - - The initial states are generated by a uniform distribution. If no upper/lower limits are specified, the limits - will be set to be the range of the states. - """ - def __init__(self): - pass - - -class NormalInitialStateGenerator(): - r"""Normal Initial State Generator - - The initial states are generated by a normal distribution, where the mean and standard deviation are specified. - The states are then truncated / clipped to be inside their corresponding range. - """ - def __init__(self): - pass - - -class LearnableInitialStateGenerator(): - r"""Learnable Initial State Generator - - This uses Generative Models. - """ - def __init__(self, model): - self.model = model - - -class AEBOInitialStateGenerator(): - r"""AutoEncoder (AE) - Bayesian Optimization (BO) Initial State Generator - - Using a pretrained AE on plausible states, and keeping the decoder allows us to explore in the lower dimensional - state space using BO (GP). The BO will provide the reduced state vector based on the uncertainty / objective - fct value. Then, the outputted vector can be fed to the decoder which will return the corresponding high- - dimensional state. - - In addition, we fix a certain capacity to the kernel matrix of the GP underlying the BO. If when inserting a - new (low-dimensional) state, the capacity is exceeded, the oldest state is removed from the kernel to allow - the incoming state. - - If the states have a certain range, we use the encoder part to get the corresponding low-dimensional state limits. - The exploration will then be carried out in the hyperrectangle formed by these 2 reduced state vector limits. - """ - def __init__(self, autoencoder, kernel_capacity=100): - pass - - -class VAEInitialStateGenerator(): - r"""Variational Autoencoder (VAE) Initial State Generator - - This uses the decoder a pretrained VAE to generate initial states. - """ - def __init__(self, states, model): - pass - - -class GANInitialStateGenerator(): - r"""Generative Adversarial Network (GAN) Initial State Generator - - This uses the generator of a trained GAN model to generate similar states. - """ - - def __init__(self, states, model, distribution=None, mapping=None): - """ - - :param states: states that need to be generated - :param model: GAN or generator of GAN - :param distribution: distribution over the noise vector - :param mapping: - """ - - # checking and setting the model - if isinstance(model, GAN): - self.generator = model.getGenerator() - elif isinstance(model, Generator): - self.generator = model - else: - raise TypeError("The `model` parameter should be an instance of GAN or Generator.") - - # checking and setting the distribution - if distribution is None: - # create normal distribution with dimension of the generator input - pass - else: - if not isinstance(distribution, Distribution): - raise TypeError("The given `distribution` is not an instance of Distribution.") - - self.distribution = distribution - - # setting mapping - self.mapping = mapping - - def generate(self): - noise_vector = self.distribution.sample() - states = self.generator(noise_vector) - if self.mapping is not None: - return self.mapping(states) - return states - - -class GMMInitialStateGenerator(): - r"""Gaussian Mixture Model Initial State Generator - - This uses a pretrained GMM to generate the states. - """ - - def __init__(self): - pass diff --git a/pyrobolearn/states/time_states.py b/pyrobolearn/states/time_states.py index 3d86021..3293687 100644 --- a/pyrobolearn/states/time_states.py +++ b/pyrobolearn/states/time_states.py @@ -38,7 +38,7 @@ class AbsoluteTimeState(TimeState): super(AbsoluteTimeState, self).__init__(data=data) def _read(self): - self.data = time.time() + self.data = np.array([time.time()]) class RelativeTimeState(TimeState): @@ -53,7 +53,7 @@ class RelativeTimeState(TimeState): def _reset(self): self.current_time = time.time() - self.data = 0.0 + self.data = np.array([0.0]) def _read(self): next_time = time.time() @@ -72,7 +72,7 @@ class CumulativeTimeState(TimeState): super(CumulativeTimeState, self).__init__(data=data) def _reset(self): - self.data = 0.0 + self.data = np.array([0.0]) self.current_time = time.time() def _read(self): @@ -237,5 +237,24 @@ if __name__ == '__main__': print("\nCombined state: {}".format(combined)) print("\nFused state: {}".format(fused)) for i in range(4): - print(combined.read()) - print(fused.read()) + print("combined.read: {}".format(combined.read())) + print("fused.read: {}".format(fused.read())) + + print("Fused does not update the data...") + + s1 = CumulativeTimeState() + s2 = PhaseState() + s3 = AbsoluteTimeState() + s_c1 = s1 + s2 + s_c2 = s2 + s3 + s = s_c1 + s_c2 + print("In the following, we just update the s_c1[s1, s2]: \n") + for i in range(3): + print("s[s_c1, s_c2]: {}".format(s)) + print("s_c1[s1,s2]: {}".format(s_c1)) + print("s_c2[s2,s3]: {}".format(s_c2)) + s_c1() + print("") + print(s.data) + print(s.merged_data) + print(s1.merged_data) \ No newline at end of file diff --git a/pyrobolearn/tasks/__init__.py b/pyrobolearn/tasks/__init__.py index eaa5005..76f511e 100644 --- a/pyrobolearn/tasks/__init__.py +++ b/pyrobolearn/tasks/__init__.py @@ -19,3 +19,6 @@ from .inverse_reinforcement import IRLTask # import curriculum learning task from .curriculum import CLTask + +# import knowledge distillation task +from .distillation import DistillationTask diff --git a/pyrobolearn/tasks/distillation.py b/pyrobolearn/tasks/distillation.py new file mode 100644 index 0000000..e8112ba --- /dev/null +++ b/pyrobolearn/tasks/distillation.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python +"""Define the knowledge distillation task. + +This type of tasks takes one or multiple models that have been trained on one or several tasks, and use them to train +a single model to compress the acquired knowledge. The target model can also be smaller than the source model(s) +reducing the space and possibly the time complexity. + +References: + [1] "Distilling the Knowledge in a Neural Network", Hinton et al., 2015 +""" + +import collections +import torch + +from pyrobolearn.models.model import Model +from pyrobolearn.approximators.approximator import Approximator + +__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 DistillationTask(object): + r"""Knowledge Distillation Task + + This type of tasks takes one or multiple models that have been trained on one or several tasks, and use them to + train a single model to compress the acquired knowledge. The target model can also be smaller than the source + model(s) reducing the space and possibly the time complexity. + + References: + [1] "Distilling the Knowledge in a Neural Network", Hinton et al., 2015 + """ + + def __init__(self, source_models, target_model, datasets=None): + """ + Initialize the Distillation task. + + Args: + source_models ((list of) Approximator / Model / torch.nn.Module): source approximators / learning models. + target_model (Approximator / Model / torch.nn.Module): target approximator / learning model. + datasets (list of Dataset, Dataset): dataset to which train the target approximator / learning model on. + """ + self.source_models = source_models + self.target_model = target_model + self.datasets = datasets + + ############## + # Properties # + ############## + + @property + def source_models(self): + """Return the source models which possess the knowledge.""" + return self._source_models + + @source_models.setter + def source_models(self, models): + """Set the source models.""" + if not isinstance(models, (list, tuple, set)): + models = [models] + for model in models: + if not isinstance(model, (Model, Approximator, torch.nn.Module)): + raise TypeError("Expecting the given source model to be an instance of `Model`, `Approximator`, or " + "`torch.nn.Module`, instead got: {}".format(type(model))) + self._source_models = models + + @property + def target_model(self): + """Return the target model which will contained the distilled knowledge once trained.""" + return self._target_model + + @target_model.setter + def target_model(self, model): + """Set the target model.""" + if not isinstance(model, (Model, Approximator, torch.nn.Module)): + raise TypeError("Expecting the given target model to be an instance of `Model`, `Approximator`, or " + "`torch.nn.Module`, instead got: {}".format(type(model))) + self._target_model = model + + @property + def datasets(self): + """Return the datasets.""" + return self._datasets + + @datasets.setter + def datasets(self, datasets): + if not isinstance(datasets, (list, tuple, set)): + datasets = [datasets] + if len(datasets) != len(self.source_models): + raise ValueError("The number of datasets (={}) does not match the number of source models (={})" + ".".format(len(datasets), len(self.source_models))) + for dataset in datasets: + if not isinstance(dataset, torch.utils.data.Dataset): + raise TypeError("Expecting the dataset to be an instance of `Dataset`, or `torch.utils.data.Dataset`," + " instead got: {}".format(type(dataset))) + self._datasets = datasets + + ########### + # Methods # + ########### + + def train(self, datasets=None, method=None): + """ + Train the target model on the provided dataset(s) and the predicted output from the source models. + + Args: + datasets (None): If None, it will use the original datasets given at the initialization. + method (None): specify which method to use to distill the knowledge. + """ + pass diff --git a/pyrobolearn/tasks/imitation.py b/pyrobolearn/tasks/imitation.py index 84b817a..fde2e87 100644 --- a/pyrobolearn/tasks/imitation.py +++ b/pyrobolearn/tasks/imitation.py @@ -351,7 +351,9 @@ class ILTask(Task): dt = 1. / 240 # run several steps in the environment + print('Test: resetting...') self.reset() + # time.sleep(10) for t in count(): if t >= num_steps or self.end_testing: self.end_testing = False diff --git a/pyrobolearn/tasks/misc.py b/pyrobolearn/tasks/misc.py index fafc111..d975a71 100644 --- a/pyrobolearn/tasks/misc.py +++ b/pyrobolearn/tasks/misc.py @@ -2,7 +2,7 @@ """Define the miscellaneous tasks. """ -from pyrobolearn.tasks.tasks import Task, ILTask, RLTask +from pyrobolearn.tasks import Task, ILTask, RLTask __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -35,7 +35,7 @@ class WalkingTask(RLTask): # define world world = World(simulator) - world.setGravity() + world.set_gravity() # define reward rewards = [Reward()] diff --git a/pyrobolearn/tasks/reinforcement.py b/pyrobolearn/tasks/reinforcement.py index ca4f65f..ad90c4d 100644 --- a/pyrobolearn/tasks/reinforcement.py +++ b/pyrobolearn/tasks/reinforcement.py @@ -2,7 +2,7 @@ """Define the reinforcement learning task. """ -# import gym +import gym from pyrobolearn.tasks.task import Task diff --git a/pyrobolearn/tasks/task.py b/pyrobolearn/tasks/task.py index 1885e5a..2aa9155 100644 --- a/pyrobolearn/tasks/task.py +++ b/pyrobolearn/tasks/task.py @@ -237,6 +237,8 @@ class Task(object): """ if render: self.env.render() + else: + self.env.hide() # results = [] rewards = [] diff --git a/pyrobolearn/terminal_conditions/terminal_condition.py b/pyrobolearn/terminal_conditions/terminal_condition.py index 779edd6..0563a5a 100644 --- a/pyrobolearn/terminal_conditions/terminal_condition.py +++ b/pyrobolearn/terminal_conditions/terminal_condition.py @@ -108,7 +108,7 @@ class HasFallen(FailedCondition): def _compute_angle(self): """Compute angle between the initial base up vector and current base up vector.""" - up_vector = get_matrix_from_quaternion(self.robot.get_base_orientation(False))[:, 2] + up_vector = get_matrix_from_quaternion(self.robot.get_base_orientation())[:, 2] angle = np.arccos(np.dot(self.robot.base_up_vector, up_vector)) return angle