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

add pendulum environment example + general updates

Browse Source
This commit is contained in:
Brian Delhaisse
2019-07-17 02:35:10 +02:00
parent 96b497ded8
commit 5369657b89
28 changed files with 1700 additions and 480 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/source/models.rst
+1 -1
View File
@@ -13,7 +13,7 @@ Learning models can be divided into 2 categories:
- Polynomial models
- Deep Neural Networks (DNNs)
- Gaussian processes (GPs)
- Trajectory based learning models:
- Trajectory based learning models
- Dynamic Movement Primitives (DMPs)
- Central Pattern Generators (CPGs)
- Gaussian Mixture Models and Gaussian Mixture Regression (GMMs/GMRs)
docs/source/overview.rst
View File
docs/source/priorities.rst
-3
View File
@@ -1,3 +0,0 @@
Priority Tasks
==============
docs/source/pyrobolearn.rst
+1 -1
View File
@@ -14,7 +14,7 @@ General idea.
- lack of benchmarks
- lack of flexibility and modularity
- lack of generalization
- high coupling
- high coupling between different modules
For instance:
pyrobolearn/actions/robot_actions/joint_actions.py
+21 -4
View File
@@ -8,7 +8,7 @@ import copy
import numpy as np
from abc import ABCMeta
from pyrobolearn.actions.robot_actions.robot_actions import RobotAction
from pyrobolearn.actions.robot_actions.robot_actions import RobotAction, Robot
__author__ = "Brian Delhaisse"
@@ -85,11 +85,18 @@ class JointPositionAction(JointAction):
joint_ids (int, list of int, None): joint id(s). If None, it will take all the actuated joints.
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.
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).
"""
self.kp, self.kd, self.max_force = kp, kd, max_force
super(JointPositionAction, self).__init__(robot, joint_ids)
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
self.data = robot.get_joint_positions(self.joints)
def _write(self, data):
@@ -303,6 +310,16 @@ class JointTorqueAction(JointAction):
"""
super(JointTorqueAction, self).__init__(robot, joint_ids)
self.data = robot.get_joint_torques(self.joints)
# check torque 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
if np.allclose(f_max, 0):
f_max = np.infty
self.f_min = f_min
self.f_max = f_max
pyrobolearn/actions/robot_actions/robot_actions.py
+9 -1
View File
@@ -34,9 +34,15 @@ class RobotAction(Action):
__metaclass__ = ABCMeta
def __init__(self, robot):
"""Initialize the abstract robot action.
Args:
robot (Robot): a robot instance.
"""
super(RobotAction, self).__init__()
if not isinstance(robot, Robot):
raise TypeError("The 'robot' parameter has to be an instance of Robot")
raise TypeError("The 'robot' parameter has to be an instance of Robot, but instead got: "
"{}".format(type(robot)))
self._robot = robot
@property
@@ -44,9 +50,11 @@ class RobotAction(Action):
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 __copy__(self):
pyrobolearn/envs/control/pendulum.py
+57 -21
View File
@@ -9,7 +9,6 @@ References:
- [1] Pendulum environment in OpenAI Gym: https://gym.openai.com/envs/Pendulum-v0/
"""
import os
import numpy as np
import pyrobolearn as prl
@@ -29,22 +28,25 @@ __status__ = "Development"
class InvertedPendulumSwingUpEnv(ControlEnv):
r"""Inverted Pendulum Swing-up Environment
This is based on the control problem proposed in OpenAI Gym:
This is based on the control problem proposed in OpenAI Gym [1]:
"The inverted pendulum swingup problem is a classic problem in the control literature. In this version of the
problem, the pendulum starts in a random position, and the goal is to swing it up so it stays upright." [1]
Here are the various environment features:
- world: basic world with gravity, a basic floor, and the pendulum.
- state: the state is given by :math:`[np.cos(q_1), np.sin(q_1), dq_1]`
- world: basic world with gravity, a basic floor, and the pendulum loaded at the center.
- state: the state is given by :math:`[cos(q_1), sin(q_1), \dot{q}_1]`
- action: the action is the joint torque :math:`\tau_1`
- reward:
- cost: :math:`||d(q,q_{target})||^2 + 0.1 * ||\dot{q}||^2 + 0.001 * ||\tau||^2`, where :math:`d(\cdot, \cdot)`
is the minimum angle difference between two angles.
- initial state generator: initialize the joint angle between [-pi, pi] (q=0 when the pendulum is pointing up)
- physics randomizer: uniform distribution of the mass of the pendulum [mass - mass/10, mass + mass/10]
References:
- [1] Pendulum environment in OpenAI Gym: https://gym.openai.com/envs/Pendulum-v0/
"""
def __init__(self, simulator=None, verbose=False):
def __init__(self, simulator, verbose=False):
"""
Initialize the inverted pendulum swing-up environment.
@@ -52,46 +54,80 @@ class InvertedPendulumSwingUpEnv(ControlEnv):
simulator (Simulator): simulator instance.
verbose (bool): if True, it will print information when creating the environment
"""
# create basic world
# create basic world with the robot
world = prl.worlds.BasicWorld(simulator)
robot = world.load_robot('pendulum')
robot.disable_motor()
# robot.print_info()
if verbose:
robot.print_info()
# create state
position_state = prl.states.JointTrigonometricPositionState(robot=robot)
trig_position_state = prl.states.JointTrigonometricPositionState(robot=robot)
velocity_state = prl.states.JointVelocityState(robot=robot)
state = position_state + velocity_state
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.)
if verbose:
print("\nAction: {}".format(action))
# create reward
# create reward/cost
position_cost = prl.rewards.JointPositionCost(prl.states.JointPositionState(robot),
np.zeros(len(robot.joints)))
target_state=np.zeros(len(robot.joints)),
update_state=True)
velocity_cost = prl.rewards.JointVelocityCost(velocity_state)
torque_cost = prl.rewards.JointTorqueCost(prl.states.JointForceTorqueState(robot=robot))
torque_cost = prl.rewards.JointTorqueCost(prl.states.JointForceTorqueState(robot=robot), update_state=True)
reward = position_cost + 0.1 * velocity_cost + 0.001 * torque_cost
if verbose:
print("Reward: {}".format(reward))
# create initial state generator
initial_state_generator = None
# create initial state generator: generate the state each time we reset the environment
def reset_robot(robot): # function to disable the motors every time we reset the joint state
def reset():
robot.disable_motor()
return reset
init_state = prl.states.JointPositionState(robot)
low, high = np.array([-np.pi] * len(robot.joints)), np.array([np.pi] * len(robot.joints))
# init_state.data = np.array([np.pi / 2]) # initial data
# initial_state_generator = prl.states.generators.FixedStateGenerator(state=init_state, fct=reset_robot(robot))
initial_state_generator = prl.states.generators.UniformStateGenerator(state=init_state, low=low, high=high,
fct=reset_robot(robot))
# create physics randomizer: randomize the mass each time we reset the environment
masses = robot.get_link_masses(link_ids=robot.joints)
masses = (masses - masses/10., masses + masses/10.)
physics_randomizer = prl.physics.LinkPhysicsRandomizer(robot, link_ids=robot.joints, masses=masses)
# could create terminal conditions (if necessary) such as:
# - success if we stay at the upper position for more than 20 steps
# - failure if we can achieve the goal after 10,000 steps
# In the OpenAI gym, there are no terminal conditions for this problem
terminal_condition = None
# create environment using composition
super(InvertedPendulumSwingUpEnv, self).__init__(world=world, states=state, rewards=reward, actions=action,
initial_state_generators=initial_state_generator)
initial_state_generators=initial_state_generator,
physics_randomizers=physics_randomizer,
terminal_conditions=terminal_condition)
# Test
if __name__ == "__main__":
from itertools import count
import pyrobolearn as prl
# create simulator
sim = prl.simulators.Bullet()
# create environment
env = InvertedPendulumSwingUpEnv(sim)
env = InvertedPendulumSwingUpEnv(sim, verbose=True)
# run simulation
for _ in count():
env.step(sleep_dt=1./240)
env.reset()
for t in prl.count():
# if (t % 800) == 0:
# env.reset() # test reset function
states, rewards, done, info = env.step(sleep_dt=1./240)
# print("State: {}".format(states))
print("Reward: {}".format(rewards))
pyrobolearn/envs/env.py
+1 -1
View File
@@ -27,7 +27,7 @@ from pyrobolearn.states.generators import StateGenerator
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "MIT"
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
pyrobolearn/physics/body_physics_randomizer.py
+7 -6
View File
@@ -6,11 +6,13 @@ Dependencies:
- `pyrobolearn.robots`
"""
from abc import ABCMeta
from pyrobolearn.physics.physics_randomizer import PhysicsRandomizer
# from pyrobolearn.robots.base import Object # TODO: change to Body or MultiBody
from pyrobolearn.robots.base import Body
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__copyright__ = "Copyright 2019, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
@@ -25,6 +27,7 @@ class BodyPhysicsRandomizer(PhysicsRandomizer):
The body physics randomizer can randomize the physical attributes of a body. It is an abstract class which is
inherited notably by `LinkPhysicsRandomizer` and `JointPhysicsRandomizer`.
"""
__metaclass__ = ABCMeta
def __init__(self, body):
"""
@@ -49,10 +52,8 @@ class BodyPhysicsRandomizer(PhysicsRandomizer):
@body.setter
def body(self, body):
"""Set the body / object instance."""
# TODO: uncomment the following lines
# if not isinstance(body, Object):
# raise TypeError("Expecting the given body to be an instance of `Object`, instead got: "
# "{}".format(type(body)))
if not isinstance(body, Body):
raise TypeError("Expecting the given body to be an instance of `Body`, instead got: {}".format(type(body)))
self._body = body
@property
pyrobolearn/physics/joint_physics_randomizer.py
+106 -23
View File
@@ -7,12 +7,14 @@ Dependencies:
"""
import collections
import numpy as np
from pyrobolearn.physics.body_physics_randomizer import BodyPhysicsRandomizer
from pyrobolearn.robots.robot import Robot
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__copyright__ = "Copyright 2019, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
@@ -25,30 +27,33 @@ class JointPhysicsRandomizer(BodyPhysicsRandomizer):
r"""Joint Physics Randomizer
The joint physics randomizer can randomize the physical attributes of a joint. For instance, this can be the
joint friction or damping coefficients. Other attributes can be the maximum force or velocity the joint(s) can
achieve.
joint friction or damping coefficients.
"""
def __init__(self, body, joint_ids=None, joint_damping=None, **kwargs):
def __init__(self, robot, joint_ids=None, joint_frictions=None, joint_dampings=None):
"""
Initialize the joint physics randomizer.
Args:
body (Body): multi-body object.
robot (Robot): robot instance.
joint_ids (int, list of int, None): joint id(s).
joint_damping (float, list of float, tuple of float, list of tuple of float, None): joint damping
coefficient. If None, it will take the default joint damping value associated with the given
`joint_ids` of the given `body`. If float, it will set that value to the specified joints and will
always return this value when sampling. If list of float, it will set each value to each joint and will
always return these values when sampling. If tuple of float, the first item is the lower bound of the
joint damping and the second item is its upper bound. It will set these bounds for each joint. If list
of tuples of joints, it will have a tuple of lower / upper bound for each joint.
**kwargs (dict): range of possible physical properties. If given one value, that property won't be
randomized. Each range is a tuple of two values `[lower_bound, upper_bound]`.
joint_frictions (float, list/tuple of float, np.array[N], None): joint friction bounds. If None, it
doesn't randomize this parameter.
joint_dampings (float, list/tuple of float, np.array[N], None): joint damping bounds. If None, it doesn't
randomize this parameter.
"""
super(JointPhysicsRandomizer, self).__init__(body)
if not isinstance(robot, Robot):
raise TypeError("Expecting the given 'robot' to be an instance of `Robot`, instead got: "
"{}".format(type(robot)))
super(JointPhysicsRandomizer, self).__init__(robot)
# set joint ids
self.joints = joint_ids
# set bounds
self.joint_friction_bounds = joint_frictions
self.joint_damping_bounds = joint_dampings
##############
# Properties #
##############
@@ -75,6 +80,27 @@ class JointPhysicsRandomizer(BodyPhysicsRandomizer):
"{}".format(type(joints)))
self._joints = joints
@property
def joint_frictions(self):
"""Return the joint friction associated with the joints."""
return self.body.get_joint_frictions(joint_ids=self.joints)
@joint_frictions.setter
def joint_frictions(self, values):
"""Set the given joint friction values to each joint."""
raise NotImplementedError("This feature is not available yet.")
@property
def joint_friction_bounds(self):
"""Return the joint friction bounds."""
return self._joint_friction_bounds
@joint_friction_bounds.setter
def joint_friction_bounds(self, values):
"""Set the given joint frictions bounds."""
self._check_bounds('joint_frictions', values)
self._joint_friction_bounds = values
@property
def joint_dampings(self):
"""Return the joint dampings associated with the joints."""
@@ -83,8 +109,27 @@ class JointPhysicsRandomizer(BodyPhysicsRandomizer):
@joint_dampings.setter
def joint_dampings(self, values):
"""Set the given joint damping values to each joint."""
# check values
if isinstance(values, (float, int)):
values = [values] * len(self.joints)
elif len(values) != len(self.joints):
raise ValueError("The number of given joint damping values (={}) does not match with the number of "
"joints (={})".format(len(values), len(self.joints)))
# set the joint dampings
for joint, value in zip(self.joints, values):
self.body.set_joint_damping(joint, value)
self.simulator.change_dynamics(body_id=self.body.id, link_id=joint, joint_damping=value)
@property
def joint_damping_bounds(self):
"""Return the joint damping bounds."""
return self._joint_damping_bounds
@joint_damping_bounds.setter
def joint_damping_bounds(self, values):
"""Set the given joint damping bounds."""
self._check_bounds('joint_dampings', values)
self._joint_damping_bounds = values
###########
# Methods #
@@ -92,21 +137,25 @@ class JointPhysicsRandomizer(BodyPhysicsRandomizer):
def names(self):
"""Return an iterator over the property names."""
for name in ['joint_damping']:
yield name
yield 'joint_damping'
yield 'joint_friction'
def bounds(self):
"""Return an iterator over the bounds for each property."""
pass
yield self.joint_damping_bounds
yield self.joint_friction_bounds
def get_properties(self):
"""
Get the physics properties.
Get the current physical properties.
Returns:
dict: current physic property values.
dict: current physical property values.
"""
pass
joint_dampings = self.joint_dampings
joint_frictions = self.joint_frictions
return {joint_id: {'joint_damping': joint_dampings[joint_id], 'joint_friction': joint_frictions[joint_id]}
for joint_id in self.joints}
def set_properties(self, properties):
"""
@@ -115,4 +164,38 @@ class JointPhysicsRandomizer(BodyPhysicsRandomizer):
Args:
properties (dict): the physic property values to be set in the simulator.
"""
pass
# check the given properties
if not isinstance(properties, dict):
raise TypeError("Expecting the given 'properties' to be a dictionary, instead got: "
"{}".format(type(properties)))
# set the properties of each joint in the simulator
if len(properties) > 0:
for joint_id in self.joints:
self.simulator.change_dynamics(self.body.id, link_id=joint_id, **properties[joint_id])
def sample(self, seed=None):
"""
Sample a new set of physics properties and returns it. Note that it doesn't set them in the simulator.
This sampling can be useful if the user wishes to check more carefully the sampled physic property values.
Once satisfied, the user can set them by calling the `set_properties` method.
Note that it samples uniformly the physics properties between their specified lower and upper bounds.
Args:
seed (int, None): random seed.
Returns:
dict: sampled physic properties.
"""
# set random seed
if seed is not None:
np.random.seed(seed)
# sample each property
properties = dict()
for joint in self.joints:
for name, bound in zip(self.names(), self.bounds()):
if bound is not None:
properties.setdefault(joint, {})[name] = np.random.uniform(low=bound[0], high=bound[1])
return properties
pyrobolearn/physics/link_physics_randomizer.py
+323 -21
View File
@@ -7,12 +7,13 @@ Dependencies:
"""
import collections
import numpy as np
from pyrobolearn.physics.body_physics_randomizer import BodyPhysicsRandomizer
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__copyright__ = "Copyright 2019, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
@@ -30,20 +31,56 @@ class LinkPhysicsRandomizer(BodyPhysicsRandomizer):
def __init__(self, body, link_ids=None, masses=None, local_inertia_diagonals=None, local_inertia_positions=None,
local_inertia_orientations=None, lateral_frictions=None, spinning_frictions=None,
rolling_frictions=None, restitutions=None, linear_dampings=None, angular_dampings=None,
contact_stiffnesses=None, contact_dampings=None, **kwargs):
contact_stiffnesses=None, contact_dampings=None):
"""
Initialize the link physics randomizer.
Args:
body (Body): multi-body object.
body (Body, Robot): a body or robot instance.
link_ids (int, list of int, None): link id(s).
**kwargs (dict): range of possible physical properties. If given one value, that property won't be
randomized. Each range is a tuple of two values `[lower_bound, upper_bound]`.
masses (tuple/list of 2 float/np.array[N], np.array[2,N], None): lower and upper bounds for the mass of
each given link. If None, it won't randomize them.
local_inertia_diagonals (tuple/list of 2 np.array[3], np.array[2,N,3]): lower and upper bounds for the
local inertia diagonal for each given link. If None, it won't randomize them.
local_inertia_positions: lower and upper bounds for the local inertia position for each given link. If
None, it won't randomize them.
local_inertia_orientations: lower and upper bounds for the local inertia orientation for each given link.
If None, it won't randomize them.
lateral_frictions: lower and upper bounds for the lateral friction for each given link. If None, it won't
randomize them.
spinning_frictions: lower and upper bounds for the spinning friction for each given link. If None, it
won't randomize them.
rolling_frictions: lower and upper bounds for the rolling friction for each given link. If None, it won't
randomize them.
restitutions: lower and upper bounds for the restitution coefficient for each given link. If None, it
won't randomize them.
linear_dampings: lower and upper bounds for the linear damping for each given link. If None, it won't
randomize them.
angular_dampings: lower and upper bounds for the angular damping for each given link. If None, it won't
randomize them.
contact_stiffnesses: lower and upper bounds for the contact stiffness for each given link. If None, it
won't randomize them.
contact_dampings: lower and upper bounds for the contact damping for each given link. If None, it won't
randomize them.
"""
super(LinkPhysicsRandomizer, self).__init__(body)
self.links = link_ids
# set the bounds
self.mass_bounds = masses
self.local_inertia_diagonal_bounds = local_inertia_diagonals
# self.local_inertia_position_bounds = local_inertia_positions
# self.local_inertia_orientation_bounds = local_inertia_orientations
# self.linear_damping_bounds = linear_dampings
# self.angular_damping_bounds = angular_dampings
self.lateral_friction_bounds = lateral_frictions
self.spinning_friction_bounds = spinning_frictions
self.rolling_friction_bounds = rolling_frictions
self.restitution_bounds = restitutions
self.contact_stiffness_bounds = contact_stiffnesses
self.contact_damping_bounds = contact_dampings
##############
# Properties #
@@ -57,7 +94,9 @@ class LinkPhysicsRandomizer(BodyPhysicsRandomizer):
@links.setter
def links(self, links):
"""Set the link id or the list of link ids."""
if isinstance(links, int):
if links is None:
links = list(range(self.body.num_links))
elif isinstance(links, int):
links = [links]
elif isinstance(links, collections.Iterable):
for idx, link in enumerate(links):
@@ -72,54 +111,286 @@ class LinkPhysicsRandomizer(BodyPhysicsRandomizer):
@property
def masses(self):
"""Return the mass of each specified link."""
return self.body.get_masses(self.links)
return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[0] for link in self.links]
@masses.setter
def masses(self, values):
"""Set the mass values."""
self.body.set_masses(self.links, values)
# check values
values = self._check_values('mass', values)
# set the link masses
for link, value in zip(self.links, values):
self.simulator.change_dynamics(body_id=self.body.id, link_id=link, mass=value)
@property
def mass_bounds(self):
"""Return the lower and upper bounds of each link mass."""
"""Return the lower and upper bounds of each link's mass."""
return self._mass_bounds
@mass_bounds.setter
def mass_bounds(self, bounds):
"""Set the mass bound for each link."""
if isinstance(bounds, (float, int)):
bounds = [(bounds, bounds) for _ in self.links]
elif isinstance(bounds, (list, tuple, np.ndarray)):
pass
self._check_bounds('masses', bounds)
self._mass_bounds = bounds
@property
def dynamics(self):
return None
def local_inertia_diagonals(self):
"""Return the local inertial diagonal of each specified link."""
return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[2] for link in self.links]
@local_inertia_diagonals.setter
def local_inertia_diagonals(self, values):
"""Set the given local inertia diagonals."""
if not isinstance(values, (list, tuple, np.ndarray)):
raise TypeError("Expecting the given local inertia diagonals to be list/tuple/np.ndarray of float/int, "
"but got instead: {}".format(values))
if len(values) != len(self.links):
raise ValueError("The number of given mass values (={}) does not match with the number of links "
"(={})".format(len(values), len(self.links)))
# set the link local inertia diagonal
for link, value in zip(self.links, values):
if len(value) != 3:
raise ValueError("Expecting the given local inertia diagonal to be a tuple/list/np.ndarray of length "
"3, but instead got a length of: {}".format(len(value)))
self.simulator.change_dynamics(body_id=self.body.id, link_id=link, local_inertia_diagonal=value)
@property
def local_inertia_diagonal_bounds(self):
"""Return the lower and upper bounds of each link's local inertia diagonal."""
return self._local_inertia_diagonal_bounds
@local_inertia_diagonal_bounds.setter
def local_inertia_diagonal_bounds(self, bounds):
"""Set the local inertia diagonal bound for each link."""
self._check_bounds('local_inertia_diagonals', bounds)
self._local_inertia_diagonal_bounds = bounds
@property
def local_inertia_positions(self):
"""Return the local inertia position of each specified link."""
return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[3] for link in self.links]
@local_inertia_positions.setter
def local_inertia_positions(self, values):
"""Set the given local inertia positions."""
raise NotImplementedError("This features is not yet available.")
@property
def local_inertia_orientations(self):
"""Return the local inertia orientation of each specified link."""
return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[4] for link in self.links]
@local_inertia_orientations.setter
def local_inertia_orientations(self, values):
"""Set the given local inertia orientations."""
raise NotImplementedError("This features is not yet available.")
@property
def lateral_frictions(self):
"""Return the lateral friction of each specified link."""
return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[1] for link in self.links]
@lateral_frictions.setter
def lateral_frictions(self, values):
"""Set the given lateral frictions."""
# check values
self._check_values('lateral_friction', values)
# set the link masses
for link, value in zip(self.links, values):
self.simulator.change_dynamics(body_id=self.body.id, link_id=link, lateral_friction=value)
@property
def lateral_friction_bounds(self):
"""Return the lower and upper bounds of each link's lateral friction."""
return self._lateral_friction_bounds
@lateral_friction_bounds.setter
def lateral_friction_bounds(self, bounds):
"""Set the lateral friction bound for each link."""
self._check_bounds('lateral_frictions', bounds)
self._lateral_friction_bounds = bounds
@property
def restitutions(self):
"""Return the restitution coefficient of each link."""
return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[5] for link in self.links]
@restitutions.setter
def restitutions(self, values):
"""Set the given restitution coefficients."""
# check values
self._check_values('restitution', values)
# set the link masses
for link, value in zip(self.links, values):
self.simulator.change_dynamics(body_id=self.body.id, link_id=link, restitution=value)
@property
def restitution_bounds(self):
"""Return the lower and upper bounds of each link's restitution coefficient."""
return self._restitution_bounds
@restitution_bounds.setter
def restitution_bounds(self, bounds):
"""Set the restitution coefficient bound for each link."""
self._check_bounds('restitutions', bounds)
self._restitution_bounds = bounds
@property
def rolling_frictions(self):
"""Return the rolling friction of each link."""
return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[6] for link in self.links]
@rolling_frictions.setter
def rolling_frictions(self, values):
"""Set the given rolling frictions."""
# check values
self._check_values('rolling_friction', values)
# set the link masses
for link, value in zip(self.links, values):
self.simulator.change_dynamics(body_id=self.body.id, link_id=link, rolling_friction=value)
@property
def rolling_friction_bounds(self):
"""Return the lower and upper bounds of each link's rolling friction."""
return self._rolling_friction_bounds
@rolling_friction_bounds.setter
def rolling_friction_bounds(self, bounds):
"""Set the rolling friction bound for each link."""
self._check_bounds('rolling_frictions', bounds)
self._rolling_friction_bounds = bounds
@property
def spinning_frictions(self):
"""Return the spinning friction of each link."""
return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[7] for link in self.links]
@spinning_frictions.setter
def spinning_frictions(self, values):
"""Set the given spinning frictions."""
# check values
self._check_values('spinning_friction', values)
# set the link masses
for link, value in zip(self.links, values):
self.simulator.change_dynamics(body_id=self.body.id, link_id=link, spinning_friction=value)
@property
def spinning_friction_bounds(self):
"""Return the lower and upper bounds of each link's spinning friction."""
return self._spinning_friction_bounds
@spinning_friction_bounds.setter
def spinning_friction_bounds(self, bounds):
"""Set the spinning friction bound for each link."""
self._check_bounds('spinning_frictions', bounds)
self._spinning_friction_bounds = bounds
@property
def contact_dampings(self):
"""Return the contact damping of each link."""
return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[8] for link in self.links]
@contact_dampings.setter
def contact_dampings(self, values):
"""Set the given contact dampings."""
# check values
self._check_values('contact_damping', values)
# set the link masses
for link, value in zip(self.links, values):
self.simulator.change_dynamics(body_id=self.body.id, link_id=link, contact_damping=value)
@property
def contact_damping_bounds(self):
"""Return the lower and upper bounds of each link's contact damping."""
return self._contact_damping_bounds
@contact_damping_bounds.setter
def contact_damping_bounds(self, bounds):
"""Set the contact damping bound for each link."""
self._check_bounds('contact_dampings', bounds)
self._contact_damping_bounds = bounds
@property
def contact_stiffnesses(self):
"""Return the contact stiffness of each link."""
return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[9] for link in self.links]
@contact_stiffnesses.setter
def contact_stiffnesses(self, values):
"""Set the given contact stiffnesses."""
# check values
self._check_values('contact_stiffness', values)
# set the link masses
for link, value in zip(self.links, values):
self.simulator.change_dynamics(body_id=self.body.id, link_id=link, contact_stiffness=value)
@property
def contact_stiffness_bounds(self):
"""Return the lower and upper bounds of each link's contact stiffness."""
return self._contact_stiffness_bounds
@contact_stiffness_bounds.setter
def contact_stiffness_bounds(self, bounds):
"""Set the contact stiffness bound for each link."""
self._check_bounds('contact_stiffnesses', bounds)
self._contact_stiffness_bounds = bounds
###########
# Methods #
###########
def _check_values(self, name, values):
"""Check the given values to be set."""
if isinstance(values, (float, int)):
values = [values] * len(self.links)
elif not isinstance(values, (list, tuple, np.ndarray)):
raise TypeError("Expecting the given '" + name + "' to be a list/tuple/np.array of float/int, but got "
"instead: {}".format(type(values)))
if len(values) != len(self.links):
raise ValueError("The number of given '" + name + "' values (={}) does not match with the number of links "
"(={})".format(len(values), len(self.links)))
return values
def names(self):
"""Return an iterator over the property names."""
for name in ['mass']:
# 'local_inertia_position', 'local_inertia_orientation', 'linear_damping', 'angular_damping'
for name in ['mass', 'local_inertia_diagonal', 'lateral_friction', 'spinning_friction', 'rolling_friction',
'restitution', 'contact_stiffness', 'contact_damping']:
yield name
def bounds(self):
"""Return an iterator over the bounds for each property."""
yield self.mass_bounds
yield self.local_inertia_diagonal_bounds
yield self.lateral_friction_bounds
yield self.spinning_friction_bounds
yield self.rolling_friction_bounds
yield self.restitution_bounds
yield self.contact_stiffness_bounds
yield self.contact_damping_bounds
def get_properties(self):
"""
Get the physics properties.
Get the current physics properties.
Returns:
dict: current physic property values.
dict: current physical property values {physic property name: corresponding value}.
"""
properties = dict()
# properties['mass'] =
return properties
infos = [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link) for link in self.links]
return {link: {'mass': info[0], 'local_inertia_diagonal': info[2], 'local_inertia_position': info[3],
'local_inertia_orientation': info[4], 'lateral_friction': info[1], 'spinning_friction': info[7],
'rolling_friction': info[6], 'restitution': info[5], 'contact_stiffness': info[9],
'contact_damping': info[8]}
for link, info in zip(self.links, infos)}
def set_properties(self, properties):
"""
@@ -128,7 +399,38 @@ class LinkPhysicsRandomizer(BodyPhysicsRandomizer):
Args:
properties (dict): the physic property values to be set in the simulator.
"""
# check the given properties
if not isinstance(properties, dict):
raise TypeError("Expecting the given 'properties' to be a dictionary, instead got: "
"{}".format(type(properties)))
# set the properties in the simulator
if len(properties) > 0:
for link in self.links:
self.simulator.change_dynamics(self.body.id, link_id=link, **properties[link])
def sample(self, seed=None):
"""
Sample a new set of physics properties and returns it. Note that it doesn't set them in the simulator.
This sampling can be useful if the user wishes to check more carefully the sampled physic property values.
Once satisfied, the user can set them by calling the `set_properties` method.
Note that it samples uniformly the physics properties between their specified lower and upper bounds.
Args:
seed (int, None): random seed.
Returns:
dict: sampled physic properties.
"""
# set random seed
if seed is not None:
np.random.seed(seed)
# sample each property
properties = dict()
for link in self.links:
for name, bound in zip(self.names(), self.bounds()):
if bound is not None:
properties.setdefault(link, {})[name] = np.random.uniform(low=bound[0], high=bound[1])
return properties
pyrobolearn/physics/physics_randomizer.py
+54 -15
View File
@@ -11,8 +11,9 @@ import numpy as np
from pyrobolearn.simulators import Simulator
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__copyright__ = "Copyright 2019, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
@@ -25,18 +26,18 @@ class PhysicsRandomizer(object):
r"""Physics Randomizer
This the main abstract class from which all the physics randomizers inherit from. A physics randomizer randomize
the physics properties of a certain object. This can be, for instance, the world, a robot, or a particular link
of that robot.
the physics properties of a certain object. This can be, for instance, the world, a robot, or a particular link /
joint of that robot.
It can change for example the dynamics of a particular object such as the mass, inertia, or others. It can also
change their physical properties such as the friction, bounciness, etc.
change their physical properties such as the friction, bounciness (restitution coefficient), etc.
Note that the physics randomizer instance has access to the simulator in order to modify the physical properties.
Also, note that normally the physics randomizer is called at the beginning of an episode, and not at each time
step. Changing the physical properties at each time step can lead to weird behaviors.
It is possible to not randomize some physical properties by specifying a specific value instead of a range (=tuple
of 2 values; lower and upper bound).
of 2 values; lower and upper bound), or by setting None.
"""
def __init__(self, simulator):
@@ -60,16 +61,35 @@ class PhysicsRandomizer(object):
@simulator.setter
def simulator(self, simulator):
"""Set the simulator instance."""
# TODO: uncomment the following lines
# if not isinstance(simulator, Simulator):
# raise TypeError("Expecting the given simulator to be an instance of `Simulator`, instead got: "
# "{}".format(type(simulator)))
if not isinstance(simulator, Simulator):
raise TypeError("Expecting the given simulator to be an instance of `Simulator`, instead got: "
"{}".format(type(simulator)))
self._simulator = simulator
###########
# Methods #
###########
@staticmethod
def _check_bounds(name, bounds):
"""Check that the bounds are of the correct type and size.
Args:
name (str): name of the physical property
bounds (list/tuple of float, np.array[2, N], None): bounds. The first item is supposed to be the lower
bound and the second item the upper bound. If None, it doesn't do anything.
"""
if bounds is not None:
# check bounds type and length
if not isinstance(bounds, (tuple, list, np.ndarray)):
raise TypeError("Expecting the given '" + name + "' to be a tuple/list/array of len(2) where the "
"first item represents the lower bound, and the second item represents the upper "
"bound, but got instead a type of: {}".format(type(bounds)))
if len(bounds) != 2:
raise ValueError("Expecting the given '" + name + "' to be a tuple/list/array of len(2) where the "
"first item represents the lower bound, and the second item represents the upper "
"bound, but got instead a length of: {}".format(len(bounds)))
def properties(self):
"""Return an iterator over the properties."""
properties = self.get_properties()
@@ -84,11 +104,13 @@ class PhysicsRandomizer(object):
def names(self):
"""Return an iterator over the property names."""
pass
properties = self.get_properties()
for name in properties.keys():
yield name
def bounds(self):
"""Return an iterator over the bounds for each property."""
pass
raise NotImplementedError
def named_bounds(self):
"""Return an iterator over the property bounds with their name and value."""
@@ -97,16 +119,16 @@ class PhysicsRandomizer(object):
def get_properties(self):
"""
Get the physics properties.
Get the physics properties. This method has to be implemented in the child class.
Returns:
dict: current physic property values.
dict: current physic property values {physic property name: corresponding value}
"""
pass
def set_properties(self, properties):
"""
Set the given physic property values using the simulator.
Set the given physic property values using the simulator. This method has to be implemented in the child class.
Args:
properties (dict): the physic property values to be set in the simulator.
@@ -134,7 +156,8 @@ class PhysicsRandomizer(object):
# sample each property
properties = dict()
for name, bound in zip(self.names(), self.bounds()):
properties[name] = np.random.uniform(low=bound[0], high=bound[1])
if bound is not None:
properties[name] = np.random.uniform(low=bound[0], high=bound[1])
return properties
def randomize(self, seed=None):
@@ -156,3 +179,19 @@ class PhysicsRandomizer(object):
"""
if seed is not None:
np.random.seed(seed)
#############
# Operators #
#############
def __str__(self):
"""Return a string describing the class."""
return
def __call__(self, seed=None):
"""Randomize the physics properties and set them in the simulator.
Args:
seed (int, None): random seed.
"""
self.randomize(seed=seed)
pyrobolearn/physics/robot_physics_randomizer.py
+1 -1
View File
@@ -13,7 +13,7 @@ from pyrobolearn.physics.joint_physics_randomizer import JointPhysicsRandomizer
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__copyright__ = "Copyright 2019, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
pyrobolearn/physics/world_physics_randomizer.py
+1 -1
View File
@@ -13,7 +13,7 @@ from pyrobolearn.physics.physics_randomizer import PhysicsRandomizer
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__copyright__ = "Copyright 2019, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
pyrobolearn/rewards/__init__.py
+5
View File
@@ -9,8 +9,13 @@ from .basic_rewards import *
# import gym wrapper reward
from .gym_reward import GymReward
# import terminal rewards
from .terminal_rewards import TerminalReward
# import costs
from .cost import *
from .joint_cost import *
from .link_cost import *
# import processors
from .processors import *
pyrobolearn/rewards/action_cost.py
+55
View File
@@ -0,0 +1,55 @@
#!/usr/bin/env python
"""Define the costs used on actions.
"""
from abc import ABCMeta
import numpy as np
import pyrobolearn as prl
from pyrobolearn.rewards.cost import Cost
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class ActionCost(Cost):
r"""(Abstract) Action Cost."""
__metaclass__ = ABCMeta
pass
class ActionDifferenceCost(ActionCost):
r"""Action Difference Cost
This computes the difference between two actions:
.. math:: \text{cost} = || a_{t-1} - a_{t} ||^2
References:
- [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
"""
def __init__(self, action):
"""
Initialize the action difference cost.
Args:
action (Action): action instance.
"""
super(ActionDifferenceCost, self).__init__()
if not isinstance(action, prl.actions.Action):
raise TypeError("Expecting the given 'action' to be an instance of `Action`, instead got: "
"{}".format(type(action)))
self.action = action
def _compute(self):
data = self.action.merged_data
prev_data = self.action.merged_data
return np.sum([- np.sum((curr - prev)**2) for curr, prev in zip(data, prev_data)])
pyrobolearn/rewards/cost.py
+16 -276
View File
@@ -27,6 +27,16 @@ import pyrobolearn.actions as actions
from pyrobolearn.rewards.reward import Reward
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
# class Cost(Objective):
class Cost(Reward):
r"""Abstract `Cost` class which inherits from the `Objective` class, and is set to be minimized.
@@ -57,23 +67,6 @@ def logistic_kernel_function(error, alpha):
return 1. / (np.exp(alpha * error) + 2. + np.exp(- alpha * error))
def min_angle_difference(q1, q2):
r"""
Return the minimum angle difference between two angles.
Args:
q1 (float, np.array[N]): first angle(s)
q2 (float, np.array[N]): second angle(s)
Returns:
float, np.array[N]: minimum angle difference(s)
"""
diff = np.maximum(q1, q2) - np.minimum(q1, q2)
if diff > np.pi:
diff = 2 * np.pi - diff
return diff
class AngularVelocityErrorCost(Cost):
r"""Angular Velocity Error Cost
@@ -140,26 +133,6 @@ class HeightCost(Cost):
return 0
class JointPositionErrorCost(Cost):
r"""Joint Position Error Cost
Return the joint position error as defined in [1] as :math:`d(\hat{\phi}, \phi) \in [0, \pi]` where :math:`d(.,.)`
is the minimum angle difference between two angles, and :math:`\hat{\phi}` and :math:`\phi` are the target and
current angles.
References:
- [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
"""
def __init__(self, joint_state, target_joint_state):
super(JointPositionErrorCost, self).__init__()
self.state = joint_state
self.target_state = target_joint_state
def _compute(self):
return - min_angle_difference(self.state.data[0], self.target_state.data[0])
class OrientationGravityCost(Cost):
r"""Orientation Gravity Cost
@@ -176,81 +149,6 @@ class OrientationGravityCost(Cost):
return np.linalg.norm(self.gravity_state.data - self.gravity)
class JointPositionCost(Cost):
r"""Joint Position Cost
Return the cost such that measures the L2 norm between the current joint positions and the target joint positions:
:math:`||d(q_{target},q)||^2` where :math:`d(\cdot, \cdot) \in [-\pi, \pi]` is the minimum distance between two
angles.
"""
def __init__(self, joint_position_state, target_joint_position):
r"""
Initialize the joint position cost.
Args:
joint_position_state (JointPositionState): joint position state.
target_joint_position (np.array[N]): target joint positions.
"""
super(JointPositionCost, self).__init__()
if not isinstance(joint_position_state, prl.states.JointPositionState):
raise TypeError("Expecting the given 'joint_position_state' to be an instance of `JointPositionState`, "
"but instead got: {}".format(type(joint_position_state)))
self.q = joint_position_state
self.target_q = target_joint_position
def _compute(self):
"""Compute and return the cost value."""
return - np.sum(min_angle_difference(self.q.data[0], self.target_q)**2)
class JointVelocityCost(Cost):
r"""Joint Velocity Cost
Return the cost due to the joint velocities: :math:`|| \dot{q} ||^2`
"""
def __init__(self, joint_velocity_state):
"""
Initialize the joint velocity cost.
Args:
joint_velocity_state (JointVelocityState): joint velocity state.
"""
super(JointVelocityCost, self).__init__()
if not isinstance(joint_velocity_state, prl.states.JointVelocityState):
raise TypeError("Expecting the given 'joint_velocity_state' to be an instance of `JointVelocityState`, "
"but instead got: {}".format(type(joint_velocity_state)))
self.dq = joint_velocity_state
def _compute(self):
"""Compute and return the cost value."""
return - np.sum(self.dq.data[0] ** 2)
class JointTorqueCost(Cost):
r"""Torque Cost
Return the cost due to the joint torques; :math:`|| \tau ||^2`.
"""
def __init__(self, joint_torque_state):
"""
Initialize the joint torque cost.
Args:
joint_torque_state (JointForceTorqueState): joint torque state.
"""
super(JointTorqueCost, self).__init__()
if not isinstance(joint_torque_state, prl.states.JointForceTorqueState):
raise TypeError("Expecting the given 'joint_torque_state' to be an instance of `JointForceTorqueState`, "
"but instead got: {}".format(type(joint_torque_state)))
self.tau = joint_torque_state
def _compute(self):
"""Compute and return the cost value."""
return - np.sum(self.tau.data[0]**2)
class PowerCost(Cost):
r"""Power Consumption Cost
@@ -268,85 +166,6 @@ class PowerCost(Cost):
return - np.sum(np.maximum(self.tau.data[0] * self.vel.data[0], 0))
class JointPowerConsumptionCost(Cost):
r"""Joint Power Consumption Cost
Return the joint power consumption cost, where the power is computed as the torque times the velocity.
"""
def __init__(self, state, joint_ids=None, update_state=False):
"""
Initialize the Joint Power Consumption cost.
Args:
torque (Robot, State): robot instance, or the state. The state must contains the `JointForceTorqueState`
and the `JointVelocityState`.
joint_ids (None, int, list of int): joint ids. This used if `torque` is a `Robot` instance.
update_state (bool): If True, it will update the state.
"""
self.update_state = update_state
if isinstance(state, Robot):
torque = states.JointForceTorqueState(state, joint_ids=joint_ids)
velocity = states.JointVelocityState(state, joint_ids=joint_ids)
self.update_state = True
# state = torque + velocity
elif isinstance(state, states.State):
# check if they have the correct state
torque = state.lookfor(states.JointForceTorqueState)
if torque is None:
raise ValueError("Didn't find a `JointForceTorqueState` instance in the given states.")
velocity = state.lookfor(states.JointVelocityState)
if velocity is None:
raise ValueError("Didn't find a `JointVelocityState` instance in the given states.")
else:
raise TypeError("Expecting the state to be an instance of `State` or `Robot`.")
super(JointPowerConsumptionCost, self).__init__() # state=state)
self.tau = torque
self.vel = velocity
self.update_state = update_state
def compute(self):
if self.update_state:
self.tau()
self.vel()
return - np.sum((self.tau.data[0] * self.vel.data[0])**2)
class JointAccelerationCost(Cost):
r"""Joint Acceleration Cost
Return the joint acceleration cost defined notably in [1] as :math:`cost = ||\ddot{q}||^2`
References:
- [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
"""
def __init__(self, joint_acceleration_state):
super(JointAccelerationCost, self).__init__()
self.ddq = joint_acceleration_state
def compute(self):
return - np.sum(self.ddq.data**2)
class JointSpeedCost(Cost):
r"""Joint Speed Cost
Return the joint speed cost as computed in [1].
References:
- [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
"""
def __init__(self, joint_velocity_state, max_joint_speed=None):
super(JointSpeedCost, self).__init__()
self.dq = joint_velocity_state
self.dq_max = max_joint_speed
if max_joint_speed is None:
self.dq_max = joint_velocity_state.max
def compute(self):
return - np.sum(np.maximum(self.dq_max - np.abs(self.dq.data), 0)**2)
class BodyImpulseCost(Cost):
r"""Body Impulse Cost
@@ -425,20 +244,6 @@ class SelfCollisionCost(Cost):
pass
class ActionDifferenceCost(Cost):
r"""Action Difference Cost
References:
- [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
"""
def __init__(self, action):
super(ActionDifferenceCost, self).__init__()
self.action = action
def compute(self):
return - (self.action.data - self.action.prev_data)**2
class PhysicsViolationCost(Cost):
"""Physics Violation Cost.
@@ -470,81 +275,16 @@ class ContactInvariantCost(Cost):
super(ContactInvariantCost, self).__init__()
class DistanceCost(Cost):
"""Distance Cost.
It penalizes the distance between 2 objects. One of the 2 objects must be movable in order for this
cost to change.
Mathematically, the cost is given by:
.. math:: c(l1, l2) = d(l1, l2) = - || l1 - l2 ||_2
where :math:`l1` represents a link attached to the first body, and :math:`l2` represents a link attached on the
second body. The distance function used is the Euclidean distance (=L2 norm).
"""
def __init__(self, body1, body2, link_id1=-1, link_id2=-1, offset=None):
r"""
Initialize the distance cost.
Args:
body1 (BasePositionState, PositionState, LinkWorldPositionState, Body, Robot): first position state. If
Body, it will wrap it with a `PositionState`. If Robot, it will wrap it with a `PositionState` or
`LinkPositionState` depending on the value of :attr:`link_id1`.
body2 (BasePositionState, PositionState, LinkWorldPositionState, Body, Robot): second position state. If
Body, it will wrap it with a `PositionState`. If Robot, it will wrap it with a `PositionState` or
`LinkPositionState` depending on the value of :attr:`link_id1`.
link_id1 (int): link id associated with the first body that we are interested in. This is only used if
the given :attr:`body1` is not a state.
link_id2 (int): link id associated with the second body that we are interested in. This is only used if
the given :attr:`body2` is not a state.
offset (None, np.array[3]): 3d offset between body1 and body2.
"""
super(DistanceCost, self).__init__()
def check_body_type(body, id_, link_id):
update_state = False
if isinstance(body, prl.robots.Body):
body = states.PositionState(body)
update_state = True
elif isinstance(body, Robot):
if link_id == -1:
body = states.PositionState(body)
else:
body = states.LinkWorldPositionState(body, link_ids=link_id)
update_state = True
elif not isinstance(body, (states.BasePositionState, states.PositionState, states.LinkWorldPositionState)):
raise TypeError("Expecting the given 'body"+str(id_)+"' to be an instance of `Body`, `Robot`, "
"`BasePositionState`, `PositionState` or `LinkWorldPositionState`, instead got: "
"{}".format(type(body), id_))
return body, update_state
self.body1, self.update_state1 = check_body_type(body1, id_=1, link_id=link_id1)
self.body2, self.update_state2 = check_body_type(body2, id_=2, link_id=link_id2)
def compute(self):
if self.update_state1:
self.body1()
if self.update_state2:
self.body2()
p1 = self.body1.data[0]
p2 = self.body2.data[0]
# print("P1: {}".format(p1))
# print("P2: {}".format(p2))
return - np.linalg.norm(p1 - p2)
class ImpactCost(Cost):
"""Impact cost.
Calculates the impact force using the kinetic energy.
"""
def __init__(self):
def __init__(self, body1, body2):
super(ImpactCost, self).__init__()
def compute(self, object1, object2):
def _compute(self):
pass
@@ -554,10 +294,10 @@ class DriftCost(Cost):
Calculates the drift of a moving object wrt a direction.
"""
def __init__(self):
def __init__(self, body, direction):
super(DriftCost, self).__init__()
def compute(self, object, direction):
def _compute(self):
pass
@@ -566,10 +306,10 @@ class ShakeCost(Cost):
Calculates the
"""
def __init__(self):
def __init__(self, body, direction):
super(ShakeCost, self).__init__()
def compute(self, object, direction):
def _compute(self):
pass
pyrobolearn/rewards/joint_cost.py
+358
View File
@@ -0,0 +1,358 @@
#!/usr/bin/env python
"""Define the costs used on joint states / actions.
"""
from abc import ABCMeta
import numpy as np
import pyrobolearn as prl
from pyrobolearn.rewards.cost import Cost
from pyrobolearn.utils.transformation import min_angle_difference
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class JointCost(Cost):
r"""(Abstract) Joint Cost."""
__metaclass__ = ABCMeta
def __init__(self, update_state=False):
"""
Initialize the abstract joint cost.
Args:
update_state (bool): if True it will update the given states before computing the cost.
"""
super(JointCost, self).__init__()
self.update_state = update_state
@staticmethod
def _check_state(state, cls, update_state=False):
# check given state
if isinstance(state, prl.robots.Robot): # if robot, instantiate state class with robot as param.
state = cls(robot=state)
update_state = True
if not isinstance(state, cls): # if not an instance of the given state, class, raise error
raise TypeError("Expecting the given 'state' to be an instance of `Robot` or `" + cls.__name__ + "`, "
"but instead got: {}".format(type(state)))
return state, update_state
@staticmethod
def _check_target_state(state, target_state, cls, update_state=False):
# check given target state
if target_state is None: # if the target is None, initialize it zero
target_state = np.zeros(state.total_size())
if isinstance(target_state, (int, float, np.ndarray)): # if target is a np.array/float/int, create FixedState
target_state = prl.states.FixedState(value=target_state)
update_state = True
elif isinstance(target_state, prl.robots.Robot): # if robot, instantiate state class with robot as param.
target_state = cls(robot=target_state)
update_state = True
elif not isinstance(target_state, cls): # if not an instance of the given state class, raise error
raise TypeError("Expecting the given 'target_state' to be None, a np.array, or an instance of "
"`Robot` or `" + cls.__name__ + "`, but instead got: {}".format(type(target_state)))
return target_state, update_state
# class JointPositionErrorCost(JointCost):
# r"""Joint Position Error Cost
#
# Return the joint position error as defined in [1] as :math:`d(\hat{\phi}, \phi) \in [0, \pi]` where :math:`d(.,.)`
# is the minimum angle difference between two angles, and :math:`\hat{\phi}` and :math:`\phi` are the target and
# current angles.
#
# References:
# - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
# """
#
# def __init__(self, joint_state, target_joint_state, update_state=False):
# """
# Initialize the joint position error cost.
#
# Args:
# joint_state (JointPositionState):
# target_joint_state (JointPositionState):
# update_state (bool): if True it will update the given states before computing the cost.
# """
# super(JointPositionErrorCost, self).__init__(update_state=update_state)
# self.state = joint_state
# self.target_state = target_joint_state
#
# def _compute(self):
# return - min_angle_difference(self.state.data[0], self.target_state.data[0])
class JointPositionCost(JointCost):
r"""Joint Position Cost
Return the cost such that measures the L2 norm between the current joint positions and the target joint positions:
:math:`||d(q_{target},q)||^2` where :math:`d(\cdot, \cdot) \in [-\pi, \pi]` is the minimum distance between two
angles.
References:
- [1] OpenAI Gym
- [2] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
"""
def __init__(self, state, target_state, update_state=False):
r"""
Initialize the joint position cost.
Args:
state (JointPositionState, Robot): joint position state.
target_state (JointPositionState, np.array[N], None): target joint position state. If None, it will be set
to 0.
update_state (bool): if True it will update the given states before computing the cost.
"""
super(JointPositionCost, self).__init__(update_state)
# check given joint position state
self.q, self.update_state = self._check_state(state, prl.states.JointPositionState,
update_state=self.update_state)
# check target joint position state
self.q_target, self.update_target_state = self._check_target_state(self.q, target_state,
prl.states.JointPositionState,
self.update_state)
if self.q.total_size() != self.q_target.total_size():
raise ValueError("The given state and target_state do not have the same size: "
"{} != {}".format(self.q.total_size(), self.q_target.total_size()))
def _compute(self):
"""Compute and return the cost value."""
if self.update_state:
self.q()
if self.update_target_state:
self.q_target()
return - np.sum(min_angle_difference(self.q.data[0], self.q_target.data[0])**2)
class JointVelocityCost(JointCost):
r"""Joint Velocity Cost
Return the cost due to the joint velocities: :math:`|| \dot{q}_{target} - \dot{q} ||^2`, where
:math:`\dot{q}_{target}` can be set to zero if wished.
"""
def __init__(self, state, target_state=None, update_state=False):
"""
Initialize the joint velocity cost.
Args:
state (JointVelocityState, Robot): joint velocity state.
target_state (JointVelocityState, np.array[N], Robot, None): target joint velocity state. If None, it
will be set to 0.
update_state (bool): if True it will update the given states before computing the cost.
"""
super(JointVelocityCost, self).__init__(update_state)
# check given joint velocity state
self.dq, self.update_state = self._check_state(state, prl.states.JointVelocityState,
update_state=self.update_state)
# check target joint velocity state
self.dq_target, self.update_target_state = self._check_target_state(self.dq, target_state,
prl.states.JointVelocityState,
self.update_state)
def _compute(self):
"""Compute and return the cost value."""
if self.update_state:
self.dq()
if self.update_target_state:
self.dq_target()
return - np.sum((self.dq_target.data[0] - self.dq.data[0])**2)
class JointAccelerationCost(JointCost):
r"""Joint Acceleration Cost
Return the joint acceleration cost defined notably in [1] as :math:`cost = || \ddot{q}_{target} - \ddot{q} ||^2`,
where :math:`\ddot{q}_{target}` can be set to zero if wished.
References:
- [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
"""
def __init__(self, state, target_state=None, update_state=False):
"""
Initialize the joint acceleration cost.
Args:
state (JointAccelerationState, Robot): joint acceleration state.
target_state (JointAccelerationState, np.array[N], Robot, None): target joint acceleration state. If None,
it will be set to 0.
update_state (bool): if True it will update the given states before computing the cost.
"""
super(JointAccelerationCost, self).__init__(update_state=update_state)
# check given joint acceleration state
self.ddq, self.update_state = self._check_state(state, prl.states.JointAccelerationState,
update_state=self.update_state)
# check target joint acceleration state
self.ddq_target, self.update_target_state = self._check_target_state(self.ddq, target_state,
prl.states.JointAccelerationState,
self.update_state)
def _compute(self):
"""Compute and return the cost value."""
if self.update_state:
self.ddq()
if self.update_target_state:
self.ddq_target()
return - np.sum((self.ddq_target.data[0] - self.ddq.data[0])**2)
class JointTorqueCost(JointCost):
r"""Torque Cost
Return the cost due to the joint torques; :math:`|| \tau_{target} - \tau ||^2`, where :math:`\tau_{target}` can
be set to zero if wished.
"""
def __init__(self, state, target_state=None, update_state=False):
"""
Initialize the joint torque cost.
Args:
state (JointForceTorqueState, Robot): joint torque state.
target_state (JointForceTorqueState, np.array[N], Robot, None): target joint torque state. If None, it
will be set to 0.
update_state (bool): if True it will update the given states before computing the cost.
"""
super(JointTorqueCost, self).__init__(update_state)
# check given joint torque state
self.tau, self.update_state = self._check_state(state, prl.states.JointForceTorqueState,
update_state=self.update_state)
# check target joint torque state
self.tau_target, self.update_target_state = self._check_target_state(self.tau, target_state,
prl.states.JointForceTorqueState,
self.update_state)
def _compute(self):
"""Compute and return the cost value."""
if self.update_state:
self.tau()
if self.update_target_state:
self.tau_target()
return - np.sum((self.tau_target.data[0] - self.tau.data[0])**2)
class JointPowerCost(JointCost):
r"""Joint Power Consumption Cost
Return the joint power consumption cost, where the power is computed as the torque times the velocity.
"""
def __init__(self, state, joint_ids=None, update_state=False):
"""
Initialize the Joint Power Consumption cost.
Args:
state (Robot, State): robot instance, or the state. The state must contains the `JointForceTorqueState`
and the `JointVelocityState`. Note that if they are multiple torque or velocity states, it will look
for the first instance.
joint_ids (None, int, list of int): joint ids. This used if `torque` is a `Robot` instance.
update_state (bool): if True it will update the given states before computing the cost.
"""
self.update_state = update_state
# Check the state
# if the given state is a robot, create the torque and velocity states
if isinstance(state, prl.robots.Robot):
torque = prl.states.JointForceTorqueState(state, joint_ids=joint_ids)
velocity = prl.states.JointVelocityState(state, joint_ids=joint_ids)
self.update_state = True
# state = torque + velocity
# elif the given state is a composite state, look for the torque and velocity states.
else:
if isinstance(state, prl.states.State):
state = [state]
# if the given state is a list of states, check each one of them by looking for the torque/velocity state
if isinstance(state, (list, tuple)):
# for each state, check if it is a torque, velocity or composite state
torque, velocity = None, None
for s in state:
if isinstance(s, prl.states.JointForceTorqueState):
if torque is None:
torque = s
elif isinstance(s, prl.states.JointVelocityState):
if velocity is None:
velocity = s
elif isinstance(s, prl.states.State):
if torque is None:
torque = s.lookfor(prl.states.JointForceTorqueState)
if velocity is None:
velocity = s.lookfor(prl.states.JointVelocityState)
else:
raise TypeError("Expecting the state to be an instance of `State` or `Robot`, or a list of "
"`State`, instead got: {}".format(type(s)))
# if we have found the states, get out of the loop
if torque is not None and velocity is not None:
break
# check that we have the torque and velocity states
if torque is None:
raise ValueError("Didn't find a `JointForceTorqueState` instance in the given states.")
if velocity is None:
raise ValueError("Didn't find a `JointVelocityState` instance in the given states.")
else:
raise TypeError("Expecting the state to be an instance of `State` or `Robot`, instead got: "
"{}".format(type(state)))
super(JointPowerCost, self).__init__()
self.tau = torque
self.vel = velocity
self.update_state = update_state
def _compute(self):
"""Compute and return the cost value."""
if self.update_state:
self.tau()
self.vel()
return - np.sum((self.tau.data[0] * self.vel.data[0])**2)
# class JointSpeedCost(Cost):
# r"""Joint Speed Cost
#
# Return the joint speed cost as computed in [1].
#
# .. math:: \text{cost} = || \max(\dot{q}_{max} - |\dot{q}|, 0) ||^2
#
# References:
# - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
# """
#
# def __init__(self, state, max_joint_speed=None):
# """
# Initialize the joint speed state.
#
# Args:
# state: joint velocity state.
# max_joint_speed:
# """
# super(JointSpeedCost, self).__init__()
# self.dq = state
# self.dq_max = max_joint_speed
# if max_joint_speed is None:
# self.dq_max = state.max
#
# def _compute(self):
# """Compute and return the cost value."""
# return - np.sum(np.maximum(self.dq_max - np.abs(self.dq.data[0]), 0)**2)
pyrobolearn/rewards/link_cost.py
+195
View File
@@ -0,0 +1,195 @@
#!/usr/bin/env python
"""Define the costs used on link states / actions.
"""
from abc import ABCMeta
import numpy as np
import pyrobolearn as prl
from pyrobolearn.rewards.cost import Cost
from pyrobolearn.utils.transformation import quaternion_distance
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class LinkCost(Cost):
r"""(Abstract) Link Cost."""
__metaclass__ = ABCMeta
def __init__(self, update_state):
"""
Initialize the abstract link cost.
Args:
update_state (bool): if True it will update the given states before computing the cost.
"""
super(LinkCost, self).__init__()
self.update_state = update_state
class DistanceCost(LinkCost):
"""Distance Cost.
It penalizes the distance between 2 objects. One of the 2 objects must be movable in order for this
cost to change.
Mathematically, the cost is given by:
.. math:: c(p_1, p_2) = - d(p_1, p_2) = - || p_2 - p_1 ||^2
where :math:`p_1` represents the position of the specified link attached to the first body with respect to a
frame, and :math:`p_2` represents the position of the specified link attached on the second body with respect to
that same frame. The distance function used is the Euclidean distance (=L2 norm).
"""
def __init__(self, body1, body2, link_id1=-1, link_id2=-1, offset=None, update_state=False):
r"""
Initialize the distance cost.
Args:
body1 (BasePositionState, PositionState, LinkWorldPositionState, Body, Robot): first position state. If
Body, it will wrap it with a `PositionState`. If Robot, it will wrap it with a `PositionState` or
`LinkPositionState` depending on the value of :attr:`link_id1`.
body2 (BasePositionState, PositionState, LinkWorldPositionState, Body, Robot): second position state. If
Body, it will wrap it with a `PositionState`. If Robot, it will wrap it with a `PositionState` or
`LinkPositionState` depending on the value of :attr:`link_id1`.
link_id1 (int): link id associated with the first body that we are interested in. This is only used if
the given :attr:`body1` is not a state.
link_id2 (int): link id associated with the second body that we are interested in. This is only used if
the given :attr:`body2` is not a state.
offset (None, np.array[3]): 3d offset between body1 and body2.
update_state (bool): if True it will update the given states before computing the cost.
"""
super(DistanceCost, self).__init__(update_state=update_state)
# check body type function
def check_body_type(body, id_, link_id):
update_state = False
if isinstance(body, prl.robots.Body):
body = prl.states.PositionState(body)
update_state = True
elif isinstance(body, prl.robots.Robot):
if link_id == -1:
body = prl.states.PositionState(body)
else:
body = prl.states.LinkWorldPositionState(body, link_ids=link_id)
update_state = True
elif not isinstance(body, (prl.states.BasePositionState, prl.states.PositionState,
prl.states.LinkWorldPositionState)):
raise TypeError("Expecting the given 'body"+str(id_)+"' to be an instance of `Body`, `Robot`, "
"`BasePositionState`, `PositionState` or `LinkWorldPositionState`, instead got: "
"{}".format(type(body), id_))
return body, update_state
self.p1, self.update_state1 = check_body_type(body1, id_=1, link_id=link_id1)
self.p2, self.update_state2 = check_body_type(body2, id_=2, link_id=link_id2)
def _compute(self):
"""Compute and return the cost value."""
if self.update_state1:
self.p1()
if self.update_state2:
self.p2()
return - np.sum((self.p1.data[0] - self.p2.data[0])**2)
# alias
PositionCost = DistanceCost
class OrientationCost(LinkCost):
r"""Orientation Cost
The orientation cost (which uses the distance between two quaternions :math:`q_1` and :math:`q_2`) is given by:
.. math::
c(q_1, q_2) = \left\{ \begin{array}{ll}
2\pi, & q1 * \bar{q}_2 = -1 + [0,0,0]^\top \\
2 || \log(q_1 * \bar{q}_2) ||, & \text{otherwise}
\end{array} \right.
where :math:`*` is the quaternion product, :math:`\bar{q}` is the conjugate of the quaternion,
:math:`-1 + [0,0,0]^\top` is the only singularity on :math:`\mathbb{S}^3`, and
:math:`\log: \mathbb{S}^3 \rightarrow \mathbb{R}^3` is the logarithm map.
"""
def __init__(self, state, target_state, update_state=False):
"""
Initialize the orientation cost.
Args:
state (OrientationState, BaseOrientationState, LinkWorldOrientationState, LinkOrientationState, Body,
Robot): the orientation state.
target_state (np.array[4], OrientationState, BaseOrientationState, LinkWorldOrientationState,
LinkOrientationState, Body, Robot, None): target orientation state. Note that if a np.array is given,
it will wrap it with the `FixedState`. If None, it will be initialize to the unit quaternion.
update_state (bool): if True it will update the given states before computing the cost.
"""
super(OrientationCost, self).__init__(update_state=update_state)
# TODO
self.q1 = state
self.q2 = target_state
def _compute(self):
"""Compute and return the cost value."""
return - quaternion_distance(self.q1.data[0], self.q2.data[0])
class LinearVelocityCost(LinkCost):
r"""Linear velocity cost
The linear velocity cost is expressed as:
.. math:: c(v_1, v_2) = || v_2 - v_1 ||^2
"""
def __init__(self, state, target_state, update_state=False):
"""
Initialize the linear velocity cost.
Args:
state (LinearVelocityState): the linear velocity state.
target_state (LinearVelocityState, np.array[3], None): target linear velocity state. Note that if a
np.array is given, it will wrap it with the `FixedState`. If None, it will be initialize to zeros.
update_state (bool): if True it will update the given states before computing the cost.
"""
raise NotImplementedError
def _compute(self):
"""Compute and return the cost value."""
pass
class AngularVelocityCost(LinkCost):
r"""Angular velocity cost
The angular velocity cost is expressed as:
.. math:: c(\omega_1, \omega_2) = || \omega_2 - \omega_1 ||^2
"""
def __init__(self, state, target_state, update_state=False):
"""
Initialize the angular velocity cost.
Args:
state (LinearVelocityState): the angular velocity state.
target_state (LinearVelocityState, np.array[3], None): target angular velocity state. Note that if a
np.array is given, it will wrap it with the `FixedState`. If None, it will be initialize to zeros.
update_state (bool): if True it will update the given states before computing the cost.
"""
raise NotImplementedError
def _compute(self):
"""Compute and return the cost value."""
pass
pyrobolearn/rewards/terminal_rewards.py
+101
View File
@@ -0,0 +1,101 @@
#!/usr/bin/env python
"""Define terminal rewards used in RL.
"""
import numpy as np
from pyrobolearn.rewards.reward import Reward
from pyrobolearn.rewards.basic_rewards import FixedReward
from pyrobolearn.terminal_conditions.terminal_condition import TerminalCondition
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2019, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class TerminalReward(Reward):
r"""Terminal reward.
This computes the provided subreward until the terminal condition is not fulfilled. Once it has achieved it, it
computes the final given reward function. This reward is useful specially for sparse rewards that only returns a
value once the goal has been achieved (e.g. games).
"""
def __init__(self, terminal_condition, subreward, final_reward):
r"""
Terminal reward.
Args:
terminal_condition (TerminalCondition): terminal condition.
subreward (Reward, float, int): sub reward that is called until the terminal condition is not fulfilled.
final_reward (Reward, float, int): final reward that is called when the terminal condition has been reached.
"""
super(TerminalReward, self).__init__()
# set the attributes
self.terminal_condition = terminal_condition
self.subreward = subreward
self.final_reward = final_reward
##############
# Properties #
##############
@property
def terminal_condition(self):
"""Return the terminal condition instance."""
return self._terminal_condition
@terminal_condition.setter
def terminal_condition(self, condition):
"""Set the terminal condition instance."""
if not isinstance(condition, TerminalCondition):
raise TypeError("Expecting the given 'terminal_condition' to be an instance of `TerminalCondition`, "
"instead got: {}".format(type(condition)))
self._terminal_condition = condition
@property
def subreward(self):
"""Return the sub-reward instance."""
return self._reward
@subreward.setter
def subreward(self, reward):
"""Set the sub-reward instance."""
if isinstance(reward, (int, float)):
reward = FixedReward(value=reward)
elif not isinstance(reward, Reward):
raise TypeError("Expecting the given 'subreward' to be an instance of `Reward`, instead got: "
"{}".format(type(reward)))
self._reward = reward
@property
def final_reward(self):
"""Return the final reward instance."""
return self._final_reward
@final_reward.setter
def final_reward(self, reward):
"""Set the final reward instance."""
if isinstance(reward, (int, float)):
reward = FixedReward(value=reward)
elif not isinstance(reward, Reward):
raise TypeError("Expecting the given 'final_reward' to be an instance of `Reward`, instead got: "
"{}".format(type(reward)))
self._final_reward = reward
###########
# Methods #
###########
def _compute(self):
"""Compute the terminal reward."""
done = self.terminal_condition()
if done:
return self.final_reward()
return self.subreward()
pyrobolearn/robots/robot.py
+20 -18
View File
@@ -99,7 +99,7 @@ class Robot(ControllableBody):
# we rescale manually the mass and inertia matrices of each link
for link in range(self.num_links):
info = self.sim.get_dynamics_info(self.id, link)
mass, local_inertia_diagonal = info[0], np.array(info[2])
mass, local_inertia_diagonal = info[0], np.asarray(info[2])
mass *= scale ** 3 # because the density is unchanged when scaling
local_inertia_diagonal *= scale ** 5 # 5 = 3+2; 3 is for the mass, and 2 is for the distance: I~mr^2
self.sim.change_dynamics(self.id, link, mass=mass, local_inertia_diagonal=local_inertia_diagonal)
@@ -1423,7 +1423,7 @@ class Robot(ControllableBody):
return self.sim.get_link_states(self.id, link_ids, compute_velocity=compute_link_velocity,
compute_forward_kinematics=compute_forward_kinematics)
def get_link_local_position(self, link_ids=None):
def get_link_local_positions(self, link_ids=None):
"""
Get the local position offset of the inertial frame (CoM) of the specified links expressed in the URDF link
frame.
@@ -1496,7 +1496,7 @@ class Robot(ControllableBody):
return self.sim.get_dynamics_info(self.id, link_ids)[0]
if link_ids is None:
link_ids = list(range(self.num_links))
return np.array([self.sim.get_dynamics_info(self.id, link)[0] for link in link_ids])
return np.asarray([self.sim.get_dynamics_info(self.id, link)[0] for link in link_ids])
def get_link_frames(self, link_ids=None, flatten=False):
r"""
@@ -1535,10 +1535,10 @@ class Robot(ControllableBody):
np.array[N*3], np.array[N,3]: link frame position of each link in world space
"""
if isinstance(link_ids, int):
return np.array(self.sim.get_link_state(self.id, link_ids)[4])
return np.asarray(self.sim.get_link_state(self.id, link_ids)[4])
if link_ids is None:
link_ids = self.joints
pos = np.array([self.sim.get_link_state(self.id, link)[4] for link in link_ids])
pos = np.asarray([self.sim.get_link_state(self.id, link)[4] for link in link_ids])
if flatten:
return pos.reshape(-1) # 1D array
return pos # 2D array
@@ -1562,7 +1562,7 @@ class Robot(ControllableBody):
return self.sim.get_link_state(self.id, link_ids)[5]
if link_ids is None:
link_ids = self.joints
orientation = np.array([self.sim.get_link_state(self.id, link)[5] for link in link_ids])
orientation = np.asarray([self.sim.get_link_state(self.id, link)[5] for link in link_ids])
if flatten:
return orientation.reshape(-1) # 1D array
return orientation # 2D array
@@ -1649,7 +1649,7 @@ class Robot(ControllableBody):
return self.sim.get_link_state(self.id, link_ids)[1]
if link_ids is None:
link_ids = self.joints
orientation = np.array([self.sim.get_link_state(self.id, link)[1] for link in link_ids])
orientation = np.asarray([self.sim.get_link_state(self.id, link)[1] for link in link_ids])
if flatten:
return orientation.reshape(-1)
return orientation # 2D array
@@ -1677,7 +1677,8 @@ class Robot(ControllableBody):
if isinstance(wrt_link_id, int):
q0 = get_quaternion_inverse(self.get_link_world_orientations(wrt_link_id))
else:
q0 = np.array([get_quaternion_inverse(self.get_link_world_orientations(link)) for link in wrt_link_id])
q0 = np.asarray([get_quaternion_inverse(self.get_link_world_orientations(link))
for link in wrt_link_id])
q = get_quaternion_product(q0, q1)
if flatten:
@@ -1728,10 +1729,10 @@ class Robot(ControllableBody):
np.array[N*3], np.array[N,3]: linear velocity of each link
"""
if isinstance(link_ids, int):
return np.array(self.sim.get_link_state(self.id, link_ids, compute_velocity=True)[6])
return np.asarray(self.sim.get_link_state(self.id, link_ids, compute_velocity=True)[6])
if link_ids is None:
link_ids = self.joints
vel = np.array([self.sim.get_link_state(self.id, link, compute_velocity=True)[6] for link in link_ids])
vel = np.asarray([self.sim.get_link_state(self.id, link, compute_velocity=True)[6] for link in link_ids])
if flatten:
return vel.reshape(-1) # 1D array
return vel # 2D array
@@ -1752,10 +1753,10 @@ class Robot(ControllableBody):
np.array[N*3], np.array[N,3]: angular velocity of each link
"""
if isinstance(link_ids, int):
return np.array(self.sim.get_link_state(self.id, link_ids, compute_velocity=True)[7])
return np.asarray(self.sim.get_link_state(self.id, link_ids, compute_velocity=True)[7])
if link_ids is None:
link_ids = self.joints
vel = np.array([self.sim.get_link_state(self.id, link, compute_velocity=True)[7] for link in link_ids])
vel = np.asarray([self.sim.get_link_state(self.id, link, compute_velocity=True)[7] for link in link_ids])
if flatten:
return vel.reshape(-1) # 1d array
return vel # 2D array
@@ -2152,7 +2153,7 @@ class Robot(ControllableBody):
return self.sim.get_dynamics_info(body_id=self.id, link_id=link_ids)[2]
if link_ids is None:
link_ids = list(range(self.num_links))
return np.array([self.sim.get_dynamics_info(self.id, link)[2] for link in link_ids])
return np.asarray([self.sim.get_dynamics_info(self.id, link)[2] for link in link_ids])
def set_link_positions(self, link_ids, positions, orientations=None):
"""
@@ -3135,7 +3136,7 @@ class Robot(ControllableBody):
q = self.get_joint_positions()
# compute and return joint accelerations
torques = np.array(torques)
torques = np.asarray(torques)
if not self.fixed_base: # if floating base
torques = np.concatenate((np.zeros(6), torques))
Hinv = np.linalg.inv(self.get_mass_matrix(q))
@@ -3175,8 +3176,8 @@ class Robot(ControllableBody):
q_aug[self.joints] = q
if q_idx is None:
return np.array(self.sim.calculate_mass_matrix(self.id, q_aug))
return np.array(self.sim.calculate_mass_matrix(self.id, q_aug))[q_idx, q_idx]
return np.asarray(self.sim.calculate_mass_matrix(self.id, q_aug))
return np.asarray(self.sim.calculate_mass_matrix(self.id, q_aug))[q_idx, q_idx]
# alias
get_inertia_matrix = get_mass_matrix
@@ -3267,7 +3268,7 @@ class Robot(ControllableBody):
H = self.get_mass_matrix(q, q_idx)
return 1./2 * dq.dot(H.dot(dq))
def get_gravity_potential_energy(self, q=None, q_idx=None, g=np.array((0., 0., -9.81))):
def get_gravity_potential_energy(self, q=None, q_idx=None, g=(0., 0., -9.81)):
r"""
Return the potential energy due to gravity.
@@ -3281,11 +3282,12 @@ class Robot(ControllableBody):
NOT USED, as we can get the link positions from the simulator (instead of using forward kinematics).
q_idx (int[M], None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N),
and the joint velocities vector.
g (np.array[3]): gravity vector.
g (np.array[3], tuple/list of 3 float): gravity vector.
Returns:
float: potential energy due to gravity
"""
g = np.asarray(g)
link_ids = list(range(self.num_links))
p = self.get_link_world_positions(link_ids=link_ids, flatten=False)
m = self.get_link_masses(link_ids=link_ids)
pyrobolearn/simulators/dart.py
+4
View File
@@ -26,12 +26,15 @@ References:
- paper: http://joss.theoj.org/papers/10.21105/joss.00500
- webpage: https://dartsim.github.io/
- github: https://github.com/dartsim/dart/
- dartpy: http://dartsim.github.io/install_dartpy_on_ubuntu.html
[2] PyDART
- source code: https://pydart2.readthedocs.io/en/latest/
- documentation: https://pydart2.readthedocs.io/en/latest/
[3] PEP8: https://www.python.org/dev/peps/pep-0008/
"""
# TODO: finish to implement this interface and use dartpy instead of pydart2
# import pydart2
try:
import pydart2 as pydart
@@ -68,6 +71,7 @@ class Dart(Simulator):
[1] Dart:
- webpage: https://dartsim.github.io/
- github: https://github.com/dartsim/dart/
- dartpy: http://dartsim.github.io/install_dartpy_on_ubuntu.html
[2] PyDART:
- source code: https://pydart2.readthedocs.io/en/latest/
- documentation: https://pydart2.readthedocs.io/en/latest/
pyrobolearn/states/__init__.py
+3 -3
View File
@@ -19,10 +19,10 @@ from .gym_states import *
# # import state generators
# from .generators import *
#
from . import generators
# # import state processors
# from .processors import *
# from . import processors
#
# # import interface states
# from .interfaces import *
pyrobolearn/states/generators/state_generator.py
+126 -48
View File
@@ -13,6 +13,7 @@ See Also:
import queue
import numpy as np
from abc import ABCMeta
from pyrobolearn.states import State
@@ -42,13 +43,15 @@ class StateGenerator(object):
The mapping function has to return a `state` object.
"""
def __init__(self, state):
def __init__(self, state, fct=None):
"""Initialize the state generator.
Args:
state (State): state instance.
fct (callable, None): callback function to be called after generating the data.
"""
self.state = state
self.fct = fct if callable(fct) else None
@property
def state(self):
@@ -63,24 +66,48 @@ class StateGenerator(object):
"{}".format(type(state)))
self._state = state
def generate(self, set_data=True):
def generate(self, set_data=True, reset_state=True):
"""Generate the state.
Args:
set_data (bool): If True, it will set the generated data to the state.
reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
to True).
Returns:
(list of) np.array: state data
"""
# generate the data
data = self._generate(set_data=set_data, reset_state=reset_state)
# call the user function
if self.fct is not None:
self.fct()
return data
def _generate(self, set_data=True, reset_state=True):
"""Generate the state.
Args:
set_data (bool): If True, it will set the generated data to the state.
reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
to True).
Returns:
(list of) np.array: state data
"""
raise NotImplementedError
def __repr__(self):
return self.__class__.__name__
# def __repr__(self):
# return self.__class__.__name__
def __str__(self):
"""Return a string describing the class."""
return self.__class__.__name__
def __call__(self, set_data=True):
"""Call the generator."""
return self.generate(set_data=set_data)
@@ -90,26 +117,31 @@ class FixedStateGenerator(StateGenerator):
This generator returns the same initial state each time it is called.
"""
def __init__(self, state):
def __init__(self, state, fct=None):
"""Initialize the fixed state generator.
Args:
state (State): state instance.
fct (callable, None): callback function to be called after generating the data.
"""
super(FixedStateGenerator, self).__init__(state)
super(FixedStateGenerator, self).__init__(state, fct=fct)
self.initial_data = self.state.data
def generate(self, set_data=True):
def _generate(self, set_data=True, reset_state=True):
"""Generate the state.
Args:
set_data (bool): If True, it will set the generated data to the state.
reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
to True).
Returns:
(list of) np.array: state data
"""
if set_data:
self.state.data = self.initial_data
if reset_state:
self.state.reset()
return self.initial_data
@@ -117,8 +149,16 @@ class QueueStateGenerator(StateGenerator):
r"""Abstract Queue state generator
"""
def __init__(self, state, queue):
super(QueueStateGenerator, self).__init__(state)
def __init__(self, state, queue, fct=None):
"""
Initialize the queue state generator.
Args:
state (State): initial state.
queue (queue.Queue): queue instance which contains the data.
fct (callable, None): callback function to be called after generating the data.
"""
super(QueueStateGenerator, self).__init__(state, fct=fct)
self.queue = queue
self.initial_data = self.state.data
@@ -129,10 +169,11 @@ class QueueStateGenerator(StateGenerator):
@queue.setter
def queue(self, q):
"""Set the queue instance."""
if not isinstance(q, queue.Queue):
raise TypeError("Expecting the given queue to be an instance of `queue.Queue`, instead got: "
"{}".format(type(q)))
self._queue = queue
self._queue = q
def put(self, item, block=False, timeout=None):
"""Put an item into the queue.
@@ -159,7 +200,7 @@ class QueueStateGenerator(StateGenerator):
('timeout' is ignored in that case).
"""
if self.queue.empty():
return self.initial_data
return self.initial_data
return self.queue.get(block=block, timeout=timeout)
# alias
@@ -177,11 +218,13 @@ class QueueStateGenerator(StateGenerator):
"""Return the approximate size of the queue (not reliable!)."""
return self.queue.qsize()
def generate(self, set_data=True):
def _generate(self, set_data=True, reset_state=True):
"""Generate the state.
Args:
set_data (bool): If True, it will set the generated data to the state.
reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
to True).
Returns:
(list of) np.array: state data
@@ -191,6 +234,8 @@ class QueueStateGenerator(StateGenerator):
data = data.data
if set_data:
self.state.data = data
if reset_state:
self.state.reset()
return data
def __len__(self):
@@ -222,15 +267,16 @@ class FIFOQueueStateGenerator(QueueStateGenerator):
The queue is filled by the user during training.
"""
def __init__(self, state, maxsize=0):
def __init__(self, state, maxsize=0, fct=None):
"""Initialize the FIFO queue state generator.
Args:
state (State): state instance.
maxsize (int): maximum size of the queue. If :attr:`maxsize` is <= 0, the queue size is infinite.
fct (callable, None): callback function to be called after generating the data.
"""
q = queue.Queue(maxsize)
super(FIFOQueueStateGenerator, self).__init__(state, queue=q)
super(FIFOQueueStateGenerator, self).__init__(state, queue=q, fct=fct)
class LIFOQueueStateGenerator(QueueStateGenerator):
@@ -240,15 +286,16 @@ class LIFOQueueStateGenerator(QueueStateGenerator):
The queue is filled by the user during training.
"""
def __init__(self, state, maxsize=0):
def __init__(self, state, maxsize=0, fct=None):
"""Initialize the LIFO queue state generator.
Args:
state (State): state instance.
maxsize (int): maximum size of the queue. If :attr:`maxsize` is <= 0, the queue size is infinite.
fct (callable, None): callback function to be called after generating the data.
"""
q = queue.LifoQueue(maxsize)
super(LIFOQueueStateGenerator, self).__init__(state, queue=q)
super(LIFOQueueStateGenerator, self).__init__(state, queue=q, fct=fct)
class PriorityQueueStateGenerator(QueueStateGenerator):
@@ -261,16 +308,17 @@ class PriorityQueueStateGenerator(QueueStateGenerator):
poorly during the training.
"""
def __init__(self, state, maxsize=0, ascending=True):
def __init__(self, state, maxsize=0, ascending=True, fct=None):
"""Initialize the priority queue state generator.
Args:
state (State): state instance.
maxsize (int): maximum size of the queue. If :attr:`maxsize` is <= 0, the queue size is infinite.
ascending (bool): if True, the item with the lowest priority will be the first one to be retrieved.
fct (callable, None): callback function to be called after generating the data.
"""
q = queue.PriorityQueue(maxsize)
super(PriorityQueueStateGenerator, self).__init__(state, queue=q)
super(PriorityQueueStateGenerator, self).__init__(state, queue=q, fct=fct)
self.ascending = ascending
def get(self, block=False, timeout=None):
@@ -315,15 +363,17 @@ class StateDistributionGenerator(StateGenerator):
The initial states :math:`s` are generated by a probability distribution :math:`p(s)`, that is :math:`s \sim p(s)`.
The probability distribution can be learned using generative models.
"""
__metaclass__ = ABCMeta
def __init__(self, state, seed=None):
def __init__(self, state, seed=None, fct=None):
"""Initialize the state distribution generator.
Args:
state (State): state instance.
seed (None, int): random seed.
fct (callable, None): callback function to be called after generating the data.
"""
super(StateDistributionGenerator, self).__init__(state)
super(StateDistributionGenerator, self).__init__(state, fct=fct)
self.seed = seed
@property
@@ -338,8 +388,11 @@ class StateDistributionGenerator(StateGenerator):
Args:
seed (int): random seed
"""
if seed is not None and not isinstance(seed, int):
raise TypeError("Expecting the given 'seed' to be an integer, instead got: {}".format(type(seed)))
if seed is not None:
np.random.seed(seed)
self._seed = seed
class UniformStateGenerator(StateDistributionGenerator):
@@ -349,15 +402,17 @@ class UniformStateGenerator(StateDistributionGenerator):
will be set to be the range of the states.
"""
def __init__(self, state, low=None, high=None):
def __init__(self, state, low=None, high=None, seed=None, fct=None):
"""Initialize the state distribution generator.
Args:
state (State): state instance.
low (None, float, np.array, list of np.array): lower bound
high (None, float, np.array, list of np.array): upper bound
seed (None, int): random seed.
fct (callable, None): callback function to be called after generating the data.
"""
super(UniformStateGenerator, self).__init__(state)
super(UniformStateGenerator, self).__init__(state, seed=seed, fct=fct)
self.low = low
self.high = high
@@ -373,12 +428,13 @@ class UniformStateGenerator(StateDistributionGenerator):
low = [-np.infty] * len(self.state)
elif isinstance(low, (int, float)):
low = [low] * len(self.state)
elif isinstance(low, (list, tuple)):
elif isinstance(low, (list, tuple, np.ndarray)):
if len(low) != len(self.state):
raise ValueError("The lower bound doesn't have the same size as the number of states; len(low) = {} "
"and len(state) = {}".format(len(low), len(self.state)))
else:
raise TypeError
raise TypeError("Expecting the 'low' bound to be an int, float, or list/tuple/np.array of int/float, "
"instead got: {}".format(type(low)))
self._low = low
@property
@@ -393,29 +449,37 @@ class UniformStateGenerator(StateDistributionGenerator):
high = [-np.infty] * len(self.state)
elif isinstance(high, (int, float)):
high = [high] * len(self.state)
elif isinstance(high, (list, tuple)):
elif isinstance(high, (list, tuple, np.ndarray)):
if len(high) != len(self.state):
raise ValueError("The higher bound doesn't have the same size as the number of states; len(high) = {} "
"and len(state) = {}".format(len(high), len(self.state)))
else:
raise TypeError
raise TypeError("Expecting the 'high' bound to be an int, float, or list/tuple/np.array of int/float, "
"instead got: {}".format(type(high)))
self._high = high
def generate(self, set_data=True):
def _generate(self, set_data=True, reset_state=True):
"""Generate the state.
Args:
set_data (bool): If True, it will set the generated data to the state.
reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
to True).
Returns:
(list of) np.array: state data
"""
spaces = self.state.space
data = [space.sample() for space in spaces]
for idx, datum, low, high in np.clip(zip(data, self.low, self.high)):
data[idx] = np.clip(datum, low, high)
# spaces = self.state.space
# data = [space.sample() for space in spaces]
# for idx, datum, low, high in enumerate(zip(data, self.low, self.high)):
# data[idx] = np.clip(datum, low, high)
data = [np.random.uniform(low=low, high=high, size=len(state))
for state, low, high in zip(self.state, self.low, self.high)]
if set_data:
self.state.data = data
if reset_state:
self.state.reset()
return data
@@ -426,7 +490,7 @@ class NormalStateGenerator(StateDistributionGenerator):
The states are then truncated / clipped to be inside their corresponding range.
"""
def __init__(self, state, means=0, scales=1.):
def __init__(self, state, means=0, scales=1., seed=None, fct=None):
"""
Initialize the Normal state generator.
@@ -434,10 +498,12 @@ class NormalStateGenerator(StateDistributionGenerator):
state (State): state instance.
means:
scales:
seed (None, int): random seed.
fct (callable, None): callback function to be called after generating the data.
"""
super(NormalStateGenerator, self).__init__(state)
super(NormalStateGenerator, self).__init__(state, seed=seed, fct=fct)
def generate(self, set_data=True):
def _generate(self, set_data=True, reset_state=True):
pass
@@ -446,16 +512,18 @@ class GenerativeStateGenerator(StateGenerator):
This uses a generative model that has been trained to learn a distribution to generate the initial states.
"""
__metaclass__ = ABCMeta
def __init__(self, state, model):
def __init__(self, state, model, fct=None):
"""
Initialize the Generative initial state generator.
Args:
state (State): state instance.
model (Model): generative model instance.
fct (callable, None): callback function to be called after generating the data.
"""
super(GenerativeStateGenerator, self).__init__(state)
super(GenerativeStateGenerator, self).__init__(state, fct=fct)
self.model = model
@@ -465,14 +533,16 @@ class VAEStateGenerator(GenerativeStateGenerator):
This uses the decoder a pretrained VAE to generate initial states.
"""
def __init__(self, state, model):
super(VAEStateGenerator, self).__init__(state, model)
def __init__(self, state, model, fct=None):
super(VAEStateGenerator, self).__init__(state, model, fct=fct)
def generate(self, set_data=True):
def _generate(self, set_data=True, reset_state=True):
"""Generate the state.
Args:
set_data (bool): If True, it will set the generated data to the state.
reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
to True).
Returns:
(list of) np.array: state data
@@ -486,12 +556,12 @@ class GANStateGenerator(GenerativeStateGenerator):
This uses the generator of a trained GAN model to generate similar states.
"""
def __init__(self, state, model, distribution=None, mapping=None):
def __init__(self, state, model, distribution=None, mapping=None, fct=None):
"""
Initialize the GAN initial state generator.
Args:
states: states that need to be generated
state: states that need to be generated
model: GAN or generator of GAN
distribution: distribution over the noise vector
mapping:
@@ -517,13 +587,15 @@ class GANStateGenerator(GenerativeStateGenerator):
# setting mapping
self.mapping = mapping
super(GANStateGenerator, self).__init__(state, model)
super(GANStateGenerator, self).__init__(state, model, fct=fct)
def generate(self, set_data=True):
def _generate(self, set_data=True, reset_state=True):
"""Generate the state.
Args:
set_data (bool): If True, it will set the generated data to the state.
reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
to True).
Returns:
(list of) np.array: state data
@@ -541,14 +613,16 @@ class GMMStateGenerator(GenerativeStateGenerator):
This uses a pretrained GMM to generate the states.
"""
def __init__(self, state, model):
super(GMMStateGenerator, self).__init__(state, model)
def __init__(self, state, model, fct=None):
super(GMMStateGenerator, self).__init__(state, model, fct=fct)
def generate(self, set_data=True):
def _generate(self, set_data=True, reset_state=True):
"""Generate the state.
Args:
set_data (bool): If True, it will set the generated data to the state.
reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
to True).
Returns:
(list of) np.array: state data
@@ -559,6 +633,7 @@ class GMMStateGenerator(GenerativeStateGenerator):
class UncertaintyStateGenerator(StateGenerator):
r"""State generator that exploits the uncertainty of initial states.
"""
__metaclass__ = ABCMeta
pass
@@ -567,6 +642,7 @@ class BOStateGenerator(UncertaintyStateGenerator):
We use Bayesian Optimization to generate the initial states.
"""
__metaclass__ = ABCMeta
pass
@@ -586,7 +662,7 @@ class AEBOStateGenerator(GenerativeStateGenerator):
The exploration will then be carried out in the hyperrectangle formed by these 2 reduced state vector limits.
"""
def __init__(self, state, model, kernel_capacity=100):
def __init__(self, state, model, kernel_capacity=100, fct=None):
"""
Initialize the autoencoder + bayesian optimization initial state generator.
@@ -597,11 +673,13 @@ class AEBOStateGenerator(GenerativeStateGenerator):
"""
super(AEBOStateGenerator, self).__init__(state, model)
def generate(self, set_data=True):
def _generate(self, set_data=True, reset_state=True):
"""Generate the state.
Args:
set_data (bool): If True, it will set the generated data to the state.
reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
to True).
Returns:
(list of) np.array: state data
pyrobolearn/states/robot_states/__init__.py
+3 -2
View File
@@ -8,8 +8,9 @@ from .joint_states import JointState, JointPositionState, JointTrigonometricPosi
JointForceTorqueState, JointAccelerationState
# import the link states
from .link_states import LinkState, LinkPositionState, LinkWorldPositionState, LinkOrientationState, \
LinkVelocityState, LinkLinearVelocityState, LinkAngularVelocityState
from .link_states import LinkState, LinkPositionState, LinkOrientationState, LinkVelocityState, \
LinkLinearVelocityState, LinkAngularVelocityState, LinkWorldPositionState, LinkWorldOrientationState, \
LinkWorldVelocityState, LinkWorldLinearVelocityState, LinkWorldAngularVelocityState
# import the sensor states
from .sensor_states import SensorState, CameraState, ContactState, FeetContactState
pyrobolearn/states/robot_states/joint_states.py
+14
View File
@@ -113,10 +113,24 @@ class JointPositionState(JointState):
"""
super(JointPositionState, self).__init__(robot, joint_ids, window_size=window_size, axis=axis, ticks=ticks)
# define the space based on the joint type # TODO
def _read(self):
"""Read the next joint position state."""
self.data = self.robot.get_joint_positions(self.joints)
def _reset(self):
"""Reset the state."""
# reset counter
self.cnt = 0.
# reset the robot joint position based on the data
if len(self.data) > 0:
self.robot.reset_joint_states(q=self.data[0], joint_ids=self.joints)
# read the next data
self._read()
class JointTrigonometricPositionState(JointState):
r"""Joint Trigonometric Position State
pyrobolearn/states/robot_states/link_states.py
+156 -13
View File
@@ -145,13 +145,48 @@ class LinkWorldPositionState(LinkState):
def _read(self):
"""Read the next link world position state."""
self.data = self.robot.get_link_positions(self.links)
self.data = self.robot.get_link_world_positions(self.links, flatten=True)
class LinkOrientationState(LinkState):
r"""Link Orientation state
"""
def __init__(self, robot, link_ids=None, wrt_link_id=None, window_size=1, axis=None, ticks=1):
"""
Initialize the link world orientation state.
Args:
robot (Robot): robot instance
link_ids (int, int[N]): link id or list of link ids
wrt_link_id (int, None): link with respect to which we compute the position of the other links. If None,
it will be the base.
window_size (int): window size of the state. This is the total number of states we should remember. That
is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`,
the window size is 2. By default, the :attr:`window_size` is one which means we only remember the
current state. The window size has to be bigger than 1. If it is below, it will be set automatically
to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states,
but is given some :attr:`data`.
axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with
shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting
state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack
the states in the specified axis. With the example, for axis=0, the resulting state has a shape of
(w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the
state is not a combination of states, but is given some :attr:`data`.
ticks (int): number of ticks to sleep before getting the next state data.
"""
self.wrt_link_id = wrt_link_id
super(LinkOrientationState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
def _read(self): # TODO: convert
"""Read the next link orientation state."""
self.data = self.robot.get_link_orientations(self.links, wrt_link_id=self.wrt_link_id, flatten=True)
class LinkWorldOrientationState(LinkState):
r"""Link World Orientation state
"""
def __init__(self, robot, link_ids=None, window_size=1, axis=None, ticks=1):
"""
Initialize the link world orientation state.
@@ -173,17 +208,53 @@ class LinkOrientationState(LinkState):
state is not a combination of states, but is given some :attr:`data`.
ticks (int): number of ticks to sleep before getting the next state data.
"""
super(LinkOrientationState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
super(LinkWorldOrientationState, self).__init__(robot, link_ids, window_size=window_size, axis=axis,
ticks=ticks)
def _read(self): # TODO: convert
"""Read the next link orientation state."""
self.data = self.robot.get_link_orientations(self.links)
"""Read the next link world orientation state."""
self.data = self.robot.get_link_world_orientations(self.links, flatten=True)
class LinkVelocityState(LinkState):
r"""Link velocity state
"""
def __init__(self, robot, link_ids=None, wrt_link_id=None, window_size=1, axis=None, ticks=1):
"""
Initialize the link velocity state.
Args:
robot (Robot): robot instance
link_ids (int, int[N]): link id or list of link ids
wrt_link_id (int, None): link with respect to which we compute the position of the other links. If None,
it will be the base.
window_size (int): window size of the state. This is the total number of states we should remember. That
is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`,
the window size is 2. By default, the :attr:`window_size` is one which means we only remember the
current state. The window size has to be bigger than 1. If it is below, it will be set automatically
to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states,
but is given some :attr:`data`.
axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with
shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting
state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack
the states in the specified axis. With the example, for axis=0, the resulting state has a shape of
(w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the
state is not a combination of states, but is given some :attr:`data`.
ticks (int): number of ticks to sleep before getting the next state data.
"""
self.wrt_link_id = wrt_link_id
super(LinkVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
def _read(self):
"""Read the next link velocity state."""
self.data = self.robot.get_link_velocities(self.links, wrt_link_id=self.wrt_link_id, flatten=True)
class LinkWorldVelocityState(LinkState):
r"""Link world velocity state
"""
def __init__(self, robot, link_ids=None, window_size=1, axis=None, ticks=1):
"""
Initialize the link velocity state.
@@ -205,17 +276,52 @@ class LinkVelocityState(LinkState):
state is not a combination of states, but is given some :attr:`data`.
ticks (int): number of ticks to sleep before getting the next state data.
"""
super(LinkVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
super(LinkWorldVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
def _read(self):
"""Read the next link velocity state."""
self.data = self.robot.get_link_velocities(self.links)
"""Read the next link world velocity state."""
self.data = self.robot.get_link_world_velocities(self.links, flatten=True)
class LinkLinearVelocityState(LinkState):
r"""Link linear velocity state
"""
def __init__(self, robot, link_ids=None, wrt_link_id=None, window_size=1, axis=None, ticks=1):
"""
Initialize the link linear velocity state.
Args:
robot (Robot): robot instance
link_ids (int, int[N]): link id or list of link ids
wrt_link_id (int, None): link with respect to which we compute the position of the other links. If None,
it will be the base.
window_size (int): window size of the state. This is the total number of states we should remember. That
is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`,
the window size is 2. By default, the :attr:`window_size` is one which means we only remember the
current state. The window size has to be bigger than 1. If it is below, it will be set automatically
to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states,
but is given some :attr:`data`.
axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with
shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting
state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack
the states in the specified axis. With the example, for axis=0, the resulting state has a shape of
(w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the
state is not a combination of states, but is given some :attr:`data`.
ticks (int): number of ticks to sleep before getting the next state data.
"""
self.wrt_link_id = wrt_link_id
super(LinkLinearVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
def _read(self):
"""Read the next link linear velocity state."""
self.data = self.robot.get_link_linear_velocities(self.links, wrt_link_id=self.wrt_link_id, flatten=True)
class LinkWorldLinearVelocityState(LinkState):
r"""Link world linear velocity state
"""
def __init__(self, robot, link_ids=None, window_size=1, axis=None, ticks=1):
"""
Initialize the link linear velocity state.
@@ -237,17 +343,53 @@ class LinkLinearVelocityState(LinkState):
state is not a combination of states, but is given some :attr:`data`.
ticks (int): number of ticks to sleep before getting the next state data.
"""
super(LinkLinearVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
super(LinkWorldLinearVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis,
ticks=ticks)
def _read(self):
"""Read the next link linear velocity state."""
self.data = self.robot.get_link_linear_velocities(self.links)
"""Read the next link world linear velocity state."""
self.data = self.robot.get_link_world_linear_velocities(self.links, flatten=True)
class LinkAngularVelocityState(LinkState):
r"""Link angular velocity state
"""
def __init__(self, robot, link_ids=None, wrt_link_id=None, window_size=1, axis=None, ticks=1):
"""
Initialize the link angular velocity state.
Args:
robot (Robot): robot instance
link_ids (int, int[N]): link id or list of link ids
wrt_link_id (int, None): link with respect to which we compute the position of the other links. If None,
it will be the base.
window_size (int): window size of the state. This is the total number of states we should remember. That
is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`,
the window size is 2. By default, the :attr:`window_size` is one which means we only remember the
current state. The window size has to be bigger than 1. If it is below, it will be set automatically
to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states,
but is given some :attr:`data`.
axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with
shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting
state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack
the states in the specified axis. With the example, for axis=0, the resulting state has a shape of
(w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the
state is not a combination of states, but is given some :attr:`data`.
ticks (int): number of ticks to sleep before getting the next state data.
"""
self.wrt_link_id = wrt_link_id
super(LinkAngularVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
def _read(self):
"""Read the next link angular velocity state."""
self.data = self.robot.get_link_angular_velocities(self.links, wrt_link_id=self.wrt_link_id, flatten=True)
class LinkWorldAngularVelocityState(LinkState):
r"""Link world angular velocity state
"""
def __init__(self, robot, link_ids=None, window_size=1, axis=None, ticks=1):
"""
Initialize the link angular velocity state.
@@ -269,8 +411,9 @@ class LinkAngularVelocityState(LinkState):
state is not a combination of states, but is given some :attr:`data`.
ticks (int): number of ticks to sleep before getting the next state data.
"""
super(LinkAngularVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
super(LinkWorldAngularVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis,
ticks=ticks)
def _read(self):
"""Read the next link angular velocity state."""
self.data = self.robot.get_link_angular_velocities(self.links)
"""Read the next link world angular velocity state."""
self.data = self.robot.get_link_world_angular_velocities(self.links, flatten=True)
pyrobolearn/utils/transformation.py
+62 -21
View File
@@ -27,6 +27,24 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
def min_angle_difference(q1, q2):
r"""
Return the minimum angle difference between two (set of) angles (expressed in radians). The differences are
always between [-pi, pi].
Args:
q1 (float, np.array[N]): first angle(s)
q2 (float, np.array[N]): second angle(s)
Returns:
float, np.array[N]: minimum angle difference(s)
"""
diff = np.maximum(q1, q2) - np.minimum(q1, q2)
if diff > np.pi:
diff = 2 * np.pi - diff
return diff
def get_homogeneous_transform(position, orientation):
r"""
Return the Homogeneous transform matrix given the position vector and the orientation.
@@ -457,31 +475,42 @@ def get_symbolic_matrix_from_quaternion(q, convention='xyzw'):
def get_rpy_from_quaternion(q, convention='xyzw'):
"""
Get the Roll-Pitch-Yaw angle from the given quaternion.
Get the Roll-Pitch-Yaw angle(s) from the given quaternion(s).
Args:
q (np.array[4], quaternion.quaternion): quaternion
q (np.array[4], np.array[N,4], (list of) quaternion.quaternion): quaternion(s)
convention: convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
'wxyz'.
Returns:
np.float[3]: roll-pitch-yaw angles.
np.float[3], np.float[N,3]: roll-pitch-yaw angles.
"""
if isinstance(q, quaternion.quaternion):
x, y, z, w = q.x, q.y, q.z, q.w
multiple_quaternions = True
if isinstance(q, quaternion.quaternion) or (isinstance(q, np.ndarray) and len(q.shape) == 1):
multiple_quaternions = False
q = np.array([q]) # (1,4)
if isinstance(q[0], quaternion.quaternion):
q = quaternion.as_float_array(q)
x, y, z, w = q[:, 1], q[:, 2], q[:, 3], q[:, 0] # (N,)
elif isinstance(q, Iterable):
if convention == 'xyzw':
x, y, z, w = q
x, y, z, w = q[:, 0], q[:, 1], q[:, 2], q[:, 3] # (N,)
elif convention == 'wxyz':
w, x, y, z = q
w, x, y, z = q[:, 0], q[:, 1], q[:, 2], q[:, 3] # (N,)
else:
raise NotImplementedError("Asking for a convention that has not been implemented")
else:
raise TypeError
roll = np.arctan2(2*(w*x + y*z), 1 - 2 * (x**2 + y**2))
pitch = np.arcsin(2 * (w*y - z*x))
yaw = np.arctan2(2 * (w*z + x*y), 1 - 2 * (y**2 + z**2))
return np.array([roll, pitch, yaw])
roll = np.arctan2(2 * (w*x + y*z), 1 - 2 * (x**2 + y**2)) # (N,)
pitch = np.arcsin(2 * (w*y - z*x)) # (N,)
yaw = np.arctan2(2 * (w*z + x*y), 1 - 2 * (y**2 + z**2)) # (N,)
rpy = np.vstack((roll, pitch, yaw)).T # (N,3)
if not multiple_quaternions:
return rpy[0]
return np.array(rpy)
def get_symbolic_rpy_from_quaternion(q, convention='xyzw'):
@@ -520,7 +549,7 @@ def get_quaternion_from_rpy(rpy, convert_to_quat=False, convention='xyzw'):
Get quaternion from Roll-Pitch-Yaw angle.
Args:
rpy (np.float[3]): roll-pitch-yaw angles
rpy (np.float[3], np.float[N,3]): roll-pitch-yaw angles
convert_to_quat (bool): If True, it will return an instance of `quaternion.quaternion`.
convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
'wxyz'.
@@ -528,26 +557,36 @@ def get_quaternion_from_rpy(rpy, convert_to_quat=False, convention='xyzw'):
Returns:
np.array[4], quaternion.quaternion: quaternion
"""
r, p, y = rpy
rpy = np.asarray(rpy)
multiple_rpy = True
if len(rpy.shape) < 2:
multiple_rpy = False
rpy = np.array([rpy]) # (1,3)
r, p, y = rpy[:, 0], rpy[:, 1], rpy[:, 2]
cr, sr = np.cos(r/2.), np.sin(r/2.)
cp, sp = np.cos(p/2.), np.sin(p/2.)
cy, sy = np.cos(y/2.), np.sin(y/2.)
w = cr * cp * cy + sr * sp * sy
x = sr * cp * cy - cr * sp * sy
y = cr * sp * cy + sr * cp * sy
z = cr * cp * sy - sr * sp * cy
w = cr * cp * cy + sr * sp * sy # (N,)
x = sr * cp * cy - cr * sp * sy # (N,)
y = cr * sp * cy + sr * cp * sy # (N,)
z = cr * cp * sy - sr * sp * cy # (N,)
if convert_to_quat:
return quaternion.quaternion(w, x, y, z)
q = quaternion.from_float_array(np.vstack((w, x, y, z)).T)
else:
if convention == 'xyzw':
return np.array([x, y, z, w])
q = np.vstack([x, y, z, w]).T
elif convention == 'wxyz':
return np.array([w, x, y, z])
q = np.vstack([w, x, y, z]).T
else:
raise NotImplementedError("Asking for a convention that has not been implemented")
if not multiple_rpy:
return q[0]
return q
def get_symbolic_quaternion_from_rpy(rpy, convention='xyzw'):
"""
@@ -956,11 +995,13 @@ def angular_velocity_from_quaternion(q1, q2):
def quaternion_distance(q1, q2):
r"""
Compute the distance metric (on :math:`\mathbb{S}^3`) betwen two quaternions :math:`q_1` and :math:`q_2`:
Compute the distance metric (on :math:`\mathbb{S}^3`) between two quaternions :math:`q_1` and :math:`q_2`:
Assuming a quaternion :math:`q` is represented as :math:`s + \pmb{v}` where :math:`s \in \mathbb{R}` is the scalar
part and :math:`\pmb{v} \in \mathbb{R}^3` is the vector part, the distance is given by:
.. math::
d(q_1, q_2) = \left\{ \begin{array}{ll}
2\pi, & q1 * \bar{q}_2 = -1 + [0,0,0]^\top \\
2 || \log(q_1 * \bar{q}_2) ||, & \text{otherwise}