diff --git a/README.md b/README.md index 8dc46cc..4564a39 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,8 @@ This repository contains the code for the *PyRoboLearn* (PRL) framework: a Python framework for Robot Learning. This framework revolves mainly around 7 axes: simulators, worlds, robots, interfaces, learning tasks (= environment and policy), learning models, and learning algorithms. +This development of this framework is ongoing. + ## Requirements The framework has been tested with Python 2.7 and Ubuntu 16.04 and 18.04. We also tested parts of it with Python 3.5 on Ubuntu 16.04 and so far so good, but there might be some errors that escaped me. diff --git a/pyrobolearn/physics/README.md b/pyrobolearn/physics/README.md new file mode 100644 index 0000000..32639f4 --- /dev/null +++ b/pyrobolearn/physics/README.md @@ -0,0 +1,9 @@ +## Physics randomizer + +This folder provides physics randomizers which randomizes the dynamical attributes / properties of an object. +For instance, it can randomize the mass or inertial matrix of a link, the bounciness of an object, the gravity of +the world, the contact friction coefficients of the floor and various links, and so on. + +Note that physics randomizer instances have access to the simulator in order to modify the physical properties. +Also, note that normally the physics randomizer is called at the beginning of an episode, and not at each time +step. Changing the physical properties at each time step can lead to weird behaviors in the simulator. diff --git a/pyrobolearn/physics/__init__.py b/pyrobolearn/physics/__init__.py new file mode 100644 index 0000000..3c63597 --- /dev/null +++ b/pyrobolearn/physics/__init__.py @@ -0,0 +1,18 @@ + +# import physics +from .physics_randomizer import * + +# import world physics randomizer +from .world_physics_randomizer import * + +# import body physics randomizer +from .body_physics_randomizer import * + +# import link physics randomizer +from .link_physics_randomizer import * + +# import joint physics randomizer +from .joint_physics_randomizer import * + +# import robot physics randomizer +from .robot_physics_randomizer import * diff --git a/pyrobolearn/physics/body_physics_randomizer.py b/pyrobolearn/physics/body_physics_randomizer.py new file mode 100644 index 0000000..03fc41a --- /dev/null +++ b/pyrobolearn/physics/body_physics_randomizer.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python +"""Define the `BodyPhysicsRandomizer` class which randomizes the physical attributes / properties of a body. + +Dependencies: +- `pyrobolearn.physics` +- `pyrobolearn.robots` +""" + +from pyrobolearn.physics.physics_randomizer import PhysicsRandomizer +# from pyrobolearn.robots.base import Object # TODO: change to Body or MultiBody + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "MIT" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class BodyPhysicsRandomizer(PhysicsRandomizer): + r"""Body Physics Randomizer + + The body physics randomizer can randomize the physical attributes of a body. It is an abstract class which is + inherited notably by `LinkPhysicsRandomizer` and `JointPhysicsRandomizer`. + """ + + def __init__(self, body): + """ + Initialize the body physics randomizer. + + Args: + body (Body): multi-body object. + """ + self.body = body + simulator = self.body.sim + super(BodyPhysicsRandomizer, self).__init__(simulator) + + ############## + # Properties # + ############## + + @property + def body(self): + """Return the body / object instance.""" + return self._body + + @body.setter + def body(self, body): + """Set the body / object instance.""" + # TODO: uncomment the following lines + # if not isinstance(body, Object): + # raise TypeError("Expecting the given body to be an instance of `Object`, instead got: " + # "{}".format(type(body))) + self._body = body + + @property + def num_links(self): + """Return the number of links of the body.""" + return self.body.num_links + + @property + def num_joints(self): + """Return the number of joints of the body.""" + return self.body.num_joints diff --git a/pyrobolearn/physics/joint_physics_randomizer.py b/pyrobolearn/physics/joint_physics_randomizer.py new file mode 100644 index 0000000..554ff3f --- /dev/null +++ b/pyrobolearn/physics/joint_physics_randomizer.py @@ -0,0 +1,118 @@ +#!/usr/bin/env python +"""Define the `JointPhysicsRandomizer` class which randomizes the physical attributes / properties of a joint or +multiple joints of a specific body. + +Dependencies: +- `pyrobolearn.physics` +""" + +import collections + +from pyrobolearn.physics.body_physics_randomizer import BodyPhysicsRandomizer + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "MIT" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class JointPhysicsRandomizer(BodyPhysicsRandomizer): + r"""Joint Physics Randomizer + + The joint physics randomizer can randomize the physical attributes of a joint. For instance, this can be the + joint friction or damping coefficients. Other attributes can be the maximum force or velocity the joint(s) can + achieve. + """ + + def __init__(self, body, joint_ids=None, joint_damping=None, **kwargs): + """ + Initialize the joint physics randomizer. + + Args: + body (Body): multi-body object. + joint_ids (int, list of int, None): joint id(s). + joint_damping (float, list of float, tuple of float, list of tuple of float, None): joint damping + coefficient. If None, it will take the default joint damping value associated with the given + `joint_ids` of the given `body`. If float, it will set that value to the specified joints and will + always return this value when sampling. If list of float, it will set each value to each joint and will + always return these values when sampling. If tuple of float, the first item is the lower bound of the + joint damping and the second item is its upper bound. It will set these bounds for each joint. If list + of tuples of joints, it will have a tuple of lower / upper bound for each joint. + **kwargs (dict): range of possible physical properties. If given one value, that property won't be + randomized. Each range is a tuple of two values `[lower_bound, upper_bound]`. + """ + super(JointPhysicsRandomizer, self).__init__(body) + self.joints = joint_ids + + ############## + # Properties # + ############## + + @property + def joints(self): + """Return the list of joint ids.""" + return self._joints + + @joints.setter + def joints(self, joints): + """Set the joint id or the list of joint ids.""" + if joints is None: + joints = self.body.joints + elif isinstance(joints, int): + joints = [joints] + elif isinstance(joints, collections.Iterable): + for idx, joint in enumerate(joints): + if not isinstance(joint, int): + raise TypeError("The {} element of the given list of joints is not an integer, instead got: " + "{}".format(idx, type(joint))) + else: + raise TypeError("Expecting the given joints to be an integer or a list of integers, instead got: " + "{}".format(type(joints))) + self._joints = joints + + @property + def joint_dampings(self): + """Return the joint dampings associated with the joints.""" + return self.body.get_joint_dampings(self.joints) + + @joint_dampings.setter + def joint_dampings(self, values): + """Set the given joint damping values to each joint.""" + for joint, value in zip(self.joints, values): + self.body.set_joint_damping(joint, value) + + ########### + # Methods # + ########### + + def names(self): + """Return an iterator over the property names.""" + for name in ['joint_damping']: + yield name + + def bounds(self): + """Return an iterator over the bounds for each property.""" + pass + + def get_properties(self): + """ + Get the physics properties. + + Returns: + dict: current physic property values. + """ + pass + + def set_properties(self, properties): + """ + Set the given physic property values using the simulator. + + Args: + properties (dict): the physic property values to be set in the simulator. + """ + pass diff --git a/pyrobolearn/physics/link_physics_randomizer.py b/pyrobolearn/physics/link_physics_randomizer.py new file mode 100644 index 0000000..5dabe35 --- /dev/null +++ b/pyrobolearn/physics/link_physics_randomizer.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python +"""Define the `LinkPhysicsRandomizer` class which randomizes the physical attributes / properties of a link or +multiple links of a specific body. + +Dependencies: +- `pyrobolearn.physics` +""" + +import collections + +from pyrobolearn.physics.body_physics_randomizer import BodyPhysicsRandomizer + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "MIT" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class LinkPhysicsRandomizer(BodyPhysicsRandomizer): + r"""Link Physics Randomizer + + The link physics randomizer can randomize the physical attributes of a link. + """ + + def __init__(self, body, link_ids=None, masses=None, local_inertia_diagonals=None, local_inertia_positions=None, + local_inertia_orientations=None, lateral_frictions=None, spinning_frictions=None, + rolling_frictions=None, restitutions=None, linear_dampings=None, angular_dampings=None, + contact_stiffnesses=None, contact_dampings=None, **kwargs): + """ + Initialize the link physics randomizer. + + Args: + body (Body): multi-body object. + link_ids (int, list of int, None): link id(s). + **kwargs (dict): range of possible physical properties. If given one value, that property won't be + randomized. Each range is a tuple of two values `[lower_bound, upper_bound]`. + """ + super(LinkPhysicsRandomizer, self).__init__(body) + self.links = link_ids + + # set the bounds + + ############## + # Properties # + ############## + + @property + def links(self): + """Return the list of link ids.""" + return self._links + + @links.setter + def links(self, links): + """Set the link id or the list of link ids.""" + if isinstance(links, int): + links = [links] + elif isinstance(links, collections.Iterable): + for idx, link in enumerate(links): + if not isinstance(link, int): + raise TypeError("The {} element of the given list of links is not an integer, instead got: " + "{}".format(idx, type(link))) + else: + raise TypeError("Expecting the given links to be an integer or a list of integers, instead got: " + "{}".format(type(links))) + self._links = links + + @property + def masses(self): + """Return the mass of each specified link.""" + return self.body.get_masses(self.links) + + @masses.setter + def masses(self, values): + """Set the mass values.""" + self.body.set_masses(self.links, values) + + @property + def mass_bounds(self): + """Return the lower and upper bounds of each link mass.""" + return self._mass_bounds + + @mass_bounds.setter + def mass_bounds(self, bounds): + """Set the mass bound for each link.""" + if isinstance(bounds, (float, int)): + bounds = [(bounds, bounds) for _ in self.links] + elif isinstance(bounds, (list, tuple, np.ndarray)): + pass + self._mass_bounds = bounds + + @property + def dynamics(self): + return None + + ########### + # Methods # + ########### + + def names(self): + """Return an iterator over the property names.""" + for name in ['mass']: + yield name + + def bounds(self): + """Return an iterator over the bounds for each property.""" + yield self.mass_bounds + + def get_properties(self): + """ + Get the physics properties. + + Returns: + dict: current physic property values. + """ + properties = dict() + # properties['mass'] = + return properties + + def set_properties(self, properties): + """ + Set the given physic property values using the simulator. + + Args: + properties (dict): the physic property values to be set in the simulator. + """ + if not isinstance(properties, dict): + raise TypeError("Expecting the given 'properties' to be a dictionary, instead got: " + "{}".format(type(properties))) + diff --git a/pyrobolearn/physics/physics_randomizer.py b/pyrobolearn/physics/physics_randomizer.py new file mode 100644 index 0000000..52f5cba --- /dev/null +++ b/pyrobolearn/physics/physics_randomizer.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python +"""Define the `PhysicsRandomizer` class which randomizes the physical attributes / properties of an object. + +This is the main abstract class from which all physics randomizers inherit from. + +Dependencies: +- `pyrobolearn.simulators` +""" + +import numpy as np + +from pyrobolearn.simulators import Simulator + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "MIT" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class PhysicsRandomizer(object): + r"""Physics Randomizer + + This the main abstract class from which all the physics randomizers inherit from. A physics randomizer randomize + the physics properties of a certain object. This can be, for instance, the world, a robot, or a particular link + of that robot. + + It can change for example the dynamics of a particular object such as the mass, inertia, or others. It can also + change their physical properties such as the friction, bounciness, etc. + + Note that the physics randomizer instance has access to the simulator in order to modify the physical properties. + Also, note that normally the physics randomizer is called at the beginning of an episode, and not at each time + step. Changing the physical properties at each time step can lead to weird behaviors. + + It is possible to not randomize some physical properties by specifying a specific value instead of a range (=tuple + of 2 values; lower and upper bound). + """ + + def __init__(self, simulator): + """ + Initialize the physics randomizer. + + Args: + simulator (Simulator): simulator instance + """ + self.simulator = simulator + + ############## + # Properties # + ############## + + @property + def simulator(self): + """Return the simulator instance.""" + return self._simulator + + @simulator.setter + def simulator(self, simulator): + """Set the simulator instance.""" + # TODO: uncomment the following lines + # if not isinstance(simulator, Simulator): + # raise TypeError("Expecting the given simulator to be an instance of `Simulator`, instead got: " + # "{}".format(type(simulator))) + self._simulator = simulator + + ########### + # Methods # + ########### + + def properties(self): + """Return an iterator over the properties.""" + properties = self.get_properties() + for p in properties.values(): + yield p + + def named_properties(self): + """Return an iterator over the properties with their name and value""" + properties = self.get_properties() + for name, p in properties.items(): + yield name, p + + def names(self): + """Return an iterator over the property names.""" + pass + + def bounds(self): + """Return an iterator over the bounds for each property.""" + pass + + def named_bounds(self): + """Return an iterator over the property bounds with their name and value.""" + for name, bound in zip(self.names(), self.bounds()): + yield name, bound + + def get_properties(self): + """ + Get the physics properties. + + Returns: + dict: current physic property values. + """ + pass + + def set_properties(self, properties): + """ + Set the given physic property values using the simulator. + + Args: + properties (dict): the physic property values to be set in the simulator. + """ + pass + + def sample(self, seed=None): + """ + Sample a new set of physics properties and returns it. Note that it doesn't set them in the simulator. + This sampling can be useful if the user wishes to check more carefully the sampled physic property values. + Once satisfied, the user can set them by calling the `set_properties` method. + + Note that it samples uniformly the physics properties between their specified lower and upper bounds. + + Args: + seed (int, None): random seed. + + Returns: + dict: sampled physic properties. + """ + # set random seed + if seed is not None: + np.random.seed(seed) + + # sample each property + properties = dict() + for name, bound in zip(self.names(), self.bounds()): + properties[name] = np.random.uniform(low=bound[0], high=bound[1]) + return properties + + def randomize(self, seed=None): + """ + Randomize the physics properties and set them in the simulator. + + Args: + seed (int, None): random seed. + """ + sampled_properties = self.sample(seed) + self.set_properties(sampled_properties) + + def seed(self, seed=None): + """ + Set the given seed when sampling or randomizing the environment. + + Args: + seed (int): random seed. + """ + if seed is not None: + np.random.seed(seed) diff --git a/pyrobolearn/physics/robot_physics_randomizer.py b/pyrobolearn/physics/robot_physics_randomizer.py new file mode 100644 index 0000000..4bbd59c --- /dev/null +++ b/pyrobolearn/physics/robot_physics_randomizer.py @@ -0,0 +1,135 @@ +#!/usr/bin/env python +"""Define the `RobotPhysicsRandomizer` class which randomizes the physical attributes / properties of links and joints. + +Dependencies: +- `pyrobolearn.physics` +""" + +import collections + +from pyrobolearn.physics.body_physics_randomizer import BodyPhysicsRandomizer +from pyrobolearn.physics.link_physics_randomizer import LinkPhysicsRandomizer +from pyrobolearn.physics.joint_physics_randomizer import JointPhysicsRandomizer + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "MIT" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class RobotPhysicsRandomizer(BodyPhysicsRandomizer): + r"""Robot Physics Randomizer + + The robot physics randomizer can randomize the physical attributes of a robot. It can notably change its mass, + the contact frictions, the inertia of the links, the friction and damping coefficients of the joints, etc. + """ + + def __init__(self, body, links=None, joints=None, **kwargs): + """ + Initialize the robot physics randomizer. + + Args: + body (Body): multi-body object. + links (int, list of int, LinkPhysicsRandomizer, list of LinkPhysicsRandomizer, None): link id(s) or link + physics randomizer(s). + joints (int, list of int, JointPhysicsRandomizer, list JointPhysicsRandomizer, None): joint id(s) or joint + physics randomizer. + **kwargs (dict): range of possible physical properties. If given one value, that property won't be + randomized. Each range is a tuple of two values `[lower_bound, upper_bound]`. + """ + super(RobotPhysicsRandomizer, self).__init__(body) + self.links = links + self.joints = joints + + ############## + # Properties # + ############## + + @property + def links(self): + """Return the list of link physics randomizers.""" + return self._links + + @links.setter + def links(self, links): + """Set the link physics randomizer or the list of link physics randomizers.""" + if isinstance(links, int): + links = [LinkPhysicsRandomizer(self.body, links)] + elif isinstance(links, LinkPhysicsRandomizer): + links = [links] + elif isinstance(links, collections.Iterable): + link_list = [] + for idx, link in enumerate(links): + if isinstance(link, int): + link = LinkPhysicsRandomizer(self.body, links) + elif not isinstance(link, LinkPhysicsRandomizer): + raise TypeError("The {} element of the given list of links is not an integer or a " + "LinkPhysicsRandomizer, instead got: {}".format(idx, type(link))) + link_list.append(link) + links = link_list + else: + raise TypeError("Expecting the given links to be an integer / `LinkPhysicsRandomizer` or a list of " + "integers / `LinkPhysicsRandomizer`, instead got: {}".format(type(links))) + self._links = links + + @property + def joints(self): + """Return the list of joint physics randomizers.""" + return self._joints + + @joints.setter + def joints(self, joints): + """Set the joint physics randomizer or the list of joint physics randomizers.""" + if isinstance(joints, int): + joints = [JointPhysicsRandomizer(self.body, joints)] + elif isinstance(joints, JointPhysicsRandomizer): + joints = [joints] + elif isinstance(joints, collections.Iterable): + joint_list = [] + for idx, joint in enumerate(joints): + if isinstance(joint, int): + joint = JointPhysicsRandomizer(self.body, joints) + elif not isinstance(joint, JointPhysicsRandomizer): + raise TypeError("The {} element of the given list of joints is not an integer or a " + "JointPhysicsRandomizer, instead got: {}".format(idx, type(joint))) + joint_list.append(joint) + joints = joint_list + else: + raise TypeError("Expecting the given joints to be an integer / `JointPhysicsRandomizer` or a list of " + "integers / `JointPhysicsRandomizer`, instead got: {}".format(type(joints))) + self._joints = joints + + ########### + # Methods # + ########### + + def names(self): + """Return an iterator over the property names.""" + pass + + def bounds(self): + """Return an iterator over the bounds for each property.""" + pass + + def get_properties(self): + """ + Get the physics properties. + + Returns: + dict: current physic property values. + """ + pass + + def set_properties(self, properties): + """ + Set the given physic property values using the simulator. + + Args: + properties (dict): the physic property values to be set in the simulator. + """ + pass diff --git a/pyrobolearn/physics/world_physics_randomizer.py b/pyrobolearn/physics/world_physics_randomizer.py new file mode 100644 index 0000000..5d1485d --- /dev/null +++ b/pyrobolearn/physics/world_physics_randomizer.py @@ -0,0 +1,462 @@ +#!/usr/bin/env python +"""Define the `WorldPhysicsRandomizer` class which randomizes the physical attributes / properties of the world. + +Dependencies: +- `pyrobolearn.physics` +- `pyrobolearn.world` +""" + +import numpy as np + +from pyrobolearn.worlds import World +from pyrobolearn.physics.physics_randomizer import PhysicsRandomizer + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "MIT" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class WorldPhysicsRandomizer(PhysicsRandomizer): + r"""World Physics Randomizer + + The world physics randomizer can randomize the physical attributes of a world. It can notably change the gravity, + the light intensity, the floor friction coefficients (including lateral, spinning, and rolling frictions), etc. + """ + + def __init__(self, world, gravity=None, lateral_friction=None, rolling_friction=None, spinning_friction=None, + restitution=None, contact_damping=None, contact_stiffness=None, **kwargs): + """ + Initialize the world physics randomizer. + + Args: + world (World): world instance. + gravity (None, np.float[3], tuple of np.float[3]): gravity bounds. If None, it will take + the default value returned by the world, and it will not sample from it. If it is a float[3], it will + set that value to the world and will always return this value when sampling from the physics randomizer. + If it is a tuple, it has to be of length 2 where the first item is the lower bound and the second item + is the upper bound. + lateral_friction (None, float, tuple of float): lateral friction coefficient bounds. If None, it will take + the default value returned by the world, and it will not sample from it. If it is a float, it will set + that value to the world and will always return this value when sampling from the physics randomizer. + If it is a tuple, it has to be of length 2 where the first item is the lower bound and the second item + is the upper bound. + rolling_friction (None, float, tuple of float): rolling friction coefficient bounds. (same types as + described in `lateral_friction`) + spinning_friction (None, float, tuple of float): spinning friction coefficient bounds. (same types as + described in `lateral_friction`) + restitution (None, float, tuple of float): restitution bounds. (same types as described in + `lateral_friction`) + contact_damping (None, float, tuple of float): contact damping bounds. (same types as described in + `lateral_friction`) + contact_stiffness (None, float, tuple of float): contact stiffness bounds. (same types as described in + `lateral_friction`) + **kwargs (dict): range of possible physical properties. If given one value, that property won't be + randomized. Each range is a tuple of two values `[lower_bound, upper_bound]`. + """ + self.world = world + simulator = self.world.simulator + super(WorldPhysicsRandomizer, self).__init__(simulator) + + # set the bounds + self.gravity_bounds = gravity + self.lateral_friction_bounds = lateral_friction + self.rolling_friction_bounds = rolling_friction + self.spinning_friction_bounds = spinning_friction + self.restitution_bounds = restitution + self.contact_damping_bounds = contact_damping + self.contact_stiffness_bounds = contact_stiffness + + ############## + # Properties # + ############## + + @property + def world(self): + """Return the world instance.""" + return self._world + + @world.setter + def world(self, world): + """Set the world instance.""" + if not isinstance(world, World): + raise TypeError("Expecting the given world to be an instance of `World`, instead got: " + "{}".format(type(world))) + self._world = world + + @property + def gravity(self): + """Return the gravity vector.""" + return self.world.gravity + + @gravity.setter + def gravity(self, gravity): + """ + Set the gravity vector. + + Args: + (np.float[3]): gravity vector [x,y,z]. + """ + self.world.gravity = gravity + + @property + def gravity_bounds(self): + """Return the upper and lower bound for the gravity vector.""" + return self._gravity_bounds + + @gravity_bounds.setter + def gravity_bounds(self, bounds): + """Set the upper and lower bounds for the gravity vector.""" + if bounds is None: + bounds = (self.gravity, self.gravity) + elif isinstance(bounds, (list, tuple, np.ndarray)): + if len(bounds) == 2: + for bound in bounds: + if not isinstance(bound, (list, tuple, np.ndarray)): + raise TypeError("Expecting one of the bounds to be a 3D vector (list, tuple, or np.ndarray), " + "instead got {}".format(type(bound))) + if len(bound) != 3: + raise ValueError("Expecting the gravity to be a 3D vector, instead received a {}D " + "vector".format(len(bounds))) + elif len(bounds) == 3: + bounds = (bounds, bounds) + else: + raise ValueError("Expecting the gravity to be a 3D vector or a tuple of length 2 where the first item " + "is the lower bound and the second item is the upper bound of 3D vectors, instead " + "the given element has a length of {}".format(len(bounds))) + else: + raise TypeError("Expecting the gravity bounds to be a 3d vector, a tuple of 3d vectors, or None. Instead " + "got {}".format(type(bounds))) + self._gravity_bounds = bounds + + @property + def lateral_friction(self): + """Return the floor lateral friction coefficient.""" + return self.world.lateral_friction + + @lateral_friction.setter + def lateral_friction(self, coefficient): + """ + Set the floor lateral friction coefficient. + + Args: + coefficient (float): lateral friction coefficient. + """ + self.world.lateral_friction = coefficient + + @property + def lateral_friction_bounds(self): + """Return the upper and lower bound for the lateral friction coefficient.""" + return self._lateral_friction_bounds + + @lateral_friction_bounds.setter + def lateral_friction_bounds(self, bounds): + """Set the upper and lower bounds for the lateral friction coefficient.""" + if bounds is None: + bounds = (self.lateral_friction, self.lateral_friction) + elif isinstance(bounds, float): + bounds = (bounds, bounds) + elif isinstance(bounds, (tuple, list)): + if len(bounds) == 2: + for bound in bounds: + if not isinstance(bound, float): + raise TypeError("Expecting one of the bounds to be a float instead got {}".format(type(bound))) + else: + raise ValueError("Expecting the bounds to be a tuple of length 2 where the first item is the lower " + "bound and the second item is the upper bound of the lateral friction, instead got " + "a length of {}".format(len(bounds))) + else: + raise TypeError("Expecting the lateral friction bounds to be a float, or a tuple of float, or None. " + "Instead got {}".format(type(bounds))) + self._lateral_friction_bounds = bounds + + @property + def rolling_friction(self): + """Return the floor rolling friction coefficient.""" + return self.world.rolling_friction + + @rolling_friction.setter + def rolling_friction(self, coefficient): + """ + Set the floor rolling friction coefficient. + + Args: + coefficient (float): rolling friction coefficient. + """ + self.world.rolling_friction = coefficient + + @property + def rolling_friction_bounds(self): + """Return the upper and lower bound for the rolling friction coefficient.""" + return self._rolling_friction_bounds + + @rolling_friction_bounds.setter + def rolling_friction_bounds(self, bounds): + """Set the upper and lower bounds for the rolling friction coefficient.""" + if bounds is None: + bounds = (self.rolling_friction, self.rolling_friction) + elif isinstance(bounds, float): + bounds = (bounds, bounds) + elif isinstance(bounds, (tuple, list)): + if len(bounds) == 2: + for bound in bounds: + if not isinstance(bound, float): + raise TypeError("Expecting one of the bounds to be a float instead got {}".format(type(bound))) + else: + raise ValueError("Expecting the bounds to be a tuple of length 2 where the first item is the lower " + "bound and the second item is the upper bound of the rolling friction, instead got " + "a length of {}".format(len(bounds))) + else: + raise TypeError("Expecting the rolling friction bounds to be a float, or a tuple of float, or None. " + "Instead got {}".format(type(bounds))) + self._rolling_friction_bounds = bounds + + @property + def spinning_friction(self): + """Return the floor spinning friction coefficient.""" + return self.world.spinning_friction + + @spinning_friction.setter + def spinning_friction(self, coefficient): + """ + Set the floor spinning friction coefficient. + + Args: + coefficient (float): spinning friction coefficient. + """ + self.world.spinning_friction = coefficient + + @property + def spinning_friction_bounds(self): + """Return the upper and lower bound for the spinning friction coefficient.""" + return self._spinning_friction_bounds + + @spinning_friction_bounds.setter + def spinning_friction_bounds(self, bounds): + """Set the upper and lower bounds for the spinning friction coefficient.""" + if bounds is None: + bounds = (self.spinning_friction, self.spinning_friction) + elif isinstance(bounds, float): + bounds = (bounds, bounds) + elif isinstance(bounds, (tuple, list)): + if len(bounds) == 2: + for bound in bounds: + if not isinstance(bound, float): + raise TypeError("Expecting one of the bounds to be a float instead got {}".format(type(bound))) + else: + raise ValueError("Expecting the bounds to be a tuple of length 2 where the first item is the lower " + "bound and the second item is the upper bound of the spinning friction, instead got " + "a length of {}".format(len(bounds))) + else: + raise TypeError("Expecting the spinning friction bounds to be a float, or a tuple of float, or None. " + "Instead got {}".format(type(bounds))) + self._spinning_friction_bounds = bounds + + @property + def restitution(self): + """Return the floor restitution (bounciness) coefficient.""" + return self.world.restitution + + @restitution.setter + def restitution(self, coefficient): + """ + Set the floor restitution (bounciness) coefficient. + + Args: + coefficient (float): restitution coefficient. + """ + self.world.restitution = coefficient + + @property + def restitution_bounds(self): + """Return the upper and lower bound for the restitution coefficient.""" + return self._restitution_bounds + + @restitution_bounds.setter + def restitution_bounds(self, bounds): + """Set the upper and lower bounds for the restitution coefficient.""" + if bounds is None: + bounds = (self.restitution, self.restitution) + elif isinstance(bounds, float): + bounds = (bounds, bounds) + elif isinstance(bounds, (tuple, list)): + if len(bounds) == 2: + for bound in bounds: + if not isinstance(bound, float): + raise TypeError("Expecting one of the bounds to be a float instead got {}".format(type(bound))) + else: + raise ValueError("Expecting the bounds to be a tuple of length 2 where the first item is the lower " + "bound and the second item is the upper bound of the restitution, instead got " + "a length of {}".format(len(bounds))) + else: + raise TypeError("Expecting the restitution bounds to be a float, or a tuple of float, or None. " + "Instead got {}".format(type(bounds))) + self._restitution_bounds = bounds + + @property + def contact_damping(self): + """Return the floor contact damping.""" + return self.world.contact_damping + + @contact_damping.setter + def contact_damping(self, value): + """ + Set the floor contact damping. + + Args: + value (float): contact damping value. + """ + self.world.contact_damping = value + + @property + def contact_damping_bounds(self): + """Return the upper and lower bound for the contact damping value.""" + return self._contact_damping_bounds + + @contact_damping_bounds.setter + def contact_damping_bounds(self, bounds): + """Set the upper and lower bounds for the contact damping value.""" + if bounds is None: + bounds = (self.contact_damping, self.contact_damping) + elif isinstance(bounds, float): + bounds = (bounds, bounds) + elif isinstance(bounds, (tuple, list)): + if len(bounds) == 2: + for bound in bounds: + if not isinstance(bound, float): + raise TypeError("Expecting one of the bounds to be a float instead got {}".format(type(bound))) + else: + raise ValueError("Expecting the bounds to be a tuple of length 2 where the first item is the lower " + "bound and the second item is the upper bound of the contact damping, instead got " + "a length of {}".format(len(bounds))) + else: + raise TypeError("Expecting the contact damping bounds to be a float, or a tuple of float, or None. " + "Instead got {}".format(type(bounds))) + self._contact_damping_bounds = bounds + + @property + def contact_stiffness(self): + """Return the floor contact stiffness.""" + return self.world.contact_stiffness + + @contact_stiffness.setter + def contact_stiffness(self, value): + """ + Set the floor contact stiffness. + + Args: + value (float): contact stiffness value. + """ + self.world.contact_stiffness = value + + @property + def contact_stiffness_bounds(self): + """Return the upper and lower bound for the contact stiffness value.""" + return self._contact_stiffness_bounds + + @contact_stiffness_bounds.setter + def contact_stiffness_bounds(self, bounds): + """Set the upper and lower bounds for the contact stiffness value.""" + if bounds is None: + bounds = (self.contact_stiffness, self.contact_stiffness) + elif isinstance(bounds, float): + bounds = (bounds, bounds) + elif isinstance(bounds, (tuple, list)): + if len(bounds) == 2: + for bound in bounds: + if not isinstance(bound, float): + raise TypeError("Expecting one of the bounds to be a float instead got {}".format(type(bound))) + else: + raise ValueError("Expecting the bounds to be a tuple of length 2 where the first item is the lower " + "bound and the second item is the upper bound of the contact stiffness, instead got " + "a length of {}".format(len(bounds))) + else: + raise TypeError("Expecting the contact stiffness bounds to be a float, or a tuple of float, or None. " + "Instead got {}".format(type(bounds))) + self._contact_stiffness_bounds = bounds + + @property + def floor_dynamics(self): + """Return the floor dynamical parameters (friction, restitution, etc). + + Returns: + float: lateral friction coefficient + float: rolling friction coefficient + float: spinning friction coefficient + float: restitution coefficient + float: contact damping value + float: contact stiffness value + """ + return self.world.floor_dynamics + + @floor_dynamics.setter + def floor_dynamics(self, dynamics): + """ + Set the floor dynamics. + + Args: + values (dict): floor dynamics. + """ + self.world.floor_dynamics = dynamics + + ########### + # Methods # + ########### + + def names(self): + """Return an iterator over the property names.""" + for name in ['gravity', 'lateral_friction', 'rolling_friction', 'spinning_friction', 'restitution', + 'contact_damping', 'contact_stiffness']: + yield name + + def bounds(self): + """Return an iterator over the bounds""" + yield self.gravity_bounds + yield self.lateral_friction_bounds + yield self.rolling_friction_bounds + yield self.spinning_friction_bounds + yield self.restitution_bounds + yield self.contact_damping_bounds + yield self.contact_stiffness_bounds + + def get_properties(self): + """ + Get the physics properties. + + Returns: + dict: current physic property values. + """ + properties = dict() + properties['gravity'] = self.gravity + floor_dynamics = self.floor_dynamics + if floor_dynamics is not None: # there is a floor + properties['lateral_friction'] = floor_dynamics[0] + properties['rolling_friction'] = floor_dynamics[1] + properties['spinning_friction'] = floor_dynamics[2] + properties['restitution'] = floor_dynamics[3] + properties['contact_damping'] = floor_dynamics[4] + properties['contact_stiffness'] = floor_dynamics[5] + return properties + + def set_properties(self, properties): + """ + Set the given physic property values using the simulator. + + Args: + properties (dict): the physic property values to be set in the simulator. + """ + if not isinstance(properties, dict): + raise TypeError("Expecting the given 'properties' to be a dictionary, instead got: " + "{}".format(type(properties))) + + # set gravity + if 'gravity' in properties: + self.gravity = properties['gravity'] + + # set floor dynamics + self.floor_dynamics = properties diff --git a/pyrobolearn/robots/__init__.py b/pyrobolearn/robots/__init__.py index 90c9e6d..5b018d8 100644 --- a/pyrobolearn/robots/__init__.py +++ b/pyrobolearn/robots/__init__.py @@ -5,7 +5,7 @@ import importlib import inspect # General robot class -from .base import Object, MovableObject, ControllableObject +from .base import Body, MovableBody, ControllableBody from .actuators import * from .sensors import * from .robot import Robot diff --git a/pyrobolearn/robots/base.py b/pyrobolearn/robots/base.py index c2b1067..9c41a0b 100644 --- a/pyrobolearn/robots/base.py +++ b/pyrobolearn/robots/base.py @@ -1,9 +1,17 @@ #!/usr/bin/env python -"""Define the various basic objects that are present in the simulator/world. +"""Define the various basic bodies / objects that are present in the simulator / world. + +Dependencies: +- `pyrobolearn.simulators` +- `pyrobolearn.utils` """ import numpy as np -import quaternion +# import quaternion + +from pyrobolearn.simulators import Simulator +from pyrobolearn.utils.orientation import get_rpy_from_quaternion, get_matrix_from_quaternion + __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -15,99 +23,172 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class Object(object): - r"""Object +class Body(object): + r"""Physical (Multi-)Body - Define an object in the simulator/world. + Define a physical body in the simulator/world. """ - def __init__(self, simulator, object_id): + def __init__(self, simulator, body_id, name=None): + """ + Initialize the Body. + + Args: + simulator (Simulator): simulator instance. + body_id (int): unique body id returned by the simulator. + name (str): name of the body. + """ + self.simulator = simulator + self.id = body_id + self.name = name + self.joints = None + + ############## + # Properties # + ############## + + @property + def simulator(self): + """Return the simulator.""" + return self.sim + + @simulator.setter + def simulator(self, simulator): + """Set the simulator.""" + if not isinstance(simulator, Simulator): + raise TypeError("Expecting the given simulator to be an instance of `Simulator`, instead got: " + "{}".format(type(simulator))) self.sim = simulator - self.id = object_id + + @property + def id(self): + """Return the id.""" + return self._id + + @id.setter + def id(self, id_): + """Set the unique body id.""" + if not isinstance(id_, int): + raise TypeError("Expecting the given simulator to be an integer, instead got: {}".format(type(id_))) + self._id = id_ @property def name(self): - return self.sim.getBodyInfo(self.id) + """Return the name of the body (or the base if not given).""" + if self._name is None: + return self.sim.get_body_info(self.id) + return self._name + + @name.setter + def name(self, name): + """Set the name of the body.""" + if not isinstance(name, str): + raise TypeError("Expecting the given name to be a string, instead got: {}".format(type(name))) + self._name = name + + @property + def pose(self): + """Return the body pose.""" + return self.sim.get_base_pose(self.id) @property def position(self): - return np.array(self.sim.getBasePositionAndOrientation(self.id)[0]) + """Return the body position.""" + return self.sim.get_base_position(self.id) @property - def quaternion(self): - quat = self.sim.getBasePositionAndOrientation(self.id)[1] - return quaternion.quaternion(quat[3], *quat[:3]) + def orientation(self): + """Return the body orientation as a quaternion [x,y,z,w].""" + return self.sim.get_base_orientation(self.id) # alias - orientation = quaternion + quaternion = orientation @property def rpy(self): - quat = self.sim.getBasePositionAndOrientation(self.id)[1] - y, p, r = self.sim.getEulerFromQuaternion(quat) - return np.array([r, p, y]) + """Return the orientation as the Roll-Pitch-Yaw angles.""" + return get_rpy_from_quaternion(self.orientation) @property - def rotation(self): - quat = self.sim.getBasePositionAndOrientation(self.id)[1] - rot = self.sim.getMatrixFromQuaternion(quat) - return np.array(rot).reshape(3, 3) - - @property - def state(self): - pos, quat = self.sim.getBasePositionAndOrientation(self.id) - rpy = self.sim.getEulerFromQuaternion(quat)[::-1] - # return np.array(pos), quaternion.quaternion(quat[3], *quat[:3]) - return np.array(pos+rpy) + def rotation_matrix(self): + """Return the orientation as a rotation matrix.""" + return get_matrix_from_quaternion(self.orientation) @property def linear_velocity(self): - return np.array(self.sim.getBaseVelocity(self.id)[0]) + """Return the linear velocity of the body's base.""" + return self.sim.get_base_linear_velocity(self.id) @property def angular_velocity(self): - return np.array(self.sim.getBaseVelocity(self.id)[1]) + """Return the angular velocity of the body's base.""" + return self.sim.get_base_angular_velocity(self.id) @property def velocity(self): - lin, ang = self.sim.getBaseVelocity(self.id) - return np.array(lin+ang) + """Return the linear and angular velocity of the body.""" + return self.sim.get_base_velocity(self.id) @property def color(self): - return self.sim.getVisualShapeData(self.id)[0][-1] - - # alias - rgba_color = color + return self.sim.get_visual_shape_data(self.id)[0][-1] @property def mass(self): - links = [-1] + list(range(self.sim.getNumJoints(self.id))) - return np.sum([self.sim.getDynamicsInfo(self.id, linkId)[0] for linkId in links]) + """Return the total mass of the body.""" + return self.sim.get_mass(self.id) @property def dimensions(self): - return np.array(self.sim.getVisualShapeData(self.id)[0][3]) + """Return the dimensions of the body. Warnings: this should not be trusted too much...""" + return np.array(self.sim.get_visual_shape_data(self.id)[0][3]) + + @property + def num_joints(self): + """Return the total number of joints.""" + return self.sim.num_joints(self.id) + + @property + def num_links(self): + """Return the total number of links. This is the same as the number of joints.""" + return self.sim.num_links(self.id) + + @property + def num_actuated_joints(self): + """Return the total number of actuated joints. This property should be overwritten in the child class.""" + return self.sim.num_actuated_joints(self.id) + + @property + def actuated_joints(self): + """Return the total number of actuated joints.""" + if self.joints is None: + self.joints = self.sim.get_actuated_joint_ids(self.id) + return self.joints + + @property + def center_of_mass(self): + """Return the center of mass.""" + return self.sim.get_center_of_mass(self.id) -class MovableObject(Object): - r"""Movable Object +class MovableBody(Body): + r"""Movable Body Define a movable object in the world. """ - def __init__(self, simulator, object_id): - super(MovableObject, self).__init__(simulator, object_id) + def __init__(self, simulator, object_id, name=None): + super(MovableBody, self).__init__(simulator, object_id, name=name) - def move(self, new_position=None, new_orientation=None): - pass + # def move(self, position=None, orientation=None): + # pass -class ControllableObject(MovableObject): - r"""Controllable Object +class ControllableBody(MovableBody): + r"""Controllable Body Define a controllable object in the world. """ - def __init__(self, simulator, object_id): - super(ControllableObject, self).__init__(simulator, object_id) + def __init__(self, simulator, object_id, name=None): + super(ControllableBody, self).__init__(simulator, object_id, name=name) diff --git a/pyrobolearn/robots/cartpole.py b/pyrobolearn/robots/cartpole.py index ee884d4..5059b64 100644 --- a/pyrobolearn/robots/cartpole.py +++ b/pyrobolearn/robots/cartpole.py @@ -9,7 +9,7 @@ import sympy import sympy.physics.mechanics as mechanics from pyrobolearn.robots.robot import Robot -from pyrobolearn.utils.orientation import getSymbolicMatrixFromAxisAngle +from pyrobolearn.utils.orientation import get_symbolic_matrix_from_axis_angle class CartPole(Robot): diff --git a/pyrobolearn/simulators/__init__.py b/pyrobolearn/simulators/__init__.py index fe94980..aae9ae0 100644 --- a/pyrobolearn/simulators/__init__.py +++ b/pyrobolearn/simulators/__init__.py @@ -4,6 +4,9 @@ # basic simulator from .simulator import Simulator +# bullet simulator +from .bullet import Bullet + # PyBullet simulator import pybullet import pybullet_data diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py index 175295e..109e2cd 100644 --- a/pyrobolearn/simulators/bullet.py +++ b/pyrobolearn/simulators/bullet.py @@ -21,12 +21,13 @@ References: import time import numpy as np import quaternion -from pyrobolearn.utils.converter import NumpyListConverter, QuaternionListConverter import pybullet +import pybullet_data from pybullet_envs.bullet.bullet_client import BulletClient -from simulator import Simulator +from pyrobolearn.utils.converter import NumpyListConverter, QuaternionListConverter +from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" @@ -74,9 +75,11 @@ class Bullet(Simulator): Erwin Coumans and Yunfei Bai, 2017/2018 """ - def __init__(self, render=True): # , converter=None): + def __init__(self, render=True, **kwargs): # , converter=None): super(Bullet, self).__init__() + # parse the kwargs + # Connect to pybullet if render: self.sim = BulletClient(connection_mode=pybullet.GUI) @@ -84,6 +87,9 @@ class Bullet(Simulator): self.sim = BulletClient(connection_mode=pybullet.DIRECT) self.id = self.sim._client + # add additional search path when loading URDFs, SDFs, MJCFs, etc. + self.sim.setAdditionalSearchPath(pybullet_data.getDataPath()) + # Converters # if converter is None: self.conv = NumpyListConverter() @@ -133,12 +139,22 @@ class Bullet(Simulator): """ self.sim.resetSimulation() + def close(self): + """Close the simulator.""" + try: + self.sim.disconnect(physicsClientId=self.id) + except pybullet.error: + pass + def step(self, sleep_time=0.): """Perform a step in the simulator. "stepSimulation will perform all the actions in a single forward dynamics simulation step such as collision detection, constraint solving and integration. The default timestep is 1/240 second, it can be changed using the setTimeStep or setPhysicsEngineParameter API." [1] + + Args: + sleep_time (float): time to sleep after performing one step in the simulation. """ self.sim.stepSimulation() time.sleep(sleep_time) @@ -170,7 +186,7 @@ class Bullet(Simulator): """ self.sim.setTimeStep(timeStep=time_step) - def set_real_time(self, flag=True): + def set_real_time(self, enable=True): """Enable/disable real time in the simulator. "By default, the physics server will not step the simulation, unless you explicitly send a 'stepSimulation' @@ -185,9 +201,9 @@ class Bullet(Simulator): allows the physicsserver thread to add additional calls to stepSimulation." [1] Args: - flag (bool): If True, it will enable the real-time simulation. If False, it will disable it. + enable (bool): If True, it will enable the real-time simulation. If False, it will disable it. """ - self.sim.setRealTimeSimulation(enableRealTimeSimulation=int(flag)) + self.sim.setRealTimeSimulation(enableRealTimeSimulation=int(enable)) def pause(self): """Pause the simulator if in real-time.""" @@ -201,18 +217,25 @@ class Bullet(Simulator): """Get the physics engine parameters. Returns: - dict: dictionary containing the following tags with their corresponding values: ['gravityAccelerationX', - 'useRealTimeSimulation', 'gravityAccelerationZ', 'numSolverIterations', 'gravityAccelerationY', - 'numSubSteps', 'fixedTimeStep'] + dict: dictionary containing the following tags with their corresponding values: ['gravity', + 'num_solver_iterations', 'use_real_time_simulation', 'num_sub_steps', 'fixed_time_step'] """ - return self.sim.getPhysicsEngineParameters() + d = self.sim.getPhysicsEngineParameters() + properties = dict() + properties['gravity'] = np.array([d['gravityAccelerationX'], d['gravityAccelerationY'], + d['gravityAccelerationZ']]) + properties['num_solver_iterations'] = d['numSolverIterations'] + properties['use_real_time_simulation'] = d['useRealTimeSimulation'] + properties['num_sub_steps'] = d['numSubSteps'] + properties['fixed_time_step'] = d['fixedTimeStep'] + return properties def set_physics_properties(self, time_step=None, num_solver_iterations=None, use_split_impulse=None, split_impulse_penetration_threshold=None, num_sub_steps=None, collision_filter_mode=None, contact_breaking_threshold=None, max_num_cmd_per_1ms=None, enable_file_caching=None, restitution_velocity_threshold=None, erp=None, contact_erp=None, friction_erp=None, enable_cone_friction=None, - deterministic_overlapping_pairs=None, solver_residual_threshold=None): + deterministic_overlapping_pairs=None, solver_residual_threshold=None, **kwargs): """Set the physics engine parameters. Args: @@ -415,7 +438,7 @@ class Bullet(Simulator): Args: plugin_id (int): unique plugin id. - args (list): list of argument values to be interpreted by the plugin. One can be a string, while the + *args (list): list of argument values to be interpreted by the plugin. One can be a string, while the others must be integers or float. """ kwargs = {} @@ -462,9 +485,9 @@ class Bullet(Simulator): Args: filename (str): a relative or absolute path to the URDF file on the file system of the physics server. - position (vec3): create the base of the object at the specified position in world space coordinates [X,Y,Z] + position (vec3): create the base of the object at the specified position in world space coordinates [x,y,z] orientation (quat): create the base of the object at the specified orientation as world space quaternion - [X,Y,Z,W] + [x,y,z,w] use_maximal_coordinates (int): Experimental. By default, the joints in the URDF file are created using the reduced coordinate method: the joints are simulated using the Featherstone Articulated Body algorithm (btMultiBody in Bullet 2.x). The useMaximalCoordinates option will create a 6 degree of freedom rigid @@ -529,7 +552,7 @@ class Bullet(Simulator): return self.sim.loadMJCF(filename, globalScaling=scaling) def load_mesh(self, filename, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), - color=None, flags=None): + color=None, flags=None, *args, **kwargs): """ Load a mesh in the world (only available in the simulator). @@ -541,7 +564,7 @@ class Bullet(Simulator): If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) mass (float): mass of the mesh (in kg). If mass = 0, it won't move even if there is a collision. scale (float[3]): scale the mesh in the (x,y,z) directions - color (int[4]): color of the mesh (by default: white and opaque) + color (int[4], None): color of the mesh (by default: white and opaque) flags (int, None): if flag = `sim.GEOM_FORCE_CONCAVE_TRIMESH` (=1), this will create a concave static triangle mesh. This should not be used with dynamic/moving objects, only for static (mass=0) terrain. @@ -648,9 +671,10 @@ class Bullet(Simulator): def create_constraint(self, parent_body_id, parent_link_id, child_body_id, child_link_id, joint_type, joint_axis, parent_frame_position, child_frame_position, - parent_frame_orientation=(0., 0., 0., 1.), child_frame_orientation=(0., 0., 0., 1.)): + parent_frame_orientation=(0., 0., 0., 1.), child_frame_orientation=(0., 0., 0., 1.), + *args, **kwargs): """ - Create a constaint. + Create a constraint. "URDF, SDF and MJCF specify articulated bodies as a tree-structures without loops. The 'createConstraint' allows you to connect specific links of bodies to close those loops. In addition, you can create arbitrary @@ -697,7 +721,8 @@ class Bullet(Simulator): self.sim.removeConstraint(constraint_id) def change_constraint(self, constraint_id, child_joint_pivot=None, child_frame_orientation=None, max_force=None, - gear_ratio=None, gear_auxiliary_link=None, relative_position_target=None, erp=None): + gear_ratio=None, gear_auxiliary_link=None, relative_position_target=None, erp=None, *args, + **kwargs): """ Change the parameters of an existing constraint. @@ -804,7 +829,11 @@ class Bullet(Simulator): return np.sum(self.get_link_masses(body_id, [-1] + list(range(self.num_links(body_id))))) def get_base_mass(self, body_id): - """Return the base mass of the robot.""" + """Return the base mass of the robot. + + Args: + body_id (int): unique object id. + """ return self.get_link_masses(body_id, -1) def get_base_name(self, body_id): @@ -991,7 +1020,7 @@ class Bullet(Simulator): self.sim.resetBaseVelocity(body_id, angularVelocity=angular_velocity) def apply_external_force(self, body_id, link_id=-1, force=(0., 0., 0.), position=(0., 0., 0.), - flags=pybullet.LINK_FRAME): + frame=pybullet.LINK_FRAME): """ Apply the specified external force on the specified position on the body / link. @@ -1006,10 +1035,10 @@ class Bullet(Simulator): force (np.float[3]): external force to be applied. position (np.float[3]): position on the link where the force is applied. See `flags` for coordinate systems. - flags (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for + frame (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates. """ - self.sim.applyExternalForce(body_id, link_id, force, position, flags) + self.sim.applyExternalForce(body_id, link_id, force, position, frame) def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.)): """ @@ -1091,7 +1120,11 @@ class Bullet(Simulator): [15] np.float[4]: joint orientation in parent frame [16] int: parent link index, -1 for base """ - return self.sim.getJointInfo(body_id, joint_id) + info = self.sim.getJointInfo(body_id, joint_id) + info[13] = np.array(info[13]) + info[14] = np.array(info[14]) + info[15] = np.array(info[15]) + return info def get_joint_state(self, body_id, joint_id): """ @@ -1227,7 +1260,8 @@ class Bullet(Simulator): joint_ids (list of int): list of joint id. control_mode (int): POSITION_CONTROL (=2) (which is in fact CONTROL_MODE_POSITION_VELOCITY_PD), VELOCITY_CONTROL (=0), TORQUE_CONTROL (=1) and PD_CONTROL (=3). - positions (list of float): list of target joint positions (used in POSITION_CONTROL) the target value is target position of the joint. + positions (list of float): list of target joint positions (used in POSITION_CONTROL) the target value is + target position of the joint. velocities (list of float): list of target joint velocities (used in PD_CONTROL, VELOCITY_CONTROL and POSITION_CONTROL). forces (list of float): list of forces. In POSITION_CONTROL and VELOCITY_CONTROL, these are the maximum @@ -1663,7 +1697,7 @@ class Bullet(Simulator): velocities (None, float, np.float[N]): desired velocity, or list of desired velocities [rad/s] kps (None, float, np.float[N]): position gain(s) kds (None, float, np.float[N]): velocity gain(s) - forces (float): maximum motor force(s)/torque(s) used to reach the target values. + forces (None, float, np.float[N]): maximum motor force(s)/torque(s) used to reach the target values. """ if isinstance(joint_ids, int): self.set_joint_motor_control(body_id, joint_ids, control_mode=pybullet.POSITION_CONTROL, position=positions, @@ -1699,7 +1733,7 @@ class Bullet(Simulator): body_id (int): unique body id. joint_ids (int, list of int): joint id, or list of joint ids. velocities (float, np.float[N]): desired velocity, or list of desired velocities [rad/s] - max_force (bool, float, float[N]): maximum motor forces/torques + max_force (None, float, np.float[N]): maximum motor forces/torques """ if isinstance(joint_ids, int): if max_force is None: @@ -1820,7 +1854,7 @@ class Bullet(Simulator): Args: body_id (int): unique body id. joint_ids (int, list of int): joint id, or list of joint ids. - torque (float, list of float): desired torque(s) to apply to the joint(s) [N]. + torques (float, list of float): desired torque(s) to apply to the joint(s) [N]. """ if isinstance(joint_ids, int): self.sim.setJointMotorControl2(body_id, joint_ids, self.sim.TORQUE_CONTROL, force=torques) @@ -2077,7 +2111,7 @@ class Bullet(Simulator): * orthographic projection * perspective projection - For the perspective projection, see `computeProjectionMatrixFOV(self) + For the perspective projection, see `computeProjectionMatrixFOV(self)`. Args: left (float): left screen (canvas) coordinate @@ -2989,50 +3023,50 @@ class Bullet(Simulator): def calculate_forward_dynamics(self, body_id, q, dq, torques): r""" - Given the specified joint positions :math:`q` and velocities :math:`\dot{q}`, and joint torques :math:`\tau`, - it computes the joint accelerations :math:`\ddot{q}`. That is, :math:`\ddot{q} = FD(model, q, \dot{q}, \tau)`. + Given the specified joint positions :math:`q` and velocities :math:`\dot{q}`, and joint torques :math:`\tau`, + it computes the joint accelerations :math:`\ddot{q}`. That is, :math:`\ddot{q} = FD(model, q, \dot{q}, \tau)`. - Specifically, it uses the rigid-body equation of motion in joint space given by (see [1]): + Specifically, it uses the rigid-body equation of motion in joint space given by (see [1]): - .. math:: \ddot{q} = H(q)^{-1} (\tau - C(q,\dot{q})) + .. math:: \ddot{q} = H(q)^{-1} (\tau - C(q,\dot{q})) - where :math:`\tau` is the vector of applied torques, :math:`H(q)` is the inertia matrix, and - :math:`C(q,\dot{q}) \dot{q}` is the vector accounting for Coriolis, centrifugal forces, gravity, and any - other forces acting on the system except the applied torques :math:`\tau`. + where :math:`\tau` is the vector of applied torques, :math:`H(q)` is the inertia matrix, and + :math:`C(q,\dot{q}) \dot{q}` is the vector accounting for Coriolis, centrifugal forces, gravity, and any + other forces acting on the system except the applied torques :math:`\tau`. - Normally, a more popular form of this equation of motion (in joint space) is given by: + Normally, a more popular form of this equation of motion (in joint space) is given by: - .. math:: H(q) \ddot{q} + S(q,\dot{q}) \dot{q} + g(q) = \tau + J^T(q) F + .. math:: H(q) \ddot{q} + S(q,\dot{q}) \dot{q} + g(q) = \tau + J^T(q) F - which is the same as the first one with :math:`C = S\dot{q} + g(q) - J^T(q) F`. However, this last formulation - is useful to understand what happens when we set some variables to 0. - Assuming that there are no forces acting on the system, and giving desired joint torques of 0, this - method will return :math:`\ddot{q} = - H(q)^{-1} (S(q,\dot{q}) \dot{q} + g(q))`. If in addition - the joint velocities are also 0, it will return :math:`\ddot{q} = - H(q)^{-1} g(q)` which are - the accelerations due to gravity. + which is the same as the first one with :math:`C = S\dot{q} + g(q) - J^T(q) F`. However, this last formulation + is useful to understand what happens when we set some variables to 0. + Assuming that there are no forces acting on the system, and giving desired joint torques of 0, this + method will return :math:`\ddot{q} = - H(q)^{-1} (S(q,\dot{q}) \dot{q} + g(q))`. If in addition + the joint velocities are also 0, it will return :math:`\ddot{q} = - H(q)^{-1} g(q)` which are + the accelerations due to gravity. - For inverse dynamics, which computes the joint torques given the joint positions, velocities, and - accelerations (that is, :math:`\tau = ID(model, q, \dot{q}, \ddot{q})`, this can be computed using - :math:`\tau = H(q)\ddot{q} + C(q,\dot{q})`. For more information about different - control schemes (position, force, impedance control and others), or about the formulation of the equation - of motion in task/operational space (instead of joint space), check the references [1-4]. + For inverse dynamics, which computes the joint torques given the joint positions, velocities, and + accelerations (that is, :math:`\tau = ID(model, q, \dot{q}, \ddot{q})`, this can be computed using + :math:`\tau = H(q)\ddot{q} + C(q,\dot{q})`. For more information about different + control schemes (position, force, impedance control and others), or about the formulation of the equation + of motion in task/operational space (instead of joint space), check the references [1-4]. - Args: - body_id (int): unique body id. - q (np.float[N]): joint positions - dq (np.float[N]): joint velocities - torques (np.float[N]): desired joint torques + Args: + body_id (int): unique body id. + q (np.float[N]): joint positions + dq (np.float[N]): joint velocities + torques (np.float[N]): desired joint torques - Returns: - float[N]: joint accelerations computed using the rigid-body equation of motion + Returns: + float[N]: joint accelerations computed using the rigid-body equation of motion - References: - [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1 - [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010 - [3] "Springer Handbook of Robotics", Siciliano et al., 2008 - [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma, - http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf - """ + References: + [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1 + [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010 + [3] "Springer Handbook of Robotics", Siciliano et al., 2008 + [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma, + http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf + """ # convert numpy arrays to lists if isinstance(q, np.ndarray): q = q.ravel().tolist() diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index cef859a..9fe03ea 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -44,7 +44,7 @@ class Simulator(object): [2] PEP8: https://www.python.org/dev/peps/pep-0008/ """ - def __init__(self, render=True): + def __init__(self, render=True, **kwargs): self._render = render self.real_time = False @@ -79,7 +79,7 @@ class Simulator(object): # Simulators - def reset(self): + def reset(self, *args, **kwargs): """Reset the simulator.""" pass @@ -92,11 +92,19 @@ class Simulator(object): pass def step(self, sleep_time=0): - """Perform a step in the simulator, and sleep the specified time.""" + """Perform a step in the simulator, and sleep the specified time. + + Args: + sleep_time (float): time to sleep after performing one step in the simulation. + """ pass def render(self, flag=True): - """Render the simulation.""" + """Render the simulation. + + Args: + flag (bool): If True, it will render the simulator by enabling the GUI. + """ pass def hide(self): @@ -104,11 +112,19 @@ class Simulator(object): self.render(False) def set_time_step(self, time_step): - """Set the time step in the simulator.""" + """Set the time step in the simulator. + + Args: + time_step (float): Each time you call 'step' the time step will proceed with 'time_step'. + """ pass - def set_real_time(self): - """Enable real time in the simulator.""" + def set_real_time(self, enable=True): + """Enable real time in the simulator. + + Args: + enable (bool): If True, it will enable the real-time simulation. If False, it will disable it. + """ pass def pause(self): @@ -139,42 +155,101 @@ class Simulator(object): """Set the gravity in the simulator.""" pass - def save(self, on_disk=False): - """Save the state of the simulator.""" + def save(self, filename=None, *args, **kwargs): + """Save the state of the simulator. + + Args: + filename (None, str): path to file to store the state of the simulator. If None, it will save it in + memory instead of the disk. + + Returns: + int: unique state id. This id can be used to load the state. + """ pass - def load(self, state): - """Load the simulator to a previous state.""" + def load(self, state, *args, **kwargs): + """Load / Restore the simulator to a previous state. + + Args: + state (int, str): unique state id, or path to the file containing the state. + """ pass - def load_plugin(self, plugin): - """Load a certain plugin in the simulator.""" + def load_plugin(self, plugin_path, name, *args, **kwargs): + """Load a certain plugin in the simulator. + + Args: + plugin_path (str): path, location on disk where to find the plugin + name (str): postfix name of the plugin that is appended to each API + + Returns: + int: unique plugin id. If this id is negative, the plugin is not loaded. Once a plugin is loaded, you can + send commands to the plugin using `execute_plugin_commands` + """ pass - def execute_plugin_commands(self, plugin_id, commands): - """Execute the commands on the specified plugin.""" + def execute_plugin_commands(self, plugin_id, *args, **kwargs): + """Execute the commands on the specified plugin. + + Args: + plugin_id (int): unique plugin id. + *args (list): list of argument values to be interpreted by the plugin. One can be a string, while the + others must be integers or float. + """ pass - def unload_plugin(self, plugin_id): - """Unload the specified plugin from the simulator.""" + def unload_plugin(self, plugin_id, *args, **kwargs): + """Unload the specified plugin from the simulator. + + Args: + plugin_id (int): unique plugin id. + """ pass # loading URDFs, SDFs, MJCFs - def load_urdf(self, filename, position, orientation): - """Load a URDF file in the simulator.""" + def load_urdf(self, filename, position, orientation, use_fixed_base=0, scale=1.0, *args, **kwargs): + """Load a URDF file in the simulator. + + Args: + filename (str): a relative or absolute path to the URDF file on the file system of the physics server. + position (vec3): create the base of the object at the specified position in world space coordinates [x,y,z] + orientation (quat): create the base of the object at the specified orientation as world space quaternion + [x,y,z,w] + use_fixed_base (bool): force the base of the loaded object to be static + scale (float): scale factor to the URDF model. + + Returns: + int (non-negative): unique id associated to the load model. + """ pass - def load_sdf(self, filename): - """Load a SDF file in the simulator.""" + def load_sdf(self, filename, scaling=1., *args, **kwargs): + """Load a SDF file in the simulator. + + Args: + filename (str): a relative or absolute path to the SDF file on the file system of the physics server. + scaling (float): scale factor for the object + + Returns: + list(int): list of object unique id for each object loaded + """ pass - def load_mjcf(self, filename): - """Load a Mujoco file in the simulator.""" + def load_mjcf(self, filename, scaling=1., *args, **kwargs): + """Load a Mujoco file in the simulator. + + Args: + filename (str): a relative or absolute path to the MJCF file on the file system of the physics server. + scaling (float): scale factor for the object + + Returns: + list(int): list of object unique id for each object loaded + """ pass - def load_mesh(self, filename, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), color=(1, 1, 1, 1), - flags=None): + def load_mesh(self, filename, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), color=None, + flags=None, *args, **kwargs): """Load a mesh into the simulator. Args: @@ -185,7 +260,7 @@ class Simulator(object): If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) mass (float): mass of the mesh (in kg). If mass = 0, it won't move even if there is a collision. scale (float[3]): scale the mesh in the (x,y,z) directions - color (int[4]): color of the mesh (by default: white and opaque) + color (int[4], None): color of the mesh (by default: white and opaque) flags (int, None): if flag = `sim.GEOM_FORCE_CONCAVE_TRIMESH` (=1), this will create a concave static triangle mesh. This should not be used with dynamic/moving objects, only for static (mass=0) terrain. @@ -196,208 +271,2017 @@ class Simulator(object): # bodies - def create_visual_shape(self, shape_type, radius=0.5, half_extents=(1, 1, 1), length=1, filename='.obj'): - pass + def create_body(self, visual_shape_id=-1, collision_shape_id=-1, mass=0, position=(0., 0., 0.), + orientation=(0., 0., 0., 1.)): + """Create a body in the simulator. - def get_visual_shape_data(self, object_id): - pass + Args: + visual_shape_id (int): unique id from createVisualShape or -1. You can reuse the visual shape (instancing) + collision_shape_id (int): unique id from createCollisionShape or -1. You can re-use the collision shape + for multiple multibodies (instancing) + mass (int): mass of the base, in kg (if using SI units) + position (np.float[3]): Cartesian world position of the base + orientation (np.float[4]): Orientation of base as quaternion [x,y,z,w] - def create_collision_shape(self, shape_type, radius=0.5, half_extents=(1, 1, 1), length=1): - pass - - def get_collision_shape_data(self): - pass - - def create_body(self): - """Create a body in the simulator.""" + Returns: + int: non-negative unique id or -1 for failure. + """ pass def remove_body(self, body_id): - """Remove a particular body in the simulator.""" + """Remove a particular body in the simulator. + + Args: + body_id (int): unique body id. + """ pass def num_bodies(self): - """Return the number of bodies present in the simulator.""" + """Return the number of bodies present in the simulator. + + Returns: + int: number of bodies + """ pass def get_body_info(self, body_id): - """Get the specified body information.""" + """Get the specified body information. + + Args: + body_id (int): unique body id. + + Returns: + dict, list: info + """ pass - def get_body_id(self): + def get_body_id(self, index): + """Get the body id associated to the index which is between 0 and `num_bodies()`. + + Args: + index (int): index between [0, `num_bodies()`] + + Returns: + int: unique body id. + """ pass # constraint - def create_constraint(self): + def create_constraint(self, parent_body_id, parent_link_id, child_body_id, child_link_id, joint_type, + joint_axis, parent_frame_position, child_frame_position, + parent_frame_orientation=(0., 0., 0., 1.), child_frame_orientation=(0., 0., 0., 1.), + *args, **kwargs): + """ + Create a constraint. + + Args: + parent_body_id (int): parent body unique id + parent_link_id (int): parent link index (or -1 for the base) + child_body_id (int): child body unique id, or -1 for no body (specify a non-dynamic child frame in world + coordinates) + child_link_id (int): child link index, or -1 for the base + joint_type (int): joint type: JOINT_PRISMATIC (=1), JOINT_FIXED (=4), JOINT_POINT2POINT (=5), + JOINT_GEAR (=6) + joint_axis (np.float[3]): joint axis, in child link frame + parent_frame_position (np.float[3]): position of the joint frame relative to parent CoM frame. + child_frame_position (np.float[3]): position of the joint frame relative to a given child CoM frame (or + world origin if no child specified) + parent_frame_orientation (np.float[4]): the orientation of the joint frame relative to parent CoM + coordinate frame + child_frame_orientation (np.float[4]): the orientation of the joint frame relative to the child CoM + coordinate frame (or world origin frame if no child specified) + + Returns: + int: constraint unique id. + """ pass - def remove_constraint(self): + def remove_constraint(self, constraint_id): + """ + Remove the specified constraint. + + Args: + constraint_id (int): constraint unique id. + """ pass - def change_constraint(self): + def change_constraint(self, constraint_id, *args, **kwargs): + """ + Change the parameters of an existing constraint. + + Args: + constraint_id (int): constraint unique id. + """ pass - def get_num_constraint(self): + def num_constraints(self): + """ + Get the number of constraints created. + + Returns: + int: number of constraints created. + """ pass - def get_constraint_id(self): + def get_constraint_id(self, index): + """ + Get the constraint unique id associated with the index which is between 0 and `num_constraints()`. + + Args: + index (int): index between [0, `num_constraints()`] + + Returns: + int: constraint unique id. + """ pass - def get_constraint_info(self): + def get_constraint_info(self, constraint_id): + """ + Get information about the given constaint id. + + Args: + constraint_id (int): constraint unique id. + + Returns: + dict, list: info + """ pass - def get_constraint_state(self): + def get_constraint_state(self, constraint_id): + """ + Get the state of the given constraint. + + Args: + constraint_id (int): constraint unique id. + + Returns: + dict, list: state + """ pass # objects - def get_base_pose(self): + def get_mass(self, body_id): + """ + Return the total mass of the robot (=sum of all mass links). + + Args: + body_id (int): unique object id, as returned from `load_urdf`. + + Returns: + float: total mass of the robot [kg] + """ pass - def reset_base_pose(self): + def get_base_mass(self, body_id): + """Return the base mass of the robot. + + Args: + body_id (int): unique object id. + """ pass - def get_base_position(self): + def get_base_name(self, body_id): + """ + Return the base name. + + Args: + body_id (int): unique object id. + + Returns: + str: base name + """ pass - def reset_base_position(self): + def get_center_of_mass(self, body_id, link_ids=None): + """ + Return the center of mass position. + + Args: + body_id (int): unique body id. + link_ids (list of int): link ids associated with the given body id. If None, it will take all the links + of the specified body. + + Returns: + np.float[3]: center of mass position in the Cartesian world coordinates + """ pass - def get_base_orientation(self): + def get_base_pose(self, body_id): + """ + Get the current position and orientation of the base (or root link) of the body in Cartesian world coordinates. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.float[3]: base position + np.float[4]: base orientation (quaternion [x,y,z,w]) + """ pass - def reset_base_orientation(self): + def get_base_position(self, body_id): + """ + Return the base position of the specified body. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.float[3]: base position. + """ pass - def get_base_velocity(self): + def get_base_orientation(self, body_id): + """ + Get the base orientation of the specified body. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.float[4]: base orientation in the form of a quaternion (x,y,z,w) + """ pass - def reset_base_velocity(self): + def reset_base_pose(self, body_id, position, orientation): + """ + Reset the base position and orientation of the specified object id. + + Args: + body_id (int): unique object id. + position (np.float[3]): new base position. + orientation (np.float[4]): new base orientation (expressed as a quaternion [x,y,z,w]) + """ pass - def apply_external_force(self): + def reset_base_position(self, body_id, position): + """ + Reset the base position of the specified body/object id while preserving its orientation. + + Args: + body_id (int): unique object id. + position (np.float[3]): new base position. + """ pass - def apply_external_torque(self): + def reset_base_orientation(self, body_id, orientation): + """ + Reset the base orientation of the specified body/object id while preserving its position. + + Args: + body_id (int): unique object id. + orientation (np.float[4]): new base orientation (expressed as a quaternion [x,y,z,w]) + """ + pass + + def get_base_velocity(self, body_id): + """ + Return the base linear and angular velocities. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.float[3]: linear velocity of the base in Cartesian world space coordinates + np.float[3]: angular velocity of the base in Cartesian world space coordinates + """ + pass + + def get_base_linear_velocity(self, body_id): + """ + Return the linear velocity of the base. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.float[3]: linear velocity of the base in Cartesian world space coordinates + """ + pass + + def get_base_angular_velocity(self, body_id): + """ + Return the angular velocity of the base. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.float[3]: angular velocity of the base in Cartesian world space coordinates + """ + pass + + def reset_base_velocity(self, body_id, linear_velocity=None, angular_velocity=None): + """ + Reset the base velocity. + + Args: + body_id (int): unique object id. + linear_velocity (np.float[3]): new linear velocity of the base. + angular_velocity (np.float[3]): new angular velocity of the base. + """ + pass + + def reset_base_linear_velocity(self, body_id, linear_velocity): + """ + Reset the base linear velocity. + + Args: + body_id (int): unique object id. + linear_velocity (np.float[3]): new linear velocity of the base + """ + pass + + def reset_base_angular_velocity(self, body_id, angular_velocity): + """ + Reset the base angular velocity. + + Args: + body_id (int): unique object id. + angular_velocity (np.float[3]): new angular velocity of the base + """ + pass + + def apply_external_force(self, body_id, link_id=-1, force=(0., 0., 0.), position=(0., 0., 0.), frame=1): + """ + Apply the specified external force on the specified position on the body / link. + + Args: + body_id (int): unique body id. + link_id (int): unique link id. If -1, it will be the base. + force (np.float[3]): external force to be applied. + position (np.float[3]): position on the link where the force is applied. See `flags` for coordinate + systems. + frame (int): if frame = 1, then the force / position is described in the link frame. If frame = 2, they + are described in the world frame. + """ + pass + + def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.)): + """ + Apply an external torque on a body, or a link of the body. Note that after each simulation step, the external + torques are cleared to 0. + + Args: + body_id (int): unique body id. + link_id (int): link id to apply the torque, if -1 it will apply the torque on the base + torque (float[3]): Cartesian torques to be applied on the body + """ pass # robots (joints and links) - def get_num_joints(self): + def num_joints(self, body_id): + """ + Return the total number of joints of the specified body. This is the same as calling `num_links`. + + Args: + body_id (int): unique body id. + + Returns: + int: number of joints with the associated body id. + """ pass - def get_joint_info(self): + def num_links(self, body_id): + """ + Return the total number of links of the specified body. This is the same as calling `num_joints`. + + Args: + body_id (int): unique body id. + + Returns: + int: number of links with the associated body id. + """ + return self.num_joints(body_id) + + def get_joint_info(self, body_id, joint_id): + """ + Return information about the given joint about the specified body. + + Note that this method returns a lot of information, so specific methods have been implemented that return + only the desired information. Also, note that we do not convert the data here. + + Args: + body_id (int): unique body id. + joint_id (int): joint id is included in [0..`num_joints(body_id)`]. + + Returns: + dict, list: joint info + """ pass - def get_joint_state(self): + def get_joint_state(self, body_id, joint_id): + """ + Get the joint state. + + Args: + body_id (int): unique body id. + joint_id (int): joint index in range [0..num_joints(body_id)] + + Returns: + float: The position value of this joint. + float: The velocity value of this joint. + np.float[6]: These are the joint reaction forces, if a torque sensor is enabled for this joint it is + [Fx, Fy, Fz, Mx, My, Mz]. Without torque sensor, it is [0, 0, 0, 0, 0, 0]. + float: This is the motor torque applied during the last stepSimulation. 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. + """ pass - def get_joint_states(self): + def get_joint_states(self, body_id, joint_ids): + """ + Get the joint state of the specified joints. + + Args: + body_id (int): unique body id. + joint_ids (list of int): list of joint ids. + + Returns: + list: + float: The position value of this joint. + float: The velocity value of this joint. + np.float[6]: These are the joint reaction forces, if a torque sensor is enabled for this joint it is + [Fx, Fy, Fz, Mx, My, Mz]. Without torque sensor, it is [0, 0, 0, 0, 0, 0]. + float: 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. + """ pass - def reset_joint_state(self): + def reset_joint_state(self, body_id, joint_id, target_position, target_velocity=0.): + """ + Reset the state of the joint. It is best only to do this at the start, while not running the simulation: + `reset_joint_state` overrides all physics simulation. + + Args: + body_id (int): unique body id. + joint_id (int): joint index in range [0..num_joints(body_id)] + target_position (float): the joint position (angle in radians [rad] or position [m]) + target_velocity (float): the joint velocity (angular [rad/s] or linear velocity [m/s]) + """ pass - def enable_joint_force_torque_sensor(self): + def enable_joint_force_torque_sensor(self, body_id, joint_id, enable=True): + """ + You can enable or disable a joint force/torque sensor in each joint. + + Args: + body_id (int): body unique id. + joint_id (int): joint index in range [0..num_joints(body_id)] + enable (bool): True to enable, False to disable the force/torque sensor + """ pass - def set_joint_motor_control(self): + def set_joint_motor_control(self, body_id, joint_id, control_mode=2, position=None, + velocity=None, force=None, kp=None, kd=None, max_velocity=None): + """ + Set the joint motor control. + + In position control: + .. math:: error = Kp (x_{des} - x) + Kd (\dot{x}_{des} - \dot{x}) + + In velocity control: + .. math:: error = \dot{x}_{des} - \dot{x} + + Note that the maximum forces and velocities are not automatically used for the different control schemes. + + Args: + body_id (int): body unique id. + joint_id (int): joint/link id. + control_mode (int): POSITION_CONTROL (=2) (which is in fact CONTROL_MODE_POSITION_VELOCITY_PD), + VELOCITY_CONTROL (=0), TORQUE_CONTROL (=1) and PD_CONTROL (=3). + position (float): target joint position (used in POSITION_CONTROL). + velocity (float): target joint velocity. In VELOCITY_CONTROL and POSITION_CONTROL, the target velocity is + the desired velocity of the joint. Note that the target velocity is not the maximum joint velocity. + In PD_CONTROL and POSITION_CONTROL/CONTROL_MODE_POSITION_VELOCITY_PD, the final target velocity is + computed using: + `kp*(erp*(desiredPosition-currentPosition)/dt)+currentVelocity+kd*(m_desiredVelocity - currentVelocity)` + force (float): in POSITION_CONTROL and VELOCITY_CONTROL, this is the maximum motor force used to reach the + target value. In TORQUE_CONTROL this is the force/torque to be applied each simulation step. + kp (float): position (stiffness) gain (used in POSITION_CONTROL). + kd (float): velocity (damping) gain (used in POSITION_CONTROL). + max_velocity (float): in POSITION_CONTROL this limits the velocity to a maximum. + """ pass - def set_joint_motor_control_array(self): + def set_joint_motor_control_array(self, body_id, joint_ids, control_mode=2, positions=None, + velocities=None, forces=None, kps=None, kds=None): + """ + Instead of making individual calls for each joint, you can pass arrays for all inputs to reduce calling + overhead dramatically. + + Args: + body_id (int): body unique id. + joint_ids (list of int): list of joint id. + control_mode (int): POSITION_CONTROL (=2) (which is in fact CONTROL_MODE_POSITION_VELOCITY_PD), + VELOCITY_CONTROL (=0), TORQUE_CONTROL (=1) and PD_CONTROL (=3). + positions (list of float): list of target joint positions (used in POSITION_CONTROL) the target value is + target position of the joint. + velocities (list of float): list of target joint velocities (used in PD_CONTROL, VELOCITY_CONTROL and + POSITION_CONTROL). + forces (list of float): list of forces. In POSITION_CONTROL and VELOCITY_CONTROL, these are the maximum + motor forces used to reach the target values. In TORQUE_CONTROL these are the forces/torques to be + applied each simulation step. + kps (list of float): list of position (stiffness) gains (used in POSITION_CONTROL). + kds (list of float): list of velocity (damping) gains (used in POSITION_CONTROL). + """ pass - def get_link_state(self): + def get_link_state(self, body_id, link_id, compute_velocity=False, compute_forward_kinematics=False): + """ + Get the state of the associated link. + + Args: + body_id (int): body unique id. + link_id (int): link index. + compute_velocity (bool): If True, the Cartesian world velocity will be computed and returned. + compute_forward_kinematics (bool): if True, the Cartesian world position/orientation will be recomputed + using forward kinematics. + + Returns: + np.float[3]: Cartesian position of CoM + np.float[4]: Cartesian orientation of CoM, in quaternion [x,y,z,w] + np.float[3]: local position offset of inertial frame (center of mass) expressed in the URDF link frame + np.float[4]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF link + frame + np.float[3]: world position of the URDF link frame + np.float[4]: world orientation of the URDF link frame + np.float[3]: Cartesian world linear velocity. Only returned if `compute_velocity` is True. + np.float[3]: Cartesian world angular velocity. Only returned if `compute_velocity` is True. + """ + pass + + def get_link_states(self, body_id, link_ids, compute_velocity=False, compute_forward_kinematics=False): + """ + Get the state of the associated links. + + Args: + body_id (int): body unique id. + link_ids (list of int): list of link index. + compute_velocity (bool): If True, the Cartesian world velocity will be computed and returned. + compute_forward_kinematics (bool): if True, the Cartesian world position/orientation will be recomputed + using forward kinematics. + + Returns: + list: + np.float[3]: Cartesian position of CoM + np.float[4]: Cartesian orientation of CoM, in quaternion [x,y,z,w] + np.float[3]: local position offset of inertial frame (center of mass) expressed in the URDF link frame + np.float[4]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF + link frame + np.float[3]: world position of the URDF link frame + np.float[4]: world orientation of the URDF link frame + np.float[3]: Cartesian world linear velocity. Only returned if `compute_velocity` is True. + np.float[3]: Cartesian world angular velocity. Only returned if `compute_velocity` is True. + """ + pass + + def get_link_names(self, body_id, link_ids): + """ + Return the name of the given link(s). + + Args: + body_id (int): unique body id. + link_ids (int, list of int): link id, or list of link ids. + + Returns: + if 1 link: + str: link name + if multiple links: + str[N]: link names + """ + pass + + def get_link_masses(self, body_id, link_ids): + """ + Return the mass of the given link(s). + + Args: + body_id (int): unique body id. + link_ids (int, list of int): link id, or list of link ids. + + Returns: + if 1 link: + float: mass of the given link + else: + float[N]: mass of each link + """ + pass + + def get_link_frames(self, body_id, link_ids): + pass + + def get_link_world_positions(self, body_id, link_ids): + """ + Return the CoM position (in the Cartesian world space coordinates) of the given link(s). + + Args: + body_id (int): unique body id. + link_ids (list of int): list of link indices. + + Returns: + if 1 link: + np.float[3]: the link CoM position in the world space + if multiple links: + np.float[N,3]: CoM position of each link in world space + """ + pass + + def get_link_positions(self, body_id, link_ids): + pass + + def get_link_world_orientations(self, body_id, link_ids): + """ + Return the CoM orientation (in the Cartesian world space) of the given link(s). + + Args: + body_id (int): unique body id. + link_ids (list of int): list of link indices. + + Returns: + if 1 link: + np.float[4]: Cartesian orientation of the link CoM (x,y,z,w) + if multiple links: + np.float[N,4]: CoM orientation of each link (x,y,z,w) + """ + pass + + def get_link_orientations(self, body_id, link_ids): + pass + + def get_link_world_linear_velocities(self, body_id, link_ids): + """ + Return the linear velocity of the link(s) expressed in the Cartesian world space coordinates. + + Args: + body_id (int): unique body id. + link_ids (list of int): list of link indices. + + Returns: + if 1 link: + np.float[3]: linear velocity of the link in the Cartesian world space + if multiple links: + np.float[N,3]: linear velocity of each link + """ + pass + + def get_link_world_angular_velocities(self, body_id, link_ids): + """ + Return the angular velocity of the link(s) in the Cartesian world space coordinates. + + Args: + body_id (int): unique body id. + link_ids (list of int): list of link indices. + + Returns: + if 1 link: + np.float[3]: angular velocity of the link in the Cartesian world space + if multiple links: + np.float[N,3]: angular velocity of each link + """ + pass + + def get_link_world_velocities(self, body_id, link_ids): + """ + Return the linear and angular velocities (expressed in the Cartesian world space coordinates) for the given + link(s). + + Args: + body_id (int): unique body id. + link_ids (list of int): list of link indices. + + Returns: + if 1 link: + np.float[6]: linear and angular velocity of the link in the Cartesian world space + if multiple links: + np.float[N,6]: linear and angular velocity of each link + """ + pass + + def get_link_velocities(self, body_id, link_ids): + pass + + def get_qindex(self, body_id, joint_ids): + """ + Get the corresponding q index of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): a joint id, or list of joint ids. + + Returns: + if 1 joint: + int: q index + if multiple joints: + np.int[N]: q indices + """ + pass + + def get_actuated_joint_ids(self, body_id): + """ + Get the actuated joint ids associated with the given body id. + + Warnings: this checks through the list of all joints each time it is called. It might be a good idea to call + this method one time and cache the actuated joint ids. + + Args: + body_id (int): unique body id. + + Returns: + list of int: actuated joint ids. + """ + pass + + def get_joint_names(self, body_id, joint_ids): + """ + Return the name of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): a joint id, or list of joint ids. + + Returns: + if 1 joint: + str: name of the joint + if multiple joints: + str[N]: name of each joint + """ + pass + + def get_joint_dampings(self, body_id, joint_ids): + """ + Get the damping coefficient of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): a joint id, or list of joint ids. + + Returns: + if 1 joint: + float: damping coefficient of the given joint + if multiple joints: + np.float[N]: damping coefficient for each specified joint + """ + pass + + def get_joint_frictions(self, body_id, joint_ids): + """ + Get the friction coefficient of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): a joint id, or list of joint ids. + + Returns: + if 1 joint: + float: friction coefficient of the given joint + if multiple joints: + float[N]: friction coefficient for each specified joint + """ + pass + + def get_joint_limits(self, body_id, joint_ids): + """ + Get the joint limits of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): a joint id, or list of joint ids. + + Returns: + if 1 joint: + np.float[2]: lower and upper limit + if multiple joints: + np.float[N,2]: lower and upper limit for each specified joint + """ + pass + + def get_joint_max_forces(self, body_id, joint_ids): + """ + Get the maximum force that can be applied on the given joint(s). + + Warning: Note that this is not automatically used in position, velocity, or torque control. + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): a joint id, or list of joint ids. + + Returns: + if 1 joint: + float: maximum force [N] + if multiple joints: + float[N]: maximum force for each specified joint [N] + """ + pass + + def get_joint_max_velocities(self, body_id, joint_ids): + """ + Get the maximum velocity that can be applied on the given joint(s). + + Warning: Note that this is not automatically used in position, velocity, or torque control. + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): a joint id, or list of joint ids. + + Returns: + if 1 joint: + float: maximum velocity [rad/s] + if multiple joints: + np.float[N]: maximum velocities for each specified joint [rad/s] + """ + pass + + def get_joint_axes(self, body_id, joint_ids): + """ + Get the joint axis about the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): a joint id, or list of joint ids. + + Returns: + if 1 joint: + np.float[3]: joint axis + if multiple joint: + np.float[N,3]: list of joint axis + """ + pass + + def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None): + """ + Set the position of the given joint(s) (using position control). + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): joint id, or list of joint ids. + positions (float, np.float[N]): desired position, or list of desired positions [rad] + velocities (None, float, np.float[N]): desired velocity, or list of desired velocities [rad/s] + kps (None, float, np.float[N]): position gain(s) + kds (None, float, np.float[N]): velocity gain(s) + forces (None, float, np.float[N]): maximum motor force(s)/torque(s) used to reach the target values. + """ + pass + + def get_joint_positions(self, body_id, joint_ids): + """ + Get the position of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): joint id, or list of joint ids. + + Returns: + if 1 joint: + float: joint position [rad] + if multiple joints: + np.float[N]: joint positions [rad] + """ + pass + + def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None): + """ + Set the velocity of the given joint(s) (using velocity control). + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): joint id, or list of joint ids. + velocities (float, np.float[N]): desired velocity, or list of desired velocities [rad/s] + max_force (None, float, np.float[N]): maximum motor forces/torques + """ + pass + + def get_joint_velocities(self, body_id, joint_ids): + """ + Get the velocity of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): joint id, or list of joint ids. + + Returns: + if 1 joint: + float: joint velocity [rad/s] + if multiple joints: + np.float[N]: joint velocities [rad/s] + """ + pass + + def set_joint_accelerations(self, body_id, joint_ids, accelerations, q=None, dq=None): + """ + Set the acceleration of the given joint(s) (using force control). This is achieved by performing inverse + dynamic which given the joint accelerations compute the joint torques to be applied. + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): joint id, or list of joint ids. + accelerations (float, np.float[N]): desired joint acceleration, or list of desired joint accelerations + [rad/s^2] + """ + pass + + def get_joint_accelerations(self, body_id, joint_ids, q=None, dq=None): + """ + Get the acceleration at the given joint(s). This is carried out by first getting the joint torques, then + performing forward dynamics to get the joint accelerations from the joint torques. + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): joint id, or list of joint ids. + q (list of int, None): all the joint positions. If None, it will compute it. + dq (list of int, None): all the joint velocities. If None, it will compute it. + + Returns: + if 1 joint: + float: joint acceleration [rad/s^2] + if multiple joints: + np.float[N]: joint accelerations [rad/s^2] + """ + pass + + def set_joint_torques(self, body_id, joint_ids, torques): + """ + Set the torque/force to the given joint(s) (using force/torque control). + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): joint id, or list of joint ids. + torques (float, list of float): desired torque(s) to apply to the joint(s) [N]. + """ + pass + + def get_joint_torques(self, body_id, joint_ids): + """ + Get the applied torque(s) on the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): a joint id, or list of joint ids. + + Returns: + if 1 joint: + float: torque [Nm] + if multiple joints: + np.float[N]: torques associated to the given joints [Nm] + """ + pass + + def get_joint_reaction_forces(self, body_id, joint_ids): + """Return the joint reaction forces at the given joint. Note that the torque sensor must be enabled, otherwise + it will always return [0,0,0,0,0,0]. + + Args: + body_id (int): unique body id. + joint_ids (int, int[N]): joint id, or list of joint ids + + Returns: + if 1 joint: + np.float[6]: joint reaction force (fx,fy,fz,mx,my,mz) [N,Nm] + if multiple joints: + np.float[N,6]: joint reaction forces [N, Nm] + """ + pass + + def get_joint_powers(self, body_id, joint_ids): + """Return the applied power at the given joint(s). Power = torque * velocity. + + Args: + body_id (int): unique body id. + joint_ids (int, int[N]): joint id, or list of joint ids + + Returns: + if 1 joint: + float: joint power [W] + if multiple joints: + np.float[N]: power at each joint [W] + """ pass # visualization - def compute_view_matrix(self): + def create_visual_shape(self, shape_type, radius=0.5, half_extents=(1., 1., 1.), length=1., filename=None, + mesh_scale=(1., 1., 1.), plane_normal=(0., 0., 1.), flags=-1, rgba_color=None, + specular_color=None, visual_frame_position=None, vertices=None, indices=None, uvs=None, + normals=None, visual_frame_orientation=None): + """ + Create a visual shape in the simulator. + + Args: + shape_type (int): type of shape; GEOM_SPHERE (=2), GEOM_BOX (=3), GEOM_CAPSULE (=7), GEOM_CYLINDER (=4), + GEOM_PLANE (=6), GEOM_MESH (=5) + radius (float): only for GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER + half_extents (np.float[3], list/tuple of 3 floats): only for GEOM_BOX. + length (float): only for GEOM_CAPSULE, GEOM_CYLINDER (length = height). + filename (str): Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each + object (marked as 'o') in the .obj file. + mesh_scale (np.float[3], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH). + plane_normal (np.float[3], list/tuple of 3 floats): plane normal (only for GEOM_PLANE). + flags (int): unused / to be decided + rgba_color (list/tuple of 4 floats): color components for red, green, blue and alpha, each in range [0..1]. + specular_color (list/tuple of 3 floats): specular reflection color, red, green, blue components in range + [0..1] + visual_frame_position (np.float[3]): translational offset of the visual shape with respect to the link frame + vertices (list of np.float[3]): Instead of creating a mesh from obj file, you can provide vertices, indices, + uvs and normals + indices (list of int): triangle indices, should be a multiple of 3. + uvs (list of np.float[2]): uv texture coordinates for vertices. Use changeVisualShape to choose the + texture image. The number of uvs should be equal to number of vertices + normals (list of np.float[3]): vertex normals, number should be equal to number of vertices. + visual_frame_orientation (np.float[4]): rotational offset (quaternion x,y,z,w) of the visual shape with + respect to the link frame + + Returns: + int: The return value is a non-negative int unique id for the visual shape or -1 if the call failed. + """ pass - def compute_projection_matrix(self): + def get_visual_shape_data(self, object_id, flags=-1): + """ + Get the visual shape data associated with the given object id. + + Args: + object_id (int): object unique id. + flags (int, None): VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) will also provide `texture_unique_id`. + + Returns: + int: object unique id. + int: link index or -1 for the base + int: visual geometry type (TBD) + np.float[3]: dimensions (size, local scale) of the geometry + str: path to the triangle mesh, if any. Typically relative to the URDF, SDF or MJCF file location, but + could be absolute + np.float[3]: position of local visual frame, relative to link/joint frame + np.float[4]: orientation of local visual frame relative to link/joint frame + list of 4 floats: URDF color (if any specified) in Red / Green / Blue / Alpha + int: texture unique id of the shape or -1 if None. This field only exists if using + VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) flag. + """ pass - def get_camera_image(self): + def change_visual_shape(self, object_id, link_id, shape_id=None, texture_id=None, rgba_color=None, + specular_color=None): + """ + Allows to change the texture of a shape, the RGBA color and other properties. + + Args: + object_id (int): unique object id. + link_id (int): link id. + shape_id (int): shape id. + texture_id (int): texture id. + rgba_color (float[4]): RGBA color. Each is in the range [0..1]. Alpha has to be 0 (invisible) or 1 + (visible) at the moment. + specular_color (int[3]): specular color components, RED, GREEN and BLUE, can be from 0 to large number + (>100). + """ pass - def load_texture(self): + def load_texture(self, filename): + """ + Load a texture from file and return a non-negative texture unique id if the loading succeeds. + This unique id can be used with changeVisualShape. + + Args: + filename (str): path to the file. + + Returns: + int: texture unique id. If non-negative, the texture was loaded successfully. + """ + pass + + def compute_view_matrix(self, eye_position, target_position, up_vector): + """Compute the view matrix. + + The view matrix is the 4x4 matrix that maps the world coordinates into the camera coordinates. Basically, + it applies a rotation and translation such that the world is in front of the camera. That is, instead + of turning the camera to capture what we want in the world, we keep the camera fixed and turn the world. + + Args: + eye_position (np.float[3]): eye position in Cartesian world coordinates + target_position (np.float[3]): position of the target (focus) point in Cartesian world coordinates + up_vector (np.float[3]): up vector of the camera in Cartesian world coordinates + + Returns: + np.float[4,4]: the view matrix + """ + pass + + def compute_view_matrix_from_ypr(self, target_position, distance, yaw, pitch, roll, up_axis_index=2): + """Compute the view matrix from the yaw, pitch, and roll angles. + + The view matrix is the 4x4 matrix that maps the world coordinates into the camera coordinates. Basically, + it applies a rotation and translation such that the world is in front of the camera. That is, instead + of turning the camera to capture what we want in the world, we keep the camera fixed and turn the world. + + Args: + target_position (np.float[3]): target focus point in Cartesian world coordinates + distance (float): distance from eye to focus point + yaw (float): yaw angle in radians left/right around up-axis + pitch (float): pitch in radians up/down. + roll (float): roll in radians around forward vector + up_axis_index (int): either 1 for Y or 2 for Z axis up. + + Returns: + np.float[4,4]: the view matrix + """ + pass + + def compute_projection_matrix(self, left, right, bottom, top, near, far): + """Compute the orthographic projection matrix. + + The projection matrix is the 4x4 matrix that maps from the camera/eye coordinates to clipped coordinates. + It is applied after the view matrix. + + There are 2 projection matrices: + * orthographic projection + * perspective projection + + For the perspective projection, see `computeProjectionMatrixFOV(self)`. + + Args: + left (float): left screen (canvas) coordinate + right (float): right screen (canvas) coordinate + bottom (float): bottom screen (canvas) coordinate + top (float): top screen (canvas) coordinate + near (float): near plane distance + far (float): far plane distance + + Returns: + np.float[4,4]: the perspective projection matrix + """ + pass + + def compute_projection_matrix_fov(self, fov, aspect, near, far): + """Compute the perspective projection matrix using the field of view (FOV). + + Args: + fov (float): field of view + aspect (float): aspect ratio + near (float): near plane distance + far (float): far plane distance + + Returns: + np.float[4,4]: the perspective projection matrix + """ + pass + + def get_camera_image(self, width, height, view_matrix=None, projection_matrix=None, light_direction=None, + light_color=None, light_distance=None, shadow=None, light_ambient_coeff=None, + light_diffuse_coeff=None, light_specular_coeff=None, renderer=None, flags=None): + """ + The `get_camera_image` API will return a RGB image, a depth buffer and a segmentation mask buffer with body + unique ids of visible objects for each pixel. + + Args: + width (int): horizontal image resolution in pixels + height (int): vertical image resolution in pixels + view_matrix (np.float[4,4]): 4x4 view matrix, see `compute_view_matrix` + projection_matrix (np.float[4,4]): 4x4 projection matrix, see `compute_projection` + light_direction (np.float[3]): `light_direction` specifies the world position of the light source, + the direction is from the light source position to the origin of the world frame. + light_color (np.float[3]): directional light color in [RED,GREEN,BLUE] in range 0..1 + light_distance (float): distance of the light along the normalized `light_direction` + shadow (bool): True for shadows, False for no shadows + light_ambient_coeff (float): light ambient coefficient + light_diffuse_coeff (float): light diffuse coefficient + light_specular_coeff (float): light specular coefficient + renderer (int): renderer + flags (int): flags + + Returns: + int: width image resolution in pixels (horizontal) + int: height image resolution in pixels (vertical) + np.int[width, height, 4]: RBGA pixels (each pixel is in the range [0..255] for each channel R, G, B, A) + np.float[width, heigth]: Depth buffer. + np.int[width, height]: Segmentation mask buffer. For each pixels the visible object unique id. + """ + pass + + def get_rgba_image(self, width, height, view_matrix=None, projection_matrix=None, light_direction=None, + light_color=None, light_distance=None, shadow=None, light_ambient_coeff=None, + light_diffuse_coeff=None, light_specular_coeff=None, renderer=None, flags=None): + """ + The `get_rgba_image` API will return a RGBA image. + + Args: + width (int): horizontal image resolution in pixels + height (int): vertical image resolution in pixels + view_matrix (np.float[4,4]): 4x4 view matrix, see `compute_view_matrix` + projection_matrix (np.float[4,4]): 4x4 projection matrix, see `compute_projection` + light_direction (np.float[3]): `light_direction` specifies the world position of the light source, + the direction is from the light source position to the origin of the world frame. + light_color (np.float[3]): directional light color in [RED,GREEN,BLUE] in range 0..1 + light_distance (float): distance of the light along the normalized `light_direction` + shadow (bool): True for shadows, False for no shadows + light_ambient_coeff (float): light ambient coefficient + light_diffuse_coeff (float): light diffuse coefficient + light_specular_coeff (float): light specular coefficient + renderer (int): renderer. + flags (int): flags. + + Returns: + np.int[width, height, 4]: RBGA pixels (each pixel is in the range [0..255] for each channel R, G, B, A) + """ + pass + + def get_depth_image(self, width, height, view_matrix=None, projection_matrix=None, light_direction=None, + light_color=None, light_distance=None, shadow=None, light_ambient_coeff=None, + light_diffuse_coeff=None, light_specular_coeff=None, renderer=None, flags=None): + """ + The `get_depth_image` API will return a depth buffer. + + Args: + width (int): horizontal image resolution in pixels + height (int): vertical image resolution in pixels + view_matrix (np.float[4,4]): 4x4 view matrix, see `compute_view_matrix` + projection_matrix (np.float[4,4]): 4x4 projection matrix, see `compute_projection` + light_direction (np.float[3]): `light_direction` specifies the world position of the light source, + the direction is from the light source position to the origin of the world frame. + light_color (np.float[3]): directional light color in [RED,GREEN,BLUE] in range 0..1 + light_distance (float): distance of the light along the normalized `light_direction` + shadow (bool): True for shadows, False for no shadows + light_ambient_coeff (float): light ambient coefficient + light_diffuse_coeff (float): light diffuse coefficient + light_specular_coeff (float): light specular coefficient + renderer (int): renderer. + flags (int): flags. + + Returns: + np.float[width, heigth]: Depth buffer. + """ + pass + + def get_segmentation_image(self, width, height, view_matrix=None, projection_matrix=None, light_direction=None, + light_color=None, light_distance=None, shadow=None, light_ambient_coeff=None, + light_diffuse_coeff=None, light_specular_coeff=None, renderer=None, flags=None): + """ + The `get_segmentation_image` API will return a segmentation mask buffer with body unique ids of visible objects + for each pixel. + + Args: + width (int): horizontal image resolution in pixels + height (int): vertical image resolution in pixels + view_matrix (np.float[4,4]): 4x4 view matrix, see `compute_view_matrix` + projection_matrix (np.float[4,4]): 4x4 projection matrix, see `compute_projection` + light_direction (np.float[3]): `light_direction` specifies the world position of the light source, + the direction is from the light source position to the origin of the world frame. + light_color (np.float[3]): directional light color in [RED,GREEN,BLUE] in range 0..1 + light_distance (float): distance of the light along the normalized `light_direction` + shadow (bool): True for shadows, False for no shadows + light_ambient_coeff (float): light ambient coefficient + light_diffuse_coeff (float): light diffuse coefficient + light_specular_coeff (float): light specular coefficient + renderer (int): renderer + flags (int): flags + + Returns: + np.int[width, height]: Segmentation mask buffer. For each pixels the visible object unique id. + """ pass # collisions - def get_overlapping_objects(self): + def create_collision_shape(self, shape_type, radius=0.5, half_extents=(1., 1., 1.), height=1., filename=None, + mesh_scale=(1., 1., 1.), plane_normal=(0., 0., 1.), flags=-1, + collision_frame_position=None, collision_frame_orientation=None): + """ + Create collision shape in the simulator. + + Args: + shape_type (int): type of shape; GEOM_SPHERE (=2), GEOM_BOX (=3), GEOM_CAPSULE (=7), GEOM_CYLINDER (=4), + GEOM_PLANE (=6), GEOM_MESH (=5) + radius (float): only for GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER + half_extents (np.float[3], list/tuple of 3 floats): only for GEOM_BOX. + height (float): only for GEOM_CAPSULE, GEOM_CYLINDER (length = height). + filename (str): Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each + object (marked as 'o') in the .obj file. + mesh_scale (np.float[3], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH). + plane_normal (np.float[3], list/tuple of 3 floats): plane normal (only for GEOM_PLANE). + flags (int): unused / to be decided + collision_frame_position (np.float[3]): translational offset of the collision shape with respect to the + link frame + collision_frame_orientation (np.float[4]): rotational offset (quaternion x,y,z,w) of the collision shape + with respect to the link frame + + Returns: + int: The return value is a non-negative int unique id for the collision shape or -1 if the call failed. + """ pass - def get_aabb(self): + def get_collision_shape_data(self, object_id, link_id=-1): + """ + Get the collision shape data associated with the specified object id and link id. + + Args: + object_id (int): object unique id. + link_id (int): link index or -1 for the base. + + Returns: + int: object unique id. + int: link id. + int: geometry type; GEOM_BOX (=3), GEOM_SPHERE (=2), GEOM_CAPSULE (=7), GEOM_MESH (=5), GEOM_PLANE (=6) + np.float[3]: depends on geometry type: + for GEOM_BOX: extents, + for GEOM_SPHERE: dimensions[0] = radius, + for GEOM_CAPSULE and GEOM_CYLINDER: dimensions[0] = height (length), dimensions[1] = radius. + For GEOM_MESH: dimensions is the scaling factor. + str: Only for GEOM_MESH: file name (and path) of the collision mesh asset. + np.float[3]: Local position of the collision frame with respect to the center of mass/inertial frame + np.float[4]: Local orientation of the collision frame with respect to the inertial frame + """ pass - def get_contact_points(self): + def get_overlapping_objects(self, aabb_min, aabb_max): + """ + This query will return all the unique ids of objects that have Axis Aligned Bounding Box (AABB) overlap with + a given axis aligned bounding box. Note that the query is conservative and may return additional objects that + don't have actual AABB overlap. This happens because the acceleration structures have some heuristic that + enlarges the AABBs a bit (extra margin and extruded along the velocity vector). + + Args: + aabb_min (np.float[3]): minimum coordinates of the aabb + aabb_max (np.float[3]): maximum coordinates of the aabb + + Returns: + list of int: list of object unique ids. + """ pass - def get_closest_points(self): + def get_aabb(self, body_id, link_id=-1): + """ + You can query the axis aligned bounding box (in world space) given an object unique id, and optionally a link + index. (when you don't pass the link index, or use -1, you get the AABB of the base). + + Args: + body_id (int): object unique id as returned by creation methods + link_id (int): link index in range [0..`getNumJoints(..)] + + Returns: + np.float[3]: minimum coordinates of the axis aligned bounding box + np.float[3]: maximum coordinates of the axis aligned bounding box + """ pass - def ray_test(self): + def get_contact_points(self, body_a, body_b, link_id_a=None, link_id_b=None): + """ + Returns the contact points computed during the most recent call to `step`. + + Args: + body_a (int): only report contact points that involve body A + body_b (int): only report contact points that involve body B. Important: you need to have a valid body A + if you provide body B + link_id_a (int): only report contact points that involve link index of body A + link_id_b (int): only report contact points that involve link index of body B + + Returns: + list: + int: contact flag (reserved) + int: body unique id of body A + int: body unique id of body B + int: link index of body A, -1 for base + int: link index of body B, -1 for base + np.float[3]: contact position on A, in Cartesian world coordinates + np.float[3]: contact position on B, in Cartesian world coordinates + np.float[3]: contact normal on B, pointing towards A + float: contact distance, positive for separation, negative for penetration + float: normal force applied during the last `step` + float: lateral friction force in the first lateral friction direction (see next returned value) + np.float[3]: first lateral friction direction + float: lateral friction force in the second lateral friction direction (see next returned value) + np.float[3]: second lateral friction direction + """ pass - def ray_test_batch(self): + def get_closest_points(self, body_a, body_b, distance, link_id_a=None, link_id_b=None): + """ + Computes the closest points, independent from `step`. This also lets you compute closest points of objects + with an arbitrary separating distance. In this query there will be no normal forces reported. + + Args: + body_a (int): only report contact points that involve body A + body_b (int): only report contact points that involve body B. Important: you need to have a valid body A + if you provide body B + distance (float): If the distance between objects exceeds this maximum distance, no points may be returned. + link_id_a (int): only report contact points that involve link index of body A + link_id_b (int): only report contact points that involve link index of body B + + Returns: + list: + int: contact flag (reserved) + int: body unique id of body A + int: body unique id of body B + int: link index of body A, -1 for base + int: link index of body B, -1 for base + np.float[3]: contact position on A, in Cartesian world coordinates + np.float[3]: contact position on B, in Cartesian world coordinates + np.float[3]: contact normal on B, pointing towards A + float: contact distance, positive for separation, negative for penetration + float: normal force applied during the last `step`. Always equal to 0. + float: lateral friction force in the first lateral friction direction (see next returned value) + np.float[3]: first lateral friction direction + float: lateral friction force in the second lateral friction direction (see next returned value) + np.float[3]: second lateral friction direction + """ + pass + + def ray_test(self, from_position, to_position): + """ + Performs a single raycast to find the intersection information of the first object hit. + + Args: + from_position (np.float[3]): start of the ray in world coordinates + to_position (np.float[3]): end of the ray in world coordinates + + Returns: + int: object unique id of the hit object + int: link index of the hit object, or -1 if none/parent + float: hit fraction along the ray in range [0,1] along the ray. + np.float[3]: hit position in Cartesian world coordinates + np.float[3]: hit normal in Cartesian world coordinates + """ + pass + + def ray_test_batch(self, from_positions, to_positions, parent_object_id=None, parent_link_id=None): + """Perform a batch of raycasts to find the intersection information of the first objects hit. + + This is similar to the rayTest, but allows you to provide an array of rays, for faster execution. The size of + 'rayFromPositions' needs to be equal to the size of 'rayToPositions'. You can one ray result per ray, even if + there is no intersection: you need to use the objectUniqueId field to check if the ray has hit anything: if + the objectUniqueId is -1, there is no hit. In that case, the 'hit fraction' is 1. + + Args: + from_positions (np.array[N,3]): list of start points for each ray, in world coordinates + to_positions (np.array[N,3]): list of end points for each ray in world coordinates + parent_object_id (int): ray from/to is in local space of a parent object + parent_link_id (int): ray from/to is in local space of a parent object + + Returns: + list: + int: object unique id of the hit object + int: link index of the hit object, or -1 if none/parent + float: hit fraction along the ray in range [0,1] along the ray. + np.float[3]: hit position in Cartesian world coordinates + np.float[3]: hit normal in Cartesian world coordinates + """ + pass + + def set_collision_filter_group_mask(self, body_id, link_id, filter_group, filter_mask): + """ + Enable/disable collision detection between groups of objects. Each body is part of a group. It collides with + other bodies if their group matches the mask, and vise versa. The following check is performed using the group + and mask of the two bodies involved. It depends on the collision filter mode. + + Args: + body_id (int): unique id of the body to be configured + link_id (int): link index of the body to be configured + filter_group (int): bitwise group of the filter + filter_mask (int): bitwise mask of the filter + """ + pass + + def set_collision_filter_pair(self, body_a, body_b, link_a=-1, link_b=-1, enable=True): + """ + Enable/disable collision between two bodies/links. + + Args: + body_a (int): unique id of body A to be filtered + body_b (int): unique id of body B to be filtered, A==B implies self-collision + link_a (int): link index of body A + link_b (int): link index of body B + enable (bool): True to enable collision, False to disable collision + """ pass # kinematics and dynamics - def get_dynamics_info(self): + def get_dynamics_info(self, body_id, link_id=-1): + """ + Get dynamic information about the mass, center of mass, friction and other properties of the base and links. + + Args: + body_id (int): body/object unique id. + link_id (int): link/joint index or -1 for the base. + + Returns: + float: mass in kg + float: friction coefficient + np.float[3]: local inertia diagonal. Note that links and base are centered around the center of mass and + aligned with the principal axes of inertia. + np.float[3]: position of inertial frame in local coordinates of the joint frame + np.float[4]: orientation of inertial frame in local coordinates of joint frame + float: coefficient of restitution + float: rolling friction coefficient orthogonal to contact normal + float: spinning friction coefficient around contact normal + float: damping of contact constraints. -1 if not available. + float: stiffness of contact constraints. -1 if not available. + """ pass - def change_dynamics(self): + def change_dynamics(self, body_id, link_id=-1, mass=None, lateral_friction=None, spinning_friction=None, + rolling_friction=None, restitution=None, linear_damping=None, angular_damping=None, + contact_stiffness=None, contact_damping=None, friction_anchor=None, + local_inertia_diagonal=None, joint_damping=None): + """ + Change dynamic properties such as mass, friction and restitution coefficients . + + Args: + body_id (int): object unique id, as returned by `load_urdf`, etc. + link_id (int): link index or -1 for the base. + mass (float): change the mass of the link (or base for link index -1) + lateral_friction (float): lateral (linear) contact friction + spinning_friction (float): torsional friction around the contact normal + rolling_friction (float): torsional friction orthogonal to contact normal + restitution (float): bouncyness of contact. Keep it a bit less than 1. + linear_damping (float): linear damping of the link (0.04 by default) + angular_damping (float): angular damping of the link (0.04 by default) + contact_stiffness (float): stiffness of the contact constraints, used together with `contact_damping` + contact_damping (float): damping of the contact constraints for this body/link. Used together with + `contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact + section. + friction_anchor (int): enable or disable a friction anchor: positional friction correction (disabled by + default, unless set in the URDF contact section) + local_inertia_diagonal (np.float[3]): diagonal elements of the inertia tensor. Note that the base and + links are centered around the center of mass and aligned with the principal axes of inertia so there + are no off-diagonal elements in the inertia tensor. + joint_damping (float): joint damping coefficient applied at each joint. This coefficient is read from URDF + joint damping field. Keep the value close to 0. + `joint_damping_force = -damping_coefficient * joint_velocity`. + """ pass - def calculate_jacobian(self): + def calculate_jacobian(self, body_id, link_id, local_position, q, dq, des_ddq): + """ + Return the full geometric Jacobian matrix :math:`J(q) = [J_{lin}(q), J_{ang}(q)]^T`, such that: + + .. math:: v = [\dot{p}, \omega]^T = J(q) \dot{q} + + where :math:`\dot{p}` is the Cartesian linear velocity of the link, and :math:`\omega` is its angular velocity. + + Warnings: if we have a floating base then the Jacobian will also include columns corresponding to the root + link DoFs (at the beginning). If it is a fixed base, it will only have columns associated with the joints. + + Args: + body_id (int): unique body id. + link_id (int): link id. + local_position (np.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). + q (np.float[N]): joint positions of size N, where N is the number of DoFs. + dq (np.float[N]): joint velocities of size N, where N is the number of DoFs. + des_ddq (np.float[N]): desired joint accelerations of size N. + + Returns: + np.float[6,N], np.float[6,(6+N)]: full geometric (linear and angular) Jacobian matrix. The number of + columns depends if the base is fixed or floating. + """ pass - def calculate_mass_matrix(self): + def calculate_mass_matrix(self, body_id, q): + """ + Return the mass/inertia matrix :math:`H(q)`, which is used in the rigid-body equation of motion (EoM) in joint + space given by (see [1]): + + .. math:: \tau = H(q)\ddot{q} + C(q,\dot{q}) + + where :math:`\tau` is the vector of applied torques, :math:`H(q)` is the inertia matrix, and + :math:`C(q,\dot{q}) \dot{q}` is the vector accounting for Coriolis, centrifugal forces, gravity, and any + other forces acting on the system except the applied torques :math:`\tau`. + + Warnings: If the base is floating, it will return a [6+N,6+N] inertia matrix, where N is the number of actuated + joints. If the base is fixed, it will return a [N,N] inertia matrix + + Args: + body_id (int): body unique id. + q (np.float[N]): joint positions of size N, where N is the total number of DoFs. + + Returns: + np.float[N,N], np.float[6+N,6+N]: inertia matrix + """ pass - def calculate_inverse_kinematics(self): + def calculate_inverse_kinematics(self, body_id, link_id, position, orientation=None, lower_limits=None, + upper_limits=None, joint_ranges=None, rest_poses=None, joint_dampings=None, + solver=None, q_curr=None, max_iters=None, threshold=None): + """ + Compute the FULL Inverse kinematics; it will return a position for all the actuated joints. + + "You can compute the joint angles that makes the end-effector reach a given target position in Cartesian world + space. Internally, Bullet uses an improved version of Samuel Buss Inverse Kinematics library. At the moment + only the Damped Least Squares method with or without Null Space control is exposed, with a single end-effector + target. Optionally you can also specify the target orientation of the end effector. In addition, there is an + option to use the null-space to specify joint limits and rest poses. This optional null-space support requires + all 4 lists (lower_limits, upper_limits, joint_ranges, rest_poses), otherwise regular IK will be used." [1] + + Args: + body_id (int): body unique id, as returned by `load_urdf`, etc. + link_id (int): end effector link index. + position (np.float[3]): target position of the end effector (its link coordinate, not center of mass + coordinate!). By default this is in Cartesian world space, unless you provide `q_curr` joint angles. + orientation (np.float[4]): target orientation in Cartesian world space, quaternion [x,y,w,z]. If not + specified, pure position IK will be used. + lower_limits (np.float[N], list of N floats): lower joint limits. Optional null-space IK. + upper_limits (np.float[N], list of N floats): upper joint limits. Optional null-space IK. + joint_ranges (np.float[N], list of N floats): range of value of each joint. + rest_poses (np.float[N], list of N floats): joint rest poses. Favor an IK solution closer to a given rest + pose. + joint_dampings (np.float[N], list of N floats): joint damping factors. Allow to tune the IK solution using + joint damping factors. + solver (int): p.IK_DLS (=0) or p.IK_SDLS (=1), Damped Least Squares or Selective Damped Least Squares, as + described in the paper by Samuel Buss "Selectively Damped Least Squares for Inverse Kinematics". + q_curr (np.float[N]): list of joint positions. By default PyBullet uses the joint positions of the body. + If provided, the targetPosition and targetOrientation is in local space! + max_iters (int): maximum number of iterations. Refine the IK solution until the distance between target + and actual end effector position is below this threshold, or the `max_iters` is reached. + threshold (float): residual threshold. Refine the IK solution until the distance between target and actual + end effector position is below this threshold, or the `max_iters` is reached. + + Returns: + np.float[N]: joint positions (for each actuated joint). + """ pass - def calculate_inverse_dynamics(self): + def calculate_inverse_dynamics(self, body_id, q, dq, des_ddq): + r""" + Starting from the specified joint positions :math:`q` and velocities :math:`\dot{q}`, it computes the joint + torques :math:`\tau` required to reach the desired joint accelerations :math:`\ddot{q}_{des}`. That is, + :math:`\tau = ID(model, q, \dot{q}, \ddot{q}_{des})`. + + Specifically, it uses the rigid-body equation of motion in joint space given by (see [1]): + + .. math:: \tau = H(q)\ddot{q} + C(q,\dot{q}) + + where :math:`\tau` is the vector of applied torques, :math:`H(q)` is the inertia matrix, and + :math:`C(q,\dot{q}) \dot{q}` is the vector accounting for Coriolis, centrifugal forces, gravity, and any + other forces acting on the system except the applied torques :math:`\tau`. + + Normally, a more popular form of this equation of motion (in joint space) is given by: + + .. math:: H(q) \ddot{q} + S(q,\dot{q}) \dot{q} + g(q) = \tau + J^T(q) F + + which is the same as the first one with :math:`C = S\dot{q} + g(q) - J^T(q) F`. However, this last formulation + is useful to understand what happens when we set some variables to 0. + Assuming that there are no forces acting on the system, and giving desired joint accelerations of 0, this + method will return :math:`\tau = S(q,\dot{q}) \dot{q} + g(q)`. If in addition joint velocities are also 0, + it will return :math:`\tau = g(q)` which can for instance be useful for gravity compensation. + + For forward dynamics, which computes the joint accelerations given the joint positions, velocities, and + torques (that is, :math:`\ddot{q} = FD(model, q, \dot{q}, \tau)`, this can be computed using + :math:`\ddot{q} = H^{-1} (\tau - C)` (see also `computeFullFD`). For more information about different + control schemes (position, force, impedance control and others), or about the formulation of the equation + of motion in task/operational space (instead of joint space), check the references [1-4]. + + Args: + body_id (int): body unique id. + q (np.float[N]): joint positions + dq (np.float[N]): joint velocities + des_ddq (np.float[N]): desired joint accelerations + + Returns: + np.float[N]: joint torques computed using the rigid-body equation of motion + + References: + [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1 + [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010 + [3] "Springer Handbook of Robotics", Siciliano et al., 2008 + [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma, + http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf + """ pass - def calculate_forward_dynamics(self): + def calculate_forward_dynamics(self, body_id, q, dq, torques): + r""" + Given the specified joint positions :math:`q` and velocities :math:`\dot{q}`, and joint torques :math:`\tau`, + it computes the joint accelerations :math:`\ddot{q}`. That is, :math:`\ddot{q} = FD(model, q, \dot{q}, \tau)`. + + Specifically, it uses the rigid-body equation of motion in joint space given by (see [1]): + + .. math:: \ddot{q} = H(q)^{-1} (\tau - C(q,\dot{q})) + + where :math:`\tau` is the vector of applied torques, :math:`H(q)` is the inertia matrix, and + :math:`C(q,\dot{q}) \dot{q}` is the vector accounting for Coriolis, centrifugal forces, gravity, and any + other forces acting on the system except the applied torques :math:`\tau`. + + Normally, a more popular form of this equation of motion (in joint space) is given by: + + .. math:: H(q) \ddot{q} + S(q,\dot{q}) \dot{q} + g(q) = \tau + J^T(q) F + + which is the same as the first one with :math:`C = S\dot{q} + g(q) - J^T(q) F`. However, this last formulation + is useful to understand what happens when we set some variables to 0. + Assuming that there are no forces acting on the system, and giving desired joint torques of 0, this + method will return :math:`\ddot{q} = - H(q)^{-1} (S(q,\dot{q}) \dot{q} + g(q))`. If in addition + the joint velocities are also 0, it will return :math:`\ddot{q} = - H(q)^{-1} g(q)` which are + the accelerations due to gravity. + + For inverse dynamics, which computes the joint torques given the joint positions, velocities, and + accelerations (that is, :math:`\tau = ID(model, q, \dot{q}, \ddot{q})`, this can be computed using + :math:`\tau = H(q)\ddot{q} + C(q,\dot{q})`. For more information about different + control schemes (position, force, impedance control and others), or about the formulation of the equation + of motion in task/operational space (instead of joint space), check the references [1-4]. + + Args: + body_id (int): unique body id. + q (np.float[N]): joint positions + dq (np.float[N]): joint velocities + torques (np.float[N]): desired joint torques + + Returns: + float[N]: joint accelerations computed using the rigid-body equation of motion + + References: + [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1 + [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010 + [3] "Springer Handbook of Robotics", Siciliano et al., 2008 + [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma, + http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf + """ pass # debug - def add_user_debug_line(self): + def add_user_debug_line(self, from_pos, to_pos, rgb_color=None, width=None, lifetime=None, parent_object_id=None, + parent_link_id=None, line_id=None): + """Add a user debug line in the simulator. + + You can add a 3d line specified by a 3d starting point (from) and end point (to), a color [red,green,blue], + a line width and a duration in seconds. + + Args: + from_pos (np.float[3]): starting point of the line in Cartesian world coordinates + to_pos (np.float[3]): end point of the line in Cartesian world coordinates + rgb_color (np.float[3]): RGB color (each channel in range [0,1]) + width (float): line width (limited by OpenGL implementation). + lifetime (float): use 0 for permanent line, or positive time in seconds (afterwards the line with be + removed automatically) + parent_object_id (int): draw line in local coordinates of a parent object. + parent_link_id (int): draw line in local coordinates of a parent link. + line_id (int): replace an existing line item (to avoid flickering of remove/add). + + Returns: + int: unique user debug line id. + """ pass - def add_user_debug_text(self): + def add_user_debug_text(self, text, position, rgb_color=None, size=None, lifetime=None, orientation=None, + parent_object_id=None, parent_link_id=None, text_id=None): + """ + Add 3D text at a specific location using a color and size. + + Args: + text (str): text. + position (np.float[3]): 3d position of the text in Cartesian world coordinates. + rgb_color (list/tuple of 3 floats): RGB color; each component in range [0..1] + size (float): text size + lifetime (float): use 0 for permanent text, or positive time in seconds (afterwards the text with be + removed automatically) + orientation (np.float[4]): By default, debug text will always face the camera, automatically rotation. + By specifying a text orientation (quaternion), the orientation will be fixed in world space or local + space (when parent is specified). Note that a different implementation/shader is used for camera + facing text, with different appearance: camera facing text uses bitmap fonts, text with specified + orientation uses TrueType font. + parent_object_id (int): draw text in local coordinates of a parent object. + parent_link_id (int): draw text in local coordinates of a parent link. + text_id (int): replace an existing text item (to avoid flickering of remove/add). + + Returns: + int: unique user debug text id. + """ pass - def add_user_debug_parameter(self): + def add_user_debug_parameter(self, name, min_range, max_range, start_value): + """ + Add custom sliders to tune parameters. + + Args: + name (str): name of the parameter. + min_range (float): minimum value. + max_range (float): maximum value. + start_value (float): starting value. + + Returns: + int: unique user debug parameter id. + """ pass - def add_user_data(self): + def read_user_debug_parameter(self, parameter_id): + """ + Read the value of the parameter / slider. + + Args: + parameter_id: unique user debug parameter id. + + Returns: + float: reading of the parameter. + """ pass - def configure_debug_visualizer(self): + def remove_user_debug_item(self, item_id): + """ + Remove the specified user debug item (line, text, parameter) from the simulator. + + Args: + item_id (int): unique id of the debug item to be removed (line, text etc) + """ + pass + + def remove_all_user_debug_items(self): + """ + Remove all user debug items from the simulator. + """ + pass + + def set_debug_object_color(self, object_id, link_id, rgb_color=(1, 0, 0)): + """ + Override the color of a specific object and link. + + Args: + object_id (int): unique object id. + link_id (int): link id. + rgb_color (float[3]): RGB debug color. + """ + pass + + def add_user_data(self, object_id, key, value): + """ + Add user data (at the moment text strings) attached to any link of a body. You can also override a previous + given value. You can add multiple user data to the same body/link. + + Args: + object_id (int): unique object/link id. + key (str): key string. + value (str): value string. + + Returns: + int: user data id. + """ + pass + + def num_user_data(self, object_id): + """ + Return the number of user data associated with the specified object/link id. + + Args: + object_id (int): unique object/link id. + + Returns: + int: the number of user data + """ + pass + + def get_user_data(self, user_data_id): + """ + Get the specified user data value. + + Args: + user_data_id (int): unique user data id. + + Returns: + str: value string. + """ + pass + + def get_user_data_id(self, object_id, key): + """ + Get the specified user data id. + + Args: + object_id (int): unique object/link id. + key (str): key string. + + Returns: + int: user data id. + """ + pass + + def get_user_data_info(self, object_id, index): + """ + Get the user data info associated with the given object and index. + + Args: + object_id (int): unique object id. + index (int): index (should be between [0, self.num_user_data(object_id)]). + + Returns: + int: user data id. + str: key. + int: body id. + int: link index + int: visual shape index. + """ + pass + + def remove_user_data(self, user_data_id): + """ + Remove the specified user data. + + Args: + user_data_id (int): user data id. + """ + pass + + def sync_user_data(self): + """ + Synchronize the user data. + """ + pass + + def configure_debug_visualizer(self, flag, enable): + """Configure the debug visualizer camera. + + Configure some settings of the built-in OpenGL visualizer, such as enabling or disabling wireframe, + shadows and GUI rendering. + + Args: + flag (int): The feature to enable or disable, such as + COV_ENABLE_WIREFRAME (=3): show/hide the collision wireframe + COV_ENABLE_SHADOWS (=2): show/hide shadows + COV_ENABLE_GUI (=1): enable/disable the GUI + COV_ENABLE_VR_PICKING (=5): enable/disable VR picking + COV_ENABLE_VR_TELEPORTING (=4): enable/disable VR teleporting + COV_ENABLE_RENDERING (=7): enable/disable rendering + COV_ENABLE_TINY_RENDERER (=12): enable/disable tiny renderer + COV_ENABLE_VR_RENDER_CONTROLLERS (=6): render VR controllers + COV_ENABLE_KEYBOARD_SHORTCUTS (=9): enable/disable keyboard shortcuts + COV_ENABLE_MOUSE_PICKING (=10): enable/disable mouse picking + COV_ENABLE_Y_AXIS_UP (Z is default world up axis) (=11): enable/disable Y axis up + COV_ENABLE_RGB_BUFFER_PREVIEW (=13): enable/disable RGB buffer preview + COV_ENABLE_DEPTH_BUFFER_PREVIEW (=14): enable/disable Depth buffer preview + COV_ENABLE_SEGMENTATION_MARK_PREVIEW (=15): enable/disable segmentation mark preview + enable (bool): False (disable) or True (enable) + """ pass def get_debug_visualizer(self): + """Get information about the debug visualizer camera. + + Returns: + float: width of the visualizer camera + float: height of the visualizer camera + np.float[4,4]: view matrix [4,4] + np.float[4,4]: perspective projection matrix [4,4] + np.float[3]: camera up vector expressed in the Cartesian world space + np.float[3]: forward axis of the camera expressed in the Cartesian world space + np.float[3]: This is a horizontal vector that can be used to generate rays (for mouse picking or creating + a simple ray tracer for example) + np.float[3]: This is a vertical vector that can be used to generate rays (for mouse picking or creating a + simple ray tracer for example) + float: yaw angle (in radians) of the camera, in Cartesian local space coordinates + float: pitch angle (in radians) of the camera, in Cartesian local space coordinates + float: distance between the camera and the camera target + np.float[3]: target of the camera, in Cartesian world space coordinates + """ pass - def reset_debug_visualizer(self): + def reset_debug_visualizer(self, distance, yaw, pitch, target_position): + """Reset the debug visualizer camera. + + Reset the 3D OpenGL debug visualizer camera distance (between eye and camera target position), camera yaw and + pitch and camera target position + + Args: + distance (float): distance from eye to camera target position + yaw (float): camera yaw angle (in radians) left/right + pitch (float): camera pitch angle (in radians) up/down + target_position (np.float[3]): target focus point of the camera + """ pass # events (mouse, keyboard) def get_keyboard_events(self): + """Get the key events. + + Returns: + dict: {keyId: keyState} + * `keyID` is an integer (ascii code) representing the key. Some special keys like shift, arrows, + and others are are defined in pybullet such as `B3G_SHIFT`, `B3G_LEFT_ARROW`, `B3G_UP_ARROW`,... + * `keyState` is an integer. 3 if the button has been pressed, 1 if the key is down, 2 if the key has + been triggered. + """ pass def get_mouse_events(self): + """Get the mouse events. + + Returns: + list of mouse events: + eventType (int): 1 if the mouse is moving, 2 if a button has been pressed or released + mousePosX (float): x-coordinates of the mouse pointer + mousePosY (float): y-coordinates of the mouse pointer + buttonIdx (int): button index for left/middle/right mouse button. It is -1 if nothing, + 0 if left button, 1 if scroll wheel (pressed), 2 if right button + buttonState (int): 0 if nothing, 3 if the button has been pressed, 4 is the button has been released, + 1 if the key is down (never observed), 2 if the key has been triggered (never + observed). + """ pass def get_mouse_and_keyboard_events(self): + """Get the mouse and key events. + + Returns: + list: list of mouse events + dict: dictionary of key events + """ pass diff --git a/pyrobolearn/states/__init__.py b/pyrobolearn/states/__init__.py index 15acfca..597c6d3 100644 --- a/pyrobolearn/states/__init__.py +++ b/pyrobolearn/states/__init__.py @@ -6,7 +6,7 @@ from .state import State from .basic_states import * # import object states -from .object_states import * +from .body_states import * # import time/count states from .time_states import * diff --git a/pyrobolearn/states/object_states.py b/pyrobolearn/states/body_states.py similarity index 78% rename from pyrobolearn/states/object_states.py rename to pyrobolearn/states/body_states.py index a4650bc..7e4c319 100644 --- a/pyrobolearn/states/object_states.py +++ b/pyrobolearn/states/body_states.py @@ -7,8 +7,8 @@ This includes notably the joint positions, velocities, and force/torque states. from abc import ABCMeta, abstractmethod from pyrobolearn.states.state import State -from pyrobolearn.worlds.world import World -from pyrobolearn.robots import Object +from pyrobolearn.worlds import World +from pyrobolearn.robots import Body __author__ = "Brian Delhaisse" @@ -21,15 +21,15 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class ObjectState(State): - """Object state (abstract) +class BodyState(State): + """Body state (abstract) """ __metaclass__ = ABCMeta def __init__(self, obj, world=None): - super(ObjectState, self).__init__() - if not isinstance(obj, (Object, int)): - raise TypeError("Expecting an instance of Object, or an identifier from the simulator/world.") + super(BodyState, self).__init__() + if not isinstance(obj, (Body, int)): + raise TypeError("Expecting an instance of Body, or an identifier from the simulator/world.") if isinstance(obj, int): if not isinstance(world, World): # try to look for the world in global variables @@ -37,7 +37,7 @@ class ObjectState(State): world = globals()['world'] else: raise ValueError("When giving the object identifier, the world need to be given as well.") - obj = Object(world.getSimulator(), obj) + obj = Body(world.simulator, obj) self.obj = obj @abstractmethod @@ -45,7 +45,7 @@ class ObjectState(State): pass -class PositionState(ObjectState): +class PositionState(BodyState): """Position of an object. """ def __init__(self, obj, world=None): @@ -56,7 +56,7 @@ class PositionState(ObjectState): self.data = self.obj.position -class OrientationState(ObjectState): +class OrientationState(BodyState): """Orientation of an object. """ def __init__(self, obj, world=None): @@ -67,7 +67,7 @@ class OrientationState(ObjectState): self.data = self.obj.orientation -class VelocityState(ObjectState): +class VelocityState(BodyState): """Velocity of an object. """ def __init__(self, obj, world=None): diff --git a/pyrobolearn/utils/orientation.py b/pyrobolearn/utils/orientation.py index 1407565..5d40286 100644 --- a/pyrobolearn/utils/orientation.py +++ b/pyrobolearn/utils/orientation.py @@ -1,5 +1,11 @@ -# utils code to transform orientation expressed in different forms -# This includes rotation matrices, euler angles (RPY), axis-angle, and quaternions +#!/usr/bin/env python +"""Provide utils code to transform orientation expressed in different forms + +This includes rotation matrices, euler angles (RPY), axis-angle, and quaternions. + +References: + [1] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010, chapter 2 +""" import numpy as np import quaternion @@ -8,8 +14,26 @@ from collections import Iterable from pyrobolearn.utils.converter import QuaternionNumpyConverter +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "MIT" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" -def getMatrixFromAxisAngle(axis, angle): + +def get_matrix_from_axis_angle(axis, angle): + """Return the rotation matrix from the specified axis and angle. + + Args: + axis (np.float[3], list of 3 float): 3d axis vector. + angle (float): angle. + + Returns: + np.float[3,3]: rotation matrix. + """ x, y, z = axis a = angle c, s = np.cos(a), np.sin(a) @@ -20,7 +44,16 @@ def getMatrixFromAxisAngle(axis, angle): return R -def getSymbolicMatrixFromAxisAngle(axis, angle): +def get_symbolic_matrix_from_axis_angle(axis, angle): + """Return the symbolic rotation matrix from the specified axis and angle. + + Args: + axis (np.float[3], list of 3 float, list of 3 sympy.Symbol): 3d axis vector. + angle (float, sympy.Symbol): angle. + + Returns: + np.float[3,3]: rotation matrix. + """ x, y, z = axis a = angle c, s = sympy.cos(a), sympy.sin(a) @@ -31,19 +64,49 @@ def getSymbolicMatrixFromAxisAngle(axis, angle): return R -def getAxisAngleFromMatrix(R): +def get_axis_angle_from_matrix(R): + """Return the associated axis and angle from the specified rotation matrix. + + Args: + R (np.float[3,3]): 3-by-3 rotation matrix. + + Returns: + float: angle. + np.float[3]: 3d axis vector. + """ angle = np.arccos((R[0, 0] + R[1, 1] + R[2, 2] - 1) / 2.) axis = 1. / (2. * np.sin(angle)) * np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]) return angle, axis -def getSymbolicAxisAngleFromMatrix(R): +def get_symbolic_axis_angle_from_matrix(R): + """Return the symbolic axis and angle from the specified rotation matrix. + + Args: + R (np.array of sympy.Symbol): 3-by-3 rotation matrix. + + Returns: + sympy.Symbol: angle. + np.array of 3 sympy.Symbol: 3d axis vector. + """ angle = sympy.acos((R[0, 0] + R[1, 1] + R[2, 2] - 1) / 2.) axis = 1. / (2. * sympy.sin(angle)) * np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]) return angle, axis -def getQuaternionFromAxisAngle(axis, angle, convert_to_quat=False, convention='xyzw'): +def get_quaternion_from_axis_angle(axis, angle, convert_to_quat=False, convention='xyzw'): + """Get the quaternion associated from the axis/angle representation. + + Args: + axis (np.float[3]): 3d axis vector. + angle (float): angle. + convert_to_quat (bool): If True, it will return an instance of `quaternion.quaternion`. + convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or + 'wxyz'. + + Returns: + np.float[4], quaternion.quaternion: quaternion. + """ w = np.cos(angle / 2.) x, y, z = np.sin(angle / 2.) * axis if convert_to_quat: @@ -57,7 +120,18 @@ def getQuaternionFromAxisAngle(axis, angle, convert_to_quat=False, convention='x raise NotImplementedError("Asking for a convention that has not been implemented") -def getSymbolicQuaternionFromAxisAngle(axis, angle, convention='xyzw'): +def get_symbolic_quaternion_from_axis_angle(axis, angle, convention='xyzw'): + """Get the symbolic quaternion associated from the axis/angle representation. + + Args: + axis (np.float[3], np.array of 3 sympy.Symbol): 3d axis vector. + angle (float, sympy.Symbol): angle. + convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or + 'wxyz'. + + Returns: + np.float[4]: symbolic quaternion. + """ w = sympy.cos(angle / 2.) x, y, z = sympy.sin(angle / 2.) * axis if convention == 'xyzw': @@ -68,21 +142,55 @@ def getSymbolicQuaternionFromAxisAngle(axis, angle, convention='xyzw'): raise NotImplementedError("Asking for a convention that has not been implemented") -def getRPYFromMatrix(R): - r = np.arctan2(R[1, 0], R[0, 0]) - p = np.arctan2(-R[2, 0], np.sqrt(R[2, 1]**2 + R[2, 2]**2)) - y = np.arctan2(R[2, 1], R[2, 2]) +def get_rpy_from_matrix(R): + """Get the Roll-Pitch-Yaw angle values from the given rotation matrix. + + Args: + R (np.float[3,3]): 3-by-3 rotation matrix. + + Returns: + np.float[3]: roll-pitch-yaw angle values. + """ + # r = np.arctan2(R[1, 0], R[0, 0]) + # p = np.arctan2(-R[2, 0], np.sqrt(R[2, 1]**2 + R[2, 2]**2)) + # y = np.arctan2(R[2, 1], R[2, 2]) + + r = np.arctan2(R[2, 1], R[2, 2]) + p = np.arctan2(-R[2, 0], np.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2)) + y = np.arctan2(R[1, 0], R[0, 0]) + return np.array([r, p, y]) -def getSymbolicRPYFromMatrix(R): - r = sympy.atan2(R[1, 0], R[0, 0]) +def get_symbolic_rpy_from_matrix(R): + """Get the symbolic Roll-Pitch-Yaw angles from the given rotation matrix. + + Args: + R (np.float[3,3], np.array of sympy.Symbol): symbolic 3-by-3 rotation matrix. + + Returns: + np.array of 3 sympy.Symbol: symbolic roll-pitch-yaw angles. + """ + # r = sympy.atan2(R[1, 0], R[0, 0]) + # p = sympy.atan2(-R[2, 0], sympy.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2)) + # y = sympy.atan2(R[2, 1], R[2, 2]) + + r = sympy.atan2(R[2, 1], R[2, 2]) p = sympy.atan2(-R[2, 0], sympy.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2)) - y = sympy.atan2(R[2, 1], R[2, 2]) + y = sympy.atan2(R[1, 0], R[0, 0]) + return np.array([r, p, y]) -def getMatrixFromRPY(rpy): +def get_matrix_from_rpy(rpy): + """Get rotation matrix from the given Roll-Pitch-Yaw angles. + + Args: + rpy (np.float[3]): roll-pitch-yaw angles + + Returns: + np.float[3,3]: rotation matrix. + """ cr, cp, cy = [np.cos(i) for i in rpy] sr, sp, sy = [np.sin(i) for i in rpy] R = np.array([[cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr], @@ -91,7 +199,16 @@ def getMatrixFromRPY(rpy): return R -def getSymbolicMatrixFromRPY(rpy): +def get_symbolic_matrix_from_rpy(rpy): + """ + Get the symbolic rotation matrix from the given Roll-Pitch-Yaw angles. + + Args: + rpy (np.float[3], np.array of 3 sympy.Symbol): roll-pitch-yaw angles. + + Returns: + 3-by-3 np.array of sympy.Symbol: symbolic rotation matrix + """ cr, cp, cy = [sympy.cos(i) for i in rpy] sr, sp, sy = [sympy.sin(i) for i in rpy] R = np.array([[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr], @@ -100,7 +217,19 @@ def getSymbolicMatrixFromRPY(rpy): return R -def getQuaternionFromMatrix(R, convert_to_quat=False, convention='xyzw'): +def get_quaternion_from_matrix(R, convert_to_quat=False, convention='xyzw'): + """ + Get the quaternion from the given rotation matrix. + + Args: + R (np.float[3,3]): rotation matrix. + convert_to_quat (bool): If True, it will return an instance of `quaternion.quaternion`. + convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or + 'wxyz'. + + Returns: + np.float[4], quaternion.quaternion: quaternion + """ w = 1./2 * np.sqrt(R[0, 0] + R[1, 1] + R[2, 2] + 1) x, y, z = 1./2 * np.array([np.sign(R[2, 1] - R[1, 2]) * np.sqrt(R[0, 0] - R[1, 1] - R[2, 2] + 1), np.sign(R[0, 2] - R[2, 0]) * np.sqrt(R[1, 1] - R[2, 2] - R[0, 0] + 1), @@ -116,7 +245,18 @@ def getQuaternionFromMatrix(R, convert_to_quat=False, convention='xyzw'): raise NotImplementedError("Asking for a convention that has not been implemented") -def getSymbolicQuaternionFromMatrix(R, convention='xyzw'): +def get_symbolic_quaternion_from_matrix(R, convention='xyzw'): + """ + Get the symbolic quaternion from the given rotation matrix. + + Args: + R (3-by-3 np.array of sympy.Symbol, np.float[3,3]): (symbolic) rotation matrix + convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or + 'wxyz'. + + Returns: + np.array of 4 sympy.Symbol: symbolic quaternion. + """ w = 1. / 2 * sympy.sqrt(R[0, 0] + R[1, 1] + R[2, 2] + 1) x, y, z = 1. / 2 * np.array([sympy.sign(R[2, 1] - R[1, 2]) * sympy.sqrt(R[0, 0] - R[1, 1] - R[2, 2] + 1), sympy.sign(R[0, 2] - R[2, 0]) * sympy.sqrt(R[1, 1] - R[2, 2] - R[0, 0] + 1), @@ -129,7 +269,18 @@ def getSymbolicQuaternionFromMatrix(R, convention='xyzw'): raise NotImplementedError("Asking for a convention that has not been implemented") -def getMatrixFromQuaternion(q, convention='xyzw'): +def get_matrix_from_quaternion(q, convention='xyzw'): + """ + Get rotation matrix from the given quaternion. + + Args: + q (np.float[4], quaternion.quaternion): quaternion + convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or + 'wxyz'. + + Returns: + np.float[3,3]: rotation matrix. + """ if isinstance(q, quaternion.quaternion): x, y, z, w = q.x, q.y, q.z, q.w elif isinstance(q, Iterable): @@ -147,11 +298,146 @@ def getMatrixFromQuaternion(q, convention='xyzw'): return R -def getSymbolicMatrixFromQuaternion(q, convention='xyzw'): - return getMatrixFromQuaternion(q, convention=convention) +def get_symbolic_matrix_from_quaternion(q, convention='xyzw'): + """ + Get symbolic rotation matrix from the given quaternion. + + Args: + q (np.array of 4 sympy.Symbol, np.float[4]): (symbolic) quaternion. + convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or + 'wxyz'. + + Returns: + 3-by-3 np.array of sympy.Symbol: symbolic rotation matrix. + """ + return get_matrix_from_quaternion(q, convention=convention) -def skew(vector): +def get_rpy_from_quaternion(q, convention='xyzw'): + """ + Get the Roll-Pitch-Yaw angle from the given quaternion. + + Args: + q (np.float[4], quaternion.quaternion): quaternion + convention: convention to be adopted when representing the quaternion. You can choose between 'xyzw' or + 'wxyz'. + + Returns: + np.float[3]: roll-pitch-yaw angles. + """ + if isinstance(q, quaternion.quaternion): + x, y, z, w = q.x, q.y, q.z, q.w + elif isinstance(q, Iterable): + if convention == 'xyzw': + x, y, z, w = q + elif convention == 'wxyz': + w, x, y, z = q + else: + raise NotImplementedError("Asking for a convention that has not been implemented") + else: + raise TypeError + roll = np.arctan2(2*(w*x + y*z), 1 - 2 * (x**2 + y**2)) + pitch = np.arcsin(2 * (w*y - z*x)) + yaw = np.arctan2(2 * (w*z + x*y), 1 - 2 * (y**2 + z**2)) + return np.array([roll, pitch, yaw]) + + +def get_symbolic_rpy_from_quaternion(q, convention='xyzw'): + """ + Get the symbolic Roll-Pitch-Yaw angle from the given quaternion. + + Args: + q (np.float[4], np.array of 4 sympy.Symbol): quaternion + convention: convention to be adopted when representing the quaternion. You can choose between 'xyzw' or + 'wxyz'. + + Returns: + np.array of 3 sympy.Symbol: symbolic roll-pitch-yaw angles. + """ + if isinstance(q, quaternion.quaternion): + x, y, z, w = q.x, q.y, q.z, q.w + elif isinstance(q, Iterable): + if convention == 'xyzw': + x, y, z, w = q + elif convention == 'wxyz': + w, x, y, z = q + else: + raise NotImplementedError("Asking for a convention that has not been implemented") + else: + raise TypeError + + roll = sympy.atan2(2*(w*x + y*z), 1 - 2 * (x**2 + y**2)) + pitch = sympy.asin(2 * (w*y - z*x)) + yaw = sympy.atan2(2 * (w*z + x*y), 1 - 2 * (y**2 + z**2)) + + return np.array([roll, pitch, yaw]) + + +def get_quaternion_from_rpy(rpy, convert_to_quat=False, convention='xyzw'): + """ + Get quaternion from Roll-Pitch-Yaw angle. + + Args: + rpy (np.float[3]): roll-pitch-yaw angles + convert_to_quat (bool): If True, it will return an instance of `quaternion.quaternion`. + convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or + 'wxyz'. + + Returns: + np.float[4], quaternion.quaternion: quaternion + """ + r, p, y = rpy + cr, sr = np.cos(r/2.), np.sin(r/2.) + cp, sp = np.cos(p/2.), np.sin(p/2.) + cy, sy = np.cos(y/2.), np.sin(y/2.) + + w = cr * cp * cy + sr * sp * sy + x = sr * cp * cy - cr * sp * sy + y = cr * sp * cy + sr * cp * sy + z = cr * cp * sy - sr * sp * cy + + if convert_to_quat: + return quaternion.quaternion(w, x, y, z) + else: + if convention == 'xyzw': + return np.array([x, y, z, w]) + elif convention == 'wxyz': + return np.array([w, x, y, z]) + else: + raise NotImplementedError("Asking for a convention that has not been implemented") + + +def get_symbolic_quaternion_from_rpy(rpy, convention='xyzw'): + """ + Get symbolic quaternion from Roll-Pitch-Yaw angle. + + Args: + rpy (np.float[3], np.array of 3 sympy.Symbol): (symbolic) roll-pitch-yaw angles + convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or + 'wxyz'. + + Returns: + np.array of 4 sympy.Symbol: symbolic quaternion + """ + r, p, y = rpy + cr, sr = sympy.cos(r/2.), sympy.sin(r/2.) + cp, sp = sympy.cos(p/2.), sympy.sin(p/2.) + cy, sy = sympy.cos(y/2.), sympy.sin(y/2.) + + w = cr * cp * cy + sr * sp * sy + x = sr * cp * cy - cr * sp * sy + y = cr * sp * cy + sr * cp * sy + z = cr * cp * sy - sr * sp * cy + + if convention == 'xyzw': + return np.array([x, y, z, w]) + elif convention == 'wxyz': + return np.array([w, x, y, z]) + else: + raise NotImplementedError("Asking for a convention that has not been implemented") + + +def skew_matrix(vector): r""" Return the skew-symmetric matrix of the given vector, which allows to represents the cross product between the given vector and another vector, as the multiplication of the returned skew-symmetric matrix with the other @@ -184,7 +470,7 @@ def skew(vector): [-y, x, 0.]]) -def RotX(angle): +def rotation_matrix_x(angle): """ Return the rotation matrix around the x-axis by the given angle. @@ -200,7 +486,7 @@ def RotX(angle): [0., s, c]]) -def RotY(angle): +def rotation_matrix_y(angle): """ Return the rotation matrix around the y-axis by the given angle. @@ -216,7 +502,7 @@ def RotY(angle): [-s, 0, c]]) -def RotZ(angle): +def rotation_matrix_z(angle): """ Return the rotation matrix around the z-axis by the given angle. @@ -239,7 +525,17 @@ def RotZ(angle): quat_converter = QuaternionNumpyConverter(convention=1) -def getQuaternionInverse(q, convention='xyzw'): +def get_quaternion_inverse(q, convention='xyzw'): + """Return the inverse of the given quaternion. + + Args: + q (np.float[4], quaternion.quaternion): quaternion. + convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or + 'wxyz'. + + Returns: + np.float[4], quaternion.quaternion: quaternion inverse. + """ if isinstance(q, quaternion.quaternion): return q.inverse() elif isinstance(q, Iterable): @@ -255,7 +551,18 @@ def getQuaternionInverse(q, convention='xyzw'): raise TypeError -def getQuaternionProduct(q1, q2, convention='xyzw'): +def get_quaternion_product(q1, q2, convention='xyzw'): + """Return the quaternion product between two quaternions. + + Args: + q1 (np.float[4], quaternion.quaternion): first quaternion + q2 (np.float[4], quaternion.quaternion): second quaternion + convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or + 'wxyz'. + + Returns: + np.float[4], quaternion.quaternion: resulting quaternion. + """ if type(q1) != type(q2): raise TypeError("Expecting q1 and q2 to be of the same type") if isinstance(q1, quaternion.quaternion): @@ -318,7 +625,7 @@ def exponential_map(r): def angular_velocity_from_quaternion(q1, q2): - """ + r""" Convert the difference between 2 quaternions using the logarithm map. Args: @@ -331,3 +638,22 @@ def angular_velocity_from_quaternion(q1, q2): q1 = quat_converter.convertTo(q1) q2 = quat_converter.convertTo(q2) return 2 * logarithm_map(q1 * q2) + + +# Tests +if __name__ == "__main__": + import pybullet + import tf.transformations as tft + + q = np.array([-0.043, 0.567, 0.368, 0.736]) + rpy = get_rpy_from_quaternion(q) + + print('\nRPY from quaternion: {}'.format(get_rpy_from_quaternion(q))) + print('RPY <- matrix <- quaternion: {}'.format(get_rpy_from_matrix(get_matrix_from_quaternion(q)))) + print('Using pybullet: {}'.format(pybullet.getEulerFromQuaternion(q))) + print('Using tf.transformations: {}'.format(tft.euler_from_quaternion(q))) + + print('\nQuaternion from RPY: {}'.format(get_quaternion_from_rpy(rpy))) + print('Quaternion <- matrix <- RPY: {}'.format(get_quaternion_from_matrix(get_matrix_from_rpy(rpy)))) + print('Using pybullet: {}'.format(pybullet.getQuaternionFromEuler(rpy))) + print('Using tf.transformations: {}'.format(tft.quaternion_from_euler(*rpy)))