From 134f69aeea4faade5a591bfba73a3757909d38a4 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Mon, 21 Oct 2019 07:04:15 +0200 Subject: [PATCH] update robot middlewares + imitation --- examples/imitation/demo.py | 77 ++++ pyrobolearn/__init__.py | 3 + pyrobolearn/models/dmp/dmp.py | 111 ++++-- pyrobolearn/models/gaussian.py | 22 +- pyrobolearn/policies/dmp_policy.py | 13 + pyrobolearn/robots/franka.py | 6 + pyrobolearn/robots/robot.py | 25 +- .../simulators/middlewares/__init__.py | 2 +- .../simulators/middlewares/middleware.py | 29 +- .../middlewares/robot_middleware.py | 242 ++++++++++++ .../simulators/middlewares/robots/__init__.py | 0 .../simulators/middlewares/robots/franka.py | 217 +++++++++++ pyrobolearn/simulators/middlewares/ros.py | 353 ++++++++++++------ .../simulators/middlewares/ros_publisher.py | 89 ++++- .../simulators/middlewares/ros_subscriber.py | 89 ++++- pyrobolearn/simulators/middlewares/yarp_.py | 4 +- pyrobolearn/simulators/simulator.py | 34 +- 17 files changed, 1134 insertions(+), 182 deletions(-) create mode 100644 examples/imitation/demo.py create mode 100644 pyrobolearn/simulators/middlewares/robot_middleware.py create mode 100644 pyrobolearn/simulators/middlewares/robots/__init__.py create mode 100644 pyrobolearn/simulators/middlewares/robots/franka.py diff --git a/examples/imitation/demo.py b/examples/imitation/demo.py new file mode 100644 index 0000000..5c41b56 --- /dev/null +++ b/examples/imitation/demo.py @@ -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!") diff --git a/pyrobolearn/__init__.py b/pyrobolearn/__init__.py index 2ecf378..a6efdb4 100644 --- a/pyrobolearn/__init__.py +++ b/pyrobolearn/__init__.py @@ -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 diff --git a/pyrobolearn/models/dmp/dmp.py b/pyrobolearn/models/dmp/dmp.py index d990d33..040d96c 100644 --- a/pyrobolearn/models/dmp/dmp.py +++ b/pyrobolearn/models/dmp/dmp.py @@ -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. diff --git a/pyrobolearn/models/gaussian.py b/pyrobolearn/models/gaussian.py index 7421e5f..6b1d5f8 100755 --- a/pyrobolearn/models/gaussian.py +++ b/pyrobolearn/models/gaussian.py @@ -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) diff --git a/pyrobolearn/policies/dmp_policy.py b/pyrobolearn/policies/dmp_policy.py index d77471e..d04508d 100644 --- a/pyrobolearn/policies/dmp_policy.py +++ b/pyrobolearn/policies/dmp_policy.py @@ -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 diff --git a/pyrobolearn/robots/franka.py b/pyrobolearn/robots/franka.py index df6067c..d9dd89c 100644 --- a/pyrobolearn/robots/franka.py +++ b/pyrobolearn/robots/franka.py @@ -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 diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py index ff82dbd..6560ba8 100644 --- a/pyrobolearn/robots/robot.py +++ b/pyrobolearn/robots/robot.py @@ -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: diff --git a/pyrobolearn/simulators/middlewares/__init__.py b/pyrobolearn/simulators/middlewares/__init__.py index 74bc85b..8c0835a 100644 --- a/pyrobolearn/simulators/middlewares/__init__.py +++ b/pyrobolearn/simulators/middlewares/__init__.py @@ -1,7 +1,7 @@ # -*- coding: utf-8 -*- # import middleware abstract class -from .middleware import MiddleWare +from .middleware import Middleware # import ROS try: diff --git a/pyrobolearn/simulators/middlewares/middleware.py b/pyrobolearn/simulators/middlewares/middleware.py index 939ee39..d6994f3 100644 --- a/pyrobolearn/simulators/middlewares/middleware.py +++ b/pyrobolearn/simulators/middlewares/middleware.py @@ -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. diff --git a/pyrobolearn/simulators/middlewares/robot_middleware.py b/pyrobolearn/simulators/middlewares/robot_middleware.py new file mode 100644 index 0000000..4d42873 --- /dev/null +++ b/pyrobolearn/simulators/middlewares/robot_middleware.py @@ -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 diff --git a/pyrobolearn/simulators/middlewares/robots/__init__.py b/pyrobolearn/simulators/middlewares/robots/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/simulators/middlewares/robots/franka.py b/pyrobolearn/simulators/middlewares/robots/franka.py new file mode 100644 index 0000000..745da42 --- /dev/null +++ b/pyrobolearn/simulators/middlewares/robots/franka.py @@ -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 diff --git a/pyrobolearn/simulators/middlewares/ros.py b/pyrobolearn/simulators/middlewares/ros.py index de8b4d6..803d14c 100644 --- a/pyrobolearn/simulators/middlewares/ros.py +++ b/pyrobolearn/simulators/middlewares/ros.py @@ -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.urdf dirname = str(os.path.dirname(path)) # /path/to/pyrobolearn/robots/urdfs// 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_ diff --git a/pyrobolearn/simulators/middlewares/ros_publisher.py b/pyrobolearn/simulators/middlewares/ros_publisher.py index 36d3928..9ec485c 100644 --- a/pyrobolearn/simulators/middlewares/ros_publisher.py +++ b/pyrobolearn/simulators/middlewares/ros_publisher.py @@ -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() diff --git a/pyrobolearn/simulators/middlewares/ros_subscriber.py b/pyrobolearn/simulators/middlewares/ros_subscriber.py index 459cb3b..6af7f98 100644 --- a/pyrobolearn/simulators/middlewares/ros_subscriber.py +++ b/pyrobolearn/simulators/middlewares/ros_subscriber.py @@ -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 diff --git a/pyrobolearn/simulators/middlewares/yarp_.py b/pyrobolearn/simulators/middlewares/yarp_.py index 4f07f4f..da9b8c7 100644 --- a/pyrobolearn/simulators/middlewares/yarp_.py +++ b/pyrobolearn/simulators/middlewares/yarp_.py @@ -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. diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index e3967b8..bfa7342 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -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):