diff --git a/examples/environments/inverted_pendulum.py b/examples/environments/inverted_pendulum.py index 77307d8..838a83a 100644 --- a/examples/environments/inverted_pendulum.py +++ b/examples/environments/inverted_pendulum.py @@ -32,12 +32,12 @@ state = trig_position_state + velocity_state print("\nObservation: {}".format(state)) -# create action -action = prl.actions.JointTorqueAction(robot, f_min=-2., f_max=2.) +# create action: \tau_1 +action = prl.actions.JointTorqueAction(robot, bounds=(-2., 2.)) print("\nAction: {}".format(action)) -# create reward/cost +# create reward/cost: ||d(q,q_{target})||^2 + 0.1 * ||\dot{q}||^2 + 0.001 * ||\tau||^2 position_cost = prl.rewards.JointPositionCost(prl.states.JointPositionState(robot), target_state=np.zeros(len(robot.joints)), update_state=True) diff --git a/pyrobolearn/actions/action.py b/pyrobolearn/actions/action.py index 757fe73..d6f9139 100644 --- a/pyrobolearn/actions/action.py +++ b/pyrobolearn/actions/action.py @@ -303,7 +303,7 @@ class Action(object): Set the corresponding space. This can only be used one time! """ if self.has_data() and not self.has_space() and \ - isinstance(space, (gym.spaces.Box, gym.spaces.Discrete)): + isinstance(space, (gym.spaces.Box, gym.spaces.Discrete, gym.spaces.MultiDiscrete)): self._space = space @property @@ -525,8 +525,9 @@ class Action(object): Does the action have discrete values? """ if self._data is None: - return [isinstance(action._space, gym.spaces.Discrete) for action in self._actions] - if isinstance(self._space, gym.spaces.Discrete): + return [isinstance(action._space, (gym.spaces.Discrete, gym.spaces.MultiDiscrete)) + for action in self._actions] + if isinstance(self._space, (gym.spaces.Discrete, gym.spaces.MultiDiscrete)): return [True] return [False] @@ -556,6 +557,7 @@ class Action(object): """ If the action is continuous, it returns the lower and higher bounds of the action. If the action is discrete, it returns the maximum number of discrete values that the action can take. + If the action is multi-discrete, it returns the maximum number of discrete values that each subaction can take. Returns: list/tuple: list of bounds if multiple actions, or bounds of this action @@ -566,6 +568,8 @@ class Action(object): return (self._space.low, self._space.high) elif isinstance(self._space, gym.spaces.Discrete): return (self._space.n,) + elif isinstance(self._space, gym.spaces.MultiDiscrete): + return (self._space.nvec,) raise NotImplementedError def apply(self, fct): diff --git a/pyrobolearn/actions/robot_actions/__init__.py b/pyrobolearn/actions/robot_actions/__init__.py index 8a0a894..087116d 100644 --- a/pyrobolearn/actions/robot_actions/__init__.py +++ b/pyrobolearn/actions/robot_actions/__init__.py @@ -9,5 +9,6 @@ from .joint_actions import JointAction, JointPositionAction, JointPositionChange JointAccelerationAction # import the link / end-effector actions -from .link_actions import LinkAction, LinkPositionAction, LinkPositionChangeAction, LinkVelocityAction, \ - LinkVelocityChangeAction, LinkForceAction +from .link_actions import LinkAction, LinkPositionAction, LinkPositionChangeAction, LinkOrientationAction, \ + LinkOrientationChangeAction, LinkPoseAction, LinkPoseChangeAction, LinkVelocityAction, LinkVelocityChangeAction, \ + LinkForceAction, LinkTorqueAction, LinkWrenchAction, ApplyForceAction, ApplyTorqueAction # , ApplyWrenchAction diff --git a/pyrobolearn/actions/robot_actions/joint_actions.py b/pyrobolearn/actions/robot_actions/joint_actions.py index bce8877..cb4aec9 100644 --- a/pyrobolearn/actions/robot_actions/joint_actions.py +++ b/pyrobolearn/actions/robot_actions/joint_actions.py @@ -7,6 +7,7 @@ This includes notably the joint positions, velocities, and force/torque actions. import copy import numpy as np from abc import ABCMeta +import gym from pyrobolearn.actions.robot_actions.robot_actions import RobotAction, Robot @@ -26,13 +27,17 @@ class JointAction(RobotAction): """ __metaclass__ = ABCMeta - def __init__(self, robot, joint_ids=None): + def __init__(self, robot, joint_ids=None, discrete_values=None): """ Initialize the joint action. Args: robot (Robot): robot instance joint_ids (int, int[N]): joint id or list of joint ids + discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint. + Note that by specifying this, the joint action is no more continuous but becomes discrete. By default, + the first value along the first axis / dimension are the values by default that are set if no data + is provided. """ super(JointAction, self).__init__(robot) @@ -43,13 +48,97 @@ class JointAction(RobotAction): joint_ids = [joint_ids] self.joints = joint_ids + # if discrete values, check the type and set the space + if discrete_values is not None: + + # check the type + if not isinstance(discrete_values, (list, tuple, np.ndarray)): + raise TypeError("Expecting the given 'discrete_values' to be a list/tuple/np.array of float/int, but " + "instead got: {}".format(type(discrete_values))) + + if len(discrete_values) == 0: + raise ValueError("Expecting at least one list of discrete values") + if not isinstance(discrete_values[0], (list, tuple, np.ndarray)): + discrete_values = [discrete_values] + + # check that the number of list of discrete values match the number of joints + if len(discrete_values) != len(self.joints): + raise ValueError("The number of discrete value sets (={}) does not match the number of joints " + "(={})".format(len(discrete_values), len(self.joints))) + + # check the type and shape of each discrete value set, and convert it to numpy arrays + for i, value in enumerate(discrete_values): + if not isinstance(value, (list, tuple, np.ndarray)): + raise TypeError("Expecting each discrete value set to be a list/tuple/np.ndarray, instead got: " + "{} at index {}".format(type(value), i)) + discrete_values[i] = np.asarray(value) + if len(discrete_values.shape) != 1: + raise ValueError("Expecting each discrete value set to be a 1D array, instead got a shape of: " + "{}".format(discrete_values.shape)) + + # set the discrete values + self.discrete_values = discrete_values + + # set the data and the space in the case of discrete values + if self.discrete_values is not None: + # set the space + if len(self.discrete_values) == 1: + self._space = gym.spaces.Discrete(len(self.discrete_values)) + else: + self._space = gym.spaces.MultiDiscrete([len(value) for value in self.discrete_values]) + + # set the data + if isinstance(self._space, gym.spaces.Discrete): + self.data = 0 + else: + self.data = np.zeros(len(self._space.nvec)) + # @property # def size(self): # return len(self.joints) - def bounds(self): - """Return the joint limits.""" - return self.robot.get_joint_limits(self.joints) + def _check_continuous_bounds(self, bounds): + """Check the given continuous bounds.""" + if not isinstance(bounds, (tuple, list, np.ndarray)): + raise TypeError("Expecting the given bounds to be a tuple/list/np.ndarray of float, instead got: " + "{}".format(type(bounds))) + if len(bounds) != 2: + raise ValueError("Expecting the bounds to be of length 2 (i.e. lower and upper bounds), instead got a " + "length of {}".format(len(bounds))) + if bounds[0] is not None and bounds[1] is not None: + bounds = np.asarray(bounds).reshape(2, -1) + if len(self.joints) != bounds.shape[1]: + if bounds.shape[1] == 1: + bounds = np.array([bounds[0, 0] * np.ones(len(self.joints)), + bounds[1, 0] * np.ones(len(self.joints))]) + else: + raise ValueError("Expecting the number of bounds to match up with the number of joints") + else: + bounds = tuple(bounds) + return bounds + + # def _write(self, data): + # """ + # Write the data. + # + # Args: + # data (int, np.ndarray): the data can be discrete or continuous. + # """ + # # if the action is discrete, then the data should be an index, or an array of values from which takes the max + # if self.is_discrete(): + # if isinstance(data, np.ndarray): + # data = np.argmax(data.reshape(-1)) + # data = self.discrete_values[data] + # self._write_continuous(data) + # + # def _write_continuous(self, data): + # """ + # Write the given continuous data. Child method that has to be implement in the child classes. + # + # Args: + # data (np.ndarray): continuous data to be written. + # """ + # raise NotImplementedError def __copy__(self): """Return a shallow copy of the action. This can be overridden in the child class.""" @@ -76,28 +165,45 @@ class JointPositionAction(JointAction): Set the joint positions using position control. """ - def __init__(self, robot, joint_ids=None, kp=None, kd=None, max_force=None): + def __init__(self, robot, joint_ids=None, bounds=(None, None), kp=None, kd=None, max_force=None, + discrete_values=None): """ Initialize the joint position action. Args: robot (Robot): robot instance. joint_ids (int, list of int, None): joint id(s). If None, it will take all the actuated joints. + bounds (tuple of 2 float / np.array[N] / None): lower and upper bound in the case of continuous action. + If None it will use the default joint position limits. kp (float, np.array[N], None): position gain(s) kd (float, np.array[N], None): velocity gain(s) max_force (float, np.array[N], None, bool): maximum motor torques / forces. If None, it will apply the default maximum force values (read from the URDF). + discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint. + Note that by specifying this, the joint action is no more continuous but becomes discrete. By default, + the first value along the first axis / dimension are the values by default that are set if no data + is provided. """ - super(JointPositionAction, self).__init__(robot, joint_ids) + super(JointPositionAction, self).__init__(robot, joint_ids, discrete_values=discrete_values) self.kp, self.kd, self.max_force = kp, kd, max_force - # # check max force and take the one by default - # if self.max_force is None: - # self.max_force = self.robot.get_joint_max_forces(self.joints) - # if np.allclose(self.max_force, 0): - # self.max_force = None + # check max force and take the one by default + if self.max_force is None: + self.max_force = self.robot.get_joint_max_forces(self.joints) + if np.allclose(self.max_force, 0): + self.max_force = None - self.data = robot.get_joint_positions(self.joints) + # set data and space if continuous + if self.discrete_values is None: + self.data = self.robot.get_joint_positions(self.joints) + bounds = self._check_continuous_bounds(bounds) + if bounds == (None, None): + bounds = self.robot.get_joint_limits(self.joints) + self._space = gym.spaces.Box(low=bounds[:, 0], high=bounds[:, 1]) + + def bounds(self): + """Return the joint limits.""" + return self.robot.get_joint_limits(self.joints) def _write(self, data): """apply the action data on the robot.""" @@ -133,20 +239,31 @@ class JointPositionChangeAction(JointPositionAction): changes. If none are provided, it will stay at the current configuration. """ - def __init__(self, robot, joint_ids=None, kp=None, kd=None, max_force=None): + def __init__(self, robot, joint_ids=None, bounds=(None, None), kp=None, kd=None, max_force=None, + discrete_values=None): """ Initialize the joint position change action. Args: robot (Robot): robot instance. joint_ids (int, list of int, None): joint id(s). If None, it will take all the actuated joints. + bounds (tuple of 2 float / np.array[N] / None): lower and upper bound in the case of continuous action. + If None it will use the default joint position limits. kp (float, np.array[N], None): position gain(s) kd (float, np.array[N], None): velocity gain(s) max_force (float, np.array[N], None, bool): maximum motor torques / forces. If True, it will apply the default maximum force values. + discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint. + Note that by specifying this, the joint action is no more continuous but becomes discrete. By default, + the first value along the first axis / dimension are the values by default that are set if no data + is provided. """ - super(JointPositionChangeAction, self).__init__(robot, joint_ids, kp=kp, kd=kd, max_force=max_force) - self.data = np.zeros(len(self.joints)) + super(JointPositionChangeAction, self).__init__(robot, joint_ids, bounds=bounds, kp=kp, kd=kd, + max_force=max_force, discrete_values=discrete_values) + + # set data if continuous + if self.discrete_values is None: + self.data = np.zeros(len(self.joints)) def _write(self, data): """apply the action data on the robot.""" @@ -161,16 +278,32 @@ class JointVelocityAction(JointAction): Set the joint velocities using velocity control. """ - def __init__(self, robot, joint_ids=None): + def __init__(self, robot, joint_ids=None, bounds=(None, None), discrete_values=None): """ Initialize the joint velocity action. Args: robot (Robot): robot instance. joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints. + bounds (tuple of 2 float / np.array[N] / None): lower and upper bound in the case of continuous action. + If None it will use the default joint position limits. + discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint. + Note that by specifying this, the joint action is no more continuous but becomes discrete. By default, + the first value along the first axis / dimension are the values by default that are set if no data + is provided. """ - super(JointVelocityAction, self).__init__(robot, joint_ids) - self.data = robot.get_joint_velocities(self.joints) + super(JointVelocityAction, self).__init__(robot, joint_ids, discrete_values=discrete_values) + + # set data and space if continuous + if self.discrete_values is None: + self.data = robot.get_joint_velocities(self.joints) + bounds = self._check_continuous_bounds(bounds) + if bounds == (None, None): + bounds = self.robot.get_joint_max_velocities(self.joints) + if np.allclose(bounds, 0): + bounds = np.array([-np.infty * np.ones(len(self.joints)), + np.infty * np.ones(len(self.joints))]) + self._space = gym.spaces.Box(low=bounds[:, 0], high=bounds[:, 1]) def _write(self, data): """apply the action data on the robot.""" @@ -185,16 +318,25 @@ class JointVelocityChangeAction(JointAction): velocity changes. If none are provided, it will keep the current joint velocities. """ - def __init__(self, robot, joint_ids=None): + def __init__(self, robot, joint_ids=None, bounds=(None, None), discrete_values=None): """ Initialize the joint velocity change action. Args: robot (Robot): robot instance. joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints. + bounds (tuple of 2 float / np.array[N] / None): lower and upper bound in the case of continuous action. + If None it will use the default joint position limits. + discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint. + Note that by specifying this, the joint action is no more continuous but becomes discrete. By default, + the first value along the first axis / dimension are the values by default that are set if no data + is provided. """ - super(JointVelocityChangeAction, self).__init__(robot, joint_ids) - self.data = np.zeros(len(self.joints)) + super(JointVelocityChangeAction, self).__init__(robot, joint_ids, discrete_values=discrete_values) + + # set data if continuous + if self.discrete_values is None: + self.data = np.zeros(len(self.joints)) def _write(self, data): """apply the action data on the robot.""" @@ -209,7 +351,8 @@ class JointPositionAndVelocityAction(JointAction): given by: :math:`error = kp * (q^* - q) - kd * (\dot{q}^* - \dot{q})`. """ - def __init__(self, robot, joint_ids=None, kp=None, kd=None, max_force=None): + def __init__(self, robot, joint_ids=None, bounds=(None, None), kp=None, kd=None, max_force=None, + discrete_values=None): """ Initialize the joint position and velocity action. @@ -220,12 +363,19 @@ class JointPositionAndVelocityAction(JointAction): kd (float, np.array[N], None): velocity gain(s) max_force (float, np.array[N], None, bool): maximum motor torques / forces. If True, it will apply the default maximum force values. + discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint. + Note that by specifying this, the joint action is no more continuous but becomes discrete. By default, + the first value along the first axis / dimension are the values by default that are set if no data + is provided. """ - super(JointPositionAndVelocityAction, self).__init__(robot, joint_ids) + super(JointPositionAndVelocityAction, self).__init__(robot, joint_ids, discrete_values=discrete_values) self.kp, self.kd, self.max_force = kp, kd, max_force - pos, vel = robot.get_joint_positions(self.joints), robot.get_joint_velocities(self.joints) - self.data = np.concatenate((pos, vel)) - self.idx = len(pos) + + # set data if continuous + if self.discrete_values is None: + pos, vel = robot.get_joint_positions(self.joints), robot.get_joint_velocities(self.joints) + self.data = np.concatenate((pos, vel)) + self.idx = len(self.joints) def _write(self, data): """apply the action data on the robot.""" @@ -261,7 +411,8 @@ class JointPositionAndVelocityChangeAction(JointPositionAndVelocityAction): given by: :math:`error = kp * (q^* - q) - kd * (\dot{q}^* - \dot{q})`. """ - def __init__(self, robot, joint_ids=None, kp=None, kd=None, max_force=None): + def __init__(self, robot, joint_ids=None, bounds=(None, None), kp=None, kd=None, max_force=None, + discrete_values=None): """ Initialize the joint position and velocity change action. @@ -272,9 +423,16 @@ class JointPositionAndVelocityChangeAction(JointPositionAndVelocityAction): kd (float, np.array[N], None): velocity gain(s) max_force (float, np.array[N], None, bool): maximum motor torques / forces. If True, it will apply the default maximum force values. + discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint. + Note that by specifying this, the joint action is no more continuous but becomes discrete. By default, + the first value along the first axis / dimension are the values by default that are set if no data + is provided. """ - super(JointPositionAndVelocityChangeAction, self).__init__(robot, joint_ids, kp=kp, kd=kd, max_force=max_force) - self.data = np.zeros(2*len(self.joints)) + super(JointPositionAndVelocityChangeAction, self).__init__(robot, joint_ids, kp=kp, kd=kd, max_force=max_force, + discrete_values=discrete_values) + # set data if continuous + if self.discrete_values is None: + self.data = np.zeros(2*len(self.joints)) def _write(self, data): """apply the action data on the robot.""" @@ -298,31 +456,41 @@ class JointTorqueAction(JointAction): Set the joint force/torque using force/torque control. """ - def __init__(self, robot, joint_ids=None, f_min=-np.infty, f_max=np.infty): + def __init__(self, robot, joint_ids=None, bounds=(None, None), discrete_values=None): """ Initialize the joint torque/force action. Args: robot (Robot): robot instance. joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints. - f_min (float, np.array[N], None): minimum torques/forces. - f_max (float, np.array[N], None): maximum torques/forces. + bounds (tuple of 2 float / np.array[N] / None): minimum and maximum torques/forces respectively. If None, + it will check the minimum/maximum torques/forces allowed. If it doesn't find them, it will set them + to -np.infty and np.infty. + discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint. + Note that by specifying this, the joint action is no more continuous but becomes discrete. By default, + the first value along the first axis / dimension are the values by default that are set if no data + is provided. """ - super(JointTorqueAction, self).__init__(robot, joint_ids) - self.data = robot.get_joint_torques(self.joints) + super(JointTorqueAction, self).__init__(robot, joint_ids, discrete_values=discrete_values) # check torque bounds + f_min, f_max = self._check_continuous_bounds(bounds) if f_min is None or f_max is None: f = robot.get_joint_max_forces(joint_ids=self.joints) f_min = -f if f_min is None else f_min f_max = f if f_max is None else f_max if np.allclose(f_min, 0): - f_min = -np.infty + f_min = -np.infty * np.ones(len(self.joints)) if np.allclose(f_max, 0): - f_max = np.infty + f_max = np.infty * np.ones(len(self.joints)) self.f_min = f_min self.f_max = f_max + # set data and space if continuous + if self.discrete_values is None: + self.data = robot.get_joint_torques(self.joints) + self._space = gym.spaces.Box(low=self.f_min, high=self.f_max) + def _write(self, data): """apply the action data on the robot.""" data = np.clip(data, self.f_min, self.f_max) @@ -359,19 +527,28 @@ class JointTorqueGravityCompensationAction(JointTorqueAction): This adds the given torques to the gravity compensation torques. That is, if a torque of 0 is provided, the robot will be in a gravity compensation mode. """ - def __init__(self, robot, joint_ids=None, f_min=-np.infty, f_max=np.infty): + def __init__(self, robot, joint_ids=None, bounds=(None, None), discrete_values=None): """ Initialize the joint torque/force action with gravity compensation. Args: robot (Robot): robot instance. joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints. - f_min (float, np.array[N], None): minimum torques/forces. - f_max (float, np.array[N], None): maximum torques/forces. + bounds (tuple of 2 float / np.array[N] / None): minimum and maximum torques/forces respectively. If None, + it will check the minimum/maximum torques/forces allowed. If it doesn't find them, it will set them + to -np.infty and np.infty. + discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint. + Note that by specifying this, the joint action is no more continuous but becomes discrete. By default, + the first value along the first axis / dimension are the values by default that are set if no data + is provided. """ - super(JointTorqueGravityCompensationAction, self).__init__(robot, joint_ids, f_min=f_min, f_max=f_max) + super(JointTorqueGravityCompensationAction, self).__init__(robot, joint_ids, bounds=bounds, + discrete_values=discrete_values) self.q_indices = self.robot.get_q_indices(joint_ids=self.joints) - self.data = np.zeros(len(self.joints)) + + # set data if continuous + if self.discrete_values is None: + self.data = np.zeros(len(self.joints)) def _write(self, data): """apply the action data on the robot.""" @@ -393,20 +570,30 @@ class JointAccelerationAction(JointAction): to be applied. """ - def __init__(self, robot, joint_ids=None, a_min=-np.infty, a_max=np.infty): + def __init__(self, robot, joint_ids=None, bounds=(None, None), discrete_values=None): """ Initialize the joint acceleration action. Args: robot (Robot): robot instance. joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints. - a_min (float, np.array[N], None): minimum accelerations. - a_max (float, np.array[N], None): maximum accelerations. + bounds (tuple of 2 float / np.array[N] / None): minimum and maximum accelerations. If None, it will use + the default joint acceleration limits. If it still doesn't find them, it will set -np.infty and + np.infty. + discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint. + Note that by specifying this, the joint action is no more continuous but becomes discrete. By default, + the first value along the first axis / dimension are the values by default that are set if no data + is provided. """ - super(JointAccelerationAction, self).__init__(robot, joint_ids) - self.data = robot.get_joint_accelerations(self.joints) - self.a_min = a_min - self.a_max = a_max + super(JointAccelerationAction, self).__init__(robot, joint_ids, discrete_values=discrete_values) + + # TODO + self.a_min = bounds[0] + self.a_max = bounds[1] + + # set data if continuous + if self.discrete_values is None: + self.data = robot.get_joint_accelerations(self.joints) def _write(self, data): """apply the action data on the robot.""" diff --git a/pyrobolearn/actions/robot_actions/link_actions.py b/pyrobolearn/actions/robot_actions/link_actions.py index b17b459..89d8521 100644 --- a/pyrobolearn/actions/robot_actions/link_actions.py +++ b/pyrobolearn/actions/robot_actions/link_actions.py @@ -7,10 +7,12 @@ This includes notably the link positions, velocities, and force/torque actions. import copy from abc import ABCMeta import numpy as np +import gym from pyrobolearn.actions.robot_actions.robot_actions import RobotAction from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy + __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] @@ -21,29 +23,78 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class LinkAction(RobotAction): +class LinkAction(RobotAction): # TODO: multiple links r"""Link Action """ __metaclass__ = ABCMeta - def __init__(self, robot, link_ids=None): + def __init__(self, robot, link_id=-1, discrete_values=None): """ Initialize the link action. Args: - robot (Robot): robot instance - link_ids (int, int[N]): link id or list of link ids + robot (Robot): robot instance. + link_id (int): link id. If -1, it is the base. + discrete_values (np.array[N, M], np.array[N], None): if provided, it represents the discrete values that + the action can take. Note that the action is no more continuous and becomes discrete at that point. + The first value will be the default value to be set if no data is provided. """ super(LinkAction, self).__init__(robot) # get the joints of the robot - if link_ids is None: - link_ids = robot.get_link_ids() - self.links = link_ids + if link_id is None: + link_id = -1 + self.link = link_id + + # if discrete values, check the type and create the space + if discrete_values is not None: + if not isinstance(discrete_values, (list, tuple, np.ndarray)): + raise TypeError("Expecting the given 'discrete_values' to be a list/tuple/np.array of float/int, but " + "instead got: {}".format(type(discrete_values))) + discrete_values = np.asarray(discrete_values) + self._space = gym.spaces.Discrete(len(self.discrete_values)) + self.discrete_values = discrete_values + + # set the data in the case it is discrete + if self.discrete_values is not None: + self.data = 0 # set the data to be the first index + + def _check_discrete_values(self, dim, last_dim): + """Check that the discrete values have the correct dimensions / shape.""" + # check discrete values + if self.discrete_values is not None: + if not (len(self.discrete_values.shape) == dim and self.discrete_values.shape[-1] == last_dim): + raise ValueError("Expecting the discrete values to have a dimension of {} and the last value of the " + "shape to be equal to {}, but instead got respectively: " + "{}, {}".format(dim, last_dim, len(self.discrete_values.shape), + self.discrete_values.shape[-1])) + + def _write(self, data): + """ + Write the data. + + Args: + data (int, np.ndarray): the data can be discrete or continuous. + """ + # if the action is discrete, then the data should be an index, or an array of values from which takes the max + if self.is_discrete(): + if isinstance(data, np.ndarray): + data = np.argmax(data.reshape(-1)) + data = self.discrete_values[data] + self._write_continuous(data) + + def _write_continuous(self, data): + """ + Write the given continuous data. Child method that has to be implement in the child classes. + + Args: + data (np.ndarray): continuous data to be written. + """ + raise NotImplementedError def __copy__(self): """Return a shallow copy of the action. This can be overridden in the child class.""" - return self.__class__(self.robot, self.links) + return self.__class__(self.robot, self.link) def __deepcopy__(self, memo={}): """Return a deep copy of the action. This can be overridden in the child class. @@ -54,271 +105,603 @@ class LinkAction(RobotAction): if self in memo: return memo[self] robot = memo.get(self.robot, self.robot) # copy.deepcopy(self.robot, memo) - links = copy.deepcopy(self.links) + links = copy.deepcopy(self.link) action = self.__class__(robot, links) memo[self] = action return action -class LinkPositionAction(LinkAction): +class LinkPositionAction(LinkAction): # TODO: multiple links r"""Link world position action - Set the world position using IK for the specified robot link(s). + Set the world position using IK for the specified robot link. """ - def __init__(self, robot, link_ids=None): + def __init__(self, robot, link_id=-1, discrete_values=None): """ Initialize the link world position action. Args: - robot (Robot): robot instance - link_ids (int, int[N]): link id or list of link ids + robot (Robot): robot instance. + link_id (int): link id. If -1, it represents the base. + discrete_values (np.array[N,3], None): if provided, it represents the discrete values that the action + can take. Note that the action is no more continuous and becomes discrete at that point. Also, note + that this parameter makes probably more sense with `LinkPositionChangeAction` instead. The first + value will be the default value to be set if no data is provided. """ - super(LinkPositionAction, self).__init__(robot, link_ids) - self.data = self.robot.get_link_world_positions(link_ids=self.links, flatten=True) # (N*3,) + super(LinkPositionAction, self).__init__(robot, link_id, discrete_values=discrete_values) - def _write(self, data): + # check discrete values + self._check_discrete_values(dim=2, last_dim=3) + + # set the original data + if self.is_continuous(): # continuous action + self.data = self.robot.get_link_world_positions(link_ids=self.link, flatten=True) # (3,) + + def _write_continuous(self, data): """apply the action data on the robot.""" - self.robot.set_link_positions(link_ids=self.links, positions=data.reshape(-1, 3)) + self.robot.set_link_positions(link_ids=self.link, positions=data) -class LinkPositionChangeAction(LinkPositionAction): +class LinkPositionChangeAction(LinkAction): # TODO: multiple links r"""Link world position change action - Set the world position using IK for the specified robot link(s). Instead of specifying directly the desired + Set the world position using IK for the specified robot link. Instead of specifying directly the desired cartesian position(s), the amount of change in the current positions is provided. """ - def __init__(self, robot, link_ids=None): + def __init__(self, robot, link_id=-1, discrete_values=None): """ Initialize the link world position change action. Args: - robot (Robot): robot instance - link_ids (int, int[N]): link id or list of link ids + robot (Robot): robot instance. + link_id (int): link id. If -1, it represents the base. + discrete_values (np.array[N,3], None): if provided, it represents the discrete values that the action + can take. Note that the action is no more continuous and becomes discrete at that point. The first + value will be the default value to be set if no data is provided. """ - super(LinkPositionChangeAction, self).__init__(robot, link_ids) - self.data = np.zeros(len(self.links) * 3) + super(LinkPositionChangeAction, self).__init__(robot, link_id, discrete_values=discrete_values) - def _write(self, data): + # check discrete values + self._check_discrete_values(dim=2, last_dim=3) + + # set the original data + if self.is_continuous(): # continuous action + self.data = np.zeros(3) + + def _write_continuous(self, data): """apply the action data on the robot.""" - data += self.robot.get_link_world_positions(link_ids=self.links) - super(LinkPositionChangeAction, self)._write(data) + data += self.robot.get_link_world_positions(link_ids=self.link, flatten=True) # (3,) + self.robot.set_link_positions(link_ids=self.link, positions=data) -class LinkOrientationAction(LinkAction): +class LinkOrientationAction(LinkAction): # TODO: multiple links r"""Link world orientation action - Set the world orientation using IK for the specified robot link(s). + Set the world orientation using IK for the specified robot link. """ - def __init__(self, robot, link_ids=None): + def __init__(self, robot, link_id=-1, discrete_values=None): """ Initialize the link world orientation action. Args: - robot (Robot): robot instance - link_ids (int, int[N]): link id or list of link ids + robot (Robot): robot instance. + link_id (int): link id. If -1, it represents the base. + discrete_values (np.array[N,4], None): if provided, it represents the discrete values that the action can + take. Note that the action is no more continuous and becomes discrete at that point. The first + value will be the default value to be set if no data is provided. """ - super(LinkOrientationAction, self).__init__(robot, link_ids) - self.data = self.robot.get_link_world_orientations(link_ids=self.links, flatten=True) # (N*4,) + super(LinkOrientationAction, self).__init__(robot, link_id, discrete_values=discrete_values) - def _write(self, data): + # check discrete values + self._check_discrete_values(dim=2, last_dim=4) + + # set the original data + if self.is_continuous(): # continuous action + self.data = self.robot.get_link_world_orientations(link_ids=self.link, flatten=True) # (4,) + + def _write_continuous(self, data): """apply the action data on the robot.""" - self.robot.set_link_positions(link_ids=self.links, orientations=data.reshape(-1, 4)) + self.robot.set_link_positions(link_ids=self.link, orientations=data) -class LinkOrientationChangeAction(LinkOrientationAction): +class LinkOrientationChangeAction(LinkAction): # TODO: multiple links r"""Link world orientation change action - Set the world orientation using IK for the specified robot link(s). Instead of specifying directly the desired + Set the world orientation using IK for the specified robot link. Instead of specifying directly the desired cartesian orientation(s), the amount of change in the current orientations is provided. Warnings: the difference in orientations should be provided as a change in roll-pitch-yaw angles (in radians), and not as quaternions. """ - def __init__(self, robot, link_ids=None): + def __init__(self, robot, link_id=-1, discrete_values=None): """ Initialize the link world orientation change action. Args: - robot (Robot): robot instance - link_ids (int, int[N]): link id or list of link ids + robot (Robot): robot instance. + link_id (int): link id. If -1, it represents the base. + discrete_values (np.array[N,3], None): if provided, it represents the discrete values that the orientation + change action can take. The orientations are represented as Roll-Pitch-Yaw angles. Note that the + action is no more continuous and becomes discrete at that point. The first value will be the default + value to be set if no data is provided. """ - super(LinkOrientationChangeAction, self).__init__(robot, link_ids) - self.data = np.zeros(len(self.links) * 4) + super(LinkOrientationChangeAction, self).__init__(robot, link_id, discrete_values=discrete_values) - def _write(self, data): + # check discrete values + self._check_discrete_values(dim=2, last_dim=3) + + # set the original data + if self.is_continuous(): # continuous action + self.data = np.zeros(3) + + def _write_continuous(self, data): """apply the action data on the robot.""" # get current orientations and convert them to RPY angles - orientations = self.robot.get_link_world_orientations(link_ids=self.links).reshape(-1, 4) # (N,4) - orientations = get_rpy_from_quaternion(orientations) # (N,3) + orientation = self.robot.get_link_world_orientations(link_ids=self.link) # (4,) + orientation = get_rpy_from_quaternion(orientation) # (3,) # add change in orientations - data = data.reshape(-1, 3) # (N,3) - data += orientations + data += orientation # (3,) # convert them back to quaternions - data = get_quaternion_from_rpy(data) # (N,4) - super(LinkOrientationChangeAction, self)._write(data) + data = get_quaternion_from_rpy(data) # (4,) + self.robot.set_link_positions(link_ids=self.link, orientations=data) -class LinkPoseAction(LinkAction): +class LinkPoseAction(LinkAction): # TODO: multiple link r"""Link world pose action - Set the world pose using IK for the specified robot link(s). + Set the world pose using IK for the specified robot link. """ - def __init__(self, robot, link_ids=None): + def __init__(self, robot, link_id=-1, discrete_values=None): """ Initialize the link world pose action. Args: - robot (Robot): robot instance - link_ids (int, int[N]): link id or list of link ids + robot (Robot): robot instance. + link_id (int): link id. If -1, it represents the base. + discrete_values (np.array[N,7], None): if provided, it represents the discrete values that the action can + take. Note that the action is no more continuous and becomes discrete at that point. The first + value will be the default value to be set if no data is provided. """ - super(LinkPoseAction, self).__init__(robot, link_ids) - self.data = self.robot.get_link_world_poses(link_ids=self.links, flatten=True) + super(LinkPoseAction, self).__init__(robot, link_id, discrete_values=discrete_values) - def _write(self, data): + # check discrete values + self._check_discrete_values(dim=2, last_dim=7) + + # set the original data + if self.is_continuous(): # continuous action + self.data = self.robot.get_link_world_poses(link_ids=self.link, flatten=True) + + def _write_continuous(self, data): """apply the action data on the robot.""" - data = data.reshape(-1, 7) - self.robot.set_link_positions(link_ids=self.links, positions=data[:, :3], orientations=data[:, 3:]) + self.robot.set_link_positions(link_ids=self.link, positions=data[:3], orientations=data[3:]) -class LinkPoseChangeAction(LinkPoseAction): +class LinkPoseChangeAction(LinkAction): # TODO: multiple link r"""Link world change pose action - Set the world pose using IK for the specified robot link(s). Instead of specifying directly the desired + Set the world pose using IK for the specified robot link. Instead of specifying directly the desired cartesian pose(s), the amount of change in the current poses is provided. - Warnings: the difference in orientations should be provided as a change in roll-pitch-yaw angles (in radians), and - not as quaternions. + Warnings: the difference in orientations should be provided as a change in roll-pitch-yaw angles (in radians), + and not as quaternions. """ - def __init__(self, robot, link_ids=None): + def __init__(self, robot, link_id=-1, discrete_values=None): """ Initialize the link world pose change action. Args: - robot (Robot): robot instance - link_ids (int, int[N]): link id or list of link ids + robot (Robot): robot instance. + link_id (int): link id. If -1, it represents the base. + discrete_values (np.array[N, 6], None): if provided, it represents the discrete values that the link pose + change action can take. Note that the orientation part is represented as Roll-Pitch-Yaw angles. + Note that the action is no more continuous and becomes discrete at that point. The first value will + be the default value to be set if no data is provided. """ - super(LinkPoseChangeAction, self).__init__(robot, link_ids) - self.data = np.zeros(len(self.links) * 6) + super(LinkPoseChangeAction, self).__init__(robot, link_id, discrete_values=discrete_values) - def _write(self, data): + # check discrete values + self._check_discrete_values(dim=2, last_dim=6) + + # set the original data + if self.is_continuous(): # continuous action + self.data = np.zeros(6) + + def _write_continuous(self, data): """apply the action data on the robot.""" - # get current poses - positions = self.robot.get_link_world_poses(link_ids=self.links, flatten=False).reshape(-1, 3) # (N,3) - orientations = self.robot.get_link_world_orientations(link_ids=self.links, flatten=False).reshape(-1, 4) # (N,4) - orientations = get_rpy_from_quaternion(orientations) # (N,3) + # get current pose + position = self.robot.get_link_world_poses(link_ids=self.link, flatten=False) # (3,) + orientation = self.robot.get_link_world_orientations(link_ids=self.link, flatten=False) # (4,) + orientation = get_rpy_from_quaternion(orientation) # (3,) # add changes - data = data.reshape(-1, 6) # (N,6) - data[:, :3] += positions - data[:, 3:] += orientations + data[:3] += position # (3,) + data[3:] += orientation # (3,) # convert back orientations to quaternions - data = np.hstack((data[:, :3], get_quaternion_from_rpy(data[:, 3:]))) + data = np.concatenate((data[:3], get_quaternion_from_rpy(data[3:]))) # (7,) # write poses - super(LinkPoseChangeAction, self)._write(data) + self.robot.set_link_positions(link_ids=self.link, positions=data[:3], orientations=data[3:]) -class LinkVelocityAction(LinkAction): +class LinkVelocityAction(LinkAction): # TODO: multiple links r"""Link world velocity action - Set the cartesian world velocity(ies) for the specified robot link(s). + Set the cartesian world velocity for the specified robot link. """ - def __init__(self, robot, link_ids=None): + def __init__(self, robot, link_id=-1, discrete_values=None): """ Initialize the link world velocity action. Args: - robot (Robot): robot instance - link_ids (int, int[N]): link id or list of link ids + robot (Robot): robot instance. + link_id (int): link id. If -1, it represents the base. + discrete_values (np.array[N, 6], None): if provided, it represents the discrete values that the action + can take. Note that the action is no more continuous and becomes discrete at that point. The first + value will be the default value to be set if no data is provided. """ - super(LinkVelocityAction, self).__init__(robot, link_ids) - self.data = self.robot.get_link_world_velocities(link_ids=self.links) + super(LinkVelocityAction, self).__init__(robot, link_id, discrete_values=discrete_values) - def _write(self, data): + # check discrete values + self._check_discrete_values(dim=2, last_dim=6) + + # set the original data + if self.is_continuous(): # continuous action + self.data = self.robot.get_link_world_velocities(link_ids=self.link) # (6,) + + def _write_continuous(self, data): """apply the action data on the robot.""" - self.robot.set_link_velocities(link_ids=self.links, positions=data.reshape(-1, 6)) + self.robot.set_link_velocities(link_ids=self.link, positions=data) -class LinkVelocityChangeAction(LinkVelocityAction): +class LinkVelocityChangeAction(LinkAction): # TODO: multiple link r"""Link world velocity change action - Set the cartesian world velocity(ies) for the specified robot link(s). Instead of specifying directly the desired - cartesian velocity(ies), the amount of change in the current velocities is provided. + Set the cartesian world velocity for the specified robot link. Instead of specifying directly the desired + cartesian velocity, the amount of change in the current velocities is provided. """ - def __init__(self, robot, link_ids=None): + def __init__(self, robot, link_id=None, discrete_values=None): """ Initialize the link world velocity change action. Args: - robot (Robot): robot instance - link_ids (int, int[N]): link id or list of link ids + robot (Robot): robot instance. + link_id (int): link id. If -1, it represents the base. + discrete_values (np.array[N, 6], None): if provided, it represents the discrete values that the action + can take. Note that the action is no more continuous and becomes discrete at that point. The first + value will be the default value to be set if no data is provided. """ - super(LinkVelocityAction, self).__init__(robot, link_ids) - self.data = np.zeros(len(self.links) * 6) + super(LinkVelocityChangeAction, self).__init__(robot, link_id, discrete_values=discrete_values) - def _write(self, data): + # check discrete values + self._check_discrete_values(dim=2, last_dim=6) + + # set the original data + if self.is_continuous(): # continuous action + self.data = np.zeros(6) + + def _write_continuous(self, data): """apply the action data on the robot.""" - data += self.robot.get_link_world_velocities(link_ids=self.links) - super(LinkVelocityChangeAction, self)._write(data) + data += self.robot.get_link_world_velocities(link_ids=self.link) + self.robot.set_link_velocities(link_ids=self.link, positions=data) -class LinkForceAction(LinkAction): +class LinkForceAction(LinkAction): # TODO: multiple links r"""Link force action - Set the cartesian force(s) for the specified robot link(s). + Set the robot joint torques in order to perform a desired cartesian force(s) with the specified robot link on + the environment. The final joint torques that are applied are: + + .. math:: \tau = N(q,\dot{q}) + J(q)^\top f + + where :math:`N(q,\dot{q})` contains the coriolis, centrifugal, and gravity effects, and :math:`f` is the cartesian + force that we wish to apply on the environment. """ - def __init__(self, robot, link_ids=None): + + def __init__(self, robot, link_id, discrete_values=None): # link_ids=None """ Initialize the link force action. Args: robot (Robot): robot instance - link_ids (int, int[N]): link id or list of link ids + link_id (int): id of the link that has to perform the desired force. + discrete_values (np.array[N, 3], None): if provided, it represents the discrete values that the action + can take. Note that the action is no more continuous and becomes discrete at that point. The first + value will be the default value to be set if no data is provided. """ - super(LinkForceAction, self).__init__(robot, link_ids) + super(LinkForceAction, self).__init__(robot, link_id, discrete_values=discrete_values) - def _write(self, data): + # check discrete values + self._check_discrete_values(dim=2, last_dim=3) + + # set the original data + if self.is_continuous(): # continuous action # TODO: check if we can sense the forces + self.data = np.zeros(3) + + def _write_continuous(self, data): """apply the action data on the robot.""" - # self.robot - pass + jacobian = self.robot.get_jacobian(link_id=self.link)[:3] # (3,N) + tau = self.robot.get_coriolis_and_gravity_compensation_torques() # (N,) + tau += jacobian.T.dot(data) # (N,) + self.robot.set_joint_torques(tau) + + +class LinkTorqueAction(LinkAction): # TODO: multiple links + r"""Link torque action + + Set the robot joint torques in order to perform a desired cartesian torque with the specified link on the + environment. The final joint torques that are applied are: + + .. math:: \tau = N(q,\dot{q}) + J(q)^\top f + + where :math:`N(q,\dot{q})` contains the coriolis, centrifugal, and gravity effects, and :math:`f` is the cartesian + torque that we wish to apply on the environment. + """ + + def __init__(self, robot, link_id, discrete_values=None): # link_ids=None + """ + Initialize the link torque action. + + Args: + robot (Robot): robot instance. + link_id (int): id of the link that has to perform the desired torque. + discrete_values (np.array[N, 3], None): if provided, it represents the discrete values that the action + can take. Note that the action is no more continuous and becomes discrete at that point. The first + value will be the default value to be set if no data is provided. + """ + super(LinkTorqueAction, self).__init__(robot, link_id, discrete_values=discrete_values) + + # check discrete values + self._check_discrete_values(dim=2, last_dim=3) + + # set the original data + if self.is_continuous(): # continuous action # TODO: check if we can sense the torques + self.data = np.zeros(3) + + def _write_continuous(self, data): + """apply the action data on the robot.""" + jacobian = self.robot.get_jacobian(link_id=self.link)[3:] # (3,N) + tau = self.robot.get_coriolis_and_gravity_compensation_torques() # (N,) + tau += jacobian.T.dot(data) # (N,) + self.robot.set_joint_torques(tau) + + +class LinkWrenchAction(LinkAction): # TODO: multiple links + r"""Link wrench action + + Set the robot joint torques in order to perform a desired cartesian wrench (concatenation of the cartesian force + and torque) with the specified link on the environment. The final joint torques that are applied are: + + .. math:: \tau = N(q,\dot{q}) + J(q)^\top f + + where :math:`N(q,\dot{q})` contains the coriolis, centrifugal, and gravity effects, and :math:`f` is the cartesian + wrench that we wish to apply on the environment. + """ + + def __init__(self, robot, link_id, discrete_values=None): # link_ids=None + """ + Initialize the link wrench action. + + Args: + robot (Robot): robot instance. + link_id (int): id of the link that has to perform the desired wrench. + discrete_values (np.array[N,6], None): if provided, it represents the discrete values that the + action can take. Note that the action is no more continuous and becomes discrete at that point. + The first value will be the default value to be set if no data is provided. + """ + super(LinkWrenchAction, self).__init__(robot, link_id, discrete_values=discrete_values) + + # check discrete values + self._check_discrete_values(dim=2, last_dim=6) + + # set the original data + if self.is_continuous(): # continuous action + self.data = np.zeros(6) + + def _write_continuous(self, data): + """apply the action data on the robot.""" + jacobian = self.robot.get_jacobian(link_id=self.link) # (6,N) + tau = self.robot.get_coriolis_and_gravity_compensation_torques() # (N,) + tau += jacobian.T.dot(data) # (N,) + self.robot.set_joint_torques(tau) + + +class ApplyForceAction(LinkAction): # TODO: multiple links + r"""Apply Force Action + + This action allows you to apply a Cartesian force on a specific link. In the simulator, it just applies the force + on the specified link at the specified position. On the real platform, it projects the Cartesian force to joint + torques and apply them on the robot. + """ + + def __init__(self, robot, link_id=-1, local_position=None, axis=None, discrete_values=None): # link_ids=None + """ + Initialize the apply force action. + + Args: + robot (Robot): robot instance. + link_id (int): id of the link on which to apply the force. If -1, it is the base. + local_position (np.array[3], list of 3 float, None): local position on the link to apply the force on. + If None, the force will be applied on the CoM of the link. + axis (np.array[3], None): axis on which to apply the force. If provided, it will create an action that + only represents the magnitude of the force. + discrete_values (np.array[N,3], np.array[N], None): if provided, it represents the forces in a discrete + manner by using the provided force values. Note that the action is no more continuous and becomes + discrete at that point. The first value will be the default value to be set if no data is provided. + """ + super(ApplyForceAction, self).__init__(robot, link_id, discrete_values=discrete_values) + + # check local position + if not isinstance(local_position, (list, tuple, np.ndarray)): + raise TypeError("Expecting the given 'local_position' to be a list/tuple/np.array of 3 float, but instead " + "got: {}".format(type(local_position))) + if len(local_position) != 3: + raise ValueError("Expecting the given 'local_position' to be a list/tuple/np.array of 3 float, but " + "instead got a length of: {}".format(len(local_position))) + self.local_position = local_position + + # check axis + if axis is not None: + if not isinstance(axis, (list, tuple, np.ndarray)): + raise TypeError("Expecting the given 'axis' to be a list/tuple/np.array, but instead got: " + "{}".format(type(axis))) + axis = np.asarray(axis) + if axis.size != 3: + raise ValueError("Expecting the given 'axis' to be list/tuple/np.array of 3 float, but instead got: " + "{}".format(axis.size)) + self.axis = axis + + # check discrete values + if self.discrete_values is not None: + # if an axis is not defined the discrete values must have a shape of (N,2) + if self.axis is None: + if not (len(self.discrete_values.shape) == 2 and self.discrete_values.shape[1] == 3): + raise ValueError("Expecting the discrete values to have a shape of (N,3), but instead got: " + "{}".format(self.discrete_values.shape)) + else: # if an axis is defined, the discrete values must have a shape of (N,) + if len(self.discrete_values.shape) > 1: + raise ValueError("Expecting the discrete values to have a shape of (N,), but instead got: " + "{}".format(self.discrete_values.shape)) + + # set the original data + if self.is_continuous(): # continuous action + if self.axis is not None: # if an axis is defined, then set the initial data to be zero + self.data = np.zeros(1) + else: # if no axis is defined, set the initial data to be a 3D zero vector + self.data = np.zeros(3) + + def _write_continuous(self, data): + """apply the action data on the robot.""" + if self.axis is not None: + data = data * self.axis + self.robot.apply_external_force(force=data, link_id=self.link[0], position=self.local_position) + + +class ApplyTorqueAction(LinkAction): # TODO: multiple links + r"""Apply Torque Action + + This action allows you to apply a Cartesian torque on a specific link. In the simulator, it just applies the torque + on the specified link at the specified position. On the real platform, it projects the Cartesian torque to joint + torques and apply them on the robot. + """ + + def __init__(self, robot, link_id=-1, axis=None, discrete_values=None): # link_ids=None + """ + Initialize the apply torque action. + + Args: + robot (Robot): robot instance. + link_id (int): id of the link on which to apply the torque. If -1, it is the base. + axis (np.array[3], None): axis around which to apply the torque. If provided, it will create an action that + only represents the magnitude of the torque. + discrete_values (np.array[N,3], np.array[N], None): if provided, it represents the torques in a discrete + manner by using the provided torque values. Note that the action is no more continuous and becomes + discrete at that point. The first value will be the default value to be set if no data is provided. + """ + super(ApplyTorqueAction, self).__init__(robot, link_id, discrete_values=discrete_values) + + # check axis + if axis is not None: + if not isinstance(axis, (list, tuple, np.ndarray)): + raise TypeError("Expecting the given 'axis' to be a list/tuple/np.array, but instead got: " + "{}".format(type(axis))) + axis = np.asarray(axis) + if axis.size != 3: + raise ValueError("Expecting the given 'axis' to be list/tuple/np.array of 3 float, but instead got: " + "{}".format(axis.size)) + self.axis = axis + + # check discrete values + if self.discrete_values is not None: + # if an axis is not defined the discrete values must have a shape of (N,2) + if self.axis is None: + if not (len(self.discrete_values.shape) == 2 and self.discrete_values.shape[1] == 3): + raise ValueError("Expecting the discrete values to have a shape of (N,3), but instead got: " + "{}".format(self.discrete_values.shape)) + else: # if an axis is defined, the discrete values must have a shape of (N,) + if len(self.discrete_values.shape) > 1: + raise ValueError("Expecting the discrete values to have a shape of (N,), but instead got: " + "{}".format(self.discrete_values.shape)) + + # set the original data + if self.is_continuous(): # continuous action + if self.axis is not None: + # if an axis is defined, then set the initial data to be zero + self.data = np.zeros(1) + else: + self.data = np.zeros(3) + + def _write_continuous(self, data): + """apply the action data on the robot.""" + if self.axis is not None: + data = data * self.axis + self.robot.apply_external_torque(torque=data, link_id=self.link[0]) + + +# class ApplyWrenchAction(LinkAction): # TODO: multiple links +# r"""Apply wrench action +# +# This action allows you to apply a Cartesian wrench (concatenation of the force and torque) on a specific link. +# In the simulator, it just applies the wrench on the specified link at the specified position. On the real +# platform, it projects the Cartesian wrench to joint torques and apply them on the robot. +# """ +# +# def __init__(self, robot, link_id, local_position=None, axis=None, discrete_values=None): # link_ids=None +# """ +# Initialize the apply wrench action. +# +# Args: +# robot (Robot): robot instance. +# link_id (int): id of the link on which to apply the force. If -1, it is the base. +# local_position (np.array[3], list of 3 float, None): local position on the link to apply the force on. +# If None, the force will be applied on the CoM of the link. +# axis (np.array[3], None): axis on which to apply the force. If provided, it will create an action that +# only represents the magnitude of the force. +# discrete_values (np.array[M]): if provided, it represents the forces in a discrete manner by using the +# provided force value. Note that the action is no more continuous and becomes discrete at that point. +# """ +# super(ApplyWrenchAction, self).__init__(robot, link_id, discrete_values=discrete_values) +# +# def _write(self, data): +# """apply the action data on the robot.""" +# pass ######################## # End Effector Actions # ######################## -class EndEffectorAction(LinkAction): - - def __init__(self, robot, end_effector_ids=None): - if end_effector_ids is None: - end_effector_ids = robot.get_end_effector_ids() - super(EndEffectorAction, self).__init__(robot, end_effector_ids) - - -class EndEffectorPositionAction(EndEffectorAction): - - def __init__(self, robot, end_effector_ids=None): - super(EndEffectorPositionAction, self).__init__(robot, end_effector_ids) - - -class EndEffectorVelocityAction(EndEffectorAction): - - def __init__(self, robot, end_effector_ids=None): - super(EndEffectorVelocityAction, self).__init__(robot, end_effector_ids) - - -class EndEffectorForceAction(EndEffectorAction): - - def __init__(self, robot, end_effector_ids=None): - super(EndEffectorForceAction, self).__init__(robot, end_effector_ids) +# class EndEffectorAction(LinkAction): +# +# def __init__(self, robot, end_effector_ids=None): +# if end_effector_ids is None: +# end_effector_ids = robot.get_end_effector_ids() +# super(EndEffectorAction, self).__init__(robot, end_effector_ids) +# +# +# class EndEffectorPositionAction(EndEffectorAction): +# +# def __init__(self, robot, end_effector_ids=None): +# super(EndEffectorPositionAction, self).__init__(robot, end_effector_ids) +# +# +# class EndEffectorVelocityAction(EndEffectorAction): +# +# def __init__(self, robot, end_effector_ids=None): +# super(EndEffectorVelocityAction, self).__init__(robot, end_effector_ids) +# +# +# class EndEffectorForceAction(EndEffectorAction): +# +# def __init__(self, robot, end_effector_ids=None): +# super(EndEffectorForceAction, self).__init__(robot, end_effector_ids) diff --git a/pyrobolearn/actions/robot_actions/robot_actions.py b/pyrobolearn/actions/robot_actions/robot_actions.py index bcd03c3..4895885 100644 --- a/pyrobolearn/actions/robot_actions/robot_actions.py +++ b/pyrobolearn/actions/robot_actions/robot_actions.py @@ -49,13 +49,13 @@ class RobotAction(Action): def robot(self): return self._robot - def is_discrete(self): - """By default, robot actions are continuous.""" - return False - - def is_continuous(self): - """By default, robot actions are continuous.""" - return True + # def is_discrete(self): + # """By default, robot actions are continuous.""" + # return False + # + # def is_continuous(self): + # """By default, robot actions are continuous.""" + # return True def __copy__(self): """Return a shallow copy of the action. This can be overridden in the child class.""" diff --git a/pyrobolearn/envs/control/pendulum.py b/pyrobolearn/envs/control/pendulum.py index 988eb0e..387f10b 100644 --- a/pyrobolearn/envs/control/pendulum.py +++ b/pyrobolearn/envs/control/pendulum.py @@ -61,19 +61,19 @@ class InvertedPendulumSwingUpEnv(ControlEnv): if verbose: robot.print_info() - # create state + # create state: [cos(q_1), sin(q_1), \dot{q}_1] trig_position_state = prl.states.JointTrigonometricPositionState(robot=robot) velocity_state = prl.states.JointVelocityState(robot=robot) state = trig_position_state + velocity_state if verbose: print("\nObservation: {}".format(state)) - # create action - action = prl.actions.JointTorqueAction(robot, f_min=-2., f_max=2.) + # create action: \tau_1 + action = prl.actions.JointTorqueAction(robot, bounds=(-2., 2.)) if verbose: print("\nAction: {}".format(action)) - # create reward/cost + # create reward/cost: ||d(q,q_{target})||^2 + 0.1 * ||\dot{q}||^2 + 0.001 * ||\tau||^2 position_cost = prl.rewards.JointPositionCost(prl.states.JointPositionState(robot), target_state=np.zeros(len(robot.joints)), update_state=True) diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py index a59c248..4f5eb4f 100644 --- a/pyrobolearn/robots/robot.py +++ b/pyrobolearn/robots/robot.py @@ -917,7 +917,7 @@ class Robot(ControllableBody): # q_idx = self.get_q_indices(joint_ids) # return accelerations[q_idx] - def get_joint_accelerations(self, joint_ids=None): # TODO: fix this!! + def get_joint_accelerations(self, joint_ids=None): r""" Get the acceleration of the specified joint(s). If the simulator doesn't provide the joint accelerations, this is computed using finite difference :math:`\ddot{q}(t) = \frac{\dot{q}(t) - \dot{q}(t-dt)}{dt}`. diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index 8dd2e91..bb1f449 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -267,6 +267,11 @@ class Simulator(object): # Static methods # ################## + @staticmethod + def in_simulation(): + """Return True if we are running in simulation instead of the real-world.""" + return True + @staticmethod def simulate_gas_dynamics(): """Return True if the simulator can simulate gases."""