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

update actions: add bounds and discrete values

Browse Source
This commit is contained in:
Brian Delhaisse
2019-07-18 01:29:49 +02:00
parent 46966da0f7
commit 18b70f01db
9 changed files with 780 additions and 200 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/environments/inverted_pendulum.py
+3 -3
View File
@@ -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)
pyrobolearn/actions/action.py
+7 -3
View File
@@ -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):
pyrobolearn/actions/robot_actions/__init__.py
+3 -2
View File
@@ -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
pyrobolearn/actions/robot_actions/joint_actions.py
+235 -48
View File
@@ -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."""
pyrobolearn/actions/robot_actions/link_actions.py
+515 -132
View File
@@ -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)
pyrobolearn/actions/robot_actions/robot_actions.py
+7 -7
View File
@@ -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."""
pyrobolearn/envs/control/pendulum.py
+4 -4
View File
@@ -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)
pyrobolearn/robots/robot.py
+1 -1
View File
@@ -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}`.
pyrobolearn/simulators/simulator.py
+5
View File
@@ -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."""