diff --git a/examples/imitation/README.md b/examples/imitation/README.md new file mode 100644 index 0000000..710a2a8 --- /dev/null +++ b/examples/imitation/README.md @@ -0,0 +1,6 @@ +## Imitation learning task + +In this folder, you can run the `trajectory_reproduction_kuka_dmp.py` file. This will create a basic world and load the kuka robot in the world. You can then record trajectories using the mouse and keyboard. Pressing `ctrl+r` will start/stop the recording of the joint trajectories, and `shift+r` will stop the recording phase and start the training phase. +In this example, we train a dynamic movement primitive (DMP) for each joint on the recorded data. Once trained, it will plot the joint values predicted by the trained DMP, and it will try to reproduce the demonstrated trajectories. + +The user can try to select the joints it wants to move/record, or increase/decrease the number of basis functions that are used for each DMP. diff --git a/examples/imitation/trajectory_reproduction_kuka_dmp.py b/examples/imitation/trajectory_reproduction_kuka_dmp.py new file mode 100644 index 0000000..621e507 --- /dev/null +++ b/examples/imitation/trajectory_reproduction_kuka_dmp.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python +"""Example on how to use an imitation learning task in PyRoboLearn using Dynamic Movement Primitives as a policy, +and a mouse keyboard interface. +""" + +# General imports +import numpy as np +import matplotlib.pyplot as plt + +# Import robots and world +from pyrobolearn.simulators import BulletSim +from pyrobolearn.worlds import BasicWorld +from pyrobolearn.robots import KukaIIWA + +# Import related to the IL task (states/actions, policy, env, interface) +from pyrobolearn.states import ExponentialPhaseState, JointPositionState +from pyrobolearn.actions import JointPositionAction +from pyrobolearn.envs import Env +from pyrobolearn.policies import BioDiscreteDMPPolicy +from pyrobolearn.tools.interfaces import MouseKeyboardInterface +from pyrobolearn.tools.bridges import BridgeMouseKeyboardImitationTask +from pyrobolearn.recorders import StateRecorder, ActionRecorder +from pyrobolearn.tasks import ILTask + + +# variables +joint_ids = None # None for all the actuated joints, or you can select which joint you want to move; e.g. [0, 1, 2] +num_basis = 20 +rate = 30 + +# Create simulator +sim = BulletSim() + +# create world +world = BasicWorld(sim) + +# load robot in the world +robot = world.loadRobot(KukaIIWA) +print("Robot's actuated joint ids: {}".format(robot.joints)) + +# create state/action +state = ExponentialPhaseState(rate=rate) +action = JointPositionAction(robot, joint_ids=joint_ids) +print("State: {}".format(state)) +print("Action: {}".format(action)) + +# create environment +env = Env(world, state) + +# create DMP policy +policy = BioDiscreteDMPPolicy(action, state, num_basis=num_basis, rate=rate) + +# create interface/bridge +interface = MouseKeyboardInterface(sim) +bridge = BridgeMouseKeyboardImitationTask(world, interface=interface, verbose=True) + +# create recorder +recorder = StateRecorder(JointPositionState(robot, joint_ids=joint_ids), rate=rate) + +# create imitation learning task +task = ILTask(env, policy, interface=bridge, recorders=recorder) + +# record, train, and test policy using the policy +# task.run() + +# record demonstrations +print("\nRecording phase: press `ctrl+r` to start/stop the recording. Once finished, press `shift+r`.") +task.record(signal_from_interface=True) +print("Recording phase: finished the recording!") + +# train policy +print("Training phase: training the policy...") +task.train(signal_from_interface=False) +print("Training phase: policy trained!") + +# plot what the DMP policy has learned by performing a rollout +y, dy, ddy = policy.rollout() +plt.figure() +plt.suptitle('DMP position trajectories in joint space') +for i in range(y.shape[0]): + plt.subplot(3, 3, i+1) + plt.title('q'+str(i)) + plt.plot(y[i]) +plt.tight_layout() +plt.show() + +# test policy +print("Reproduction phase: test policy...") +task.test(num_steps=rate*100, signal_from_interface=False) +print("Reproduction phase: Policy tested!") diff --git a/examples/interfaces/README.md b/examples/interfaces/README.md new file mode 100644 index 0000000..0a26b1a --- /dev/null +++ b/examples/interfaces/README.md @@ -0,0 +1,4 @@ +## Interfaces + +In this folder, you will find examples on what interfaces you can use and on how you can collect the data from them. +You will also be able to connect an interface with an element of the world (in this case, a robot) using bridges, and see that different bridges can lead to different behaviors while getting the data from the same interface. diff --git a/examples/interfaces/webcam.py b/examples/interfaces/webcam.py new file mode 100644 index 0000000..1c46d6d --- /dev/null +++ b/examples/interfaces/webcam.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python +"""Load the Webcam interface. +""" + +from itertools import count +import matplotlib.pyplot as plt +from pyrobolearn.tools.interfaces.camera.webcam import WebcamInterface + +# create interface +interface = WebcamInterface(use_thread=True, sleep_dt=1./10, verbose=False) + +# plotting using matplotlib in interactive mode +fig = plt.figure() +plot = None +plt.ion() # interactive mode on + +for _ in count(): + # # if don't use thread call `step` or `run` (note that `run` returns the frame but not + # interface.step() + + # get the frame and plot it with matplotlib + frame = interface.frame + if plot is None: + plot = plt.imshow(frame) + else: + plot.set_data(frame) + plt.pause(0.01) + + # check if the figure is closed, and if so, get out of the loop + if not plt.fignum_exists(fig.number): + break + +plt.ioff() # interactive mode off +plt.show() diff --git a/pyrobolearn/actions/action.py b/pyrobolearn/actions/action.py index 346c67a..162bd43 100644 --- a/pyrobolearn/actions/action.py +++ b/pyrobolearn/actions/action.py @@ -5,6 +5,7 @@ This file defines the `Action` class, which is returned by the policy and given """ import numpy as np +import torch import collections from abc import ABCMeta, abstractmethod import gym @@ -52,7 +53,7 @@ class Action(object): Args: actions (list/tuple of Action): list of actions to be combined together (if given, we can not specified data) - data (np.ndarray): data associated to this state + data (np.ndarray): data associated to this action space (gym.space): space associated with the given data Warning: @@ -79,6 +80,7 @@ class Action(object): # The following attributes should normally be set in the child classes self._data = data + self._torch_data = data if data is None else torch.from_numpy(data).float() self._space = space self._distribution = None # for sampling self._normalizer = None @@ -91,7 +93,7 @@ class Action(object): self.add(actions) # reset action - #self.reset() + # self.reset() ############################## # Properties (Getter/Setter) # @@ -108,7 +110,7 @@ class Action(object): """ Set the list of actions. """ - if self.hasData(): + if self.has_data(): raise AttributeError("Trying to add internal actions to the current action while it already has some data. " "A action should be a combination of actions or should contain some kind of data, " "but not both.") @@ -128,7 +130,7 @@ class Action(object): Returns: list of np.ndarray: list of data associated to the action """ - if self.hasData(): + if self.has_data(): return [self._data] return [action._data for action in self._actions] @@ -141,8 +143,17 @@ class Action(object): Args: data: the data to set """ + if self.has_actions(): # combined actions + if not isinstance(data, collections.Iterable): + raise TypeError("data is not an iterator") + if len(self._actions) != len(data): + raise ValueError("The number of actions is different from the number of data segments") + for action, d in zip(self._actions, data): + action.data = d + # one action: change the data - if self.hasData(): + # if self.has_data(): + else: if not isinstance(data, np.ndarray): if isinstance(data, (list, tuple)): data = np.array(data) @@ -151,34 +162,128 @@ class Action(object): else: raise TypeError("Expecting a numpy array, a list/tuple of int/float, or an int/float for 'data'") - if self._data.shape != data.shape: + if self._data is not None and self._data.shape != data.shape: raise ValueError("The given data does not have the same shape as previously.") # clip the value using the space - if self.hasSpace(): - if self.isContinuous(): # continuous case + if self.has_space(): + if self.is_continuous(): # continuous case low, high = self._space.low, self._space.high data = np.clip(data, low, high) - else: # discrete case + else: # discrete case n = self._space.n if data.size == 1: data = np.clip(data, 0, n) self._data = data + self._torch_data = torch.from_numpy(data).float() - else: # combined action + @property + def merged_data(self): + """ + Return the merged data. + """ + # fuse the data + fused_action = self.fuse() + # return the data + return fused_action.data + + @property + def torch_data(self): + """ + Return the data as a list of torch tensors. + """ + if self.has_data(): + return [self._torch_data] + return [action._torch_data for action in self._actions] + + @torch_data.setter + def torch_data(self, data): + """ + Set the torch data and update the numpy version of the data. + + Args: + data (torch.Tensor, list of torch.Tensors): data to set. + """ + if self.has_actions(): # combined actions if not isinstance(data, collections.Iterable): raise TypeError("data is not an iterator") if len(self._actions) != len(data): raise ValueError("The number of actions is different from the number of data segments") for action, d in zip(self._actions, data): - action.data = d + action.torch_data = d + + # one action: change the data + # if self.has_data(): + else: + if isinstance(data, torch.Tensor): + data = data.float() + elif isinstance(data, np.ndarray): + data = torch.from_numpy(data).float() + elif isinstance(data, (list, tuple)): + data = torch.from_numpy(np.array(data)).float() + elif isinstance(data, (int, float)): + data = data * torch.ones(self._data.shape) + 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.") + + # clip the value using the space + if self.has_space(): + if self.is_continuous(): # continuous case + low, high = torch.from_numpy(self._space.low), torch.from_numpy(self._space.high) + data = torch.min(torch.max(data, low), high) + else: # discrete case + n = self._space.n + if data.size == 1: + data = torch.clamp(data, min=0, max=n) + self._torch_data = data + if data.requires_grad: + data = data.detach().numpy() + else: + data = data.numpy() + self._data = data + + @property + def merged_torch_data(self): + """ + Return the merged torch data. + + Returns: + list of torch.Tensor: list of data torch tensors. + """ + # fuse the data + fused_action = self.fuse() + # return the data + return fused_action.torch_data + + @property + def vec_data(self): + """ + Return a vectorized form of the data. + + Returns: + np.array[N]: all the data. + """ + return np.concatenate([data.reshape(-1) for data in self.merged_data]) + + @property + def vec_torch_data(self): + """ + Return a vectorized form of all the torch tensors. + + Returns: + torch.Tensor([N]): all the torch tensors reshaped such that they are unidimensional. + """ + return torch.cat([data.reshape(-1) for data in self.merged_torch_data]) @property def space(self): """ Get the corresponding space. """ - if self.hasSpace(): + if self.has_space(): return [self._space] return [action._space for action in self._actions] @@ -187,7 +292,7 @@ class Action(object): """ Set the corresponding space. This can only be used one time! """ - if self.hasData() and not self.hasSpace() and \ + if self.has_data() and not self.has_space() and \ isinstance(space, (gym.spaces.Box, gym.spaces.Discrete)): self._space = space @@ -214,7 +319,7 @@ class Action(object): """ Return the shape of each action. Some actions, such as camera actions have more than 1 dimension. """ - # if self.hasActions(): + # if self.has_actions(): return [d.shape for d in self.data] # return [self.data.shape] @@ -223,7 +328,7 @@ class Action(object): """ Return the size of each action. """ - # if self.hasActions(): + # if self.has_actions(): return [d.size for d in self.data] # return [len(self.data)] @@ -234,6 +339,13 @@ class Action(object): """ return [len(d.shape) for d in self.data] + @property + def num_dimensions(self): + """ + Return the number of different dimensions (length of shape). + """ + return len(np.unique(self.dimension)) + @property def distribution(self): """ @@ -252,7 +364,8 @@ class Action(object): ########### # Methods # ########### - def isCombinedAction(self): + + def is_combined_actions(self): """ Return a boolean value depending if the action is a combination of actions. @@ -262,12 +375,12 @@ class Action(object): return len(self._actions) > 0 # alias - hasActions = isCombinedAction + has_actions = is_combined_actions - def hasData(self): + def has_data(self): return self._data is not None - def hasSpace(self): + def has_space(self): return self._space is not None def add(self, action): @@ -278,7 +391,7 @@ class Action(object): Args: action (Action, list/tuple of Action): action(s) to add to the internal list of actions """ - if self.hasData(): + if self.has_data(): raise AttributeError("Undefined behavior: a action should be a combination of actions or should contain " "some kind of data, but not both.") if isinstance(action, Action): @@ -303,7 +416,7 @@ class Action(object): Write the action values to the simulator for each action. This has to be overwritten by the child class. """ - if self.hasData(): # write the current action + if self.has_data(): # write the current action self._write(data) else: # read each action if self.actions: @@ -324,7 +437,7 @@ class Action(object): # Returns: # initial action # """ - # if self.hasData(): # reset the current action + # if self.has_data(): # reset the current action # self._reset() # else: # reset each action # for action in self.actions: @@ -345,7 +458,7 @@ class Action(object): # """ # return [len(d.shape) for d in self.data] - def maxDimension(self): + def max_dimension(self): """ Return the maximum dimension. """ @@ -357,13 +470,13 @@ class Action(object): # """ # return [d.size for d in self.data] - def totalSize(self): + def total_size(self): """ Return the total size of the combined action. """ return sum(self.size) - def hasDiscreteValues(self): + def has_discrete_values(self): """ Does the action have discrete values? """ @@ -373,13 +486,13 @@ class Action(object): return [True] return [False] - def isDiscrete(self): + def is_discrete(self): """ If all the actions are discrete, then it is discrete. """ - return all(self.hasDiscreteValues()) + return all(self.has_discrete_values()) - def hasContinuousValues(self): + def has_continuous_values(self): """ Does the action have continuous values? """ @@ -389,11 +502,11 @@ class Action(object): return [True] return [False] - def isContinuous(self): + def is_continuous(self): """ If one of the action is continuous, then the action is considered to be continuous. """ - return any(self.hasContinuousValues()) + return any(self.has_continuous_values()) def bounds(self): """ @@ -428,7 +541,7 @@ class Action(object): Sample some values from the action based on the given distribution. If no distribution is specified, it samples from a uniform distribution (default value). """ - if self.isCombinedAction(): + if self.is_combined_actions(): return [action.sample() for action in self._actions] if self._distribution is None: return @@ -436,7 +549,7 @@ class Action(object): pass raise NotImplementedError - def addNoise(self, noise=None, replace=True): # parameter dependent of the action + def add_noise(self, noise=None, replace=True): # parameter dependent of the action """ Add some noise to the action, and returns it. @@ -446,7 +559,7 @@ class Action(object): if self._data is None: # apply noise for action in self._actions: - action.addNoise(noise=noise) + action.add_noise(noise=noise) else: # add noise to the data noisy_data = self.data + noise @@ -466,6 +579,86 @@ class Action(object): """ pass + def fuse(self, other=None, axis=0): + """ + Fuse the actions that have the same shape together. The axis specified along which axis we concatenate the data. + If multiple actions with different shapes are present, the axis will be the one specified if possible, + otherwise it will be min(dimension, axis). + + Examples: + a0 = JointPositionAction(robot) + a1 = JointVelocityAction(robot) + a = a0 & a1 + print(a) + print(a.shape) + a = a0 + a1 + a.fuse() + print(a) + print(a.shape) + """ + # check argument + if not (other is None or isinstance(other, Action)): + raise TypeError("The 'other' argument should be None or another action.") + + # build list of all the actions + actions = [self] if self.has_data() else self._actions + if other is not None: + if other.has_data(): + actions.append(other) + else: + actions.extend(other._actions) + + # check if only one action + if len(actions) < 2: + return self # do nothing + + # build the dictionary with key=dimension of shape, value=list of actions + dic = {} + for action in actions: + dic.setdefault(len(action._data.shape), []).append(action) + + # traverse the dictionary and fuse corresponding shapes + actions = [] + for key, value in dic.items(): + if len(value) > 1: + # fuse + data = [action._data for action in value] + names = [action.name for action in value] + a = Action(data=np.concatenate(data, axis=min(axis, key)), name='+'.join(names)) + actions.append(a) + else: + # only one action + actions.append(value[0]) + + # return the fused action + if len(actions) == 1: + return actions[0] + return Action(actions) + + def lookfor(self, class_type): + """ + Look for the specified class type/name in the list of internal actions, and returns it. + + Args: + class_type (type, str): class type or name + + Returns: + Action: the corresponding instance of the Action class + """ + # if string, lowercase it + if isinstance(class_type, str): + class_type = class_type.lower() + + # if there is one action + if self.has_data(): + if self.__class__ == class_type or self.__class__.__name__.lower() == class_type: + return self + + # the action has multiple actions, thus we go through each action + for action in self.actions: + if action.__class__ == class_type or action.__class__.__name__.lower() == class_type: + return action + ######################## # Operator Overloading # ######################## @@ -513,7 +706,7 @@ class Action(object): """ Iterator over the actions. """ - if self.isCombinedAction(): + if self.is_combined_actions(): for action in self._actions: yield action else: @@ -548,7 +741,7 @@ class Action(object): item = item.data # check if continuous - # if self.isContinuous(): + # if self.is_continuous(): # low, high = self.bounds() # return np.all(low <= item) and np.all(item <= high) # else: # discrete case @@ -586,7 +779,7 @@ class Action(object): key (int, slice): index of the internal action, or index/indices for the action data value (Action, int/float, array): value to be set """ - if self.isCombinedAction(): + if self.is_combined_actions(): # set/move the action to the specified key if isinstance(value, Action) and isinstance(key, int): self._actions[key] = value diff --git a/pyrobolearn/models/dmp.py b/pyrobolearn/models/dmp.py index 265ae61..1f9065b 100644 --- a/pyrobolearn/models/dmp.py +++ b/pyrobolearn/models/dmp.py @@ -68,6 +68,7 @@ class CS(object): def reset(self): """Reset the phase variable""" self.s = self.init_phase + return self.s def rollout(self, tau=1.0, error_coupling=1.0): """Generate phase variable in an open loop fashion. @@ -116,6 +117,7 @@ class DiscreteCS(CS): def reset(self): """Reset the phase variable""" self.s = self.init_phase + return self.s def step(self, tau=1.0, error_coupling=1.0): """Generate phase value for discrete movements. @@ -161,6 +163,7 @@ class RhythmicCS(CS): def reset(self): """Reset the phase variable""" self.s = self.init_phase + return self.s def step(self, tau=1.0, error_coupling=1.0): r"""Generate phase value for rhythmic movements. @@ -686,6 +689,7 @@ class DMP(object): self.K = self.D**2 / 4. if stiffness is None else stiffness # set up the DMP system + self.prev_s = self.cs.init_phase self.reset() # target forcing term (keep a copy) @@ -828,7 +832,7 @@ class DMP(object): self.y = self.y0.copy() self.dy = self.dy0.copy() # np.zeros(self.num_dmps) self.ddy = self.ddy0.copy() - self.cs.reset() + self.prev_s = self.cs.reset() def step(self, s=None, tau=1.0, error=0.0, forcing_term=None, new_goal=None, external_force=None, rescale_force=True): """Run the DMP transformation system for a single time step. @@ -851,6 +855,10 @@ class DMP(object): elif not isinstance(s, (float, int)): raise TypeError("Expecting the phase 's' to be a float or integer. Instead, I got {}".format(type(s))) + # check if same phase as before + if s == self.prev_s: + return self.y, self.dy, self.ddy + if new_goal is None: new_goal = self.goal @@ -1360,6 +1368,12 @@ class BioDiscreteDMP(DiscreteDMP): # get phase from canonical system if s is None: s = self.cs.step(tau=tau, error_coupling=error_coupling) + elif not isinstance(s, (float, int)): + raise TypeError("Expecting the phase 's' to be a float or integer. Instead, I got {}".format(type(s))) + + # check if same phase as before + if s == self.prev_s: + return self.y, self.dy, self.ddy if new_goal is None: new_goal = self.goal diff --git a/pyrobolearn/policies/dmp_policy.py b/pyrobolearn/policies/dmp_policy.py index 5314365..7c4484f 100644 --- a/pyrobolearn/policies/dmp_policy.py +++ b/pyrobolearn/policies/dmp_policy.py @@ -35,10 +35,10 @@ class DMPPolicy(Policy): def _size(self, x): size = 0 if isinstance(x, (State, Action)): - if x.isDiscrete(): + if x.is_discrete(): size = x.space[0].n else: - size = x.totalSize() + size = x.total_size() elif isinstance(x, np.ndarray): size = x.size elif isinstance(x, torch.Tensor): @@ -50,11 +50,13 @@ class DMPPolicy(Policy): def act(self, state, deterministic=True, to_numpy=True): # return self.model.predict(state, to_numpy=to_numpy) if (self.cnt % self.rate) == 0: + # print("Policy state value: {}".format(state.data[0][0])) self.y, self.dy, self.ddy = self.model.step(state.data[0][0]) self.cnt += 1 # y, dy, ddy = self.model.step() # return np.array([y, dy, ddy]) if isinstance(self.actions, JointPositionAction): + # print("DMP action: {}".format(self.y)) self.actions.data = self.y elif isinstance(self.actions, JointVelocityAction): self.actions.data = self.dy @@ -79,7 +81,7 @@ class DMPPolicy(Policy): # dy = dy.reshape(1, -1) # if len(ddy.shape) == 1: # ddy = ddy.reshape(1, -1) - self.model.imitate(y, plot=True) # dy, ddy, plot=True) # dy, ddy) + self.model.imitate(y, plot=False) # dy, ddy, plot=True) # dy, ddy) else: print("Nothing to imitate.") diff --git a/pyrobolearn/states/state.py b/pyrobolearn/states/state.py index 5c80697..0939c17 100644 --- a/pyrobolearn/states/state.py +++ b/pyrobolearn/states/state.py @@ -6,6 +6,7 @@ models such as policies/controllers, dynamic transition functions, value estimat """ import numpy as np +import torch import collections from abc import ABCMeta, abstractmethod import gym @@ -122,6 +123,7 @@ class State(object): # The following attributes should normally be set in the child classes self._data = data + self._torch_data = data if data is None else torch.from_numpy(data).float() self._space = space self._distribution = None # for sampling self._normalizer = None @@ -152,7 +154,7 @@ class State(object): """ Set the list of states. """ - if self.hasData(): + if self.has_data(): raise AttributeError("Trying to add internal states to the current state while it already has some data. " "A state should be a combination of states or should contain some kind of data, " "but not both.") @@ -172,7 +174,7 @@ class State(object): Returns: list of np.ndarray: list of data associated to the state """ - if self.hasData(): + if self.has_data(): return [self._data] return [state._data for state in self._states] @@ -185,8 +187,17 @@ class State(object): Args: data: the data to set """ + if self.has_states(): # combined states + if not isinstance(data, collections.Iterable): + raise TypeError("data is not an iterator") + if len(self._states) != len(data): + raise ValueError("The number of states is different from the number of data segments") + for state, d in zip(self._states, data): + state.data = d + # one state: change the data - if self.hasData(): + # if self.has_data(): + else: if not isinstance(data, np.ndarray): if isinstance(data, (list, tuple)): data = np.array(data) @@ -194,12 +205,12 @@ class State(object): 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.shape != data.shape: + if self._data is not None and self._data.shape != data.shape: raise ValueError("The given data does not have the same shape as previously.") # clip the value using the space - if self.hasSpace(): - if self.isContinuous(): # continuous case + if self.has_space(): + if self.is_continuous(): # continuous case low, high = self._space.low, self._space.high data = np.clip(data, low, high) else: # discrete case @@ -207,14 +218,7 @@ class State(object): if data.size == 1: data = np.clip(data, 0, n) self._data = data - - else: # combined state - if not isinstance(data, collections.Iterable): - raise TypeError("data is not an iterator") - if len(self._states) != len(data): - raise ValueError("The number of states is different from the number of data segments") - for state, d in zip(self._states, data): - state.data = d + self._torch_data = torch.from_numpy(data).float() @property def merged_data(self): @@ -226,12 +230,103 @@ class State(object): # return the data return fused_state.data + @property + def torch_data(self): + """ + Return the data as a list of torch tensors. + """ + if self.has_data(): + return [self._torch_data] + return [state._torch_data for state in self._states] + + @torch_data.setter + def torch_data(self, data): + """ + Set the torch data and update the numpy version of the data. + + Args: + data (torch.Tensor, list of torch.Tensors): data to set. + """ + if self.has_states(): # combined states + if not isinstance(data, collections.Iterable): + raise TypeError("data is not an iterator") + if len(self._states) != len(data): + raise ValueError("The number of states is different from the number of data segments") + for state, d in zip(self._states, data): + state.torch_data = d + + # one state: change the data + # if self.has_data(): + else: + if isinstance(data, torch.Tensor): + data = data.float() + elif isinstance(data, np.ndarray): + data = torch.from_numpy(data).float() + elif isinstance(data, (list, tuple)): + data = torch.from_numpy(np.array(data)).float() + elif isinstance(data, (int, float)): + data = data * torch.ones(self._data.shape) + 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.") + + # clip the value using the space + if self.has_space(): + if self.is_continuous(): # continuous case + low, high = torch.from_numpy(self._space.low), torch.from_numpy(self._space.high) + data = torch.min(torch.max(data, low), high) + else: # discrete case + n = self._space.n + if data.size == 1: + data = torch.clamp(data, min=0, max=n) + self._torch_data = data + if data.requires_grad: + data = data.detach().numpy() + else: + data = data.numpy() + self._data = data + + @property + def merged_torch_data(self): + """ + Return the merged torch data. + + Returns: + list of torch.Tensor: list of data torch tensors. + """ + # fuse the data + fused_state = self.fuse() + # return the data + return fused_state.torch_data + + @property + def vec_data(self): + """ + Return a vectorized form of the data. + + Returns: + np.array[N]: all the data. + """ + return np.concatenate([data.reshape(-1) for data in self.merged_data]) + + @property + def vec_torch_data(self): + """ + Return a vectorized form of all the torch tensors. + + Returns: + torch.Tensor([N]): all the torch tensors reshaped such that they are unidimensional. + """ + return torch.cat([data.reshape(-1) for data in self.merged_torch_data]) + @property def space(self): """ Get the corresponding space. """ - if self.hasSpace(): + if self.has_space(): return [self._space] return [state._space for state in self._states] @@ -240,7 +335,7 @@ class State(object): """ Set the corresponding space. This can only be used one time! """ - if self.hasData() and not self.hasSpace() and \ + if self.has_data() and not self.has_space() and \ isinstance(space, (gym.spaces.Box, gym.spaces.Discrete)): self._space = space @@ -283,6 +378,13 @@ class State(object): """ return [len(d.shape) for d in self.data] + @property + def num_dimensions(self): + """ + Return the number of different dimensions (length of shape). + """ + return len(np.unique(self.dimension)) + @property def distribution(self): """ @@ -302,7 +404,7 @@ class State(object): # Methods # ########### - def isCombinedState(self): + def is_combined_states(self): """ Return a boolean value depending if the state is a combination of states. @@ -312,12 +414,12 @@ class State(object): return len(self._states) > 0 # alias - hasStates = isCombinedState + has_states = is_combined_states - def hasData(self): + def has_data(self): return self._data is not None - def hasSpace(self): + def has_space(self): return self._space is not None def add(self, state): @@ -328,7 +430,7 @@ class State(object): Args: state (State, list/tuple of State): state(s) to add to the internal list of states """ - if self.hasData(): + if self.has_data(): raise AttributeError("Undefined behavior: a state should be a combination of states or should contain " "some kind of data, but not both.") if isinstance(state, State): @@ -353,7 +455,7 @@ class State(object): Read the state values from the simulator for each state, set it and return their values. This has to be overwritten by the child class. """ - if self.hasData(): # read the current state + if self.has_data(): # read the current state self._read() else: # read each state for state in self.states: @@ -373,7 +475,7 @@ class State(object): Returns: initial state """ - if self.hasData(): # reset the current state + if self.has_data(): # reset the current state self._reset() else: # reset each state for state in self.states: @@ -382,19 +484,19 @@ class State(object): # return the first state data return self.read() - def maxDimension(self): + def max_dimension(self): """ Return the maximum dimension. """ return max(self.dimension) - def totalSize(self): + def total_size(self): """ Return the total size of the combined state. """ return sum(self.size) - def hasDiscreteValues(self): + def has_discrete_values(self): """ Does the state have discrete values? """ @@ -404,13 +506,13 @@ class State(object): return [True] return [False] - def isDiscrete(self): + def is_discrete(self): """ If all the states are discrete, then it is discrete. """ - return all(self.hasDiscreteValues()) + return all(self.has_discrete_values()) - def hasContinuousValues(self): + def has_continuous_values(self): """ Does the state have continuous values? """ @@ -420,11 +522,11 @@ class State(object): return [True] return [False] - def isContinuous(self): + def is_continuous(self): """ If one of the state is continuous, then the state is considered to be continuous. """ - return any(self.hasContinuousValues()) + return any(self.has_continuous_values()) def bounds(self): """ @@ -459,7 +561,7 @@ class State(object): Sample some values from the state based on the given distribution. If no distribution is specified, it samples from a uniform distribution (default value). """ - if self.isCombinedState(): + if self.is_combined_states(): return [state.sample() for state in self._states] if self._distribution is None: return @@ -467,7 +569,7 @@ class State(object): pass raise NotImplementedError - def addNoise(self, noise=None, replace=True): # parameter dependent of the state + def add_noise(self, noise=None, replace=True): # parameter dependent of the state """ Add some noise to the state, and returns it. @@ -477,7 +579,7 @@ class State(object): if self._data is None: # apply noise for state in self._states: - state.addNoise(noise=noise) + state.add_noise(noise=noise) else: # add noise to the data noisy_data = self.data + noise @@ -519,9 +621,9 @@ class State(object): raise TypeError("The 'other' argument should be None or another state.") # build list of all the states - states = [self] if self.hasData() else self._states + states = [self] if self.has_data() else self._states if other is not None: - if other.hasData(): + if other.has_data(): states.append(other) else: states.extend(other._states) @@ -530,7 +632,7 @@ class State(object): if len(states) < 2: return self # do nothing - # build the dictionary with key=dimension of shape, value=state + # build the dictionary with key=dimension of shape, value=list of states dic = {} for state in states: dic.setdefault(len(state._data.shape), []).append(state) @@ -556,11 +658,25 @@ class State(object): def lookfor(self, class_type): """ Look for the specified class type/name in the list of internal states, and returns it. + + Args: + class_type (type, str): class type or name + + Returns: + State: the corresponding instance of the State class """ - if self.hasData(): - return None + # if string, lowercase it + if isinstance(class_type, str): + class_type = class_type.lower() + + # if there is one state + if self.has_data(): + if self.__class__ == class_type or self.__class__.__name__.lower() == class_type: + return self + + # the state has multiple states, thus we go through each state for state in self.states: - if state.__class__ == class_type: + if state.__class__ == class_type or state.__class__.__name__.lower() == class_type: return state ######################## @@ -610,7 +726,7 @@ class State(object): """ Iterator over the states. """ - if self.isCombinedState(): + if self.is_combined_states(): for state in self._states: yield state else: @@ -645,7 +761,7 @@ class State(object): item = item.data # check if continuous - # if self.isContinuous(): + # if self.is_continuous(): # low, high = self.bounds() # return np.all(low <= item) and np.all(item <= high) # else: # discrete case @@ -683,7 +799,7 @@ class State(object): key (int, slice): index of the internal state, or index/indices for the state data value (State, int/float, array): value to be set """ - if self.isCombinedState(): + if self.is_combined_states(): # set/move the state to the specified key if isinstance(value, State) and isinstance(key, int): self._states[key] = value diff --git a/pyrobolearn/states/time_states.py b/pyrobolearn/states/time_states.py index ce8b55d..7812b36 100644 --- a/pyrobolearn/states/time_states.py +++ b/pyrobolearn/states/time_states.py @@ -37,7 +37,7 @@ class AbsoluteTimeState(TimeState): super(AbsoluteTimeState, self).__init__(data=data) def _read(self): - self._data[0] = time.time() + self.data = time.time() class RelativeTimeState(TimeState): @@ -52,11 +52,11 @@ class RelativeTimeState(TimeState): def _reset(self): self.current_time = time.time() - self._data[0] = 0.0 + self.data = 0.0 def _read(self): next_time = time.time() - self._data[0] = next_time - self.current_time + self.data = next_time - self.current_time self.current_time = next_time @@ -71,40 +71,104 @@ class CumulativeTimeState(TimeState): super(CumulativeTimeState, self).__init__(data=data) def _reset(self): - self._data[0] = 0.0 + self.data = 0.0 self.current_time = time.time() def _read(self): next_time = time.time() - self._data[0] += (next_time - self.current_time) + self.data = self._data + (next_time - self.current_time) self.current_time = next_time class PhaseState(TimeState): - r"""Phase State + r"""(Linear) Phase State + + Each call to the phase state will forward linearly in time with a value of `(end - start) / (num_steps - 1)`. + This means, it will take for the phase `num_steps` to reach the `end` value starting from the `start` value. + Once `end` is reached, it will stop forwarding in time, and will return that `end` value. """ - def __init__(self, num_steps=100, max_value=1., rate=1): - data = np.array([0.0]) + def __init__(self, num_steps=100, start=0, end=1., rate=1): self.cnt = 0 self.rate = rate - self.max_value = max_value + self.end_value = end + self.start_value = start + self.sign = np.sign(end - start) if num_steps < 2: num_steps = 2 - self.dphase = float(max_value) / (num_steps - 1) + self.dphase = float((end - start) / (num_steps - 1.)) + data = np.array([start]) - self.dphase super(PhaseState, self).__init__(data=data) def _reset(self): - self._data[0] = 0.0 + self.data = np.array([self.start_value]) - self.dphase self.cnt = 0 def _read(self): if (self.cnt % self.rate) == 0: - if self._data[0] < self.max_value: - self._data[0] += self.dphase + if self.sign > 0 and self._data[0] < self.end_value: + self.data = np.minimum(self._data + self.dphase, self.end_value) + elif self.sign < 0 and self._data[0] > self.end_value: + self.data = np.maximum(self._data + self.dphase, self.end_value) self.cnt += 1 +class ExponentialPhaseState(TimeState): + r"""Exponential Phase State + + Let's assume the phase is described by the following differential equation: `ds/dt = a * s(t)` then solving + it results in `s(t) = s(0) * exp(a * t)`. Initially, `t` starts from `t_0` and is incremented by `dt` at each call, + and reaches `t_f` after `num_steps` specified by the user. + + This class is notably useful for Phase states that decay exponentially. + """ + def __init__(self, num_steps=100, s0=1., sf=None, t0=0., tf=1., a=-1., rate=1): + """ + + Args: + num_steps (int): number of steps to reach `T`. + s0 (float): initial phase value. + sf (float): possible end phase value. Depending on the sign of `a`, it will stop + t0 (float): initial time value. + tf (float): final time value. With the `num_steps` it allows the computation of `dt`. + a (float): speed constant. + rate (int): rate at which to update the phase + """ + if tf < t0: + raise ValueError("The final time value must be bigger than the inital time value; we don't go back in " + "time!") + self.cnt = 0 + self.rate = rate + self.t0, self.tf = t0, tf + self.dt = (tf - t0) / (num_steps - 1) + self.t = self.t0 - self.dt + self.s0, self.sf = s0, sf + self.a = a + data = np.array([self.s0]) * np.exp(self.a * self.t) + super(ExponentialPhaseState, self).__init__(data=data) + + def _reset(self): + self.cnt = 0 + self.t = self.t0 - self.dt + self.data = np.array([self.s0]) * np.exp(self.a * self.t) + + def _read(self): + if (self.cnt % self.rate) == 0: + self.t += self.dt + if self.t < self.tf: + self.data = np.array([self.s0]) * np.exp(self.a * self.t) + if self.sf is not None: + if self.a < 0: + self.data = np.maximum(self._data, self.sf) + elif self.a > 0: + self.data = np.minimum(self._data, self.sf) + self.cnt += 1 + + +# alias +DecayPhaseState = ExponentialPhaseState + + # Tests the different time states if __name__ == '__main__': s = AbsoluteTimeState() @@ -124,6 +188,19 @@ if __name__ == '__main__': print(s.reset()) for i in range(10): print(s()) + print(s.torch_data) + + s = PhaseState(num_steps=100, start=1, end=-1) + print("\nPhase Time State:") + print(s.reset()) + for i in range(100): + print(s()) + + s = ExponentialPhaseState(num_steps=100, s0=1, a=-1) + print("\nPhase Time State:") + print(s.reset()) + for i in range(200): + print(s()) combined = AbsoluteTimeState() + RelativeTimeState() + CumulativeTimeState() fused = AbsoluteTimeState() & RelativeTimeState() & CumulativeTimeState() diff --git a/pyrobolearn/tools/interfaces/camera/__init__.py b/pyrobolearn/tools/interfaces/camera/__init__.py index 478f9d1..74e431f 100644 --- a/pyrobolearn/tools/interfaces/camera/__init__.py +++ b/pyrobolearn/tools/interfaces/camera/__init__.py @@ -3,16 +3,16 @@ from camera import CameraInterface # Webcam -from webcam import WebcamInterface +# from webcam import WebcamInterface # Asus Xtion -from asus_xtion import AsusXtionInterface +# from asus_xtion import AsusXtionInterface # Kinect -from kinect import * +# from kinect import * # FER -from fer import FERInterface +# from fer import FERInterface # OpenPose -from openpose import OpenPoseInterface +# from openpose import OpenPoseInterface diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py index fafbd92..f0d76d1 100644 --- a/pyrobolearn/worlds/world.py +++ b/pyrobolearn/worlds/world.py @@ -9,6 +9,7 @@ Dependencies: import collections import multiprocessing import os +import numpy as np import cv2 import time