wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-10 12:21:16 +08:00
Code Issues Packages Projects Releases Wiki Activity

add webcam interface + imitation examples

Browse Source
This commit is contained in:
Brian Delhaisse
2019-03-20 11:56:20 +01:00
parent b6f04d52cb
commit fbff80e746
11 changed files with 638 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/imitation/README.md
+6
View File
@@ -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.
examples/imitation/trajectory_reproduction_kuka_dmp.py
+90
View File
@@ -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!")
examples/interfaces/README.md
+4
View File
@@ -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.
examples/interfaces/webcam.py
+34
View File
@@ -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()
pyrobolearn/actions/action.py
+229 -36
View File
@@ -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
pyrobolearn/models/dmp.py
+15 -1
View File
@@ -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
pyrobolearn/policies/dmp_policy.py
+5 -3
View File
@@ -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.")
pyrobolearn/states/state.py
+159 -43
View File
@@ -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
pyrobolearn/states/time_states.py
+90 -13
View File
@@ -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()
pyrobolearn/tools/interfaces/camera/__init__.py
+5 -5
View File
@@ -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
pyrobolearn/worlds/world.py
+1
View File
@@ -9,6 +9,7 @@ Dependencies:
import collections
import multiprocessing
import os
import numpy as np
import cv2
import time