diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py index 02a3ee2..2792867 100644 --- a/pyrobolearn/robots/robot.py +++ b/pyrobolearn/robots/robot.py @@ -1141,11 +1141,9 @@ class Robot(ControllableBody): if multiple links: str[N]: link names """ - if isinstance(link_ids, int): - return self.sim.get_joint_info(self.id, link_ids)[12] if link_ids is None: link_ids = self.joints - return [self.sim.get_joint_info(self.id, link)[12] for link in link_ids] + return self.sim.get_link_names(self.id, link_ids) def get_link_masses(self, link_ids=None): r""" @@ -1896,10 +1894,10 @@ class Robot(ControllableBody): based on the current joint positions. Returns: - np.array[6,6,N]: CoM Jacobian + np.array[6,N]: CoM Jacobian References: - [1] "Whole-body cooperative balancing of humanoid robot using COG Jacobian", Sugihara et al., IROS, 2002 + - [1] "Whole-body cooperative balancing of humanoid robot using COG Jacobian", Sugihara et al., IROS, 2002 """ # Get current joint position if q is None: diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py index 312a733..06fab1a 100644 --- a/pyrobolearn/simulators/bullet.py +++ b/pyrobolearn/simulators/bullet.py @@ -13,9 +13,9 @@ Dependencies in PRL: * `pyrobolearn.simulators.simulator.Simulator` References: - [1] PyBullet: https://pybullet.org - [2] PyBullet Quickstart Guide: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA - [3] PEP8: https://www.python.org/dev/peps/pep-0008/ + - [1] PyBullet: https://pybullet.org + - [2] PyBullet Quickstart Guide: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA + - [3] PEP8: https://www.python.org/dev/peps/pep-0008/ """ # general imports @@ -74,9 +74,9 @@ class Bullet(Simulator): sim = Bullet() References: - [1] "PyBullet, a Python module for physics simulation for games, robotics and machine learning", Erwin Coumans - and Yunfei Bai, 2016-2019 - [1] PyBullet Quickstart Guide: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA + - [1] "PyBullet, a Python module for physics simulation for games, robotics and machine learning", Erwin + Coumans and Yunfei Bai, 2016-2019 + - [2] PyBullet Quickstart Guide: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA Erwin Coumans and Yunfei Bai, 2017/2018 """ @@ -228,6 +228,15 @@ class Bullet(Simulator): memo[self] = sim return sim + ################## + # Static methods # + ################## + + @staticmethod + def simulate_soft_bodies(): + """Return True if the simulator can simulate soft bodies.""" + return True + ########### # Methods # ########### @@ -1728,8 +1737,17 @@ class Bullet(Simulator): str[N]: link names """ if isinstance(link_ids, int): + if link_ids == -1: + return self.get_base_name(body_id) return self.sim.getJointInfo(body_id, link_ids)[12] - return [self.sim.getJointInfo(body_id, link_id)[12] for link_id in link_ids] + + link_names = [] + for link_id in link_ids: + if link_id == -1: + link_names.append(self.get_base_name(body_id)) + else: + link_names.append(self.sim.getJointInfo(body_id, link_id)[12]) + return link_names def get_link_masses(self, body_id, link_ids): """ diff --git a/pyrobolearn/simulators/bullet_ros.py b/pyrobolearn/simulators/bullet_ros.py index fe7093e..947da87 100644 --- a/pyrobolearn/simulators/bullet_ros.py +++ b/pyrobolearn/simulators/bullet_ros.py @@ -22,7 +22,8 @@ References: - [4] PEP8: https://www.python.org/dev/peps/pep-0008/ """ -# TODO +# TODO: finish this interface and move ROS stuffs to ros.py + import os import subprocess import psutil @@ -93,6 +94,33 @@ class BulletROS(Bullet): # , ROS): self.subscribers = {} self.publishers = {} + ############## + # Properties # + ############## + + @property + def is_subscribing(self): + """Return True if we are subscribing to topics.""" + return self.subscribe + + @property + def is_publishing(self): + """Return True if we are publishing to topics.""" + return self.publish + + ################## + # Static methods # + ################## + + @staticmethod + def has_middleware_communication_layer(): + """Return True if the simulator has a middleware communication layer (like ROS, YARP, etc).""" + return True + + ########### + # Methods # + ########### + def close(self): """ Close everything @@ -108,16 +136,6 @@ class BulletROS(Bullet): # , ROS): # call parent destructor super(BulletROS, self).close() - @property - def is_subscribing(self): - """Return True if we are subscribing to topics.""" - return self.subscribe - - @property - def is_publishing(self): - """Return True if we are publishing to topics.""" - return self.publish - def load_urdf(self, filename, position=None, orientation=None, use_maximal_coordinates=None, use_fixed_base=None, flags=None, scale=None): """Load the given URDF file. diff --git a/pyrobolearn/simulators/rbdl_.py b/pyrobolearn/simulators/rbdl_.py new file mode 100644 index 0000000..5dd3acc --- /dev/null +++ b/pyrobolearn/simulators/rbdl_.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python +"""Python wrapper around RBDL + +The signature of each method defined here are inspired by the `Robot` class in PyRoboLearn and [1] but in accordance +with the PEP8 style guide [2]. Most of the documentation for the methods have been copied-pasted from [1] for +completeness purposes. + +References: + - [1] RBDL: + - Webpage (with documentation): https://rbdl.bitbucket.io/ + - Bitbucket repository: https://bitbucket.org/rbdl/rbdl/ + - [2] PEP8: https://www.python.org/dev/peps/pep-0008/ +""" + +import collections +import rbdl + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Martin Felis (martin@fysx.org)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class RBDL(object): + r"""RBDL Interface. + + References: + - [1] RBDL: + - Webpage (with documentation): https://rbdl.bitbucket.io/ + - Bitbucket repository: https://bitbucket.org/rbdl/rbdl/ + - [2] PEP8: https://www.python.org/dev/peps/pep-0008/ + """ + + def __init__(self, filename=None, verbose=False, floating_base=False): + self.model = None + self.load_model(filename, verbose=verbose, floating_base=floating_base) + + def load_model(self, filename, verbose=False, floating_base=False): + """ + Load the given URDF model. + + Args: + filename (str): path to the urdf model. + verbose (bool): if True, it will output + floating_base (bool): if True, the model will be considered to have a floating base, thus 6 more DoFs will + be added. + """ + self.model = rbdl.loadModel(filename, verbose=verbose, floating_base=floating_base) + + # alias + load_urdf = load_model + + @property + def num_dofs(self): + """Return the number of degrees of freedom (DoFs); that is, if the base is not fixed, 6 (= 3 degrees for + translation + 3 degrees for orientation) + the joints that are not fixed. + """ + return self.model.dof_count + + # alias + dof_count = num_dofs + + @property + def gravity(self): + """Return the Cartesian gravity vector applied on the model.""" + return self.model.gravity + + @property + def num_joints(self): + """Return the number of joints: ROOT + Num DoFs""" + return len(self.model.mJoints) + + @property + def num_fixed_links(self): + """Return the number of fixed links.""" + return len(self.model.mFixedBodies) + + @property + def num_links(self): + """Return the number of links.""" + return len(self.model.mBodies) + + @property + def num_actuated_joints(self): + """Return the number of actuated joints.""" + return self.model.q_size + + @property + def fixed_joints(self): + """Return the list of fixed joints""" + return [] # TODO + + def get_link_names(self, link_ids): + """Return the link names.""" + if isinstance(link_ids, collections.Iterable): + return [self.model.GetBodyName(link_id) for link_id in link_ids] + return self.model.GetBodyName(link_ids) + + +# Test +if __name__ == '__main__': + import os + import pyrobolearn as prl + + fixed_base = False + + robot = prl.robots.HyQ2Max(prl.simulators.Bullet(render=False), fixed_base=fixed_base) + rbdl_ = RBDL(os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/hyq2max/hyq2max.urdf', + floating_base=not fixed_base) + + robots = [robot, rbdl_] + + def print_attr(msg, attr, *args, **kwargs): + for rob in robots: + a = getattr(rob, attr) + if callable(a): + print(msg.format(a(*args, **kwargs))) + print(msg.format(a)) + + print_attr("Num DoFs: {}", 'num_dofs') + print_attr("Num of actuated joints: {}", 'num_actuated_joints') + print_attr("Num of joints: {}", 'num_joints') + print_attr("Num of links: {}", 'num_links') + + print(robot.get_link_names([-1] + range(robot.num_links))) + print(rbdl_.get_link_names(range(rbdl_.num_links))) + + print("Num fixed links: {}".format(robot.num_links - robot.num_actuated_joints)) + print("Num fixed links: {}".format(rbdl_.num_fixed_links)) diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index 1fa6be6..7fc2765 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -262,6 +262,35 @@ class Simulator(object): memo[self] = sim return sim + ################## + # Static methods # + ################## + + @staticmethod + def simulate_gas_dynamics(): + """Return True if the simulator can simulate gases.""" + return False + + @staticmethod + def simulate_liquid_dynamics(): + """Return True if the simulator can simulate liquids.""" + return False + + @staticmethod + def simulate_fluid_dynamics(): + """Return True if the simulator can simulate fluids (gases and liquids).""" + return Simulator.simulate_gas_dynamics() and Simulator.simulate_liquid_dynamics() + + @staticmethod + def simulate_soft_bodies(): + """Return True if the simulator can simulate soft bodies.""" + return False + + @staticmethod + def has_middleware_communication_layer(): + """Return True if the simulator has a middleware communication layer (like ROS, YARP, etc).""" + return False + ########### # Methods # ########### diff --git a/pyrobolearn/states/robot_states/robot_states.py b/pyrobolearn/states/robot_states/robot_states.py index 2e47ad6..0a73e3d 100644 --- a/pyrobolearn/states/robot_states/robot_states.py +++ b/pyrobolearn/states/robot_states/robot_states.py @@ -182,6 +182,40 @@ class BaseOrientationState(RobotState): self.data = self.robot.get_base_orientation() +class BasePoseState(RobotState): + r"""Base pose state + + This is the state that returns the base position [x,y,z] and orientation (expressed as a quaternion [x,y,z,w]) + with respect to the world frame. + """ + + def __init__(self, robot, window_size=1, axis=None, ticks=1): + """ + Initialize the base pose state. + + Args: + robot (Robot): instance of Robot which allows to access to the robot state + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(BasePoseState, self).__init__(robot, window_size=window_size, axis=axis, ticks=ticks) + + def _read(self): + """Read the base position state data.""" + self.data = self.robot.get_base_pose() + + class BaseLinearVelocityState(RobotState): r"""Base linear velocity state @@ -246,3 +280,37 @@ class BaseAngularVelocityState(RobotState): def _read(self): """Read the base angular velocity state data.""" self.data = self.robot.get_base_angular_velocity() + + +class BaseVelocityState(RobotState): + r"""Base velocity state + + This is the state that returns the base linear [vx, vy, vz] and angular velocity [wx, wy, wz] with respect to the + world frame. + """ + + def __init__(self, robot, window_size=1, axis=None, ticks=1): + """ + Initialize the base velocity state. + + Args: + robot (Robot): instance of Robot which allows to access to the robot state + window_size (int): window size of the state. This is the total number of states we should remember. That + is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`, + the window size is 2. By default, the :attr:`window_size` is one which means we only remember the + current state. The window size has to be bigger than 1. If it is below, it will be set automatically + to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states, + but is given some :attr:`data`. + axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with + shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting + state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack + the states in the specified axis. With the example, for axis=0, the resulting state has a shape of + (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the + state is not a combination of states, but is given some :attr:`data`. + ticks (int): number of ticks to sleep before getting the next state data. + """ + super(BaseVelocityState, self).__init__(robot, window_size=window_size, axis=axis, ticks=ticks) + + def _read(self): + """Read the base linear velocity state data.""" + self.data = self.robot.get_base_velocity() diff --git a/pyrobolearn/tools/interfaces/interface.py b/pyrobolearn/tools/interfaces/interface.py index fbd097a..4f552c2 100644 --- a/pyrobolearn/tools/interfaces/interface.py +++ b/pyrobolearn/tools/interfaces/interface.py @@ -94,6 +94,8 @@ class Interface(object): if self.use_thread: while True: if self.stop_thread: # if the thread should stop + if self.verbose: + print("Stopping the thread") break self.run(*args, **kwargs) time.sleep(self.dt) @@ -112,6 +114,8 @@ class Interface(object): Stop and close the interface. """ if self.use_thread: + if self.verbose: + print("Asking for thread to stop...") self.stop_thread = True ############# diff --git a/pyrobolearn/tools/interfaces/phones/__init__.py b/pyrobolearn/tools/interfaces/phones/__init__.py new file mode 100644 index 0000000..40930cc --- /dev/null +++ b/pyrobolearn/tools/interfaces/phones/__init__.py @@ -0,0 +1,3 @@ + +# import the abstract phone interface +from .phone import PhoneInterface # , TabletInterface diff --git a/pyrobolearn/tools/interfaces/phones/android.py b/pyrobolearn/tools/interfaces/phones/android.py new file mode 100644 index 0000000..7b9a6a8 --- /dev/null +++ b/pyrobolearn/tools/interfaces/phones/android.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python +"""Define the android interface. +""" + +# TODO: finish this interface +# TODO: implement android application (Kotlin or Java) + +from pyrobolearn.tools.interfaces.phones import PhoneInterface + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class AndroidInterface(PhoneInterface): + r"""Android phone/tablet interface + """ + + def __init__(self, use_thread=False, sleep_dt=0, verbose=False): + """ + Initialize the android interface. + + Args: + use_thread (bool): If True, it will run the interface in a separate thread than the main one. + The interface will update its data automatically. + sleep_dt (float): If :attr:`use_thread` is True, it will sleep the specified amount before acquiring / + setting the next sample. + verbose (bool): If True, it will print information about the state of the interface. This is let to the + programmer what he / she wishes to print. + """ + super(AndroidInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose) diff --git a/pyrobolearn/tools/interfaces/phones/iphone.py b/pyrobolearn/tools/interfaces/phones/iphone.py new file mode 100644 index 0000000..6d77578 --- /dev/null +++ b/pyrobolearn/tools/interfaces/phones/iphone.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python +"""Define the iPhone interface. +""" + +# TODO: finish this interface +# TODO: implement iphone application (Swift) + +from pyrobolearn.tools.interfaces.phones import PhoneInterface + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class IPhoneInterface(PhoneInterface): + r"""IPhone / IPad interface + """ + + def __init__(self, use_thread=False, sleep_dt=0, verbose=False): + """ + Initialize the iphone interface. + + Args: + use_thread (bool): If True, it will run the interface in a separate thread than the main one. + The interface will update its data automatically. + sleep_dt (float): If :attr:`use_thread` is True, it will sleep the specified amount before acquiring / + setting the next sample. + verbose (bool): If True, it will print information about the state of the interface. This is let to the + programmer what he / she wishes to print. + """ + super(IPhoneInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose) diff --git a/pyrobolearn/tools/interfaces/phones/phone.py b/pyrobolearn/tools/interfaces/phones/phone.py new file mode 100644 index 0000000..057ea7e --- /dev/null +++ b/pyrobolearn/tools/interfaces/phones/phone.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python +"""Define the smartphone and tablet interface. + +These interfaces allows you to get sensor values sent with your phone or tablet, and values returned by the +corresponding (Android / IPhone) application. +""" + +from pyrobolearn.tools.interfaces import InputOutputInterface + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +# TODO: define common interface for phone and tablet? + +class PhoneInterface(InputOutputInterface): + r"""Smart-Phone Interface + """ + + def __init__(self, use_thread=False, sleep_dt=0, verbose=False): + """ + Initialize the smartphone interface. + + Args: + use_thread (bool): If True, it will run the interface in a separate thread than the main one. + The interface will update its data automatically. + sleep_dt (float): If :attr:`use_thread` is True, it will sleep the specified amount before acquiring / + setting the next sample. + verbose (bool): If True, it will print information about the state of the interface. This is let to the + programmer what he / she wishes to print. + """ + super(PhoneInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose) + + +# class TabletInterface(InputOutputInterface): +# r"""Tablet Interface +# """ +# +# def __init__(self, use_thread=False, sleep_dt=0, verbose=False): +# """ +# Initialize the tablet interface. +# +# Args: +# use_thread (bool): If True, it will run the interface in a separate thread than the main one. +# The interface will update its data automatically. +# sleep_dt (float): If :attr:`use_thread` is True, it will sleep the specified amount before acquiring / +# setting the next sample. +# verbose (bool): If True, it will print information about the state of the interface. This is let to the +# programmer what he / she wishes to print. +# """ +# super(TabletInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose) diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py index 6a0f22c..9cff1f2 100644 --- a/pyrobolearn/worlds/world.py +++ b/pyrobolearn/worlds/world.py @@ -1335,7 +1335,7 @@ class World(object): position (float[3]): position of the box in the Cartesian world space (in meters) orientation (float[4]): orientation of the box using quaternion [x,y,z,w]. mass (float): mass of the box (in kg). If mass = 0, the box won't move even if there is a collision. - dimensions (float[3]): dimensions of the box + dimensions (float[3]): dimensions of the box (in meter) color (int[4], None): color of the box for red, green, blue, and alpha, each in range [0,1] return_body (bool): if True, it will return an instance of the `Body`, otherwise, it will return the unique id.