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

update robot middlewares + imitation

Browse Source
This commit is contained in:
Brian Delhaisse
2019-10-21 07:04:15 +02:00
parent 64066d894b
commit 134f69aeea
17 changed files with 1134 additions and 182 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/imitation/demo.py
+77
View File
@@ -0,0 +1,77 @@
# -*- coding: utf-8 -*-
#!/usr/bin/env python
"""Imitation learning demonstration using DMPs and ROS with the Franka robot.
"""
import pyrobolearn as prl
# variables
joint_ids = None # None for all the actuated joints, or you can select which joint you want to move; e.g. [0, 1, 2]
num_basis = 20
rate = 30
# create middleware
# ros = prl.middlewares.ROS()
# create simulator
sim = prl.simulators.Bullet() # middleware=ros)
# sim.disable_middleware() # disable the middleware (get/set info only from/to simulation)
# create basic world (with gravity and floor)
world = prl.worlds.BasicWorld(sim)
# load Franka Emika Panda robot in the world
robot = prl.robots.Franka(sim)
world.load_robot(robot)
robot.print_info()
# create state/action
state = prl.states.ExponentialPhaseState(ticks=rate)
action = prl.actions.JointPositionAction(robot, joint_ids=joint_ids)
print("State: {}".format(state))
print("Action: {}".format(action))
# create environment
env = prl.envs.Env(world, state)
# create DMP policy
policy = prl.policies.BioDiscreteDMPPolicy(action, state, num_basis=num_basis, rate=rate)
# create interface/bridge
interface = prl.interfaces.MouseKeyboardInterface(sim)
bridge = prl.bridges.BridgeMouseKeyboardImitationTask(world, interface=interface, verbose=True)
# create recorder
recorder = prl.recorders.StateRecorder(prl.states.JointPositionState(robot, joint_ids=joint_ids), rate=rate)
# create imitation learning task
task = prl.tasks.ILTask(env, policy, interface=bridge, recorders=recorder)
# record, train, and test policy using the policy
# task.run()
# record demonstrations in simulation/reality
print("\nRecording phase: press `ctrl+r` to start/stop the recording. Once finished, press `shift+r`.")
task.record(signal_from_interface=True)
print("Recording phase: finished the recording!")
# train policy
print("Training phase: training the policy...")
task.train(signal_from_interface=False)
print("Training phase: policy trained!")
# plot what the DMP policy has learned by performing a rollout
policy.plot_rollout(nrows=3, ncols=3, suptitle='DMP position trajectories in joint space',
titles=['q' + str(i) for i in range(robot.num_actuated_joints)], show=True)
# test policy in simulation
print("Reproduction phase: test policy in simulation...")
task.test(num_steps=rate*100, signal_from_interface=False)
print("Reproduction phase: Policy tested!")
# test policy on real robot
print("Reproduction phase: test policy in reality...")
# sim.enable_middleware() # enable the real robot
# task.test(num_steps=rate*100, signal_from_interface=False)
print("Reproduction phase: Policy tested!")
pyrobolearn/__init__.py
+3
View File
@@ -22,6 +22,7 @@ logger.addHandler(handler)
# import simulators
from . import simulators
from .simulators import middlewares
# import robots
from . import robots
@@ -70,6 +71,8 @@ from . import dynamics
# import tools (interfaces and bridges)
from . import tools
from .tools import interfaces
from .tools import bridges
# import recorders
from . import recorders
pyrobolearn/models/dmp/dmp.py
+77 -34
View File
@@ -8,6 +8,7 @@ This file implements the DMP abstract class from which all dynamic movement prim
import numpy as np
import copy
import scipy.interpolate
import matplotlib.pyplot as plt
from pyrobolearn.models.dmp.canonical_systems import CS
from pyrobolearn.models.dmp.forcing_terms import ForcingTerm
@@ -94,17 +95,17 @@ class DMP(object):
- it can be used with RL algorithms, notably PoWER [9] and PI^2 [10]
References:
[1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013
[2] "Motor primitives in vertebrates and invertebrates", Flash et al., 2005
[3] Tutorials on DMP: https://studywolf.wordpress.com/category/robotics/dynamic-movement-primitive/
[4] PyDMPs (from DeWolf, 2013): https://github.com/studywolf/pydmps
[5] "Biologically-inspired Dynamical Systems for Movement Generation: Automatic Real-time Goal Adaptation
and Obstacle Avoidance", Hoffmann et al., 2009
[6] "Action Sequencing using Dynamic Movement Primitives", Nemec et al., 2011
[7] "Orientation in Cartesian Space Dynamic Movement Primitives", Ude et al., 2014
[8] "A Framework for Learning Biped Locomotion with Dynamical Movement Primitives", Nakanishi et al., 2004
[9] "Policy Search for Motor Primitives in Robotics", Kober et al., 2010
[10] "A Generalized Path Integral Control Approach to Reinforcement Learning", Theodorou et al., 2010
- [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013
- [2] "Motor primitives in vertebrates and invertebrates", Flash et al., 2005
- [3] Tutorials on DMP: https://studywolf.wordpress.com/category/robotics/dynamic-movement-primitive/
- [4] PyDMPs (from DeWolf, 2013): https://github.com/studywolf/pydmps
- [5] "Biologically-inspired Dynamical Systems for Movement Generation: Automatic Real-time Goal Adaptation
and Obstacle Avoidance", Hoffmann et al., 2009
- [6] "Action Sequencing using Dynamic Movement Primitives", Nemec et al., 2011
- [7] "Orientation in Cartesian Space Dynamic Movement Primitives", Ude et al., 2014
- [8] "A Framework for Learning Biped Locomotion with Dynamical Movement Primitives", Nakanishi et al., 2004
- [9] "Policy Search for Motor Primitives in Robotics", Kober et al., 2010
- [10] "A Generalized Path Integral Control Approach to Reinforcement Learning", Theodorou et al., 2010
"""
def __init__(self, canonical_system, forcing_term, y0=0, goal=1, stiffness=None, damping=None):
@@ -112,10 +113,10 @@ class DMP(object):
Args:
canonical_system (CS): canonical system which drives the DMP transformation system
forcing_terms (list): list of forcing terms (one forcing term for each DMP). Each forcing term can have
forcing_term (list): list of forcing terms (one forcing term for each DMP). Each forcing term can have
different number of basis functions.
y0 (float, float[M]): initial state of DMPs
goal (float, float[M]): goal state of DMPs
y0 (float, np.array[float[M]]): initial state of DMPs
goal (float, np.array[float[M]]): goal state of DMPs
stiffness (float): stiffness term in the transformation system for DMPs
damping (float): damping term in the transformation system for DMPs
"""
@@ -313,7 +314,7 @@ class DMP(object):
idx += size
def get_damping_ratio(self):
"""
r"""
Return the damping ratio :math:`\zeta = D / D_c` where :math:`D_c = 2 \sqrt{K}`.
* if :math:`\zeta` = 0, the system is undamped (i.e. no damping)
@@ -353,8 +354,8 @@ class DMP(object):
s (None, float): the phase value. If None, it will use the canonical system.
tau (float): Increase tau to make the system slower, and decrease it to make it faster
error (float): optional system feedback
forcing_term (float[M]): if given, it will replace the forcing term (where `M` = number of DMPs)
new_goal (float[M]): new goal (where `M` = number of DMPs)
forcing_term (np.array[float[M]]): if given, it will replace the forcing term (where `M` = number of DMPs)
new_goal (np.array[float[M]]): new goal (where `M` = number of DMPs)
rescale_force (bool): if the given forcing term should be rescaled.
"""
@@ -411,13 +412,14 @@ class DMP(object):
tau (float): Increase tau to make the system slower, and decrease it to make it faster
timesteps (None, int): the number of steps to perform
error (float): optional system feedback
forcing_term (np.ndarray): if given, it will replace the forcing term (shape [num_dmps, timesteps])
new_goal (np.ndarray): new goal (of shape [num_dmps,])
forcing_term (np.array[float[M,T]]): if given, it will replace the forcing term (shape [num_dmps,
timesteps])
new_goal (np.array[float[M]]): new goal (of shape [num_dmps,])
Returns:
float[M,T]: y (position) trajectories
float[M,T]: dy (velocity) trajectories
float[M,T]: ddy (acceleration) trajectories
np.array[float[M,T]]: y (position) trajectories
np.array[float[M,T]]: dy (velocity) trajectories
np.array[float[M,T]]: ddy (acceleration) trajectories
"""
# reset the canonical and transformation systems
@@ -460,9 +462,14 @@ class DMP(object):
"""Imitate a desired trajectory, and learn the parameters that best realizes it.
Args:
y_des (np.array): the desired position trajectories of each DMP with shape [num_dmps, timesteps]
dy_des (np.array): the desired velocities with shape [num_dmps, timesteps]
ddy_des (np.array): the desired accelerations with shape [num_dmps, timesteps]
y_des (np.array[float[M,T]], np.array[float[N,M,T]]): the desired position trajectories of each DMP with
shape [num_dmps, timesteps] or [num_trajectories, num_dmps, timesteps]. The number of timesteps for each
trajectory can be different. Note that each trajectory should have the same initial state and goal. When
giving multiple trajectories to DMPs, they will be averaged out.
dy_des (np.array[float[M,T]], np.array[float[N,M,T]]): the desired velocities with shape
[num_dmps, timesteps] or [num_trajectories, num_dmps, timesteps].
ddy_des (np.array[float[M,T]], np.array[float[N,M,T]]): the desired accelerations with shape
[num_dmps, timesteps] or [num_trajectories, num_dmps, timesteps].
interpolation (str): how to interpolate the data. Select between 'linear', 'cubic', and 'hermite'.
"""
@@ -523,7 +530,6 @@ class DMP(object):
# plot
if plot:
import matplotlib.pyplot as plt
plt.figure()
plt.plot(y_des[0], 'b', label='pos')
plt.plot(dy_des[0], 'g', label='vel')
@@ -549,10 +555,10 @@ class DMP(object):
Get the forcing terms based on the given phase value.
Args:
s (float, float[T]): phase value(s)
s (float, np.array[float[T]]): phase value(s)
Returns:
float[M], float[M,T]: forcing terms
np.array[float[M]], np.array[float[M,T]]: forcing terms
"""
return np.array([self.f[d](s) for d in range(self.num_dmps)])
@@ -561,14 +567,15 @@ class DMP(object):
Generate the goal from the initial positions, velocities, accelerations, and forces.
Args:
y0 (float[M], None): initial positions. If None, it will take the default initial positions.
dy0 (float[M], None): initial velocities. If None, it will take the default initial velocities.
ddy0 (float[M], None): initial accelerations. If None, it will take the default initial accerelations.
f0 (float[M], None): initial forcing terms. If None, it will compute it based on the learned weights.
You can also give `dmp.f_target[:,0]` to get the correct goal.
y0 (np.array[float[M]], None): initial positions. If None, it will take the default initial positions.
dy0 (np.array[float[M]], None): initial velocities. If None, it will take the default initial velocities.
ddy0 (np.array[float[M]], None): initial accelerations. If None, it will take the default initial
accelerations.
f0 (np.array[float[M]], None): initial forcing terms. If None, it will compute it based on the learned
weights. You can also give `dmp.f_target[:,0]` to get the correct goal.
Returns:
float[M]: goal position for each DMP.
np.array[float[M]]: goal position for each DMP.
"""
if y0 is None:
y0 = self.y0
@@ -600,6 +607,42 @@ class DMP(object):
raise TypeError("The given model is not an instance of DMP.")
pass
def plot_rollout(self, ax=None, nrows=1, ncols=1, suptitle=None, titles=None, show=False):
"""
Plot a complete rollout.
Args:
ax (plt.Axes.axis, None): figure axis.
nrows (int): number of rows in the subplot.
ncols (int): number of columns in the subplot.
suptitle (str): main title for the subplots.
titles (str, list[str]): title for each subplot.
show (bool): if True, it will show and block the plot.
"""
# perform a rollout
y, dy, ddy = self.rollout()
# if ax is not defined
if ax is None:
plt.figure()
if suptitle is not None:
plt.suptitle(suptitle)
# plot each subplot
for i in range(y.shape[0]):
plt.subplot(nrows, ncols, i + 1)
if titles is not None:
if isinstance(titles, str):
plt.title(titles)
elif isinstance(titles, (list, tuple, np.ndarray)) and i < len(titles):
plt.title(titles[i])
plt.plot(y[i])
# tight the layout
plt.tight_layout()
if show:
plt.show()
# def __rshift__(self, other):
# """
# Sequence DMP model with another learning model.
pyrobolearn/models/gaussian.py
+11 -11
View File
@@ -84,9 +84,9 @@ class Gaussian(object):
forward substitution, and then computing :math:`L^\top x = y` by backward substitution.
References:
[1] "Pattern Recognition and Machine Learning", Bishop, 2006
[2] "Machine Learning: A Probabilistic Perspective", Murphy, 2012, chap 3 and 4
[3] "The Matrix Cookbook", Petersen et al., 2012, sec 8
- [1] "Pattern Recognition and Machine Learning", Bishop, 2006
- [2] "Machine Learning: A Probabilistic Perspective", Murphy, 2012, chap 3 and 4
- [3] "The Matrix Cookbook", Petersen et al., 2012, sec 8
The implementation of this class was inspired by the following codes:
* `scipy`: https://docs.scipy.org/doc/scipy-0.14.0/reference/generated/scipy.stats.multivariate_normal.html
@@ -108,8 +108,8 @@ class Gaussian(object):
Initialize the multivariate normal distribution on the given manifold.
Args:
mean (np.array[D]): mean vector.
covariance (np.array[D,D]): covariance matrix.
mean (np.array[float[D]]): mean vector.
covariance (np.array[float[D,D]]): covariance matrix.
seed (int): random seed. Useful when sampling.
manifold (None): By default, it is the Euclidean space.
N (int, N): the number of data points
@@ -273,12 +273,12 @@ class Gaussian(object):
the mean vector :math:`\mu`, i.e. :math:`\max_{\mu} p(X | \mu, \Sigma)`.
Args:
X (array[N,D]): data matrix of shape NxD (if axis=0) or DxN (if axis=1), where N is the number of samples,
and D is the dimensionality of a data point
X (np.array[float[N,D]]): data matrix of shape NxD (if axis=0) or DxN (if axis=1), where N is the number
of samples, and D is the dimensionality of a data point
axis (int): axis along which the mean is computed
Returns:
float[D]: mean vector
np.array[float[D]]: mean vector
"""
# if manifold is Euclidean
mean = np.mean(X, axis=axis)
@@ -291,13 +291,13 @@ class Gaussian(object):
for the covariance matrix :math:`\Sigma`, i.e. :math:`\max_{\Sigma} p(X | \mu, \Sigma)`.
Args:
X (array[N,D]): data matrix of shape NxD where N is the number of samples, and D is the dimensionality
of a data point
X (np.array[float[N,D]]): data matrix of shape NxD where N is the number of samples, and D is the
dimensionality of a data point.
axis (int): axis along which the covariance is computed
bessels_correction (bool): if True, it will compute the covariance using `1/N-1` instead of `N`.
Returns:
float[D,D]: 2D covariance matrix
np.array[float[D,D]]: 2D covariance matrix
"""
# if manifold is Euclidean
cov = np.cov(X, rowvar=bool(axis), bias=not bessels_correction)
pyrobolearn/policies/dmp_policy.py
+13
View File
@@ -127,6 +127,19 @@ class DMPPolicy(Policy):
else:
print("Nothing to imitate.")
def plot_rollout(self, nrows=1, ncols=1, suptitle=None, titles=None, show=True):
"""
Plot the rollouts using the DMPs.
Args:
nrows (int): number of rows in the subplot.
ncols (int): number of columns in the subplot.
suptitle (str): main title for the subplots.
titles (str, list[str]): title for each subplot.
show (bool): if True, it will show and block the plot.
"""
self.model.plot_rollout(nrows=nrows, ncols=ncols, suptitle=suptitle, titles=titles, show=show)
class DiscreteDMPPolicy(DMPPolicy):
r"""Discrete DMP Policy
pyrobolearn/robots/franka.py
+6
View File
@@ -4,6 +4,7 @@
"""
import os
import numpy as np
from pyrobolearn.robots.manipulator import Manipulator
from pyrobolearn.robots.gripper import ParallelGripper
@@ -59,6 +60,11 @@ class Franka(Manipulator):
# self.disable_motor()
self._joint_configuration = {'home': np.array([0.0277854, -0.97229678, -0.028778385, -2.427800237,
-0.086976557, 1.442695354, -0.711514286, 0., 0.])}
self.set_home_joint_positions()
class FrankaGripper(ParallelGripper):
r"""Franka Emika Panda gripper
pyrobolearn/robots/robot.py
+20 -5
View File
@@ -333,6 +333,19 @@ class Robot(ControllableBody):
for actuator in actuators:
actuator()
##############
# Middleware #
##############
def get_robot_middleware(self):
"""
Get the robot middleware associated with the given robot.
Returns:
RobotMiddleware, None: robot middleware associated with the robot. None, if no middleware was defined.
"""
return self.sim.get_robot_middleware(robot_id=self.id)
########
# Base #
########
@@ -1385,7 +1398,7 @@ class Robot(ControllableBody):
simulation.
"""
# check joint_ids
if not joint_ids:
if joint_ids is None:
joint_ids = self.joints
if isinstance(joint_ids, int):
joint_ids = [joint_ids]
@@ -1435,9 +1448,11 @@ class Robot(ControllableBody):
the child class.
"""
if 'home' in self._joint_configuration:
joint_ids, joint_values = self._joint_configuration['home']
if len(joint_ids) == self.num_actuated_joints:
return joint_values
# joint_ids, joint_values = self._joint_configuration['home']
# if len(joint_ids) == self.num_actuated_joints:
# return joint_values
joint_values = self._joint_configuration['home']
return joint_values
return np.zeros(self.num_actuated_joints)
def set_home_joint_positions(self):
@@ -3638,7 +3653,7 @@ class Robot(ControllableBody):
Args:
q (np.array[float[N]], None): joint positions of size N, where N is the total number of DoFs. If None, it
will get the current joint positions.
will get the current joint positions.
q_idx (slice, None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N).
Returns:
pyrobolearn/simulators/middlewares/__init__.py
+1 -1
View File
@@ -1,7 +1,7 @@
# -*- coding: utf-8 -*-
# import middleware abstract class
from .middleware import MiddleWare
from .middleware import Middleware
# import ROS
try:
pyrobolearn/simulators/middlewares/middleware.py
+27 -2
View File
@@ -17,13 +17,13 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class MiddleWare(object):
class Middleware(object):
r"""Middleware (abstract) class
Middleware can be provided to simulators which can then use them to send/receive messages.
"""
def __init__(self, subscribe=False, publish=False, teleoperate=False):
def __init__(self, subscribe=False, publish=False, teleoperate=False, command=True):
"""
Initialize the middleware to communicate.
@@ -33,11 +33,16 @@ class MiddleWare(object):
publish (bool): if True, it will publish the given values to the topics associated to the loaded robots.
teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
previous attributes :attr:`subscribe` and :attr:`publish`.
command (bool): if True, it will subscribe/publish to some (joint) commands. If False, it will
subscribe/publish to some (joint) states.
"""
# set variables
self.is_subscribing = subscribe
self.is_publishing = publish
self.is_teleoperating = teleoperate
self.is_commanding = command
self._robots = {} # {body_id: RobotMiddleware}
##############
# Properties #
@@ -67,6 +72,14 @@ class MiddleWare(object):
def is_teleoperating(self, teleoperate):
self._teleoperate = bool(teleoperate)
@property
def is_commanding(self):
return self._command
@is_commanding.setter
def is_commanding(self, command):
self._command = bool(command)
#############
# Operators #
#############
@@ -117,6 +130,18 @@ class MiddleWare(object):
"""
pass
def get_robot_middleware(self, robot_id):
r"""
Get the robot middleware associated with the given robot id.
Args:
robot_id (int): robot unique id.
Returns:
RobotMiddleware, None: robot middleware. None if it could not find the associated robot midddleware.
"""
pass
def load_urdf(self, urdf):
"""Load the given URDF file.
pyrobolearn/simulators/middlewares/robot_middleware.py
+242
View File
@@ -0,0 +1,242 @@
# -*- coding: utf-8 -*-
#!/usr/bin/env python
"""Define the Robot middleware API.
The robot middleware interface is an interface between a particular robot and the middleware. The middleware
possesses a list of Robot middleware interfaces (one for each robot). If you have a specific robot, you have to
implement this class otherwise it will use the provided default one.
For instance, when using ROS, the `RobotMiddleWare` is inherited by the `ROSRobotMiddleware` class from which all
ROS robot middlewares have to inherit from. A `DefaultROSRobotMiddleware` that inherits from the `ROSRobotMiddleware`
is also provided.
Dependencies in PRL:
* `pyrobolearn.simulators.middlewares.robot_middleware.RobotMiddleWare`
"""
__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 RobotMiddleware(object):
r"""Robot middleware interface.
The robot middleware interface is an interface between a particular robot and the middleware. The middleware
possesses a list of Robot middleware interfaces (one for each robot).
Notably, the robot middleware has a unique id, has a list of publishers and subscribers associated with the given
robot.
Here are the possible combinations between the different values for subscribe (S), publish (P), teleoperate (T),
and command (C):
- S=1, P=0, T=0: subscribes to the topics, and get the messages when calling the corresponding getter methods.
- S=0, P=1, T=0: publishes the various messages to the topics when calling the corresponding setter methods.
- S=1, P=0, T=1, C=0/1: get messages by subscribing to the topics that publish some commands/states. The received
commands/states are then set in the simulator. Depending on the value of `C`, it will subscribe to topics that
publish some commands (C=1) or states (C=0). Example: should we subscribe to joint trajectory commands, or joint
states when teleoperating the robot in the simulator? This C value allows to specify which one we are interested
in.
- S=0, P=1, T=1: when calling the getters methods such as joint positions, velocities, and others, it also
publishes the joint states. This is useful if we are moving/teleoperating the robot in the simulator.
- S=0, P=0, T=1/0: doesn't do anything.
- S=1, P=1, T=0: subscribes to some topics and publish messages to other topics. The messages are can be
sent/received by calling the appropriate getter/setter methods.
- S=1, P=1, T=1: not allowed.
"""
def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, command=True,
control_file=None):
"""
Initialize the robot middleware interface.
Args:
robot_id (int): robot unique id.
urdf (str): path to the URDF file.
subscribe (bool): if True, it will subscribe to the topics associated to the loaded robot, and will read
the values published on these topics.
publish (bool): if True, it will publish the given values to the topics associated to the loaded robot.
teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
previous attributes :attr:`subscribe` and :attr:`publish`.
command (bool): if True, it will subscribe/publish to some (joint) commands. If False, it will
subscribe/publish to some (joint) states.
control_file (str, None): path to the YAML control file.
"""
# set variables
self.id = robot_id
self.urdf = urdf
self.control_file = control_file
self.is_subscribing = subscribe
self.is_publishing = publish
self.is_teleoperating = teleoperate
self.command = command
if self.is_teleoperating and self.is_publishing and self.is_subscribing:
raise ValueError("The three following arguments 'subscribe', 'publish', and 'teleoperate' can not be all "
"true at the same time. Select maximum two.")
def __del__(self):
"""
Close all topics.
"""
self.close()
def unregister(self):
"""
Unsubscribe from a topic. Topic instance is no longer valid after this call. Additional calls to `unregister()`
have no effect.
"""
pass
def close(self):
"""
Close all topics. Topic instances are no longer valid after this call.
"""
self.unregister()
def get_joint_positions(self, joint_ids):
"""
Get the position of the given joint(s).
Args:
joint_ids (int, list[int]): joint id, or list of joint ids.
Returns:
if 1 joint:
float: joint position [rad]
if multiple joints:
np.array[float[N]]: joint positions [rad]
"""
pass
def set_joint_positions(self, positions, joint_ids=None, velocities=None, kps=None, kds=None, forces=None):
"""
Set the position of the given joint(s) (using position control).
Args:
positions (float, np.array[float[N]]): desired position, or list of desired positions [rad]
joint_ids (int, list[int], None): joint id, or list of joint ids.
velocities (None, float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
kps (None, float, np.array[float[N]]): position gain(s)
kds (None, float, np.array[float[N]]): velocity gain(s)
forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values.
"""
pass
def get_joint_velocities(self, joint_ids):
"""
Get the velocity of the given joint(s).
Args:
joint_ids (int, list[int]): joint id, or list of joint ids.
Returns:
if 1 joint:
float: joint velocity [rad/s]
if multiple joints:
np.array[float[N]]: joint velocities [rad/s]
"""
pass
def set_joint_velocities(self, velocities, joint_ids=None, max_force=None):
"""
Set the velocity of the given joint(s) (using velocity control).
Args:
velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
joint_ids (int, list[int], None): joint id, or list of joint ids.
max_force (None, float, np.array[float[N]]): maximum motor forces/torques.
"""
pass
def get_joint_torques(self, joint_ids):
"""
Get the applied torque(s) on the given joint(s). "This is the motor torque applied during the last `step`.
Note that this only applies in VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the
applied joint motor torque is exactly what you provide, so there is no need to report it separately." [1]
Args:
joint_ids (int, list[int]): a joint id, or list of joint ids.
Returns:
if 1 joint:
float: torque [Nm]
if multiple joints:
np.array[float[N]]: torques associated to the given joints [Nm]
"""
pass
def set_joint_torques(self, torques, joint_ids=None):
"""
Set the torque/force to the given joint(s) (using force/torque control).
Args:
torques (float, list[float]): desired torque(s) to apply to the joint(s) [N].
joint_ids (int, list[int], None): joint id, or list of joint ids.
"""
pass
def has_sensor(self, name):
"""
Check if the given robot middleware has the specified sensor.
Args:
name (str): name of the sensor.
Returns:
bool: True if the robot middleware has the sensor.
"""
pass
def get_sensor_values(self, name):
"""
Get the sensor values associated with the given sensor name.
Args:
name (str): unique name of the sensor.
Returns:
object, np.array, float, int: sensor values.
"""
pass
def get_pid(self, joint_ids):
"""
Get the PID coefficients associated to the given joint ids.
Args:
joint_ids (list[int]): list of unique joint ids.
Returns:
list[np.array[float[3]]]: list of PID coefficients for each joint.
"""
pass
def set_pid(self, joint_ids, pid):
"""
Set the given PID coefficients to the given joint ids.
Args:
joint_ids (list[int]): list of unique joint ids.
pid (list[np.array[float[3]]]): list of PID coefficients for each joint. If one of the value is -1, it
will left untouched the associated PID value to the previous one.
"""
pass
def get_jacobian(self, link_id, q=None, local_position=None):
"""
Return the jacobian.
"""
pass
def get_inertia_matrix(self, q=None):
"""
Return the inertia matrix.
"""
pass
pyrobolearn/simulators/middlewares/robots/__init__.py
View File
pyrobolearn/simulators/middlewares/robots/franka.py
+217
View File
@@ -0,0 +1,217 @@
# -*- coding: utf-8 -*-
#!/usr/bin/env python
"""Define the Franka ROS Robot middleware API.
This is robot middleware interface between the Franka robot and ROS. This file should be modified by the user!!
Currently, we use the following setup:
- https://github.com/erdalpekel/franka_ros
- https://github.com/erdalpekel/panda_simulation
by launching `panda_simulation/simulation.launch`.
The topics for the joint states and joint commands (=joint trajectories) are:
- /joint_states
- /panda_arm_controller/command
- /panda_hand_controller/command
"""
# import ROS messages
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from sensor_msgs.msg import JointState
from pyrobolearn.simulators.middlewares.ros import ROSRobotMiddleware
__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 FrankaROSMiddleware(ROSRobotMiddleware):
r"""Robot middleware interface.
The robot middleware interface is an interface between a particular robot and the middleware. The middleware
possesses a list of Robot middleware interfaces (one for each robot).
Notably, the robot middleware has a unique id, has a list of publishers and subscribers associated with the given
robot.
"""
def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, command=True,
control_file=None, launch_file=None):
"""
Initialize the robot middleware interface.
Args:
robot_id (int): robot unique id.
urdf (str): path to the URDF file.
subscribe (bool): if True, it will subscribe to the topics associated to the loaded robot, and will read
the values published on these topics.
publish (bool): if True, it will publish the given values to the topics associated to the loaded robot.
teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
previous attributes :attr:`subscribe` and :attr:`publish`.
command (bool): if True, it will subscribe/publish to some (joint) commands. If False, it will
subscribe/publish to some (joint) states.
control_file (str, None): path to the YAML control file. If provided, it will be parsed.
launch_file (str, None): path to the ROS launch file. If provided, it will be parsed.
"""
super(FrankaROSMiddleware, self).__init__(robot_id, urdf, subscribe, publish, teleoperate, command,
control_file, launch_file)
# update publisher and subscriber topics
def get_joint_positions(self, joint_ids=None):
"""
Get the position of the given joint(s).
Args:
joint_ids (int, list[int], None): joint id, or list of joint ids.
Returns:
if 1 joint:
float: joint position [rad]
if multiple joints:
np.array[float[N]]: joint positions [rad]
"""
pass
def set_joint_positions(self, positions, joint_ids=None, velocities=None, kps=None, kds=None, forces=None):
"""
Set the position of the given joint(s) (using position control).
Args:
positions (float, np.array[float[N]]): desired position, or list of desired positions [rad]
joint_ids (int, list[int], None): joint id, or list of joint ids.
velocities (None, float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
kps (None, float, np.array[float[N]]): position gain(s)
kds (None, float, np.array[float[N]]): velocity gain(s)
forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values.
"""
pass
def get_joint_velocities(self, joint_ids=None):
"""
Get the velocity of the given joint(s).
Args:
joint_ids (int, list[int], None): joint id, or list of joint ids.
Returns:
if 1 joint:
float: joint velocity [rad/s]
if multiple joints:
np.array[float[N]]: joint velocities [rad/s]
"""
pass
def set_joint_velocities(self, velocities, joint_ids=None, max_force=None):
"""
Set the velocity of the given joint(s) (using velocity control).
Args:
velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
joint_ids (int, list[int], None): joint id, or list of joint ids.
max_force (None, float, np.array[float[N]]): maximum motor forces/torques.
"""
pass
def get_joint_torques(self, joint_ids=None):
"""
Get the applied torque(s) on the given joint(s). "This is the motor torque applied during the last `step`.
Note that this only applies in VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the
applied joint motor torque is exactly what you provide, so there is no need to report it separately." [1]
Args:
joint_ids (int, list[int], None): a joint id, or list of joint ids.
Returns:
if 1 joint:
float: torque [Nm]
if multiple joints:
np.array[float[N]]: torques associated to the given joints [Nm]
"""
pass
def set_joint_torques(self, torques, joint_ids=None):
"""
Set the torque/force to the given joint(s) (using force/torque control).
Args:
torques (float, list[float]): desired torque(s) to apply to the joint(s) [N].
joint_ids (int, list[int], None): joint id, or list of joint ids.
"""
pass
def has_sensor(self, name):
"""
Check if the given robot middleware has the specified sensor.
Args:
name (str): name of the sensor.
Returns:
bool: True if the robot middleware has the sensor.
"""
pass
def get_sensor_values(self, name):
"""
Get the sensor values associated with the given sensor name.
Args:
name (str): unique name of the sensor.
Returns:
object, np.array, float, int: sensor values.
"""
pass
def get_pid(self, joint_ids):
"""
Get the PID coefficients associated to the given joint ids.
Args:
joint_ids (list[int]): list of unique joint ids.
Returns:
list[np.array[float[3]]]: list of PID coefficients for each joint.
"""
pass
def set_pid(self, joint_ids, pid):
"""
Set the given PID coefficients to the given joint ids.
Args:
joint_ids (list[int]): list of unique joint ids.
pid (list[np.array[float[3]]]): list of PID coefficients for each joint. If one of the value is -1, it
will left untouched the associated PID value to the previous one.
"""
pass
def get_jacobian(self, link_id, q=None, local_position=None):
"""
Return the jacobian.
Args:
link_id (int): link id.
q (np.array[float[N]], None): joint positions of size N, where N is the number of DoFs. If None, it will
compute q based on the current joint positions.
local_position (None, np.array[float[3]]): the point on the specified link to compute the Jacobian (in link
local coordinates around its center of mass). If None, it will use the CoM position (in the link frame).
"""
pass
def get_inertia_matrix(self, q=None):
"""
Return the inertia matrix.
Args:
q (np.array[float[N]], None): joint positions of size N, where N is the total number of DoFs. If None, it
will get the current joint positions.
"""
pass
pyrobolearn/simulators/middlewares/ros.py
+244 -109
View File
@@ -21,7 +21,7 @@ Note to compile ROS packages using `catkin_make`, you might have to specify the
- catkin_make -DPYTHON_EXECUTABLE=path/to/bin/python3
Dependencies in PRL:
* `pyrobolearn.simulators.middlewares.middleware.MiddleWare`
* `pyrobolearn.simulators.middlewares.middleware.Middleware`
References:
- [1] ROS: http://www.ros.org/ and http://wiki.ros.org
@@ -56,7 +56,8 @@ import gazebo_msgs.msg as gazebo_msg
import geometry_msgs.msg as geometry_msg
import trajectory_msgs.msg as trajectory_msg
from pyrobolearn.simulators.middlewares.middleware import MiddleWare
from pyrobolearn.simulators.middlewares.middleware import Middleware
from pyrobolearn.simulators.middlewares.robot_middleware import RobotMiddleware
from pyrobolearn.simulators.middlewares.ros_publisher import PublisherData, Publisher, RobotPublisher
from pyrobolearn.simulators.middlewares.ros_subscriber import SubscriberData, Subscriber, RobotSubscriber
@@ -75,6 +76,8 @@ __status__ = "Development"
class Remapper(object):
"""Remapper from old topic to a new topic.
Note that this doesn't replace the old topic by the new one; the old topic will still be present.
"""
def __init__(self, old_topic, new_topic, msg_class, queue_size=10, new_msg_class=None, function=None):
@@ -145,17 +148,35 @@ class Remapper(object):
self.unregister()
class RobotMiddleWare(object):
r"""Robot middleware interface.
class ROSRobotMiddleware(RobotMiddleware):
r"""ROS robot middleware interface.
The robot middleware interface is an interface between a particular robot and the middleware. The middleware
possesses a list of Robot middleware interfaces (one for each robot).
Notably, the robot middleware has a unique id, has a list of publishers and subscribers associated with the given
robot.
Notably, the ROS robot middleware has a unique id, has a list of publishers and subscribers associated with the
given robot.
Here are the possible combinations between the different values for subscribe (S), publish (P), teleoperate (T),
and command (C):
- S=1, P=0, T=0: subscribes to the topics, and get the messages when calling the corresponding getter methods.
- S=0, P=1, T=0: publishes the various messages to the topics when calling the corresponding setter methods.
- S=1, P=0, T=1, C=0/1: get messages by subscribing to the topics that publish some commands/states. The received
commands/states are then set in the simulator. Depending on the value of `C`, it will subscribe to topics that
publish some commands (C=1) or states (C=0). Example: should we subscribe to joint trajectory commands, or joint
states when teleoperating the robot in the simulator? This C value allows to specify which one we are interested
in.
- S=0, P=1, T=1: when calling the getters methods such as joint positions, velocities, and others, it also
publishes the joint states. This is useful if we are moving/teleoperating the robot in the simulator.
- S=0, P=0, T=1/0: doesn't do anything.
- S=1, P=1, T=0: subscribes to some topics and publish messages to other topics. The messages are can be
sent/received by calling the appropriate getter/setter methods.
- S=1, P=1, T=1: not allowed.
"""
def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, control_file=None):
def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, command=True,
control_file=None, launch_file=None):
"""
Initialize the robot middleware interface.
@@ -167,27 +188,37 @@ class RobotMiddleWare(object):
publish (bool): if True, it will publish the given values to the topics associated to the loaded robot.
teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
previous attributes :attr:`subscribe` and :attr:`publish`.
control_file (str, None): path to the YAML control file.
command (bool): if True, it will subscribe/publish to some (joint) commands. If False, it will
subscribe/publish to some (joint) states.
control_file (str, None): path to the YAML control file. If provided, it will be parsed.
launch_file (str, None): path to the ROS launch file. If provided, it will be parsed.
"""
# set variables
self.id = robot_id
self.urdf = urdf
self.control_file = control_file
self.is_subscribing = subscribe
self.is_publishing = publish
self.is_teleoperating = teleoperate
super(ROSRobotMiddleware, self).__init__(robot_id, urdf, subscribe, publish, teleoperate, command, control_file)
self.launch_file = launch_file
if self.is_teleoperating and self.is_publishing and self.is_subscribing:
raise ValueError("The three following arguments 'subscribe', 'publish', and 'teleoperate' can not be all "
"true at the same time. Select maximum two.")
print("\n Creating RobotMiddleware")
print("\n Creating ROSRobotMiddleware")
# get path to the URDF folder
path = os.path.abspath(urdf) # /path/to/pyrobolearn/robots/urdfs/<robot>/robot.urdf
dirname = str(os.path.dirname(path)) # /path/to/pyrobolearn/robots/urdfs/<robot>/
basename = str(os.path.basename(path).split('.')[-2]) # robot name without extension
config_file = dirname + basename + ".yaml"
# parse URDF to get joint names, q indices, etc.
self.urdf_parser = URDFParser()
tree = self.urdf_parser.parse(urdf)
print("ROSRobotMiddleware - publisher - name: ", tree.name)
print("Num joints: ", tree.num_joints)
print("Num actuated joints: ", tree.num_actuated_joints)
self.tree = tree
self.q_indices = np.zeros(tree.num_joints, dtype=int)
self.joint_names = []
count = 0
for i, joint in enumerate(tree.joints.values()):
if joint.dtype != 'fixed':
print("Adding joint {} with type={}".format(joint.name, joint.dtype))
self.q_indices[i] = count
self.joint_names.append(joint.name)
count += 1
# subscriber and publisher associated with the given robot
if self.is_subscribing:
@@ -197,68 +228,6 @@ class RobotMiddleWare(object):
print("Creating Robot Publisher")
self.publisher = RobotPublisher(name=basename)
# 1. check if topics and services related to the loaded robot (such as joint_states, joint_commands, etc) are
# already advertised, if yes it will create the publishers/subscribers to these topics which would allow
# the `Simulator` instance to write/read the values on/from these topics
# check for YAML control configuration file
if os.path.isfile(config_file):
# if it exists, import it
data = yaml.load(open(config_file, 'r'), Loader=yaml.FullLoader)
else:
data = {basename: {'joint_state_controller': {'publish_rate': 50,
'type': 'joint_state_controller/JointStateController'}}}
# node = roslaunch.core.Node(package="controller_manager", name="controller_spawner", node_type="spawner",
# respawn="false", output="screen", namespace="/rrbot",
# args=' '.join(["joint_state_controller", "joint1_position_controller",
# "joint2_position_controller"]))
# 2. if the topics didn't exist, it will check if a controller_manager has been loaded beforehand, and if yes,
# it will load the corresponding controllers.
# / rrbot / controller_manager / list_controller_types
# / rrbot / controller_manager / list_controllers
# / rrbot / controller_manager / load_controller
# / rrbot / controller_manager / reload_controller_libraries
# / rrbot / controller_manager / switch_controller
# / rrbot / controller_manager / unload_controller
# create YAML file and load it
# 3. if it didn't find the topics nor the controller manager, it will create by default the various topics
# (joint_states, joint_commands, etc) but these won't be using `ros_control`.
if self.is_publishing:
urdf_parser = URDFParser()
tree = urdf_parser.parse(urdf)
print("RobotMiddleware - publisher - name: ", tree.name)
print("Num joints: ", tree.num_joints)
print("Num actuated joints: ", tree.num_actuated_joints)
self.q_indices = np.zeros(tree.num_joints, dtype=int)
topics = []
count = 0
for i, joint in enumerate(tree.joints.values()):
if joint.dtype != 'fixed':
print("Adding joint {} with type={}".format(joint.name, joint.dtype))
topic = '/' + tree.name + '/joint' + str(count+1) + '_position_controller/command'
self.q_indices[i] = count
count += 1
topics.append(topic)
print("Topics: ", topics)
publisher = self.publisher.create_publisher(name='qpos', topic=topics, data_class=std_msg.Float64)
self.publisher.init_set_joint_positions(publisher=publisher, msg_attribute_name='data')
# sensors
# /rrbot/camera1/image_raw
# /rrbot/laser/scan
def __del__(self):
"""
Close all topics.
"""
self.close()
def unregister(self):
"""
Unsubscribe from a topic. Topic instance is no longer valid after this call. Additional calls to `unregister()`
@@ -269,18 +238,69 @@ class RobotMiddleWare(object):
if self.is_publishing:
self.publisher.unregister()
def close(self):
def create_publisher(self, name, topic, msg_class, queue_size=10):
"""
Close all topics.
"""
self.unregister()
Create a publisher to the specific topic. If the publisher already exists, it unregister the previous one
and replace it by the new one.
def get_joint_positions(self, joint_ids):
Args:
name (str): unique name of the publisher. The name must be unique. You will be able to access to this
publisher using its name.
topic (str, list[str]): name of the topic(s).
msg_class (object): message class serialization.
queue_size (int): The queue size used for asynchronously publishing messages from different threads. A
size of zero means an infinite queue, which can be dangerous. When None is passed all publishing will
happen synchronously and a warning message will be printed.
Returns:
PublisherData: the publisher data holder.
"""
return self.publisher.create_publisher(name=name, topic=topic, data_class=msg_class, queue_size=queue_size)
def create_subscriber(self, name, topic, msg_class):
"""
Create a subscriber to the specific topic. If the subscriber already exists, it unregister the previous one
and replace it by the new one.
Args:
name (str): unique name of the subscriber. The name must be unique. You will be able to access to this
subscriber using its name.
topic (str, list[str]): name of the topic(s).
msg_class (object): message class serialization.
Returns:
SubscriberData: the subscriber data holder.
"""
return self.subscriber.create_subscriber(name=name, topic=topic, data_class=msg_class)
def change_topic(self, old_topic, new_topic, new_msg=None, queue_size=None):
"""
Change a publisher's or subscriber's topic name to a new one with possibly a new message class and queue size.
Args:
old_topic (str): old topic name.
new_topic (str): new topic name.
new_msg (object): message class serialization. If None, it will use the same message class than the old
topic.
queue_size (int): The queue size used for asynchronously publishing messages from different threads. A
size of zero means an infinite queue, which can be dangerous. If None, it will use the same queue size
than the old topic.
Returns:
PublisherData, SubscriberData: the publisher data holder.
"""
if self.publisher.has_topic(old_topic):
self.publisher.change_topic(old_topic=old_topic, new_topic=new_topic, new_msg=new_msg,
queue_size=queue_size)
if self.subscriber.has_subscriber(old_topic):
self.subscriber.change_topic(old_topic=old_topic, new_topic=new_topic, new_msg=new_msg)
def get_joint_positions(self, joint_ids=None):
"""
Get the position of the given joint(s).
Args:
joint_ids (int, list[int]): joint id, or list of joint ids.
joint_ids (int, list[int], None): joint id, or list of joint ids.
Returns:
if 1 joint:
@@ -289,7 +309,8 @@ class RobotMiddleWare(object):
np.array[float[N]]: joint positions [rad]
"""
if self.is_subscribing:
return self.subscriber.get_joint_positions(joint_ids)
q_indices = None if joint_ids is None else self.q_indices[joint_ids]
return self.subscriber.get_joint_positions(q_indices)
def set_joint_positions(self, positions, joint_ids=None, velocities=None, kps=None, kds=None, forces=None):
"""
@@ -308,7 +329,7 @@ class RobotMiddleWare(object):
self.publisher.set_joint_positions(positions, q_indices=q_indices)
self.publisher.publish('qpos')
def get_joint_velocities(self, joint_ids):
def get_joint_velocities(self, joint_ids=None):
"""
Get the velocity of the given joint(s).
@@ -322,7 +343,8 @@ class RobotMiddleWare(object):
np.array[float[N]]: joint velocities [rad/s]
"""
if self.is_subscribing:
return self.subscriber.get_joint_velocities(joint_ids)
q_indices = None if joint_ids is None else self.q_indices[joint_ids]
return self.subscriber.get_joint_velocities(q_indices)
def set_joint_velocities(self, velocities, joint_ids=None, max_force=None):
"""
@@ -334,10 +356,11 @@ class RobotMiddleWare(object):
max_force (None, float, np.array[float[N]]): maximum motor forces/torques.
"""
if self.is_publishing:
self.publisher.set_joint_velocities(velocities, q_indices=joint_ids)
q_indices = None if joint_ids is None else self.q_indices[joint_ids]
self.publisher.set_joint_velocities(velocities, q_indices=q_indices)
self.publisher.publish('qvel')
def get_joint_torques(self, joint_ids):
def get_joint_torques(self, joint_ids=None):
"""
Get the applied torque(s) on the given joint(s). "This is the motor torque applied during the last `step`.
Note that this only applies in VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the
@@ -353,7 +376,8 @@ class RobotMiddleWare(object):
np.array[float[N]]: torques associated to the given joints [Nm]
"""
if self.is_subscribing:
return self.subscriber.get_joint_torques(joint_ids)
q_indices = None if joint_ids is None else self.q_indices[joint_ids]
return self.subscriber.get_joint_torques(q_indices)
def set_joint_torques(self, torques, joint_ids=None):
"""
@@ -364,7 +388,8 @@ class RobotMiddleWare(object):
joint_ids (int, list[int], None): joint id, or list of joint ids.
"""
if self.is_publishing:
self.publisher.set_joint_velocities(torques, q_indices=joint_ids)
q_indices = None if joint_ids is None else self.q_indices[joint_ids]
self.publisher.set_joint_velocities(torques, q_indices=q_indices)
self.publisher.publish('torques')
def has_sensor(self, name):
@@ -381,6 +406,18 @@ class RobotMiddleWare(object):
return self.subscriber.has_subscriber(name)
return False
def get_sensor_values(self, name):
"""
Get the sensor values associated with the given sensor name.
Args:
name (str): unique name of the sensor.
Returns:
object, np.array, float, int: sensor values.
"""
pass
def get_pid(self, joint_ids):
"""
Get the PID coefficients associated to the given joint ids.
@@ -404,8 +441,99 @@ class RobotMiddleWare(object):
"""
pass
def get_jacobian(self, link_id, q=None, local_position=None):
"""
Return the full geometric jacobian.
class ROS(MiddleWare):
Args:
link_id (int): link id.
q (np.array[float[N]], None): joint positions of size N, where N is the number of DoFs. If None, it will
compute q based on the current joint positions.
local_position (None, np.array[float[3]]): the point on the specified link to compute the Jacobian (in link
local coordinates around its center of mass). If None, it will use the CoM position (in the link frame).
"""
pass
def get_inertia_matrix(self, q=None):
"""
Return the inertia matrix.
Args:
q (np.array[float[N]], None): joint positions of size N, where N is the total number of DoFs. If None, it
will get the current joint positions.
"""
pass
class DefaultROSRobotMiddleware(ROSRobotMiddleware):
r"""Default ROS robot middleware interface.
This is the default ROS robot middleware interface which can be created when no specific interfaces are provided
by the user. Specific interfaces can be found in the `robots` folder.
Here are the possible combinations between the different values for subscribe (S), publish (P), teleoperate (T),
and command (C):
- S=1, P=0, T=0: subscribes to the topics, and get the messages when calling the corresponding getter methods.
- S=0, P=1, T=0: publishes the various messages to the topics when calling the corresponding setter methods.
- S=1, P=0, T=1, C=0/1: get messages by subscribing to the topics that publish some commands/states. The received
commands/states are then set in the simulator. Depending on the value of `C`, it will subscribe to topics that
publish some commands (C=1) or states (C=0). Example: should we subscribe to joint trajectory commands, or joint
states when teleoperating the robot in the simulator? This C value allows to specify which one we are interested
in.
- S=0, P=1, T=1: when calling the getters methods such as joint positions, velocities, and others, it also
publishes the joint states. This is useful if we are moving/teleoperating the robot in the simulator.
- S=0, P=0, T=1/0: doesn't do anything.
- S=1, P=1, T=0: subscribes to some topics and publish messages to other topics. The messages are can be
sent/received by calling the appropriate getter/setter methods.
- S=1, P=1, T=1: not allowed.
"""
def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, command=True,
control_file=None):
"""
Initialize the robot middleware interface.
Args:
robot_id (int): robot unique id.
urdf (str): path to the URDF file.
subscribe (bool): if True, it will subscribe to the topics associated to the loaded robot, and will read
the values published on these topics.
publish (bool): if True, it will publish the given values to the topics associated to the loaded robot.
teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
previous attributes :attr:`subscribe` and :attr:`publish`.
command (bool): if True, it will subscribe/publish to some (joint) commands. If False, it will
subscribe/publish to some (joint) states.
control_file (str, None): path to the YAML control file.
"""
# set variables
super(DefaultROSRobotMiddleware, self).__init__(robot_id, urdf, subscribe, publish, teleoperate, command,
control_file)
if self.is_publishing:
urdf_parser = URDFParser()
tree = urdf_parser.parse(urdf)
print("ROSRobotMiddleware - publisher - name: ", tree.name)
print("Num joints: ", tree.num_joints)
print("Num actuated joints: ", tree.num_actuated_joints)
self.q_indices = np.zeros(tree.num_joints, dtype=int)
topics = []
count = 0
for i, joint in enumerate(tree.joints.values()):
if joint.dtype != 'fixed':
print("Adding joint {} with type={}".format(joint.name, joint.dtype))
topic = '/' + tree.name + '/joint' + str(count+1) + '_position_controller/command'
self.q_indices[i] = count
count += 1
topics.append(topic)
print("Publishing Topics: ", topics)
publisher = self.publisher.create_publisher(name='qpos', topic=topics, data_class=std_msg.Float64)
self.publisher.init_set_joint_positions(publisher=publisher, msg_attribute_name='data')
# sensors
class ROS(Middleware):
r"""ROS Interface middleware
This middleware class can be given to the simulator which can then interact with the various robots, sensors, and
@@ -442,7 +570,8 @@ class ROS(MiddleWare):
# Note that can also use the middleware alone to access to the various ROS nodes, publishers, subscribers.
"""
def __init__(self, subscribe=False, publish=False, teleoperate=False, master_uri=11311, init_core=True, **kwargs):
def __init__(self, subscribe=False, publish=False, teleoperate=False, command=True, master_uri=11311,
init_core=True, **kwargs):
"""
Initialize the ROS middleware.
@@ -452,10 +581,12 @@ class ROS(MiddleWare):
publish (bool): if True, it will publish the given values to the topics associated to the loaded robots.
teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2
previous attributes :attr:`subscribe` and :attr:`publish`.
command (bool): if True, it will subscribe/publish to some (joint) commands. If False, it will
subscribe/publish to some (joint) states.
master_uri (int): ROS master URI.
init_core (bool): initialize the ROS core if specified.
"""
super(ROS, self).__init__(subscribe=subscribe, publish=publish, teleoperate=teleoperate)
super(ROS, self).__init__(subscribe=subscribe, publish=publish, teleoperate=teleoperate, command=command)
# Environment variable
self.env = os.environ.copy()
@@ -471,7 +602,7 @@ class ROS(MiddleWare):
self.remappers = {}
self.models = []
self._robots = {} # {body_id: RobotMiddleware}
self._robots = {} # {body_id: ROSRobotMiddleware}
self.count_id = -1
# init roslaunch
@@ -816,7 +947,8 @@ class ROS(MiddleWare):
def remap_topic(self, old_topic, new_topic, msg_class, queue_size=10):
"""
Remap a topic name to another topic name.
Remap a old topic to a new topic. Note that this doesn't replace the old topic by the new one; the old topic
will still be present.
Args:
old_topic (str): name of the old topic.
@@ -1176,17 +1308,17 @@ class ROS(MiddleWare):
# Robots #
##########
def get_robot_middleware(self, body_id):
def get_robot_middleware(self, robot_id):
"""
Return the robot middleware associated with the given body id.
Return the robot middleware associated with the given robot id.
Args:
body_id (int): unique body id.
robot_id (int): unique robot id.
Returns:
RobotMiddleware, None: robot middleware corresponding to the given body id. None if it could not find it.
ROSRobotMiddleware, None: robot middleware corresponding to the given body id. None if it could not find it.
"""
return self._robots.get(body_id)
return self._robots.get(robot_id)
def load_urdf(self, urdf):
"""Load the given URDF file.
@@ -1210,9 +1342,12 @@ class ROS(MiddleWare):
id_ = self.count_id
self.count_id += 1
# create robot middleware
robot = RobotMiddleWare(id_, urdf=urdf, subscribe=self.is_subscribing, publish=self.is_publishing,
teleoperate=self.is_teleoperating, control_file=None)
# check if specific robot middleware exists in `robots` folder, and if so load it
# otherwise, create default robot middleware
robot = DefaultROSRobotMiddleware(id_, urdf=urdf, subscribe=self.is_subscribing,
publish=self.is_publishing, teleoperate=self.is_teleoperating,
command=self.is_commanding, control_file=None)
self._robots[id_] = robot
return id_
pyrobolearn/simulators/middlewares/ros_publisher.py
+81 -8
View File
@@ -54,6 +54,7 @@ class PublisherData(object):
# self.__dict__['msg'] = data_class()
self.topic = topic
self.queue_size = queue_size
self.msg_class = data_class
if isinstance(topic, collections.Iterable):
self.is_group = True
@@ -176,8 +177,8 @@ class PublisherData(object):
have no effect.
"""
if isinstance(self.publisher, collections.Iterable):
for subscriber in self.publisher:
subscriber.unregister()
for publisher in self.publisher:
publisher.unregister()
else:
self.publisher.unregister()
@@ -213,12 +214,15 @@ class Publisher(object):
else:
rospy.init_node(self.__class__.__name__ + str(publisher_id))
# all publishers
# all publishers {publisher name: PublisherData}
self.publishers = dict()
# all topics {topic: publisher name}
self.topics_to_publisher_name = dict()
def create_publisher(self, name, topic, data_class, queue_size=10):
"""
Create a publisher to the specific topic.
Create a publisher to the specific topic. If the publisher already exists, it unregister the previous one
and replace it by the new one.
Args:
name (str): unique name of the publisher. The name must be unique. You will be able to access to this
@@ -232,11 +236,27 @@ class Publisher(object):
Returns:
PublisherData: the publisher data holder.
"""
# if the publisher already exists, unregister and remove it
self.remove_publisher(name)
# create new publisher
publisher = PublisherData(topic, data_class, queue_size=queue_size)
self.publishers[name] = publisher
# setattr(self, name, publisher)
self.topics_to_publisher_name[topic] = name
return publisher
def remove_publisher(self, name):
"""
Remove a publisher from the list of inner publishers. This will also unregister it.
Args:
name (str): unique name of the publisher.
"""
if name in self.publishers:
self.unregister(name)
self.publishers.pop(name)
def has_publisher(self, name):
"""
Return True if the given publisher name has been created.
@@ -249,7 +269,7 @@ class Publisher(object):
"""
return name in self.publishers
def get_subscriber(self, name):
def get_publisher(self, name):
"""
Return the associated `PublisherData` given its unique name.
@@ -262,6 +282,59 @@ class Publisher(object):
"""
return self.publishers.get(name)
def has_topic(self, name):
"""
Return True if the given topic is used by the publisher.
Args:
name (str): topic name.
Returns:
bool: True if the given topic name exists.
"""
return name in self.topics_to_publisher_name
def get_publisher_name_from_topic(self, topic_name):
"""
Return the publisher's name associated with the given topic name.
Args:
topic_name (str): topic name.
Returns:
str, None: name of the publisher. None, if no publisher name is associated with the given topic name.
"""
return self.topics_to_publisher_name.get(topic_name)
def change_topic(self, old_topic, new_topic, new_msg=None, queue_size=None):
"""
Change a publisher's topic name to a new one with possibly a new message class and queue size.
Args:
old_topic (str): old topic name.
new_topic (str): new topic name.
new_msg (object): message class serialization. If None, it will use the same message class than the old
topic.
queue_size (int): The queue size used for asynchronously publishing messages from different threads. A
size of zero means an infinite queue, which can be dangerous. If None, it will use the same queue size
than the old topic.
Returns:
PublisherData: the publisher data holder.
"""
if not old_topic in self.topics_to_publisher_name:
raise ValueError("The given 'old_topic' name ({}) doesn't exist in this publisher, are you sure it is the "
"correct topic name?".format(old_topic))
name = self.topics_to_publisher_name[old_topic]
publisher = self.publishers[name]
if new_msg is None:
new_msg = publisher.msg_class
if queue_size is None:
queue_size = publisher.queue_size
return self.create_publisher(name, new_topic, new_msg, queue_size=queue_size)
def publish(self, name=None, data=None, indices=None):
"""
Publish the given data using the given publisher name.
@@ -291,8 +364,8 @@ class Publisher(object):
name (str, None): name of the topic to unsubscribe. If None, it will unsubscribe from all topics.
"""
if name is None:
for subscriber in self.publishers.values():
subscriber.unregister()
for publisher in self.publishers.values():
publisher.unregister()
else:
self.publishers[name].unregister()
pyrobolearn/simulators/middlewares/ros_subscriber.py
+84 -5
View File
@@ -47,6 +47,7 @@ class SubscriberData(object):
# self.subscriber_data = data_class()
self.topic = topic
self.msg_class = data_class
if isinstance(topic, collections.Iterable):
self.is_group = True
self.subscriber = [rospy.Subscriber(t, data_class, callback=self.callback, callback_args=idx)
@@ -138,12 +139,15 @@ class Subscriber(object):
else:
rospy.init_node(self.__class__.__name__ + str(subscriber_id))
# all subscribers
# all subscribers {subscriber name: SubscriberData}
self.subscribers = dict()
# all topics {topic: subscriber name}
self.topics_to_subscriber_name = dict()
def create_subscriber(self, name, topic, data_class):
"""
Create a subscriber to the specific topic.
Create a subscriber to the specific topic. If the subscriber already exists, it unregister the previous one
and replace it by the new one.
Args:
name (str): unique name of the subscriber. The name must be unique. You will be able to access to this
@@ -154,10 +158,26 @@ class Subscriber(object):
Returns:
SubscriberData: the subscriber data holder.
"""
# if the subscriber already exists, unregister and remove it
self.remove_subscriber(name)
# create new subscriber
subscriber = SubscriberData(topic, data_class)
self.subscribers[name] = subscriber
self.topics_to_subscriber_name[topic] = name
return subscriber
def remove_subscriber(self, name):
"""
Remove a subscriber from the list of inner subscribers. This will also unregister it.
Args:
name (str): unique name of the suscriber.
"""
if name in self.subscribers:
self.unregister(name)
self.subscribers.pop(name)
def has_subscriber(self, name):
"""
Return True if the given subscriber name has been created.
@@ -183,6 +203,54 @@ class Subscriber(object):
"""
return self.subscribers.get(name)
def has_topic(self, name):
"""
Return True if the given topic is used by the subscriber.
Args:
name (str): topic name.
Returns:
bool: True if the given topic name exists.
"""
return name in self.topics_to_subscriber_name
def get_subscriber_name_from_topic(self, topic_name):
"""
Return the subscriber's name associated with the given topic name.
Args:
topic_name (str): topic name.
Returns:
str, None: name of the subscriber. None, if no subscriber name is associated with the given topic name.
"""
return self.topics_to_subscriber_name.get(topic_name)
def change_topic(self, old_topic, new_topic, new_msg=None):
"""
Change a subscriber's topic name to a new one with possibly a new message class.
Args:
old_topic (str): old topic name.
new_topic (str): new topic name.
new_msg (object): message class serialization. If None, it will use the same message class than the old
topic.
Returns:
SubscriberData: the subscriber data holder.
"""
if not old_topic in self.topics_to_subscriber_name:
raise ValueError("The given 'old_topic' name ({}) doesn't exist in this subscriber, are you sure it is "
"the correct topic name?".format(old_topic))
name = self.topics_to_subscriber_name[old_topic]
if new_msg is None:
subscriber = self.subscribers[name]
new_msg = subscriber.msg_class
return self.create_subscriber(name, new_topic, new_msg)
# def __getattr__(self, name):
# return self.subscribers[name]
@@ -324,15 +392,26 @@ class RobotSubscriber(Subscriber):
"""
pass
def get_jacobian(self):
def get_jacobian(self, link_id, q=None, local_position=None):
"""
Return the jacobian.
Return the full geometric jacobian.
Args:
link_id (int): link id.
q (np.array[float[N]], None): joint positions of size N, where N is the number of DoFs. If None, it will
compute q based on the current joint positions.
local_position (None, np.array[float[3]]): the point on the specified link to compute the Jacobian (in link
local coordinates around its center of mass). If None, it will use the CoM position (in the link frame).
"""
pass
def get_inertia_matrix(self):
def get_inertia_matrix(self, q=None):
"""
Return the inertia matrix.
Args:
q (np.array[float[N]], None): joint positions of size N, where N is the total number of DoFs. If None, it
will get the current joint positions.
"""
pass
pyrobolearn/simulators/middlewares/yarp_.py
+2 -2
View File
@@ -14,7 +14,7 @@ import signal
import importlib
import inspect
from pyrobolearn.simulators.middlewares.middleware import MiddleWare
from pyrobolearn.simulators.middlewares.middleware import Middleware
__author__ = "Brian Delhaisse"
@@ -27,7 +27,7 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class YARP(MiddleWare):
class YARP(Middleware):
r"""YARP Interface middleware
This middleware can be given to the simulator which can then interact with robots.
pyrobolearn/simulators/simulator.py
+29 -5
View File
@@ -19,7 +19,7 @@ References:
"""
from pyrobolearn.utils.data_structures.orderedset import OrderedSet
from pyrobolearn.simulators.middlewares.middleware import MiddleWare
from pyrobolearn.simulators.middlewares.middleware import Middleware
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
@@ -194,7 +194,7 @@ class Simulator(object):
Args:
render (bool): if True, it will open the GUI, otherwise, it will just run the server.
num_instances (int): number of simulator instances.
middleware (MiddleWare, None): middleware instance.
middleware (Middleware, None): middleware instance.
**kwargs (dict): optional arguments (this is not used here).
"""
self._render = render
@@ -203,7 +203,7 @@ class Simulator(object):
self._num_instances = num_instances
self.middleware = middleware
self._middleware_enabled = True # by default
self._middleware_ids = {} # {simulator_body_id: middleware_body_id}
self._middleware_ids = {} # {simulator_body_id: middleware_robot_id}
# main camera in the simulator
self._camera = None
@@ -258,8 +258,8 @@ class Simulator(object):
@middleware.setter
def middleware(self, middleware):
"""Set the middleware."""
if middleware is not None and not isinstance(middleware, MiddleWare):
raise TypeError("Expecting the given 'middleware' to be an instance of `MiddleWare`, but got instead: "
if middleware is not None and not isinstance(middleware, Middleware):
raise TypeError("Expecting the given 'middleware' to be an instance of `Middleware`, but got instead: "
"{}".format(middleware))
self._middleware = middleware
@@ -438,6 +438,30 @@ class Simulator(object):
"""
self.enable_middleware(enable=False)
def get_middleware(self):
"""
Get the middleware given to the simulator.
Returns:
Middleware, None: Middleware.
"""
return self.middleware
def get_robot_middleware(self, robot_id):
"""
Get the robot middleware associated with the given robot id.
Args:
robot_id (int): unique robot id.
Returns:
RobotMiddleware, None: robot middleware associated with the given robot id.
"""
if self._middleware is not None:
robot_id = self._middleware_ids.get(robot_id)
if robot_id is not None:
return self._middleware.get_robot_middleware(robot_id)
# Simulators
def reset(self, *args, **kwargs):