diff --git a/examples/imitation/trajectory_reproduction_kuka_dmp.py b/examples/imitation/trajectory_reproduction_kuka_dmp.py index 04436bb..6dc5dd4 100644 --- a/examples/imitation/trajectory_reproduction_kuka_dmp.py +++ b/examples/imitation/trajectory_reproduction_kuka_dmp.py @@ -39,7 +39,7 @@ robot = world.load_robot(KukaIIWA) print("Robot's actuated joint ids: {}".format(robot.joints)) # create state/action -state = ExponentialPhaseState(rate=rate) +state = ExponentialPhaseState(ticks=rate) action = JointPositionAction(robot, joint_ids=joint_ids) print("State: {}".format(state)) print("Action: {}".format(action)) diff --git a/pyrobolearn/states/basic_states.py b/pyrobolearn/states/basic_states.py index fe760de..79420ea 100644 --- a/pyrobolearn/states/basic_states.py +++ b/pyrobolearn/states/basic_states.py @@ -25,8 +25,28 @@ class FixedState(State): This is a dummy fixed state which always returns the value it was initialized with. """ - def __init__(self, value): - super(FixedState, self).__init__(data=value) + + def __init__(self, value, window_size=1, axis=None, ticks=1): + """ + Initialize the dummy fixed state. + + Args: + value (int, float, object): always return this value. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(FixedState, self).__init__(data=value, window_size=window_size, axis=axis, ticks=ticks) class FunctionalState(State): @@ -34,16 +54,40 @@ class FunctionalState(State): This is a state which accepts a function which has to output the data. """ - def __init__(self, function, *args, **kwargs): + + def __init__(self, function, window_size=1, axis=None, ticks=1, *args, **kwargs): + """ + Initialize the functional state. + + Args: + function (callable): callable function or class that has to output the next state data. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + *args: list of arguments given to the function. + **kwargs: dictionary of arguments given to the function. + """ self.function = function self.args, self.kwargs = args, kwargs data = function(*args, **kwargs) # call one time to get data - super(FunctionalState, self).__init__(data=data) + super(FunctionalState, self).__init__(data=data, window_size=window_size, axis=axis, ticks=ticks) def _reset(self): + """Reset the functional state.""" self.data = self.function(*self.args, **self.kwargs) def _read(self): + """Read the next functional state data.""" self.data = self.function(*self.args, **self.kwargs) @@ -53,38 +97,80 @@ class CounterState(State): Counts the number of time this step has been called. """ - def __init__(self, cnt=-1): - self.cnt = cnt + def __init__(self, cnt=0, window_size=1, axis=None, ticks=1): + """ + Initialize the counter state. + + Args: + cnt (int): initial value for the counter. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + self.count = cnt if isinstance(cnt, int): cnt = np.array([cnt]) if not (isinstance(cnt, np.ndarray) and cnt.size == 1 and len(cnt.shape) == 1 and cnt.dtype.kind in np.typecodes['AllInteger']): raise TypeError("Expecting an int, or a numpy array (integer) with size 1") - super(CounterState, self).__init__(data=cnt) + super(CounterState, self).__init__(data=cnt, window_size=window_size, axis=axis, ticks=ticks) def _reset(self): - self.data = self.cnt + """Reset the counter state.""" + self.data = self.count def _read(self): + """Read the next counter state.""" self.data = self._data + 1 class PreviousActionState(State): r"""Previous Action State - This state copies the previous action. + This state copies the previous action data. """ - def __init__(self, action): + def __init__(self, action, window_size=1, axis=None, ticks=1): + """ + Initialize the previous action state. + + Args: + action (Action): action to copy the data from. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ if not isinstance(action, Action): raise TypeError("Expecting the action to be an instance of Action, instead got {}".format(action)) self.action = action - super(PreviousActionState, self).__init__() + super(PreviousActionState, self).__init__(window_size=window_size, axis=axis, ticks=ticks) def _reset(self): + """Reset the action state.""" self.data = self.action.data def _read(self): + """Read the next action state.""" self.data = self.action.data diff --git a/pyrobolearn/states/body_states.py b/pyrobolearn/states/body_states.py index 962fb01..f04e005 100644 --- a/pyrobolearn/states/body_states.py +++ b/pyrobolearn/states/body_states.py @@ -26,16 +26,29 @@ class BodyState(State): """ __metaclass__ = ABCMeta - def __init__(self, body, world=None): + def __init__(self, body, world=None, window_size=1, axis=None, ticks=1): """ Initialize the body state. Args: body (Body, int): body or unique body id. world (None, World): world instance if the body id was given. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. """ - super(BodyState, self).__init__() + super(BodyState, self).__init__(window_size=window_size, axis=axis, ticks=ticks) if not isinstance(body, (Body, int)): raise TypeError("Expecting an instance of Body, or an identifier from the simulator/world.") if isinstance(body, int): @@ -57,18 +70,32 @@ class PositionState(BodyState): """Position of a body in the world. """ - def __init__(self, body, world=None): + def __init__(self, body, world=None, window_size=1, axis=None, ticks=1): """ Initialize the position state. Args: body (Body, int): body or unique body id. world (None, World): world instance if the body id was given. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. """ - super(PositionState, self).__init__(body, world) + super(PositionState, self).__init__(body, world, window_size=window_size, axis=axis, ticks=ticks) self.data = self.body.position def _read(self): + """Read the next body position state.""" self.data = self.body.position @@ -76,18 +103,32 @@ class OrientationState(BodyState): """Orientation of a body in the world. """ - def __init__(self, body, world=None): + def __init__(self, body, world=None, window_size=1, axis=None, ticks=1): """ Initialize the orientation state. Args: body (Body, int): body or unique body id. world (None, World): world instance if the body id was given. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. """ - super(OrientationState, self).__init__(body, world) + super(OrientationState, self).__init__(body, world, window_size=window_size, axis=axis, ticks=ticks) self.data = self.body.orientation def _read(self): + """Read the next body orientation.""" self.data = self.body.orientation @@ -95,16 +136,30 @@ class VelocityState(BodyState): """Velocity of a body in the world """ - def __init__(self, body, world=None): + def __init__(self, body, world=None, window_size=1, axis=None, ticks=1): """ Initialize the velocity state. Args: body (Body, int): body or unique body id. world (None, World): world instance if the body id was given. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. """ - super(VelocityState, self).__init__(body, world) + super(VelocityState, self).__init__(body, world, window_size=window_size, axis=axis, ticks=ticks) self.data = self.body.velocity def _read(self): + """Read the next body velocity state.""" self.data = self.body.velocity diff --git a/pyrobolearn/states/gym_states.py b/pyrobolearn/states/gym_states.py index 37ea999..ee2cc99 100644 --- a/pyrobolearn/states/gym_states.py +++ b/pyrobolearn/states/gym_states.py @@ -32,12 +32,25 @@ class GymState(State): See Also: `GymEnv` """ - def __init__(self, gym_env): + def __init__(self, gym_env, window_size=1, axis=None, ticks=1): """ Initialize the OpenAI Gym state. Args: gym_env (gym.Env): OpenAI gym environment + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. """ # check types @@ -50,7 +63,7 @@ class GymState(State): data = space.sample() # call super constructor - super(GymState, self).__init__(data=data, space=space) + super(GymState, self).__init__(data=data, space=space, window_size=window_size, axis=axis, ticks=ticks) def _reset(self): pass @@ -65,7 +78,7 @@ if __name__ == '__main__': env = gym.make('CartPole-v1') # create gym state - states = GymState(env) + states = GymState(env, window_size=1, axis=None) # print some information print("State: {}".format(states)) diff --git a/pyrobolearn/states/robot_states/joint_states.py b/pyrobolearn/states/robot_states/joint_states.py index c597cfa..27edcb9 100644 --- a/pyrobolearn/states/robot_states/joint_states.py +++ b/pyrobolearn/states/robot_states/joint_states.py @@ -6,7 +6,7 @@ This includes notably the joint positions, velocities, and force/torque states. from abc import ABCMeta -from pyrobolearn.states.robot_states.robot_states import RobotState +from pyrobolearn.states.robot_states.robot_states import RobotState, Robot __author__ = "Brian Delhaisse" @@ -24,15 +24,30 @@ class JointState(RobotState): """ __metaclass__ = ABCMeta - def __init__(self, robot, joint_ids=None): + def __init__(self, robot, joint_ids=None, window_size=1, axis=None, ticks=1): """ Initialize the joint state. Args: - robot (Robot): robot instance - joint_ids (int, int[N]): joint id or list of joint ids + robot (Robot): robot instance. + joint_ids (int, int[N]): joint id or list of joint ids. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. """ - super(JointState, self).__init__(robot) + # check if robot instance + if not isinstance(robot, Robot): + raise TypeError("The 'robot' parameter has to be an instance of Robot") # get the joints of the robot if joint_ids is None: @@ -41,8 +56,7 @@ class JointState(RobotState): joint_ids = [joint_ids] self.joints = joint_ids - # read the data - self._read() + super(JointState, self).__init__(robot, window_size=window_size, axis=axis, ticks=ticks) class JointPositionState(JointState): @@ -51,10 +65,31 @@ class JointPositionState(JointState): Return the joint positions as the state. """ - def __init__(self, robot, joint_ids=None): - super(JointPositionState, self).__init__(robot, joint_ids) + def __init__(self, robot, joint_ids=None, window_size=1, axis=None, ticks=1): + """ + Initialize the joint position state. + + Args: + robot (Robot): robot instance. + joint_ids (int, int[N]): joint id or list of joint ids. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(JointPositionState, self).__init__(robot, joint_ids, window_size=window_size, axis=axis, ticks=ticks) def _read(self): + """Read the next joint position state.""" self.data = self.robot.get_joint_positions(self.joints) @@ -64,10 +99,31 @@ class JointVelocityState(JointState): Return the joint velocities as the state. """ - def __init__(self, robot, joint_ids=None): - super(JointVelocityState, self).__init__(robot, joint_ids) + def __init__(self, robot, joint_ids=None, window_size=1, axis=None, ticks=1): + """ + Initialize the joint velocity state. + + Args: + robot (Robot): robot instance. + joint_ids (int, int[N]): joint id or list of joint ids. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(JointVelocityState, self).__init__(robot, joint_ids, window_size=window_size, axis=axis, ticks=ticks) def _read(self): + """Read the next joint velocity state.""" self.data = self.robot.get_joint_velocities(self.joints) @@ -77,10 +133,31 @@ class JointForceTorqueState(JointState): Return the joint force and torques as the state. """ - def __init__(self, robot, joint_ids=None): - super(JointForceTorqueState, self).__init__(robot, joint_ids) + def __init__(self, robot, joint_ids=None, window_size=1, axis=None, ticks=1): + """ + Initialize the joint force torque state. + + Args: + robot (Robot): robot instance. + joint_ids (int, int[N]): joint id or list of joint ids. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(JointForceTorqueState, self).__init__(robot, joint_ids, window_size=window_size, axis=axis, ticks=ticks) def _read(self): + """Read the next joint force torque state.""" self.data = self.robot.get_joint_torques(self.joints) @@ -91,8 +168,29 @@ class JointAccelerationState(JointState): joint torques and then applied forward dynamics to get the corresponding joint accelerations. """ - def __init__(self, robot, joint_ids=None): - super(JointAccelerationState, self).__init__(robot, joint_ids) + def __init__(self, robot, joint_ids=None, window_size=1, axis=None, ticks=1): + """ + Initialize the joint acceleration state. + + Args: + robot (Robot): robot instance. + joint_ids (int, int[N]): joint id or list of joint ids. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(JointAccelerationState, self).__init__(robot, joint_ids, window_size=window_size, axis=axis, ticks=ticks) def _read(self): + """Read the next joint acceleration state.""" self.data = self.robot.get_joint_accelerations(self.joints) diff --git a/pyrobolearn/states/robot_states/link_states.py b/pyrobolearn/states/robot_states/link_states.py index d54089f..e123234 100644 --- a/pyrobolearn/states/robot_states/link_states.py +++ b/pyrobolearn/states/robot_states/link_states.py @@ -6,7 +6,7 @@ This includes notably the link positions and velocities. from abc import ABCMeta -from pyrobolearn.states.robot_states.robot_states import RobotState +from pyrobolearn.states.robot_states.robot_states import RobotState, Robot __author__ = "Brian Delhaisse" @@ -24,45 +24,104 @@ class LinkState(RobotState): """ __metaclass__ = ABCMeta - def __init__(self, robot, link_ids=None): + def __init__(self, robot, link_ids=None, window_size=1, axis=None, ticks=1): """ Initialize the link state. Args: robot (Robot): robot instance link_ids (int, int[N]): link id or list of link ids + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. """ - super(LinkState, self).__init__(robot) + # check if robot instance + if not isinstance(robot, Robot): + raise TypeError("The 'robot' parameter has to be an instance of Robot") # get links from robot if link_ids is None: link_ids = range(robot.num_links) self.links = link_ids - # read the data - self._read() + # call parent constructor + super(LinkState, self).__init__(robot, window_size=window_size, axis=axis, ticks=ticks) class LinkPositionState(LinkState): r"""Link Position state """ - def __init__(self, robot, link_ids=None, wrt_link_id=None): + def __init__(self, robot, link_ids=None, wrt_link_id=None, window_size=1, axis=None, ticks=1): + """ + Initialize the link position state. + + Args: + robot (Robot): robot instance + link_ids (int, int[N]): link id or list of link ids + wrt_link_id (int, None): link with respect to which we compute the position of the other links. If None, + it will be the base. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ self.wrt_link_id = wrt_link_id - super(LinkPositionState, self).__init__(robot, link_ids) + super(LinkPositionState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks) def _read(self): - self.data = self.robot.get_link_positions(self.links, wrt_link_id=self.wrt_link_id) + """Read the next link position state.""" + return self.robot.get_link_positions(self.links, wrt_link_id=self.wrt_link_id) class LinkWorldPositionState(LinkState): r"""Link World Position state """ - def __init__(self, robot, link_ids=None): - super(LinkWorldPositionState, self).__init__(robot, link_ids) + def __init__(self, robot, link_ids=None, window_size=1, axis=None, ticks=1): + """ + Initialize the link world position state. + + Args: + robot (Robot): robot instance + link_ids (int, int[N]): link id or list of link ids + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(LinkWorldPositionState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks) def _read(self): + """Read the next link world position state.""" self.data = self.robot.get_link_positions(self.links) @@ -70,10 +129,31 @@ class LinkOrientationState(LinkState): r"""Link Orientation state """ - def __init__(self, robot, link_ids=None): - super(LinkOrientationState, self).__init__(robot, link_ids) + def __init__(self, robot, link_ids=None, window_size=1, axis=None, ticks=1): + """ + Initialize the link world orientation state. + + Args: + robot (Robot): robot instance + link_ids (int, int[N]): link id or list of link ids + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(LinkOrientationState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks) def _read(self): # TODO: convert + """Read the next link orientation state.""" self.data = self.robot.get_link_orientations(self.links) @@ -81,10 +161,31 @@ class LinkVelocityState(LinkState): r"""Link velocity state """ - def __init__(self, robot, link_ids=None): - super(LinkVelocityState, self).__init__(robot, link_ids) + def __init__(self, robot, link_ids=None, window_size=1, axis=None, ticks=1): + """ + Initialize the link velocity state. + + Args: + robot (Robot): robot instance + link_ids (int, int[N]): link id or list of link ids + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(LinkVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks) def _read(self): + """Read the next link velocity state.""" self.data = self.robot.get_link_velocities(self.links) @@ -92,10 +193,31 @@ class LinkLinearVelocityState(LinkState): r"""Link linear velocity state """ - def __init__(self, robot, link_ids=None): - super(LinkLinearVelocityState, self).__init__(robot, link_ids) + def __init__(self, robot, link_ids=None, window_size=1, axis=None, ticks=1): + """ + Initialize the link linear velocity state. + + Args: + robot (Robot): robot instance + link_ids (int, int[N]): link id or list of link ids + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(LinkLinearVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks) def _read(self): + """Read the next link linear velocity state.""" self.data = self.robot.get_link_linear_velocities(self.links) @@ -103,8 +225,29 @@ class LinkAngularVelocityState(LinkState): r"""Link angular velocity state """ - def __init__(self, robot, link_ids=None): - super(LinkAngularVelocityState, self).__init__(robot, link_ids) + def __init__(self, robot, link_ids=None, window_size=1, axis=None, ticks=1): + """ + Initialize the link angular velocity state. + + Args: + robot (Robot): robot instance + link_ids (int, int[N]): link id or list of link ids + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(LinkAngularVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks) def _read(self): + """Read the next link angular velocity state.""" self.data = self.robot.get_link_angular_velocities(self.links) diff --git a/pyrobolearn/states/robot_states/robot_states.py b/pyrobolearn/states/robot_states/robot_states.py index 2368169..49d3135 100644 --- a/pyrobolearn/states/robot_states/robot_states.py +++ b/pyrobolearn/states/robot_states/robot_states.py @@ -8,7 +8,7 @@ Dependencies: - `pyrobolearn.robots` """ -from abc import ABCMeta, abstractmethod +from abc import ABCMeta import numpy as np from pyrobolearn.states import State @@ -32,27 +32,36 @@ class RobotState(State): """ __metaclass__ = ABCMeta - def __init__(self, robot): + def __init__(self, robot, window_size=1, axis=None, ticks=1): """ Initialize the robot state. Args: robot (Robot): instance of Robot which allows to access to the robot state + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. """ - super(RobotState, self).__init__() if not isinstance(robot, Robot): raise TypeError("The 'robot' parameter has to be an instance of Robot") self._robot = robot + super(RobotState, self).__init__(window_size=window_size, axis=axis, ticks=ticks) @property def robot(self): """Return the robot instance""" return self._robot - @abstractmethod - def _read(self): - pass - class BasePositionState(RobotState): r"""Base position state @@ -60,11 +69,30 @@ class BasePositionState(RobotState): This is the state that returns the base position with respect to the world frame. """ - def __init__(self, robot): - super(BasePositionState, self).__init__(robot) - self._read() + def __init__(self, robot, window_size=1, axis=None, ticks=1): + """ + Initialize the base position state. + + Args: + robot (Robot): instance of Robot which allows to access to the robot state + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(BasePositionState, self).__init__(robot, window_size=window_size, axis=axis, ticks=ticks) def _read(self): + """Read the base position state data.""" self.data = self.robot.get_base_position() @@ -74,11 +102,30 @@ class BaseHeightState(RobotState): This is the state that returns the base height with respect to the world frame. """ - def __init__(self, robot): - super(BaseHeightState, self).__init__(robot) - self._read() + def __init__(self, robot, window_size=1, axis=None, ticks=1): + """ + Initialize the base height state. + + Args: + robot (Robot): instance of Robot which allows to access to the robot state + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(BaseHeightState, self).__init__(robot, window_size=window_size, axis=axis, ticks=ticks) def _read(self): + """Read the height state data.""" self.data = np.array([self.robot.get_base_position()[-1]]) @@ -88,11 +135,30 @@ class BaseOrientationState(RobotState): This is the state that returns the base orientation with respect to the world frame. """ - def __init__(self, robot): - super(BaseOrientationState, self).__init__(robot) - self._read() + def __init__(self, robot, window_size=1, axis=None, ticks=1): + """ + Initialize the base orientation state. + + Args: + robot (Robot): instance of Robot which allows to access to the robot state + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(BaseOrientationState, self).__init__(robot, window_size=window_size, axis=axis, ticks=ticks) def _read(self): + """Read the orientation state data.""" self.data = self.robot.get_base_orientation() @@ -102,11 +168,30 @@ class BaseLinearVelocityState(RobotState): This is the state that returns the base linear velocity with respect to the world frame. """ - def __init__(self, robot): - super(BaseLinearVelocityState, self).__init__(robot) - self._read() + def __init__(self, robot, window_size=1, axis=None, ticks=1): + """ + Initialize the base linear velocity state. + + Args: + robot (Robot): instance of Robot which allows to access to the robot state + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(BaseLinearVelocityState, self).__init__(robot, window_size=window_size, axis=axis, ticks=ticks) def _read(self): + """Read the base linear velocity state data.""" self.data = self.robot.get_base_linear_velocity() @@ -116,9 +201,28 @@ class BaseAngularVelocityState(RobotState): This is the state that returns the base angular velocity with respect to the world frame. """ - def __init__(self, robot): - super(BaseAngularVelocityState, self).__init__(robot) - self._read() + def __init__(self, robot, window_size=1, axis=None, ticks=1): + """ + Initialize the base position state. + + Args: + robot (Robot): instance of Robot which allows to access to the robot state + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(BaseAngularVelocityState, self).__init__(robot, window_size=window_size, axis=axis, ticks=ticks) def _read(self): + """Read the base angular velocity state data.""" self.data = self.robot.get_base_angular_velocity() diff --git a/pyrobolearn/states/robot_states/sensor_states.py b/pyrobolearn/states/robot_states/sensor_states.py index 1f947c6..288b351 100644 --- a/pyrobolearn/states/robot_states/sensor_states.py +++ b/pyrobolearn/states/robot_states/sensor_states.py @@ -8,8 +8,12 @@ from abc import ABCMeta import collections import numpy as np -from pyrobolearn.states.robot_states.robot_states import RobotState +# from pyrobolearn.states.robot_states.robot_states import RobotState +from pyrobolearn.states.state import State from pyrobolearn.robots.legged_robot import LeggedRobot +from pyrobolearn.robots.sensors.sensor import Sensor +from pyrobolearn.robots.sensors.contact import ContactSensor +from pyrobolearn.robots.sensors.camera import CameraSensor __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -21,21 +25,67 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class SensorState(RobotState): # TODO: define refresh_rate & frequency +class SensorState(State): # RobotState # TODO: define refresh_rate & frequency r"""Sensor state (abstract class) """ __metaclass__ = ABCMeta - def __init__(self, robot): - super(SensorState, self).__init__(robot) + def __init__(self, sensors, window_size=1, axis=None, ticks=1): + """ + Initialize the sensor state. + + Args: + sensors (Sensor, list of Sensor): sensor(s). + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + if not isinstance(sensors, collections.Iterable): + sensors = [sensors] + for sensor in sensors: + if not isinstance(sensor, Sensor): + raise TypeError("Expecting the given 'sensor' to be an instance of `Sensor`, instead got: " + "{}".format(type(sensor))) + self.sensors = sensors + super(SensorState, self).__init__(window_size=window_size, axis=axis, ticks=ticks) class CameraState(SensorState): r"""Camera state """ - def __init__(self, robot, camera=None): - super(CameraState, self).__init__(robot) + def __init__(self, camera, window_size=1, axis=None, ticks=1): + """ + Initialize the camera sensor state. + + Args: + camera (CameraSensor): camera sensor(s). + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + self.camera = camera + super(CameraState, self).__init__(sensors=camera, window_size=window_size, axis=axis, ticks=ticks) def _read(self): pass @@ -47,24 +97,38 @@ class ContactState(SensorState): Return the contact states between a link of the robot and an object in the world (including the floor). """ - def __init__(self, robot, contacts=None): + def __init__(self, contacts, window_size=1, axis=None, ticks=1): """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). + contacts (ContactSensor, list of ContactSensor): list of 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. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. """ - super(ContactState, self).__init__(robot) + if not isinstance(contacts, collections.Iterable): + contacts = [contacts] + for contact in contacts: + if not isinstance(contact, ContactSensor): + raise TypeError("Expecting the given 'contact' to be an instance of `ContactSensor`, instead got: " + "{}".format(type(contact))) self.contacts = contacts - # read the data - self._read() + super(ContactState, self).__init__(sensors=contacts, window_size=window_size, axis=axis, ticks=ticks) def _read(self): - 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]) + contacts = np.array([int(contact.is_in_contact()) for contact in self.contacts]) self.data = contacts @@ -74,13 +138,34 @@ class FeetContactState(ContactState): 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): + def __init__(self, robot, contacts=None, window_size=1, axis=None, ticks=1): + """ + Initialize the feet contact state. + + Args: + robot (LeggedRobot): legged robot. + contacts (ContactSensor, list of ContactSensor, None): list of contact sensors. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ # 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.") + self.robot = robot # check if the contact sensors or link ids are valid if contacts is None: @@ -95,4 +180,4 @@ class FeetContactState(ContactState): else: raise TypeError("Expecting the list of feet ids to be a list of integers.") contacts = feet_ids - super(FeetContactState, self).__init__(robot, contacts) + super(FeetContactState, self).__init__(contacts, window_size=window_size, axis=axis, ticks=ticks) diff --git a/pyrobolearn/states/state.py b/pyrobolearn/states/state.py index 24c97bc..870a5a6 100644 --- a/pyrobolearn/states/state.py +++ b/pyrobolearn/states/state.py @@ -64,7 +64,7 @@ class State(object): """ - def __init__(self, states=(), data=None, space=None, name=None, window_size=1, axis=-1, ticks=1): + def __init__(self, states=(), data=None, space=None, name=None, window_size=1, axis=None, ticks=1): """ Initialize the state. The state contains some kind of data, or is a state combined of other states. @@ -79,12 +79,12 @@ class State(object): current state. The window size has to be bigger than 1. If it is below, it will be set automatically to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, but is given some :attr:`data`. - axis (int): axis to concatenate or stack the states in the current window. If you have a state with shape - (n,), then if the axis is -1 (by default) or 0, it will just concatenate it such that resulting state - has a shape (n*w,) where w is the window size. If the axis is `dim` (in this example, axis=1), then - it will just stack the states present in the window (in this example, it will return the state with - shape (n,w)). The :attr:`axis` attribute is only when the state is not a combination of states, - but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. ticks (int): number of ticks to sleep before getting the next state data. Warning: @@ -128,11 +128,9 @@ class State(object): if self._data is None: self.add(states) - # reset state - self.reset() - # set window self._window = None + self._torch_window = None self.window_size = window_size self.axis = axis @@ -140,6 +138,9 @@ class State(object): self.cnt = 0 self.ticks = int(ticks) + # reset state + self.reset() + ############################## # Properties (Getter/Setter) # ############################## @@ -176,9 +177,20 @@ class State(object): Returns: list of np.ndarray: list of data associated to the state """ + # if the current state has data if self.has_data(): - return [self._data] - return [state._data for state in self._states] + if len(self.window) == 1: + return [self.window[0]] # [self._data] + + # concatenate the data in the window + if self.axis is None: + return [np.concatenate(self.window.queue)] + + # stack the data in the window + return [np.stack(self.window.queue, axis=self.axis)] # stack + + # if multiple states + return [state.data[0] for state in self._states] @data.setter def data(self, data): @@ -228,6 +240,15 @@ class State(object): # set data self._data = data self._torch_data = torch.from_numpy(data).float() + self.window.append(self._data) + self.torch_window.append(self._torch_data) + + # check that the window is full: if not, copy the last data + if len(self.window) != self.window.maxsize: + for _ in range(self.window.maxsize - len(self.window)): + # copy last appended data + self.window.append(self.window[-1]) + self.torch_window.append(self.torch_window[-1]) @property def merged_data(self): @@ -245,8 +266,17 @@ class State(object): 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] + if len(self.torch_window) == 1: + return [self.torch_window[0]] # [self._torch_data] + + # concatenate the data in the window + if self.axis is None: + return [torch.cat(self.torch_window.tolist())] + + # stack the data in the window + return [torch.stack(self.torch_window.tolist(), dim=self.axis)] # stack + + return [state.torch_data[0] for state in self._states] @torch_data.setter def torch_data(self, data): @@ -290,12 +320,19 @@ class State(object): n = self._space.n if data.size == 1: data = torch.clamp(data, min=0, max=n) + + # set data self._torch_data = data - if data.requires_grad: - data = data.detach().numpy() - else: - data = data.numpy() - self._data = data + self._data = data.detach().numpy() if data.requires_grad else data.numpy() + self.torch_window.append(self._torch_data) + self.window.append(self._data) + + # check that the window is full: if not, copy the last data + if len(self.window) != self.window.maxsize: + for _ in range(self.window.maxsize - len(self.window)): + # copy last appended data + self.window.append(self.window[-1]) + self.torch_window.append(self.torch_window[-1]) @property def merged_torch_data(self): @@ -441,6 +478,11 @@ class State(object): """Return the window.""" return self._window + @property + def torch_window(self): + """Return the torch window.""" + return self._torch_window + @property def window_size(self): """Return the window size.""" @@ -454,7 +496,22 @@ class State(object): if not isinstance(size, int): raise TypeError("Expecting the given window size to be an int, instead got: {}".format(type(size))) size = size if size > 0 else 1 + + # create windows self._window = FIFOQueue(maxsize=size) + self._torch_window = FIFOQueue(maxsize=size) + + # add data if present + if self._data is not None: + self._window.append(self._data) + self._torch_window.append(self._torch_data) + + # check that the window is full: if not, copy the last data + if len(self._window) != self._window.maxsize: + for _ in range(self._window.maxsize - len(self._window)): + # copy last appended data + self._window.append(self._window[-1]) + self._torch_window.append(self._torch_window[-1]) @property def axis(self): @@ -464,11 +521,9 @@ class State(object): @axis.setter def axis(self, axis): """Set the axis to concatenate or stack the states in the current window.""" - if axis is None: - axis = -1 - if not isinstance(axis, int): - raise TypeError("Expecting the given axis to be an int, instead got: {}".format(type(axis))) - axis = axis if axis > -2 else -1 + if axis is not None and not isinstance(axis, int): + raise TypeError("Expecting the given axis to be None (concatenate) or an int (stack), instead got: " + "{}".format(type(axis))) self._axis = axis ########### @@ -496,9 +551,11 @@ class State(object): has_states = is_combined_states def has_data(self): + """Check if the state has data.""" return self._data is not None def has_space(self): + """Check if the state has a space.""" return self._space is not None def add(self, state): @@ -536,25 +593,33 @@ class State(object): """ Read the state values from the simulator for each state, set it and return their values. """ - if self.has_data(): # read the current state - self._read() - else: # read each state - for state in self.states: - state._read() + # if time to read + if self.cnt % self.ticks == 0: + + if self.has_data(): # read the current state + self._read() + else: # read each state + for state in self.states: + state._read() + + # increment counter + self.cnt += 1 # return the data - # return self.data + return self.data def _reset(self): """ Reset the state. This has to be overwritten in the child class. """ + self.cnt = 0 self._read() def reset(self): """ Some states need to be reset. It returns the initial state. """ + self.cnt = 0 if self.has_data(): # reset the current state self._reset() else: # reset each state @@ -662,7 +727,7 @@ class State(object): state.add_noise(noise=noise) else: # add noise to the data - noisy_data = self.data + noise + noisy_data = self.data[0] + noise # clip such that the data is within the bounds self.data = noisy_data @@ -715,14 +780,14 @@ class State(object): # 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) + dic.setdefault(len(state.data[0].shape), []).append(state) # traverse the dictionary and fuse corresponding shapes states = [] for key, value in dic.items(): if len(value) > 1: # fuse - data = [state._data for state in value] + data = [state.data[0] for state in value] names = [state.name for state in value] s = State(data=np.concatenate(data, axis=min(axis, key)), name='+'.join(names)) states.append(s) @@ -771,7 +836,7 @@ class State(object): lst.append(')') return '\n'.join(lst) else: - return '%s(%s)' % (self.name, self._data) + return '%s(%s)' % (self.name, self.data[0]) # def __str__(self): # """ @@ -871,7 +936,7 @@ class State(object): """ # if one state, slice the corresponding state data if len(self._states) == 0: - return self._data[key] + return self.data[0][key] # if multiple states if isinstance(key, int): # get one state @@ -898,7 +963,7 @@ class State(object): raise TypeError("Expecting key to be an int, and value to be a state.") else: # set the value on the data directly - self._data[key] = value + self.data[0][key] = value def __add__(self, other): """ @@ -955,7 +1020,7 @@ class State(object): s1 = self._states if self._data is None else OrderedSet([self]) s2 = other._states if other._data is None else OrderedSet([other]) s = s1 - s2 - if len(s) == 1: # just one element + if len(s) == 1: # just one element return s[0] return State(s) diff --git a/pyrobolearn/states/time_states.py b/pyrobolearn/states/time_states.py index 3293687..bc991cb 100644 --- a/pyrobolearn/states/time_states.py +++ b/pyrobolearn/states/time_states.py @@ -33,11 +33,30 @@ class AbsoluteTimeState(TimeState): Returns the absolute time. """ - def __init__(self): + def __init__(self, window_size=1, axis=None, ticks=1): + """ + Initialize the absolule time state. + + Args: + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ data = np.array([time.time()]) - super(AbsoluteTimeState, self).__init__(data=data) + super(AbsoluteTimeState, self).__init__(data=data, window_size=window_size, axis=axis, ticks=ticks) def _read(self): + """Read the next absolute time state.""" self.data = np.array([time.time()]) @@ -47,15 +66,35 @@ class RelativeTimeState(TimeState): Returns the time difference from last time. """ - def __init__(self): + def __init__(self, window_size=1, axis=None, ticks=1): + """ + Initialize the relative time state. + + Args: + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ data = np.array([0.0]) - super(RelativeTimeState, self).__init__(data=data) + super(RelativeTimeState, self).__init__(data=data, window_size=window_size, axis=axis, ticks=ticks) def _reset(self): + """Reset the relative time state.""" self.current_time = time.time() self.data = np.array([0.0]) def _read(self): + """Read the next relative time state.""" next_time = time.time() self.data = next_time - self.current_time self.current_time = next_time @@ -67,15 +106,35 @@ class CumulativeTimeState(TimeState): Return the cumulative time. """ - def __init__(self): + def __init__(self, window_size=1, axis=None, ticks=1): + """ + Initialize the cumulative time state. + + Args: + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ data = np.array([0.0]) - super(CumulativeTimeState, self).__init__(data=data) + super(CumulativeTimeState, self).__init__(data=data, window_size=window_size, axis=axis, ticks=ticks) def _reset(self): + """Reset the cumulative time state.""" self.data = np.array([0.0]) self.current_time = time.time() def _read(self): + """Read the next cumulative time state.""" next_time = time.time() self.data = self._data + (next_time - self.current_time) self.current_time = next_time @@ -89,30 +148,49 @@ class PhaseState(TimeState): Once `end` is reached, it will stop forwarding in time, and will return that `end` value. """ - def __init__(self, num_steps=100, start=0, end=1., rate=1): - self.cnt = 0 + def __init__(self, num_steps=100, start=0, end=1., window_size=1, axis=None, ticks=1): + """ + Initialize the linear phase state. + + Args: + num_steps (int): the number of time steps. + start (float): initial phase value. + end (float): final phase value. Once the phase value is bigger than the final phase value, it will + always return that final phase value. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ self.num_steps = num_steps - self.rate = rate self.end_value = end self.start_value = start self.sign = np.sign(end - start) if num_steps < 2: num_steps = 2 self.dphase = float((end - start) / (num_steps - 1.)) - data = np.array([start]) - self.dphase - super(PhaseState, self).__init__(data=data) + data = np.array([start]) # - self.dphase + super(PhaseState, self).__init__(data=data, window_size=window_size, axis=axis, ticks=ticks) def _reset(self): - self.data = np.array([self.start_value]) - self.dphase - self.cnt = 0 + """Reset the linear phase state.""" + self.data = np.array([self.start_value]) # - self.dphase def _read(self): - if (self.cnt % self.rate) == 0: - 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 + """Read the next linear phase state.""" + 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) class ExponentialPhaseState(TimeState): @@ -124,8 +202,10 @@ class ExponentialPhaseState(TimeState): 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): + + def __init__(self, num_steps=100, s0=1., sf=None, t0=0., tf=1., a=-1., window_size=1, axis=None, ticks=1): """ + Initialize the exponential phase state. Args: num_steps (int): number of steps to reach `T`. @@ -134,37 +214,46 @@ class ExponentialPhaseState(TimeState): 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 + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. """ 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.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) + super(ExponentialPhaseState, self).__init__(data=data, window_size=window_size, axis=axis, ticks=ticks) def _reset(self): - self.cnt = 0 - self.t = self.t0 - self.dt + """Reset the exponential phase state.""" + 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 + """Read the next exponential phase state.""" + 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) # alias @@ -179,17 +268,39 @@ class RhythmicPhase(PhaseState): automatically from `start` """ - def __init__(self, num_steps=100, start=0, end=1., rate=1): - super(RhythmicPhase, self).__init__(num_steps=num_steps, start=start, end=end, rate=rate) + def __init__(self, num_steps=100, start=0, end=1., window_size=1, axis=None, ticks=1): + """ + Initialize the rhythmic phase state. + + Args: + num_steps (int): the number of time steps. + start (float): initial phase value. + end (float): final phase value. Once the phase value is bigger than the final phase value, it will + always return that final phase value. + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(RhythmicPhase, self).__init__(num_steps=num_steps, start=start, end=end, + window_size=window_size, axis=axis, ticks=ticks) def _read(self): - if (self.cnt % self.rate) == 0: - self.data = self._data + self.dphase - if self.sign > 0 and self._data[0] >= self.end_value: - self.data = np.array([self.start_value]) - if self.sign < 0 and self._data[0] <= self.end_value: - self.data = np.array([self.start_value]) - self.cnt += 1 + """Read the next rhythmic phase state.""" + self.data = self._data + self.dphase + if self.sign > 0 and self._data[0] >= self.end_value: + self.data = np.array([self.start_value]) + if self.sign < 0 and self._data[0] <= self.end_value: + self.data = np.array([self.start_value]) # Tests the different time states @@ -206,7 +317,7 @@ if __name__ == '__main__': for i in range(10): print(s()) - s = CumulativeTimeState() + s = CumulativeTimeState() # window_size=2, axis=0, ticks=2) print("\nCumulative Time State:") print(s.reset()) for i in range(10): @@ -222,7 +333,7 @@ if __name__ == '__main__': s = ExponentialPhaseState(num_steps=100, s0=1, a=-1) print("\nPhase Time State:") print(s.reset()) - for i in range(200): + for i in range(110): print(s()) s = RhythmicPhase(num_steps=10, start=1, end=2) @@ -257,4 +368,4 @@ if __name__ == '__main__': print("") print(s.data) print(s.merged_data) - print(s1.merged_data) \ No newline at end of file + print(s1.merged_data)