From 0d15cad93ae7902f7e55159f66a4bdd0430d366c Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Mon, 29 Jul 2019 02:00:32 +0200 Subject: [PATCH] update parsers and mujoco/dart simulators --- pyrobolearn/robots/lipm.py | 153 +++ pyrobolearn/robots/slip.py | 150 +++ pyrobolearn/simulators/bullet.py | 16 +- pyrobolearn/simulators/dart.py | 901 +++++++++++++++--- pyrobolearn/simulators/mujoco.py | 1 - pyrobolearn/simulators/simulator.py | 26 +- pyrobolearn/utils/parsers/robots/__init__.py | 6 + .../utils/parsers/robots/data_structures.py | 707 ++++++++------ .../utils/parsers/robots/mujoco_parser.py | 186 +++- .../utils/parsers/robots/proto_parser.py | 2 +- .../utils/parsers/robots/sdf_parser.py | 255 ++++- .../utils/parsers/robots/skel_parser.py | 554 +++++++++++ .../utils/parsers/robots/urdf_parser.py | 10 +- .../utils/parsers/robots/world_parser.py | 1 + 14 files changed, 2535 insertions(+), 433 deletions(-) create mode 100644 pyrobolearn/robots/lipm.py create mode 100644 pyrobolearn/robots/slip.py create mode 100644 pyrobolearn/utils/parsers/robots/skel_parser.py diff --git a/pyrobolearn/robots/lipm.py b/pyrobolearn/robots/lipm.py new file mode 100644 index 0000000..2c6963f --- /dev/null +++ b/pyrobolearn/robots/lipm.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python +"""Provide the various Linear Inverted Pendulum Models (LIPMs). + +This includes: LIPM2D, DualLIPM2D, LIPM3D, DualLIPM3D +""" + +# TODO: finish to implement this + +import os + +from pyrobolearn.robots.legged_robot import LeggedRobot + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class LIPM2D(LeggedRobot): + r"""Linear Inverted Pendulum Model 2D + + This class describes the 2D Linear Inverted Pendulum Model (2D-LIPM), which underlies walking behaviors, and is + often used as a template model in locomotion. The 2D version constraints the possible motion to belong to the xz + plan. + + See Also: + - LIPM3D: the 3D version of the 2D-LIPM + - DualLIPM2D: the dual version of the 2D-LIPM + - DualLIPM3D: the dual version of the 3D-LIPM + """ + + def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1., + urdf=os.path.dirname(__file__) + '/urdfs/templates/lipm2d.xml'): + # check parameters + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1.) + if fixed_base is None: + fixed_base = False + + super(LIPM2D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale) + self.name = 'lipm_2d' + + +class DualLIPM2D(LeggedRobot): + r"""Dual Linear Inverted Pendulum Model 2D + + This class describes the dual 2D Linear Inverted Pendulum Model (dual 2D-LIPM), which underlies walking behaviors, + and is often used as a template model in locomotion. The 2D version constraints the possible motions to belong to + the xz plan. + + See Also: + - LIPM3D: the 3D version of the 2D-LIPM + - DualLIPM3D: the dual version of the 3D-LIPM + """ + + def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1., + urdf=os.path.dirname(__file__) + '/urdfs/templates/dual_lipm2d.xml'): + # check parameters + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1.) + if fixed_base is None: + fixed_base = False + + super(DualLIPM2D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale) + self.name = 'dual_lipm_2d' + + +class LIPM3D(LeggedRobot): + r"""Linear Inverted Pendulum Model 3D + + This class describes the 3D Linear Inverted Pendulum Model (3D-LIPM), which underlies walking behaviors, and is + often used as a template model in locomotion. + + See Also: + - DualLIPM3D: the dual version of the 3D-LIPM + """ + + def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1., + urdf=os.path.dirname(__file__) + '/urdfs/templates/lipm3d.xml'): + # check parameters + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1.) + if fixed_base is None: + fixed_base = False + + super(LIPM3D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale) + self.name = 'lipm_3d' + + +class DualLIPM3D(LeggedRobot): + r"""Dual Linear Inverted Pendulum Model 3D + + This class describes the dual 3D Linear Inverted Pendulum Model (dual 3D-LIPM), which underlies walking behaviors, + and is often used as a template model in locomotion. + + See Also: + - SLIP: the spring-loaded inverted pendulum which is more suitable for running and jumping behaviors. + """ + + def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1., + urdf=os.path.dirname(__file__) + '/urdfs/templates/dual_lipm3d.xml'): + # check parameters + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1.) + if fixed_base is None: + fixed_base = False + + super(DualLIPM3D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale) + self.name = 'dual_lipm_3d' + + +# Test +if __name__ == "__main__": + from itertools import count + from pyrobolearn.simulators import BulletSim + from pyrobolearn.worlds import BasicWorld + + # Create simulator + sim = BulletSim() + + # create world + world = BasicWorld(sim) + + # create robot + robot = LIPM2D(sim) + + # print information about the robot + robot.print_info() + + # run simulation + for i in count(): + # step in simulation + world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/slip.py b/pyrobolearn/robots/slip.py new file mode 100644 index 0000000..191a4ba --- /dev/null +++ b/pyrobolearn/robots/slip.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python +"""Provide the various Spring-Loaded Inverted Pendulum (SLIP) models. + +This includes: SLIP2D, DualSLIP2D, SLIP3D, DualSLIP3D +""" + +# TODO: finish to implement this + +import os + +from pyrobolearn.robots.legged_robot import LeggedRobot + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class SLIP2D(LeggedRobot): + r"""Spring-Loaded Inverted Pendulum (2D) + + This class describes the 2D Spring-Loaded Inverted Pendulum (2D-SLIP) model, which underlies dynamic locomotive + behaviors (such as running and jumping), and is often used as a template model in locomotion. The 2D version + constraints the possible motion to belong to the xz plan. + + See Also: + - SLIP3D: the 3D version of the 2D-SLIP + - DualSLIP2D: the dual version of the 2D-SLIP + - DualSLIP3D: the dual version of the 3D-SLIP + """ + + def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1., + urdf=os.path.dirname(__file__) + '/urdfs/templates/SLIP2d.xml'): + # check parameters + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1.) + if fixed_base is None: + fixed_base = False + + super(SLIP2D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale) + self.name = 'SLIP_2d' + + +class DualSLIP2D(LeggedRobot): + r"""Dual Linear Inverted Pendulum Model 2D + + This class describes the dual 2D Spring-Loaded Inverted Pendulum (dual 2D-SLIP) model, which underlies dynamic + locomotive behaviors (such as running and jumping), and is often used as a template model in locomotion. The 2D + version constraints the possible motion to belong to the xz plan. + + See Also: + - SLIP3D: the 3D version of the 2D-SLIP + - DualSLIP3D: the dual version of the 3D-SLIP + """ + + def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1., + urdf=os.path.dirname(__file__) + '/urdfs/templates/dual_SLIP2d.xml'): + # check parameters + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1.) + if fixed_base is None: + fixed_base = False + + super(DualSLIP2D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale) + self.name = 'dual_SLIP_2d' + + +class SLIP3D(LeggedRobot): + r"""Linear Inverted Pendulum Model 3D + + This class describes the 3D Spring-Loaded Inverted Pendulum (3D-SLIP) model, which underlies dynamic locomotive + behaviors (such as running and jumping), and is often used as a template model in locomotion. + + See Also: + - DualSLIP3D: the dual version of the 3D-SLIP + """ + + def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1., + urdf=os.path.dirname(__file__) + '/urdfs/templates/SLIP3d.xml'): + # check parameters + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1.) + if fixed_base is None: + fixed_base = False + + super(SLIP3D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale) + self.name = 'SLIP_3d' + + +class DualSLIP3D(LeggedRobot): + r"""Dual Linear Inverted Pendulum Model 3D + + This class describes the dual 3D Spring-Loaded Inverted Pendulum (dual 3D-SLIP) model, which underlies dynamic + locomotive behaviors (such as running and jumping), and is often used as a template model in locomotion. + """ + + def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1., + urdf=os.path.dirname(__file__) + '/urdfs/templates/dual_SLIP3d.xml'): + # check parameters + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1.) + if fixed_base is None: + fixed_base = False + + super(DualSLIP3D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale) + self.name = 'dual_SLIP_3d' + + +# Test +if __name__ == "__main__": + from itertools import count + from pyrobolearn.simulators import BulletSim + from pyrobolearn.worlds import BasicWorld + + # Create simulator + sim = BulletSim() + + # create world + world = BasicWorld(sim) + + # create robot + robot = SLIP2D(sim) + + # print information about the robot + robot.print_info() + + # run simulation + for i in count(): + # step in simulation + world.step(sleep_dt=1./240) diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py index 87909d5..d2e00bc 100644 --- a/pyrobolearn/simulators/bullet.py +++ b/pyrobolearn/simulators/bullet.py @@ -3513,10 +3513,10 @@ class Bullet(Simulator): np.array[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, + - [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 @@ -3570,10 +3570,10 @@ class Bullet(Simulator): np.array[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, + - [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 diff --git a/pyrobolearn/simulators/dart.py b/pyrobolearn/simulators/dart.py index e9bd325..592e80f 100644 --- a/pyrobolearn/simulators/dart.py +++ b/pyrobolearn/simulators/dart.py @@ -1,12 +1,12 @@ #!/usr/bin/env python """Define the DART (Dynamic Animation and Robotics Toolkit) Simulator API. -This is the main interface that communicates with the PyDART simulator [1, 2]. By defining this interface, it allows to +This is the main interface that communicates with the DART simulator [1, 2]. By defining this interface, it allows to decouple the PyRoboLearn framework from the simulator. It also converts some data types to the ones required by -PyDART. +``dartpy``. The signature of each method defined here are inspired by [1,2] but in accordance with the PEP8 style guide [3]. -Parts of the documentation for the methods have been copied-pasted from [2] for completeness purposes. +Parts of the documentation for the methods have been copied-pasted from [1] for completeness purposes. Note that there are 2 python wrappers for DART [1]: `dartpy` (which uses `pybind11` to wrap C++ code) [1], and `pydart` (which uses `SWIG` to wrap the C++ code) [2]. Currently, it seems that: @@ -16,39 +16,52 @@ repo. Note that you can only use it in Python 3 (>=3.4). It can be used with Python2.7 and Python3.5. - Both have a very poor Python documentation. +We selected to use ``dartpy`` instead of ``pydart`` as this last one is no more under active development, and +``dartpy`` is the official release. + Dependencies in PRL: * `pyrobolearn.simulators.simulator.Simulator` Dependencies in PRL: None References: - [1] DART: Dynamic Animation and Robotics Toolkit + - [1] DART: Dynamic Animation and Robotics Toolkit - paper: http://joss.theoj.org/papers/10.21105/joss.00500 - webpage: https://dartsim.github.io/ - github: https://github.com/dartsim/dart/ - dartpy: http://dartsim.github.io/install_dartpy_on_ubuntu.html - [2] PyDART + - [2] PyDART - source code: https://pydart2.readthedocs.io/en/latest/ - documentation: https://pydart2.readthedocs.io/en/latest/ - [3] PEP8: https://www.python.org/dev/peps/pep-0008/ + - [3] PEP8: https://www.python.org/dev/peps/pep-0008/ """ -# TODO: finish to implement this interface and use dartpy instead of pydart2 +# import standard libraries +import os +import time +import numpy as np +from collections import OrderedDict -# import pydart2 +# import dartpy try: - import pydart2 as pydart + import dartpy as dart except ImportError as e: - raise ImportError(e.__str__() + "\n: HINT: you can install pydart2 by following the instructions given at: " - "https://pydart2.readthedocs.io/en/latest/install.html") + raise ImportError(e.__str__() + "\n: HINT: you can install `dartpy` by following the instructions given at: " + "https://dartsim.github.io/install_dartpy_on_ubuntu.html") # import PRL simulator from pyrobolearn.simulators.simulator import Simulator +from pyrobolearn.utils.parsers.robots import URDFParser, MuJoCoParser, SDFParser, SkelParser +from pyrobolearn.utils.transformation import get_quaternion_from_matrix +# check Python version +import sys +if sys.version_info[0] < 3: + raise RuntimeError("You must use Python 3 with the MuJoCo simulator.") __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["DART", "PyDART", "Brian Delhaisse"] +__credits__ = ["DART", "Brian Delhaisse"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" @@ -57,43 +70,228 @@ __status__ = "Development" class Dart(Simulator): - r"""PyDART simulator + r"""DART simulator - This is a wrapper around the PyDART2 API [2] (which is a Python wrapper around DART [1]). + This is a wrapper around the ``dartpy`` API [1] (which is itself a Python wrapper around DART [1]). With DART, we first have to create a world, then spawn a skeleton (a `Skeleton` is a structure that consists of `BodyNode` which are connected by `Joint`. + Warnings: by default, in DART, the world frame axis are defined with x pointing forward, y pointing upward, and + z pointing on the right. To be consistent with the other simulators, we change this to be x pointing forward, + y pointing on the left, and z pointing upward. + + Notes: + - In the documentation, Isometry refers to a homogeneous transformation matrix. + Examples: sim = Dart() References: - [1] Dart: + - [1] Dart: - webpage: https://dartsim.github.io/ - github: https://github.com/dartsim/dart/ - dartpy: http://dartsim.github.io/install_dartpy_on_ubuntu.html - [2] PyDART: - - source code: https://pydart2.readthedocs.io/en/latest/ - - documentation: https://pydart2.readthedocs.io/en/latest/ - [3] PEP8: https://www.python.org/dev/peps/pep-0008/ + - [2] PEP8: https://www.python.org/dev/peps/pep-0008/ """ - def __init__(self, render=True, dt=1.0/1000.0, **kwargs): + def __init__(self, render=True, dt=0.001, **kwargs): super(Dart, self).__init__(render, **kwargs) - # init pydart - pydart.init() + # dart = {'collision': ['BulletCollisionDetector', 'BulletCollisionGroup', 'CollisionDetector', + # 'CollisionGroup', 'CollisionOption', 'CollisionResult', 'Contact', + # 'DARTCollisionDetector', 'DARTCollisionGroup', 'DistanceOption', 'DistanceResult', + # 'FCLCollisionDetector', 'FCLCollisionGroup', 'OdeCollisionDetector', + # 'OdeCollisionGroup', 'RayHit', 'RaycastOption', 'RaycastResult'], + # 'common': ['Composite', 'Observer', 'Subject', 'Uri'], + # 'constraint': ['BallJointConstraint', 'BoxedLcpConstraintSolver', 'BoxedLcpSolver', 'ConstraintBase', + # 'ConstraintSolver', 'DantzigBoxedLcpSolver', 'JointConstraint', + # 'JointCoulombFrictionConstraint', 'JointLimitConstraint', 'PgsBoxedLcpSolver', + # 'PgsBoxedLcpSolverOption', 'WeldJointConstraint'], + # 'dynamics': ['ArrowShape', 'ArrowShapeProperties', 'BallJoint', 'BallJointProperties', 'BodyNode', + # 'BodyNodeAspectProperties', 'BodyNodeProperties', 'BoxShape', 'CapsuleShape', 'Chain', + # 'ChainCriteria', 'CollisionAspect', + # 'CompositeJoiner_EmbedProperties_EulerJoint_EulerJointUniqueProperties_GenericJoint_R3Space', + # 'CompositeJoiner_EmbedProperties_PlanarJoint_PlanarJointUniqueProperties_GenericJoint_R3Space', + # 'CompositeJoiner_EmbedProperties_PrismaticJoint_PrismaticJointUniqueProperties_GenericJoint_R1Space', + # 'CompositeJoiner_EmbedProperties_RevoluteJoint_RevoluteJointUniqueProperties_GenericJoint_R1Space', + # 'CompositeJoiner_EmbedProperties_ScrewJoint_ScrewJointUniqueProperties_GenericJoint_R1Space', + # 'CompositeJoiner_EmbedProperties_TranslationalJoint2D_TranslationalJoint2DUniqueProperties_GenericJoint_R2Space', + # 'CompositeJoiner_EmbedProperties_UniversalJoint_UniversalJointUniqueProperties_GenericJoint_R2Space', + # 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R1GenericJointStateGenericJointUniqueProperties_Joint', + # 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R2GenericJointStateGenericJointUniqueProperties_Joint', + # 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R3GenericJointStateGenericJointUniqueProperties_Joint', + # 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_SE3GenericJointStateGenericJointUniqueProperties_Joint', + # 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_SO3GenericJointStateGenericJointUniqueProperties_Joint', + # 'ConeShape', 'CylinderShape', 'DegreeOfFreedom', 'Detachable', 'DynamicsAspect', + # 'EllipsoidShape', + # 'EmbedPropertiesOnTopOf_EulerJoint_EulerJointUniqueProperties_GenericJoint_R3Space', + # 'EmbedPropertiesOnTopOf_PlanarJoint_PlanarJointUniqueProperties_GenericJoint_R3Space', + # 'EmbedPropertiesOnTopOf_PrismaticJoint_PrismaticJointUniqueProperties_GenericJoint_R1Space', + # 'EmbedPropertiesOnTopOf_RevoluteJoint_RevoluteJointUniqueProperties_GenericJoint_R1Space', + # 'EmbedPropertiesOnTopOf_ScrewJoint_ScrewJointUniqueProperties_GenericJoint_R1Space', + # 'EmbedPropertiesOnTopOf_TranslationalJoint2D_TranslationalJoint2DUniqueProperties_GenericJoint_R2Space', + # 'EmbedPropertiesOnTopOf_UniversalJoint_UniversalJointUniqueProperties_GenericJoint_R2Space', + # 'EmbedProperties_EulerJoint_EulerJointUniqueProperties', + # 'EmbedProperties_Joint_JointProperties', + # 'EmbedProperties_PlanarJoint_PlanarJointUniqueProperties', + # 'EmbedProperties_PrismaticJoint_PrismaticJointUniqueProperties', + # 'EmbedProperties_RevoluteJoint_RevoluteJointUniqueProperties', + # 'EmbedProperties_ScrewJoint_ScrewJointUniqueProperties', + # 'EmbedProperties_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', + # 'EmbedProperties_UniversalJoint_UniversalJointUniqueProperties', + # 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties_Joint', + # 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties_Joint', + # 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties_Joint', + # 'EmbedStateAndPropertiesOnTopOf_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties_Joint', + # 'EmbedStateAndPropertiesOnTopOf_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties_Joint', + # 'EmbedStateAndProperties_GenericJoint_R1GenericJointState_GenericJointUniqueProperties', + # 'EmbedStateAndProperties_GenericJoint_R2GenericJointState_GenericJointUniqueProperties', + # 'EmbedStateAndProperties_GenericJoint_R3GenericJointState_GenericJointUniqueProperties', + # 'EmbedStateAndProperties_GenericJoint_SE3GenericJointState_GenericJointUniqueProperties', + # 'EmbedStateAndProperties_GenericJoint_SO3GenericJointState_GenericJointUniqueProperties', + # 'Entity', 'EulerJoint', 'EulerJointProperties', 'EulerJointUniqueProperties', 'Frame', + # 'FreeJoint', 'FreeJointProperties', 'GenericJointProperties_R1', + # 'GenericJointProperties_R2', 'GenericJointProperties_R3', 'GenericJointProperties_SE3', + # 'GenericJointProperties_SO3', 'GenericJointUniqueProperties_R1', + # 'GenericJointUniqueProperties_R2', 'GenericJointUniqueProperties_R3', + # 'GenericJointUniqueProperties_SE3', 'GenericJointUniqueProperties_SO3', + # 'GenericJoint_R1', 'GenericJoint_R2', 'GenericJoint_R3', 'GenericJoint_SE3', + # 'GenericJoint_SO3', 'InverseKinematics', 'InverseKinematicsErrorMethod', 'JacobianNode', + # 'Joint', 'JointProperties', 'LineSegmentShape', 'Linkage', 'LinkageCriteria', + # 'MeshShape', 'MetaSkeleton', 'MultiSphereConvexHullShape', 'Node', 'PlanarJoint', + # 'PlanarJointProperties', 'PlanarJointUniqueProperties', 'PlaneShape', 'PrismaticJoint', + # 'PrismaticJointProperties', 'PrismaticJointUniqueProperties', 'ReferentialSkeleton', + # 'RequiresAspect_EmbeddedPropertiesAspect_EulerJoint_EulerJointUniqueProperties', + # 'RequiresAspect_EmbeddedPropertiesAspect_Joint_JointProperties', + # 'RequiresAspect_EmbeddedPropertiesAspect_PlanarJoint_PlanarJointUniqueProperties', + # 'RequiresAspect_EmbeddedPropertiesAspect_PrismaticJoint_PrismaticJointUniqueProperties', + # 'RequiresAspect_EmbeddedPropertiesAspect_RevoluteJoint_RevoluteJointUniqueProperties', + # 'RequiresAspect_EmbeddedPropertiesAspect_ScrewJoint_ScrewJointUniqueProperties', + # 'RequiresAspect_EmbeddedPropertiesAspect_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', + # 'RequiresAspect_EmbeddedPropertiesAspect_UniversalJoint_UniversalJointUniqueProperties', + # 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties', + # 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties', + # 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties', + # 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties', + # 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties', + # 'RevoluteJoint', 'RevoluteJointProperties', 'RevoluteJointUniqueProperties', + # 'ScrewJoint', 'ScrewJointProperties', 'ScrewJointUniqueProperties', 'Shape', + # 'ShapeFrame', 'ShapeNode', 'SimpleFrame', 'Skeleton', 'SoftMeshShape', + # 'SpecializedForAspect_EmbeddedPropertiesAspect_EulerJoint_EulerJointUniqueProperties', + # 'SpecializedForAspect_EmbeddedPropertiesAspect_Joint_JointProperties', + # 'SpecializedForAspect_EmbeddedPropertiesAspect_PlanarJoint_PlanarJointUniqueProperties', + # 'SpecializedForAspect_EmbeddedPropertiesAspect_PrismaticJoint_PrismaticJointUniqueProperties', + # 'SpecializedForAspect_EmbeddedPropertiesAspect_RevoluteJoint_RevoluteJointUniqueProperties', + # 'SpecializedForAspect_EmbeddedPropertiesAspect_ScrewJoint_ScrewJointUniqueProperties', + # 'SpecializedForAspect_EmbeddedPropertiesAspect_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', + # 'SpecializedForAspect_EmbeddedPropertiesAspect_UniversalJoint_UniversalJointUniqueProperties', + # 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties', + # 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties', + # 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties', + # 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties', + # 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties', + # 'SphereShape', 'TemplatedJacobianBodyNode', 'TranslationalJoint', 'TranslationalJoint2D', + # 'TranslationalJoint2DProperties', 'TranslationalJoint2DUniqueProperties', + # 'TranslationalJointProperties', 'UniversalJoint', 'UniversalJointProperties', + # 'UniversalJointUniqueProperties', 'VisualAspect', 'WeldJoint', 'ZeroDofJoint', + # 'ZeroDofJointProperties'], + # 'gui': {'osg': ['BodyNodeDnD', 'DragAndDrop', 'GUIActionAdapter', 'GUIEventAdapter', + # 'GUIEventHandler', 'GridVisual', 'ImGuiHandler', 'ImGuiViewer', 'ImGuiWidget', + # 'InteractiveFrame', 'InteractiveFrameDnD', 'InteractiveTool', 'RealTimeWorldNode', + # 'ShadowMap', 'ShadowTechnique', 'SimpleFrameDnD', 'SimpleFrameShapeDnD', 'Viewer', + # 'ViewerAttachment', 'WorldNode']}, + # 'math': ['AngleAxis', 'Isometry3', 'Quaternion', 'Random', 'eulerXYXToMatrix', 'eulerXYZToMatrix', + # 'eulerXZXToMatrix', 'eulerXZYToMatrix', 'eulerYXYToMatrix', 'eulerYXZToMatrix', + # 'eulerYZXToMatrix', 'eulerYZYToMatrix', 'eulerZXYToMatrix', 'eulerZXZToMatrix', + # 'eulerZYXToMatrix', 'eulerZYZToMatrix', 'expAngular', 'expMap', 'expMapJac', 'expMapRot', + # 'expToQuat', 'matrixToEulerXYX', 'matrixToEulerXYZ', 'matrixToEulerXZY', 'matrixToEulerYXZ', + # 'matrixToEulerYZX', 'matrixToEulerZXY', 'matrixToEulerZYX', 'quatToExp', 'verifyRotation', + # 'verifyTransform'], + # 'optimizer': ['Function', 'GradientDescentSolver', 'GradientDescentSolverProperties', + # 'GradientDescentSolverUniqueProperties', 'ModularFunction', 'MultiFunction', + # 'NloptSolver', 'NullFunction', 'Problem', 'Solver', 'SolverProperties'], + # 'simulation': ['World'], + # 'utils': ['DartLoader', 'SkelParser']] + + # Skeleton = ['checkIndexingConsistency', 'clearConstraintImpulses', 'clearExternalForces', 'clearIK', + # 'clearInternalForces', 'clone', 'cloneMetaSkeleton', 'computeForwardDynamics', + # 'computeForwardKinematics', 'computeImpulseForwardDynamics', 'computeInverseDynamics', + # 'computeKineticEnergy', 'computeLagrangian', 'computePotentialEnergy', + # 'createBallJointAndBodyNodePair', 'createEulerJointAndBodyNodePair', + # 'createFreeJointAndBodyNodePair', 'createPlanarJointAndBodyNodePair', + # 'createPrismaticJointAndBodyNodePair', 'createRevoluteJointAndBodyNodePair', + # 'createScrewJointAndBodyNodePair', 'createTranslationalJoint2DAndBodyNodePair', + # 'createTranslationalJointAndBodyNodePair', 'createUniversalJointAndBodyNodePair', + # 'createWeldJointAndBodyNodePair', 'dirtyArticulatedInertia', 'dirtySupportPolygon', + # 'disableAdjacentBodyCheck', 'disableSelfCollisionCheck', 'enableAdjacentBodyCheck', + # 'enableSelfCollisionCheck', 'getAcceleration', 'getAccelerationLowerLimit', + # 'getAccelerationLowerLimits', 'getAccelerationUpperLimit', 'getAccelerationUpperLimits', + # 'getAccelerations', 'getAdjacentBodyCheck', 'getAngularJacobian', 'getAngularJacobianDeriv', + # 'getAugMassMatrix', 'getBodyNode', 'getBodyNodes', 'getCOM', 'getCOMJacobian', + # 'getCOMJacobianSpatialDeriv', 'getCOMLinearAcceleration', 'getCOMLinearJacobian', + # 'getCOMLinearJacobianDeriv', 'getCOMLinearVelocity', 'getCOMSpatialAcceleration', + # 'getCOMSpatialVelocity', 'getCommand', 'getCommands', 'getConfiguration', 'getConstraintForces', + # 'getCoriolisAndGravityForces', 'getCoriolisForces', 'getDof', 'getDofs', 'getExternalForces', + # 'getForce', 'getForceLowerLimit', 'getForceLowerLimits', 'getForceUpperLimit', + # 'getForceUpperLimits', 'getForces', 'getGravity', 'getGravityForces', 'getIK', 'getIndexOf', + # 'getInvMassMatrix', 'getJacobian', 'getJacobianClassicDeriv', 'getJacobianSpatialDeriv', + # 'getJoint', 'getJointConstraintImpulses', 'getJoints', 'getLinearJacobian', + # 'getLinearJacobianDeriv', 'getLockableReference', 'getMass', 'getMassMatrix', 'getName', + # 'getNumBodyNodes', 'getNumDofs', 'getNumEndEffectors', 'getNumJoints', 'getNumMarkers', + # 'getNumRigidBodyNodes', 'getNumShapeNodes', 'getNumSoftBodyNodes', 'getNumTrees', 'getPosition', + # 'getPositionDifferences', 'getPositionLowerLimit', 'getPositionLowerLimits', + # 'getPositionUpperLimit', 'getPositionUpperLimits', 'getPositions', 'getProperties', 'getPtr', + # 'getRootBodyNode', 'getRootJoint', 'getSelfCollisionCheck', 'getSkeleton', 'getState', + # 'getSupportVersion', 'getTimeStep', 'getTreeBodyNodes', 'getVelocities', 'getVelocity', + # 'getVelocityChanges', 'getVelocityDifferences', 'getVelocityLowerLimit', + # 'getVelocityLowerLimits', 'getVelocityUpperLimit', 'getVelocityUpperLimits', 'getWorldJacobian', + # 'hasBodyNode', 'hasJoint', 'integratePositions', 'integrateVelocities', + # 'isEnabledAdjacentBodyCheck', 'isEnabledSelfCollisionCheck', 'isImpulseApplied', 'isMobile', + # 'mUnionIndex', 'mUnionRootSkeleton', 'mUnionSize', 'resetAccelerations', 'resetCommands', + # 'resetGeneralizedForces', 'resetPositions', 'resetUnion', 'resetVelocities', 'setAcceleration', + # 'setAccelerationLowerLimit', 'setAccelerationLowerLimits', 'setAccelerationUpperLimit', + # 'setAccelerationUpperLimits', 'setAccelerations', 'setAdjacentBodyCheck', 'setAspectProperties', + # 'setCommand', 'setCommands', 'setConfiguration', 'setForce', 'setForceLowerLimit', + # 'setForceLowerLimits', 'setForceUpperLimit', 'setForceUpperLimits', 'setForces', 'setGravity', + # 'setImpulseApplied', 'setJointConstraintImpulses', 'setMobile', 'setName', 'setPosition', + # 'setPositionLowerLimit', 'setPositionLowerLimits', 'setPositionUpperLimit', + # 'setPositionUpperLimits', 'setPositions', 'setProperties', 'setSelfCollisionCheck', 'setState', + # 'setTimeStep', 'setVelocities', 'setVelocity', 'setVelocityLowerLimit', 'setVelocityLowerLimits', + # 'setVelocityUpperLimit', 'setVelocityUpperLimits', 'updateBiasImpulse', 'updateVelocityChange'] # create world - self.world = pydart.World(dt) + self.sim = dart.simulation.World() + self.world = self.sim + + # create urdf parser + self._urdf_parser = dart.utils.DartLoader() + + # set time step + self.dt = self.world.getTimeStep() # main camera in the simulator self._camera = None + self.viewer = None # if we need to render if render: self.render() + # keep track of visual and collision shapes + self.visual_shapes = {} # {visual_id: Visual} + self.collision_shapes = {} # {collision_id: Collision} + self.bodies = OrderedDict() # {body_id: Body} + self.textures = {} # {texture_id: Texture} + self.constraints = OrderedDict() # {constraint_id: Constraint} + + # create counters + self._visual_cnt = 0 + self._collision_cnt = 0 + self._body_cnt = 1 # 0 is for the world + self._texture_cnt = 0 + self._constraint_cnt = 0 + ############## # Properties # ############## @@ -101,7 +299,12 @@ class Dart(Simulator): @property def version(self): """Return the version of the simulator.""" - return 0 + return "6.10.0" # TODO: get it from the code + + @property + def timestep(self): + """Return the simulator time step.""" + return self.dt ############# # Operators # @@ -127,11 +330,24 @@ class Dart(Simulator): memo[self] = sim return sim + ################## + # Static methods # + ################## + + @staticmethod + def supports_acceleration(): + """Return True if the simulator provides acceleration (dynamic) information (such as joint accelerations, link + Cartesian accelerations, etc). If not, the `Robot` class will have to implement these using finite + difference.""" + return True + ########### # Methods # ########### - # Simulators + ############## + # Simulators # + ############## def reset(self, *args, **kwargs): """Reset the simulator.""" @@ -139,7 +355,7 @@ class Dart(Simulator): def close(self): """Close the simulator.""" - self.world.destroy() + del self.world def seed(self, seed=None): """Set the given seed in the simulator.""" @@ -173,19 +389,31 @@ class Dart(Simulator): enable (bool): If True, it will render the simulator by enabling the GUI. """ self._render = enable - pydart.gui.viewer.launch(self.world) + self.viewer = dart.gui.osg.Viewer() + # gui_node = dart.gui.osg.WorldNode(self.world) + gui_node = dart.gui.osg.RealTimeWorldNode(self.world) + self.viewer.addWorldNode(gui_node) + self.viewer.run() # TODO: call self.viewer.frame() instead (need to update the python wrapper) def hide(self): """Hide the GUI.""" self.render(False) + def get_time_step(self): + """Get the time step in the simulator. + + Returns: + float: time step in the simulator + """ + return self.world.getTimeStep() + def set_time_step(self, time_step): """Set the time step in the simulator. Args: time_step (float): Each time you call 'step' the time step will proceed with 'time_step'. """ - self.world.set_time_step(time_step) + self.world.setTimeStep(time_step) def set_real_time(self, enable=True): """Enable real time in the simulator. @@ -193,7 +421,8 @@ class Dart(Simulator): Args: enable (bool): If True, it will enable the real-time simulation. If False, it will disable it. """ - pass + # create real-time world node (dart.gui.osg.RealTimeWorldNode) + pass # TODO: use threads and create class that inherits from dart.gui.osg.RealTimeWorldNode def pause(self): """Pause the simulator if in real-time.""" @@ -221,7 +450,7 @@ class Dart(Simulator): def get_gravity(self): """Return the gravity set in the simulator.""" - return self.world.gravity() + return self.world.getGravity() def set_gravity(self, gravity=(0, 0, -9.81)): """Set the gravity in the simulator with the given acceleration. @@ -229,7 +458,7 @@ class Dart(Simulator): Args: gravity (list, tuple of 3 floats): acceleration in the x, y, z directions. """ - self.world.set_gravity([0.0, 0.0, -9.81]) + self.world.setGravity(gravity) def save(self, filename=None, *args, **kwargs): """Save the state of the simulator. @@ -282,9 +511,12 @@ class Dart(Simulator): """ pass - # loading URDFs, SDFs, MJCFs + ###################################### + # loading URDFs, SDFs, MJCFs, meshes # + ###################################### - def load_urdf(self, filename, position=None, orientation=(0., 0., 0., 1.), use_fixed_base=0, scale=1.0, *args, **kwargs): + def load_urdf(self, filename, position=None, orientation=(0., 0., 0., 1.), use_fixed_base=0, scale=1.0, *args, + **kwargs): """Load a URDF file in the simulator. Args: @@ -298,7 +530,9 @@ class Dart(Simulator): Returns: int (non-negative): unique id associated to the load model. """ - return self.world.add_skeleton(filename).id + skeleton = self._urdf_parser.parseSkeleton(filename) + self.world.addSkeleton(skeleton) + return self.world.getNumSkeletons() - 1 def load_sdf(self, filename, scaling=1., *args, **kwargs): """Load a SDF file in the simulator. @@ -310,7 +544,7 @@ class Dart(Simulator): Returns: list(int): list of object unique id for each object loaded """ - return self.world.add_skeleton(filename).id + pass def load_mjcf(self, filename, scaling=1., *args, **kwargs): """Load a Mujoco file in the simulator. @@ -322,7 +556,13 @@ class Dart(Simulator): Returns: list(int): list of object unique id for each object loaded """ - raise NotImplementedError("This method is not available with the DART simulator.") + mujoco_parser = MuJoCoParser(filename=filename) + urdf_generator = URDFParser() + skel_generator = SkelParser() + + # generate the URDF/Skel + + raise NotImplementedError("This method is not available yet with the DART simulator.") def load_mesh(self, filename, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), color=None, with_collision=True, flags=None, *args, **kwargs): @@ -344,7 +584,11 @@ class Dart(Simulator): Returns: int: unique id of the mesh in the world """ - self.world.add_skeleton(filename) + # compute the mesh inertia + + # create URDF with that file + + self.world.addSkeleton(filename) @staticmethod def get_available_sdfs(fullpath=False): @@ -386,7 +630,9 @@ class Dart(Simulator): """ return [] - # bodies + ########## + # Bodies # + ########## def create_body(self, visual_shape_id=-1, collision_shape_id=-1, mass=0., position=(0., 0., 0.), orientation=(0., 0., 0., 1.), *args, **kwargs): @@ -403,7 +649,15 @@ class Dart(Simulator): Returns: int: non-negative unique id or -1 for failure. """ - pass + # TODO + + # create skeleton + skeleton = dart.dynamics.Skeleton() + + # add skeleton to the world + self.world.addSkeleton(skeleton) + + return self.world.getNumSkeletons() - 1 def remove_body(self, body_id): """Remove a particular body in the simulator. @@ -411,7 +665,7 @@ class Dart(Simulator): Args: body_id (int): unique body id. """ - pass + self.world.removeSkeleton(body_id) def num_bodies(self): """Return the number of bodies present in the simulator. @@ -419,7 +673,7 @@ class Dart(Simulator): Returns: int: number of bodies """ - return self.world.num_skeletons() + return self.world.getNumSkeletons() def get_body_info(self, body_id): """Get the specified body information. @@ -443,7 +697,9 @@ class Dart(Simulator): """ pass - # constraint + ############### + # constraints # + ############### 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, @@ -472,6 +728,10 @@ class Dart(Simulator): Returns: int: constraint unique id. """ + # Check dart.constraint.* + # ['BallJointConstraint', 'BoxedLcpConstraintSolver', 'BoxedLcpSolver', 'ConstraintBase', 'ConstraintSolver', + # 'DantzigBoxedLcpSolver', 'JointConstraint', 'JointCoulombFrictionConstraint', 'JointLimitConstraint', + # 'PgsBoxedLcpSolver', 'PgsBoxedLcpSolverOption', 'WeldJointConstraint'] pass def remove_constraint(self, constraint_id): @@ -537,7 +797,9 @@ class Dart(Simulator): """ pass - # objects + ########### + # objects # + ########### def get_mass(self, body_id): """ @@ -549,7 +811,8 @@ class Dart(Simulator): Returns: float: total mass of the robot [kg] """ - return self.world.skeletons[body_id].mass() + skeleton = self.world.getSkeleton(body_id) + return skeleton.getMass() def get_base_mass(self, body_id): """Return the base mass of the robot. @@ -557,7 +820,8 @@ class Dart(Simulator): Args: body_id (int): unique object id. """ - pass + base = self.world.getSkeleton(body_id).getRootBodyNode() # this is the same as getBodyNode(0) + return base.getMass() def get_base_name(self, body_id): """ @@ -569,7 +833,8 @@ class Dart(Simulator): Returns: str: base name """ - pass + base = self.world.getSkeleton(body_id).getRootBodyNode() # this is the same as getBodyNode(0) + return base.getName() def get_center_of_mass_position(self, body_id, link_ids=None): """ @@ -583,8 +848,20 @@ class Dart(Simulator): Returns: np.float[3]: center of mass position in the Cartesian world coordinates """ + skeleton = self.world.getSkeleton(body_id) + if link_ids is None: - return self.world.skeletons[body_id].com() + com = skeleton.getCOM() # (3,1) + return com.reshape(-1) + + # if isinstance(link_ids, int): + # link_ids = [link_ids] + # + # coms = [] + # for link_id in link_ids: + # body = skeleton.getBodyNode(link_id + 1) + # + # return None # TODO def get_center_of_mass_velocity(self, body_id, link_ids=None): @@ -599,7 +876,13 @@ class Dart(Simulator): Returns: np.float[3]: center of mass linear velocity. """ - pass + skeleton = self.world.getSkeleton(body_id) + + if link_ids is None: + com_vel = skeleton.getCOMLinearVelocity() # (3,1) + return com_vel.reshape(-1) + + return None # TODO def get_base_pose(self, body_id): """ @@ -612,7 +895,12 @@ class Dart(Simulator): np.float[3]: base position np.float[4]: base orientation (quaternion [x,y,z,w]) """ - pass + base = self.world.getSkeleton(body_id).getRootBodyNode() + transform = base.getWorldTransform() + # position = transform[:-1, 3] + position = base.getCOM().reshape(-1) + orientation = get_quaternion_from_matrix(transform[:-1, :-1]) + return position, orientation def get_base_position(self, body_id): """ @@ -624,7 +912,9 @@ class Dart(Simulator): Returns: np.float[3]: base position. """ - pass + base = self.world.getSkeleton(body_id).getRootBodyNode() + # return base.getWorldTransform()[:-1, 3] + return base.getCOM().reshape(-1) def get_base_orientation(self, body_id): """ @@ -636,7 +926,9 @@ class Dart(Simulator): Returns: np.float[4]: base orientation in the form of a quaternion (x,y,z,w) """ - pass + base = self.world.getSkeleton(body_id).getRootBodyNode() + transform = base.getWorldTransform() + return get_quaternion_from_matrix(transform[:-1, :-1]) def reset_base_pose(self, body_id, position, orientation): """ @@ -680,7 +972,10 @@ class Dart(Simulator): 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 + base = self.world.getSkeleton(body_id).getRootBodyNode() + lin_vel = base.getLinearVelocity() + ang_vel = base.getAngularVelocity() + return lin_vel.reshape(-1), ang_vel.reshape(-1) def get_base_linear_velocity(self, body_id): """ @@ -692,7 +987,9 @@ class Dart(Simulator): Returns: np.float[3]: linear velocity of the base in Cartesian world space coordinates """ - pass + base = self.world.getSkeleton(body_id).getRootBodyNode() + lin_vel = base.getLinearVelocity() + return lin_vel.reshape(-1) def get_base_angular_velocity(self, body_id): """ @@ -704,7 +1001,9 @@ class Dart(Simulator): Returns: np.float[3]: angular velocity of the base in Cartesian world space coordinates """ - pass + base = self.world.getSkeleton(body_id).getRootBodyNode() + ang_vel = base.getAngularVelocity() + return ang_vel.reshape(-1) def reset_base_velocity(self, body_id, linear_velocity=None, angular_velocity=None): """ @@ -750,7 +1049,11 @@ class Dart(Simulator): 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 + link = self.world.getSkeleton(body_id).getBodyNode(link_id + 1) + force = np.asarray(force).reshape(-1, 1) # (3,1) + offset = np.asarray(position).reshape(-1, 1) # (3,1) + is_local = (frame == 1) + link.setExtForce(force=force, offset=offset, isForceLocal=is_local, isOffsetLocal=is_local) def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.), frame=1): """ @@ -764,9 +1067,18 @@ class Dart(Simulator): 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. """ - pass + link = self.world.getSkeleton(body_id).getBodyNode(link_id + 1) + torque = np.asarray(torque).reshape(-1, 1) # (3,1) + is_local = (frame == 1) + link.setExtForce(torque=torque, isLocal=is_local) - # robots (joints and links) + ################### + # transformations # + ################### + + ############################# + # robots (joints and links) # + ############################# def num_joints(self, body_id): """ @@ -778,7 +1090,8 @@ class Dart(Simulator): Returns: int: number of joints with the associated body id. """ - pass + skeleton = self.world.getSkeleton(body_id) + return skeleton.getNumJoints() # TODO: check if we have to add: -1 def num_actuated_joints(self, body_id): """ @@ -790,7 +1103,8 @@ class Dart(Simulator): Returns: int: number of actuated joints of the specified body. """ - pass + skeleton = self.world.getSkeleton(body_id) + return skeleton.getNumDofs() # TODO: check with fixed and floating base def num_links(self, body_id): """ @@ -802,6 +1116,8 @@ class Dart(Simulator): Returns: int: number of links with the associated body id. """ + # skeleton = self.world.getSkeleton(body_id) + # return skeleton.getNumBodyNodes() return self.num_joints(body_id) def get_joint_info(self, body_id, joint_id): @@ -816,7 +1132,27 @@ class Dart(Simulator): joint_id (int): joint id is included in [0..`num_joints(body_id)`]. Returns: - dict, list: joint info + [0] int: the same joint id as the input parameter + [1] str: name of the joint (as specified in the URDF/SDF/etc file) + [2] int: type of the joint which implie the number of position and velocity variables. + The types include JOINT_REVOLUTE (=0), JOINT_PRISMATIC (=1), JOINT_SPHERICAL (=2), + JOINT_PLANAR (=3), and JOINT_FIXED (=4). + [3] int: q index - the first position index in the positional state variables for this body + [4] int: dq index - the first velocity index in the velocity state variables for this body + [5] int: flags (reserved) + [6] float: the joint damping value (as specified in the URDF file) + [7] float: the joint friction value (as specified in the URDF file) + [8] float: the positional lower limit for slider and revolute joints + [9] float: the positional upper limit for slider and revolute joints + [10] float: maximum force specified in URDF. Note that this value is not automatically used. + You can use maxForce in 'setJointMotorControl2'. + [11] float: maximum velocity specified in URDF. Note that this value is not used in actual + motor control commands at the moment. + [12] str: name of the link (as specified in the URDF/SDF/etc file) + [13] np.array[3]: joint axis in local frame (ignored for JOINT_FIXED) + [14] np.array[3]: joint position in parent frame + [15] np.array[4]: joint orientation in parent frame + [16] int: parent link index, -1 for base """ pass @@ -837,7 +1173,12 @@ class Dart(Simulator): 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 + joint = self.world.getSkeleton(body_id).getJoint(joint_id + 1) + position = joint.getPosition(0) # joints can have less or more than 1 DoF (like WeldJoint, FreeJoint) + velocity = joint.getVelocity(0) + reaction_forces = np.zeros(6) # TODO + torque = joint.getForce(0) + return position, velocity, reaction_forces, torque def get_joint_states(self, body_id, joint_ids): """ @@ -857,7 +1198,9 @@ class Dart(Simulator): 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 + if isinstance(joint_ids, int): + return self.get_joint_state(body_id, joint_ids) + return [self.get_joint_state(body_id, joint_id) for joint_id in joint_ids] def reset_joint_state(self, body_id, joint_id, position, velocity=0.): """ @@ -870,7 +1213,12 @@ class Dart(Simulator): position (float): the joint position (angle in radians [rad] or position [m]) velocity (float): the joint velocity (angular [rad/s] or linear velocity [m/s]) """ - pass + joint = self.world.getSkeleton(body_id).getJoint(joint_id + 1) + # joint.resetPosition(0) + # joint.resetVelocity(0) + joint.setPosition(0, position) + if velocity is not None: + joint.setVelocity(0, velocity) def enable_joint_force_torque_sensor(self, body_id, joint_ids, enable=True): """ @@ -938,7 +1286,26 @@ class Dart(Simulator): 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 + body = self.world.getSkeleton(body_id).getBodyNode(link_id + 1) + transform = body.getWorldTransform() + com_position = body.getCOM().reshape(-1) + com_orientation = get_quaternion_from_matrix(transform[:-1, :-1]) + + local_position = body.getLocalCOM().reshape(-1) + local_orientation = np.array([0., 0., 0., 1.]) + + world_position = transform[:-1, 3] + world_orientation = np.array(com_orientation) + + results = [com_position, com_orientation, local_position, local_orientation, world_position, world_orientation] + + if compute_velocity: + linear_velocity = body.getLinearVelocity().reshape(-1) + angular_velocity = body.getAngularVelocity().reshape(-1) + results.append(linear_velocity) + results.append(angular_velocity) + + return results def get_link_states(self, body_id, link_ids, compute_velocity=False, compute_forward_kinematics=False): """ @@ -963,7 +1330,10 @@ class Dart(Simulator): 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 + if isinstance(link_ids, int): + return self.get_link_state(body_id, link_ids, compute_velocity, compute_forward_kinematics) + return [self.get_link_state(body_id, link_id, compute_velocity, compute_forward_kinematics) + for link_id in link_ids] def get_link_names(self, body_id, link_ids): """ @@ -979,7 +1349,12 @@ class Dart(Simulator): if multiple links: str[N]: link names """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(link_ids, int): + return skeleton.getBodyNode(link_ids + 1).getName() + + return [skeleton.getBodyNode(link + 1).getName() for link in link_ids] def get_link_masses(self, body_id, link_ids): """ @@ -995,10 +1370,42 @@ class Dart(Simulator): else: float[N]: mass of each link """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(link_ids, int): + return skeleton.getBodyNode(link_ids + 1).getMass() + + return [skeleton.getBodyNode(link + 1).getMass() for link in link_ids] def get_link_frames(self, body_id, link_ids): - pass + r""" + Return the link world frame position(s) and orientation(s). + + Args: + body_id (int): body id. + link_ids (int, int[N]): link id, or list of desired link ids. + + Returns: + if 1 link: + np.array[3]: the link frame position in the world space + np.array[4]: Cartesian orientation of the link frame [x,y,z,w] + if multiple links: + np.array[N,3]: link frame position of each link in world space + np.array[N,4]: orientation of each link frame [x,y,z,w] + """ + skeleton = self.world.getSkeleton(body_id) + + if isinstance(link_ids, int): + transform = skeleton.getBodyNode(link_ids + 1).getWorldTransform() + return transform[:-1, 3], get_quaternion_from_matrix(transform[:-1, :-1]) + + positions, orientations = [], [] + for link_id in link_ids: + transform = skeleton.getBodyNode(link_ids + 1).getWorldTransform() + positions.append(transform[:-1, 3]) + orientations.append(get_quaternion_from_matrix(transform[:-1, :-1])) + + return np.array(positions), np.array(orientations) def get_link_world_positions(self, body_id, link_ids): """ @@ -1014,7 +1421,12 @@ class Dart(Simulator): if multiple links: np.float[N,3]: CoM position of each link in world space """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(link_ids, int): + return skeleton.getBodyNode(link_ids + 1).getCOM().reshape(-1) + + return [skeleton.getBodyNode(link + 1).getCOM().reshape(-1) for link in link_ids] def get_link_positions(self, body_id, link_ids): pass @@ -1033,7 +1445,13 @@ class Dart(Simulator): if multiple links: np.float[N,4]: CoM orientation of each link (x,y,z,w) """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(link_ids, int): + return get_quaternion_from_matrix(skeleton.getBodyNode(link_ids + 1).getWorldTransform()[:-1, :-1]) + + return [get_quaternion_from_matrix(skeleton.getBodyNode(link + 1).getWorldTransform()[:-1, :-1]) + for link in link_ids] def get_link_orientations(self, body_id, link_ids): pass @@ -1052,7 +1470,12 @@ class Dart(Simulator): if multiple links: np.float[N,3]: linear velocity of each link """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(link_ids, int): + return skeleton.getBodyNode(link_ids + 1).getLinearVelocity().reshape(-1) + + return [skeleton.getBodyNode(link + 1).getLinearVelocity.reshape(-1) for link in link_ids] def get_link_world_angular_velocities(self, body_id, link_ids): """ @@ -1068,7 +1491,12 @@ class Dart(Simulator): if multiple links: np.float[N,3]: angular velocity of each link """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(link_ids, int): + return skeleton.getBodyNode(link_ids + 1).getAngularVelocity().reshape(-1) + + return [skeleton.getBodyNode(link + 1).getAngularVelocity.reshape(-1) for link in link_ids] def get_link_world_velocities(self, body_id, link_ids): """ @@ -1085,7 +1513,21 @@ class Dart(Simulator): if multiple links: np.float[N,6]: linear and angular velocity of each link """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(link_ids, int): + link = skeleton.getBodyNode(link_ids + 1) + lin_vel = link.getLinearVelocity().reshape(-1) + ang_vel = link.getAngularVelocity().reshape(-1) + return np.concatenate((lin_vel, ang_vel)) + + velocities = [] + for link_id in link_ids: + link = skeleton.getBodyNode(link_id + 1) + lin_vel = link.getLinearVelocity().reshape(-1) + ang_vel = link.getAngularVelocity().reshape(-1) + velocities.append(np.concatenate((lin_vel, ang_vel))) + return velocities def get_link_velocities(self, body_id, link_ids): pass @@ -1116,7 +1558,14 @@ class Dart(Simulator): Returns: list of int: actuated joint ids. """ - pass + skeleton = self.world.getSkeleton(body_id) + + joint_ids = [] + for joint_id in range(1, skeleton.getNumJoints()): + num_dofs = skeleton.getJoint(joint_id).getNumDofs() + if num_dofs > 0: + joint_ids.append(joint_id - 1) + return joint_ids def get_joint_names(self, body_id, joint_ids): """ @@ -1132,7 +1581,12 @@ class Dart(Simulator): if multiple joints: str[N]: name of each joint """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(joint_ids, int): + return skeleton.getJoint(joint_ids + 1).getName() + + return [skeleton.getJoint(joint + 1).getName() for joint in joint_ids] def get_joint_type_ids(self, body_id, joint_ids): """ @@ -1149,7 +1603,7 @@ class Dart(Simulator): """ pass - def get_joint_type_names(self, body_id, joint_ids): + def get_joint_type_names(self, body_id, joint_ids): # TODO: make sure it is the same as other simulators """ Get joint type names. @@ -1162,7 +1616,14 @@ class Dart(Simulator): str: joint type name. if multiple joints: list of above """ - pass + # bullet: ['revolute', 'prismatic', 'spherical', 'planar', 'fixed', 'point2point', 'gear'] + # dart: ['ball', 'free', 'euler', 'weld' (=fixed), 'revolute', 'universal', 'prismatic'] + skeleton = self.world.getSkeleton(body_id) + + if isinstance(joint_ids, int): + return skeleton.getJoint(joint_ids + 1).getType() + + return [skeleton.getJoint(joint + 1).getType() for joint in joint_ids] def get_joint_dampings(self, body_id, joint_ids): """ @@ -1178,7 +1639,12 @@ class Dart(Simulator): if multiple joints: np.float[N]: damping coefficient for each specified joint """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(joint_ids, int): + return skeleton.getJoint(joint_ids + 1).getDampingCoefficient(0) + + return [skeleton.getJoint(joint + 1).getDampingCoefficient(0) for joint in joint_ids] def get_joint_frictions(self, body_id, joint_ids): """ @@ -1194,7 +1660,12 @@ class Dart(Simulator): if multiple joints: np.float[N]: friction coefficient for each specified joint """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(joint_ids, int): + return skeleton.getJoint(joint_ids + 1).getCoulombFriction(0) + + return [skeleton.getJoint(joint + 1).getCoulombFriction(0) for joint in joint_ids] def get_joint_limits(self, body_id, joint_ids): """ @@ -1210,7 +1681,17 @@ class Dart(Simulator): if multiple joints: np.float[N,2]: lower and upper limit for each specified joint """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(joint_ids, int): + joint = skeleton.getJoint(joint_ids + 1) + return np.array([joint.getPositionLowerLimit(0), joint.getPositionUpperLimit(0)]) + + limits = [] + for joint_id in joint_ids: + joint = skeleton.getJoint(joint_id + 1) + limits.append([joint.getPositionLowerLimit(0), joint.getPositionUpperLimit(0)]) + return np.array(limits) def get_joint_max_forces(self, body_id, joint_ids): """ @@ -1228,7 +1709,18 @@ class Dart(Simulator): if multiple joints: np.float[N]: maximum force for each specified joint [N] """ - pass + # TODO + skeleton = self.world.getSkeleton(body_id) + + if isinstance(joint_ids, int): + joint = skeleton.getJoint(joint_ids + 1) + return np.max(np.abs([joint.getForceLowerLimit(0), joint.getForceUpperLimit(0)])) + + forces = [] + for joint_id in joint_ids: + joint = skeleton.getJoint(joint_id + 1) + forces.append(np.max(np.abs([joint.getForceLowerLimit(0), joint.getForceUpperLimit(0)]))) + return np.array(forces) def get_joint_max_velocities(self, body_id, joint_ids): """ @@ -1246,7 +1738,18 @@ class Dart(Simulator): if multiple joints: np.float[N]: maximum velocities for each specified joint [rad/s] """ - pass + # TODO + skeleton = self.world.getSkeleton(body_id) + + if isinstance(joint_ids, int): + joint = skeleton.getJoint(joint_ids + 1) + return np.max(np.abs([joint.getVelocityLowerLimit(0), joint.getVelocityUpperLimit(0)])) + + velocities = [] + for joint_id in joint_ids: + joint = skeleton.getJoint(joint_id + 1) + velocities.append(np.max(np.abs([joint.getVelocityLowerLimit(0), joint.getVelocityUpperLimit(0)]))) + return np.array(velocities) def get_joint_axes(self, body_id, joint_ids): """ @@ -1262,7 +1765,20 @@ class Dart(Simulator): if multiple joint: np.float[N,3]: list of joint axis """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(joint_ids, int): + joint = skeleton.getJoint(joint_ids + 1) + if hasattr(joint, 'getAxis'): + return joint.getAxis() + return np.zeros(3) # TODO: should we return None instead? + + axes = [] + for joint_id in joint_ids: + joint = skeleton.getJoint(joint_id + 1) + axis = np.zeros(3) if not hasattr(joint, 'getAxis') else joint.getAxis() + axes.append(axis) + return np.array(axes) def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None): """ @@ -1277,7 +1793,16 @@ class Dart(Simulator): 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 + skeleton = self.world.getSkeleton(body_id) + + # TODO: use position control + + if isinstance(joint_ids, int): + # Some joints have more or less than 1 DoF (like Free, Weld=Fixed) + return skeleton.getJoint(joint_ids + 1).setPosition(0, positions) + + for joint_id, q in zip(joint_ids, positions): + skeleton.getJoint(joint_id + 1).setPosition(0, q) def get_joint_positions(self, body_id, joint_ids): """ @@ -1293,7 +1818,13 @@ class Dart(Simulator): if multiple joints: np.float[N]: joint positions [rad] """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(joint_ids, int): + # Some joints have more or less than 1 DoF (like Free, Weld=Fixed) + return skeleton.getJoint(joint_ids + 1).getPosition(0) + + return np.array([skeleton.getJoint(joint_id + 1).getPosition(0) for joint_id in joint_ids]) def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None): """ @@ -1305,7 +1836,16 @@ class Dart(Simulator): 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 + skeleton = self.world.getSkeleton(body_id) + + # TODO: use velocity control + + if isinstance(joint_ids, int): + # Some joints have more or less than 1 DoF (like Free, Weld=Fixed) + return skeleton.getJoint(joint_ids + 1).setVelocity(0, velocities) + + for joint_id, dq in zip(joint_ids, velocities): + skeleton.getJoint(joint_id + 1).setVelocity(0, dq) def get_joint_velocities(self, body_id, joint_ids): """ @@ -1321,7 +1861,13 @@ class Dart(Simulator): if multiple joints: np.float[N]: joint velocities [rad/s] """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(joint_ids, int): + # Some joints have more or less than 1 DoF (like Free, Weld=Fixed) + return skeleton.getJoint(joint_ids + 1).getVelocity(0) + + return np.array([skeleton.getJoint(joint_id + 1).getVelocity(0) for joint_id in joint_ids]) def set_joint_accelerations(self, body_id, joint_ids, accelerations, q=None, dq=None): """ @@ -1353,7 +1899,13 @@ class Dart(Simulator): if multiple joints: np.float[N]: joint accelerations [rad/s^2] """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(joint_ids, int): + # Some joints have more or less than 1 DoF (like Free, Weld=Fixed) + return skeleton.getJoint(joint_ids + 1).getAcceleration(0) + + return np.array([skeleton.getJoint(joint_id + 1).getAcceleration(0) for joint_id in joint_ids]) def set_joint_torques(self, body_id, joint_ids, torques): """ @@ -1364,7 +1916,16 @@ class Dart(Simulator): 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 + skeleton = self.world.getSkeleton(body_id) + + # TODO: use velocity control + + if isinstance(joint_ids, int): + # Some joints have more or less than 1 DoF (like Free, Weld=Fixed) + return skeleton.getJoint(joint_ids + 1).setForce(0, torques) + + for joint_id, dq in zip(joint_ids, torques): + skeleton.getJoint(joint_id + 1).setForce(0, dq) def get_joint_torques(self, body_id, joint_ids): """ @@ -1380,7 +1941,13 @@ class Dart(Simulator): if multiple joints: np.float[N]: torques associated to the given joints [Nm] """ - pass + skeleton = self.world.getSkeleton(body_id) + + if isinstance(joint_ids, int): + # Some joints have more or less than 1 DoF (like Free, Weld=Fixed) + return skeleton.getJoint(joint_ids + 1).getForce(0) + + return np.array([skeleton.getJoint(joint_id + 1).getForce(0) for joint_id in joint_ids]) 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 @@ -1411,9 +1978,13 @@ class Dart(Simulator): if multiple joints: np.float[N]: power at each joint [W] """ - pass + torque = self.get_joint_torques(body_id, joint_ids) + velocity = self.get_joint_velocities(body_id, joint_ids) + return torque * velocity - # visualization + ################# + # Visualization # + ################# 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, @@ -1695,7 +2266,9 @@ class Dart(Simulator): """ pass - # collisions + ############## + # Collisions # + ############## 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, @@ -1806,6 +2379,8 @@ class Dart(Simulator): float: lateral friction force in the second lateral friction direction (see next returned value) np.float[3]: second lateral friction direction """ + # results: contact point, normal and penetration depth + results = self.world.checkCollision() pass def get_closest_points(self, body1, body2, distance, link1_id=None, link2_id=None): @@ -1909,7 +2484,9 @@ class Dart(Simulator): """ pass - # kinematics and dynamics + ########################### + # Kinematics and Dynamics # + ########################### def get_dynamics_info(self, body_id, link_id=-1): """ @@ -1932,7 +2509,28 @@ class Dart(Simulator): float: damping of contact constraints. -1 if not available. float: stiffness of contact constraints. -1 if not available. """ - pass + body = self.world.getSkeleton(body_id).getBodyNode(link_id + 1) + mass = body.getMass() + + dynamics = body.getShapeNode(0).getDynamicsAspect() + friction = dynamics.getFrictionCoeff() + + ixx, iyy, izz, ixy, ixz, iyz = 0., 0., 0., 0., 0., 0. + body.getMomentOfInertia(ixx, iyy, izz, ixy, ixz, iyz) + inertia = np.array([[ixx, ixy, ixz], [ixy, iyy, iyz], [ixz, iyz, izz]]).reshape(3, 3) + local_inertia_diag = np.linalg.eigh(inertia)[0] + + position = body.getLocalCOM().reshape(-1) + orientation = get_quaternion_from_matrix(body.getWorldTransform()[:-1, :-1]) + + restitution = dynamics.getRestitutionCoeff() + rolling_friction = -1 + spinning_friction = -1 + damping = -1 + stiffness = -1 + + return [mass, friction, local_inertia_diag, position, orientation, restitution, rolling_friction, + spinning_friction, damping, stiffness] 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, @@ -1964,7 +2562,30 @@ class Dart(Simulator): joint damping field. Keep the value close to 0. `joint_damping_force = -damping_coefficient * joint_velocity`. """ - pass + skeleton = self.world.getSkeleton(body_id) + body = skeleton.getBodyNode(link_id + 1) + joint = skeleton.getJoint(link_id + 1) + + if mass is not None: + body.setMass(mass) + + if lateral_friction is not None: + dynamics = body.getShapeNode(0).getDynamicsAspect() + dynamics.setFrictionCoeff(lateral_friction) + + if restitution is not None: + dynamics = body.getShapeNode(0).getDynamicsAspect() + dynamics.setRestitutionCoeff(restitution) + + if local_inertia_diagonal is not None: + ixx, iyy, izz = local_inertia_diagonal + body.setMomentOfInertia(Ixx=ixx, Iyy=iyy, Izz=izz) + + if joint_damping is not None: + joint.setDampingCoefficient(joint_damping) + + # if joint_friction is not None: + # joint.setCoulombFriction(joint_friction) def calculate_jacobian(self, body_id, link_id, local_position, q, dq=None, des_ddq=None): r""" @@ -1990,8 +2611,11 @@ class Dart(Simulator): 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. """ - body = self.world.skeletons[body_id].bodynodes[link_id] - return body.jacobian(offset=local_position) # body.world_jacobian(offset=local_position) + skeleton = self.world.getSkeleton(body_id) + body = skeleton.getBodyNode(link_id + 1) + # TODO: set q? + local_position = np.asarray(local_position).reshape(-1, 1) + return skeleton.getWorldJacobian(node=body, localOffset=local_position) def calculate_mass_matrix(self, body_id, q): r""" @@ -2014,8 +2638,9 @@ class Dart(Simulator): Returns: np.float[N,N], np.float[6+N,6+N]: inertia matrix """ - self.world.skeletons[body_id].set_positions(q) - return self.world.skeletons[body_id].mass_matrix() + skeleton = self.world.getSkeleton(body_id) + # TODO: set q? + return skeleton.getAugMassMatrix() 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, @@ -2098,13 +2723,16 @@ class Dart(Simulator): 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, + - [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 + skeleton = self.world.getSkeleton(body_id) + c = skeleton.getCoriolisAndGravityForces().reshape(-1) # (M,) + H = skeleton.getAugMassMatrix() # (M,M) + return H.dot(des_ddq) + c def calculate_forward_dynamics(self, body_id, q, dq, torques): r""" @@ -2146,15 +2774,22 @@ class Dart(Simulator): np.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, + - [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 + skeleton = self.world.getSkeleton(body_id) + c = skeleton.getCoriolisAndGravityForces().reshape(-1) # (M,) + # H_inv = skeleton.getInvMassMatrix() # (M,M) + H = skeleton.getAugMassMatrix() # (M,M) + H_inv = np.linalg.inv(H) + return H_inv.dot((torques - c)) - # debug + ######### + # Debug # + ######### 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): @@ -2403,7 +3038,9 @@ class Dart(Simulator): """ pass - # events (mouse, keyboard) + ############################ + # Events (mouse, keyboard) # + ############################ def get_keyboard_events(self): """Get the key events. @@ -2451,6 +3088,40 @@ if __name__ == '__main__': # create simulation sim = Dart(render=False) - skeleton = sim.load_urdf(os.path.dirname(__file__) + '/../robots/urdfs/coman/coman.urdf') - print(dir(skeleton)) - sim.render() + skeleton_id = sim.load_urdf(os.path.dirname(__file__) + '/../robots/urdfs/cubli/cubli.urdf') + # skeleton_id = sim.load_urdf(os.path.dirname(__file__) + '/../robots/urdfs/rrbot/pendulum.urdf') + skeleton = sim.world.getSkeleton(skeleton_id) + print("World name: {}".format(sim.world.getName())) + print("Gravity: {}".format(sim.world.getGravity())) + print("Skeleton name from world: {}".format(sim.world.getSkeleton(0).getName())) + print("Skeleton name: {}".format(skeleton.getName())) + print("DoFs: {}".format(skeleton.getNumDofs())) + print("Num of Body nodes: {}".format(skeleton.getNumBodyNodes())) + print("Num of Joints: {}".format(skeleton.getNumJoints())) + print("Positions: {}".format(skeleton.getPositions())) + base = skeleton.getRootBodyNode() + print("Base name: {}".format(base.getName())) + print("Transform: {}".format(base.getTransform())) + body1 = skeleton.getBodyNode(0) + print("body1 name: {}".format(body1.getName())) + body2 = skeleton.getBodyNode(1) + print("body2 name: {}".format(body2.getName())) + print("body2 transform: {}".format(body2.getTransform())) + # print("body2 world transform: {}".format(body2.getWorldTransform())) + # print("body2 relative transform: {}".format(body2.getRelativeTransform())) + # sim.render() + + for dof in range(skeleton.getNumDofs()): + skeleton.setPosition(dof, np.pi/5) + + for joint_id in range(skeleton.getNumJoints()): + joint = skeleton.getJoint(joint_id) + print("\njoint id: {}".format(joint_id)) + print("joint name: {}".format(joint.getName())) + print("joint type: {}".format(joint.getType())) + if hasattr(joint, 'getAxis'): + print("joint axis: {}".format(joint.getAxis())) + print("joint num DoFs: {}".format(joint.getNumDofs())) + print("joint position: {}".format(joint.getPosition(0))) + print("skeleton joint position: ", skeleton.getPosition(joint_id)) + diff --git a/pyrobolearn/simulators/mujoco.py b/pyrobolearn/simulators/mujoco.py index 8a171b2..9af6583 100644 --- a/pyrobolearn/simulators/mujoco.py +++ b/pyrobolearn/simulators/mujoco.py @@ -62,7 +62,6 @@ except ImportError as e: # import pyrobolearn related functionalities from pyrobolearn.simulators.simulator import Simulator -# from pyrobolearn.utils.parsers.robots import mujoco_parser, urdf_parser, sdf_parser from pyrobolearn.utils.parsers.robots import URDFParser, MuJoCoParser, SDFParser diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index 9630a9d..655abb7 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -12,9 +12,9 @@ dynamics, etc. Dependencies in PRL: None References: - [1] PyBullet: https://pybullet.org - [2] PyBullet Quickstart Guide: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA - [3] PEP8: https://www.python.org/dev/peps/pep-0008/ + - [1] PyBullet: https://pybullet.org + - [2] PyBullet Quickstart Guide: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA + - [3] PEP8: https://www.python.org/dev/peps/pep-0008/ """ from pyrobolearn.utils.data_structures.orderedset import OrderedSet @@ -42,8 +42,8 @@ class Simulator(object): sim = GazeboROS() References: - [1] PyBullet: https://pybullet.org - [2] PEP8: https://www.python.org/dev/peps/pep-0008/ + - [1] PyBullet: https://pybullet.org + - [2] PEP8: https://www.python.org/dev/peps/pep-0008/ """ # keep track of the instantiated simulators @@ -2350,10 +2350,10 @@ class Simulator(object): np.array[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, + - [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 @@ -2398,10 +2398,10 @@ class Simulator(object): np.array[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, + - [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 diff --git a/pyrobolearn/utils/parsers/robots/__init__.py b/pyrobolearn/utils/parsers/robots/__init__.py index 6444af6..5c7579f 100644 --- a/pyrobolearn/utils/parsers/robots/__init__.py +++ b/pyrobolearn/utils/parsers/robots/__init__.py @@ -7,3 +7,9 @@ from .mujoco_parser import MuJoCoParser # import sdf parser from .sdf_parser import SDFParser + +# import skel parser +from .skel_parser import SkelParser + +# import proto parser +# from .proto_parser import ProtoParser diff --git a/pyrobolearn/utils/parsers/robots/data_structures.py b/pyrobolearn/utils/parsers/robots/data_structures.py index d044c65..073a364 100644 --- a/pyrobolearn/utils/parsers/robots/data_structures.py +++ b/pyrobolearn/utils/parsers/robots/data_structures.py @@ -1,5 +1,5 @@ #!/usr/bin/env python -"""Provide the data structures that are shared among the various parsers and converters. +"""Provide the common data structures that are shared among the various parsers, generators, and converters. """ import numpy as np @@ -62,101 +62,18 @@ class PhysicsEngine(object): self.tolerance = None -class World(object): - r"""World data structure. - - World frame (robotics convention with the right-hand rule): - - the x axis points forward - - the y axis points to the left - - the z axis points upward - """ - - def __init__(self, name=None): - self.name = name - self.trees = OrderedDict() - self.physics = None - - @property - def physics(self): - return self._physics - - @physics.setter - def physics(self, physics): - if physics is not None and not isinstance(physics, Physics): - raise TypeError("Expecting the physics to be an instance of `Physics`, but got instead: " - "{}".format(type(physics))) - self._physics = physics - - -class Light(object): - r"""Light data structure. - - Type of light: point, directional, and spot - """ - - def __init__(self, name=None, dtype=None, cast_shadows=None, diffuse=None, specular=None, attenuation=None, - direction=None, spot=None, position=None, orientation=None): - """ - Initialize the Light data structure. - - Args: - name (str): unique name for the light. - dtype (str): type of light, select between {'point', 'directional', 'spot'} - cast_shadows (bool): if True, it will cast shadows. - diffuse (tuple of 4 float, np.array[4]): diffuse light (RGBA) color. - specular (tuple of 4 float, np.array[4]): specular light (RGBA) color. - attenuation: light attenuation - direction (np.array[3]): direction of the light if dtype='directional' or dtype='spot'. - spot: spot light parameters - position (tuple/list of 3 float, np.array[3]): position of the light in the world. - orientation (tuple/list of 3 float, np.array[3]): orientation of the light in the world. - """ - self.name = name - self.dtype =dtype - self.shadows = cast_shadows - self.diffuse = diffuse - self.specular = specular - self.attenuation = attenuation - self.direction = direction - self.spot = spot - self.position = position - self.orientation = orientation - - -class Physics(object): - r"""Physical properties of the world. - - This includes gravity, friction, viscosity, etc. - """ - - def __init__(self, gravity=(0., 0., -9.81)): - self.gravity = gravity - - class Frame(object): r"""Reference Frame""" - def __init__(self, position=None, orientation=None, dtype=None, right_handed=True, forward_axis=(1., 0., 0.), - up_axis=(0., 0., 1.)): + def __init__(self, position=None, orientation=None, dtype=None, right_handed=True): + # forward_axis=(1., 0., 0.), up_axis=(0., 0., 1.)): + self.position = position self.orientation = orientation self.dtype = dtype # world frame, body frame, joint frame, inertial frame, etc. self.right_handed = right_handed - self.forward_axis = forward_axis - self.up_axis = up_axis - - -class Tree(object): - r"""Tree data structure.""" - - def __init__(self, name=None, root=None): - self.name = name - self.root = root - self.bodies = OrderedDict() - self.joints = OrderedDict() - self.materials = {} - self.position = None - self.orientation = None + # self.forward_axis = forward_axis + # self.up_axis = up_axis @property def position(self): @@ -188,11 +105,11 @@ class Tree(object): @property def rpy(self): - return self._orientation + return self.orientation @property def quaternion(self): - return get_quaternion_from_rpy(self._orientation) + return get_quaternion_from_rpy(self.orientation) @property def rot(self): @@ -200,6 +117,8 @@ class Tree(object): @property def pose(self): + if self.position is None and self.orientation is None: + return None return self.position, self.orientation @pose.setter @@ -222,6 +141,266 @@ class Tree(object): raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray") +class Physics(object): + r"""Physical properties of the world. + + This includes gravity, friction, viscosity, etc. + """ + + def __init__(self, gravity=(0., 0., -9.81), timestep=None): + # gravity depends on the world frame; the frame axis convention that we use. + # By default, x points forward, y on the left, and z upward. + self.gravity = gravity + self.timestep = timestep + + @property + def gravity(self): + return self._gravity + + @gravity.setter + def gravity(self, gravity): + if gravity is not None: + if isinstance(gravity, str): + gravity = [float(g) for g in gravity.split()] + gravity = np.asarray(gravity).reshape(-1) + self._gravity = gravity + + @property + def timestep(self): + return self._timestep + + @timestep.setter + def timestep(self, timestep): + if timestep is not None: + timestep = float(timestep) + self._timestep = timestep + + +class World(object): + r"""World data structure. + + World frame (robotics convention with the right-hand rule): + - the x axis points forward + - the y axis points to the left + - the z axis points upward + """ + + def __init__(self, name=None): + self.name = name + self.trees = OrderedDict() + self.physics = None + self.lights = OrderedDict() + + @property + def physics(self): + return self._physics + + @physics.setter + def physics(self, physics): + if physics is not None and not isinstance(physics, Physics): + raise TypeError("Expecting the physics to be an instance of `Physics`, but got instead: " + "{}".format(type(physics))) + self._physics = physics + + +class Light(object): + r"""Light data structure. + + Type of light: point, directional, and spot + """ + + def __init__(self, name=None, dtype=None, cast_shadows=None, ambient=None, diffuse=None, specular=None, + attenuation=None, direction=None, spot=None, position=None, orientation=None, active=True): + """ + Initialize the Light data structure. + + Args: + name (str): unique name for the light. + dtype (str): type of light, select between {'point', 'directional', 'spot'} + cast_shadows (bool): if True, it will cast shadows. + diffuse (tuple of 4 float, np.array[4]): diffuse light (RGBA) color. + specular (tuple of 4 float, np.array[4]): specular light (RGBA) color. + attenuation: light attenuation + direction (np.array[3]): direction of the light if dtype='directional' or dtype='spot'. + spot: spot light parameters + position (tuple/list of 3 float, np.array[3]): position of the light in the world. + orientation (np.array[3], str): orientation of the light expressed as roll-pitch-yaw angles. + active (bool): if True, the light is on. + """ + self.name = name + self.dtype = dtype + self.shadows = cast_shadows + self.ambient = ambient + self.diffuse = diffuse + self.specular = specular + self.attenuation = attenuation + self.direction = direction + self.spot = spot + self.active = active + self.frame = Frame(position=position, orientation=orientation) + + @property + def shadows(self): + return self._shadows + + @shadows.setter + def shadows(self, enable): + if enable is not None: + if isinstance(enable, str): + enable = enable.lower() + if len(enable) == 1: + enable = int(enable) + elif enable == 'false': + enable = 0 + elif enable == 'true': + enable = 1 + enable = bool(enable) + self._shadows = enable + + @property + def position(self): + return self.frame.position + + @position.setter + def position(self, position): + self.frame.position = position + + @property + def orientation(self): + return self.frame.orientation + + @orientation.setter + def orientation(self, orientation): + self.frame.orientation = orientation + + @property + def rpy(self): + return self.frame.rpy + + @property + def quaternion(self): + return self.frame.quaternion + + @property + def rot(self): + return self.frame.quaternion + + @property + def pose(self): + return self.frame.pose + + @pose.setter + def pose(self, pose): + self.frame.pose = pose + + @property + def direction(self): + return self._direction + + @direction.setter + def direction(self, direction): + if direction is not None: + if isinstance(direction, str): + direction = [float(d) for d in direction.split()] + direction = np.asarray(direction).reshape(-1) + self._direction = direction + + @property + def ambient(self): + return self._ambient + + @ambient.setter + def ambient(self, ambient): + if ambient is not None: + if isinstance(ambient, str): # e.g. '0.5 0.1 1. 1.' + ambient = (float(c) for c in ambient.split()) + if not isinstance(ambient, (list, tuple)): + raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float") + else: + ambient = tuple(ambient) + self._ambient = ambient + + color = ambient + + @property + def diffuse(self): + return self._diffuse + + @diffuse.setter + def diffuse(self, diffuse): + if diffuse is not None: + if isinstance(diffuse, str): # e.g. '0.5 0.1 1. 1.' + diffuse = (float(c) for c in diffuse.split()) + if not isinstance(diffuse, (list, tuple)): + raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float") + else: + diffuse = tuple(diffuse) + self._diffuse = diffuse + + @property + def specular(self): + return self._specular + + @specular.setter + def specular(self, specular): + if specular is not None: + if isinstance(specular, str): # e.g. '0.5 0.1 1. 1.' + specular = (float(c) for c in specular.split()) + if not isinstance(specular, (list, tuple)): + raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float") + else: + specular = tuple(specular) + self._specular = specular + + +class Tree(object): + r"""Tree data structure.""" + + def __init__(self, name=None, root=None): + self.name = name + self.root = root + self.bodies = OrderedDict() + self.joints = OrderedDict() + self.materials = {} + self.frame = Frame() + + @property + def position(self): + return self.frame.position + + @position.setter + def position(self, position): + self.frame.position = position + + @property + def orientation(self): + return self.frame.orientation + + @orientation.setter + def orientation(self, orientation): + self.frame.orientation = orientation + + @property + def rpy(self): + return self.frame.rpy + + @property + def quaternion(self): + return self.frame.quaternion + + @property + def rot(self): + return self.frame.quaternion + + @property + def pose(self): + return self.frame.pose + + @pose.setter + def pose(self, pose): + self.frame.pose = pose + + class Body(object): r"""Body / Link data structure.""" @@ -255,6 +434,11 @@ class Joint(object): - screw: a single DoF joint wich coupled sliding and rotational motion - universal: like a ball joint, but constrains one DoF - planar: allows motion in a plane perpendicular to the axis. + + - URDF: continuous, fixed, floating, planar, prismatic, revolute + - SDF: ball, fixed, gearbox, prismatic, revolute, revolute2, screw, universal + - Dart: ball, free, euler, prismatic, weld (=fixed), revolute, universal + - MuJoCo: ball, free, hinge (=revolute), slide """ def __init__(self, joint_id, name=None, dtype=None, limits=None, parent=None, child=None, axis=None, @@ -266,13 +450,15 @@ class Joint(object): self.parent = parent self.child = child self.axis = axis - self.position = position - self.orientation = orientation + self.frame = Frame(position=position, orientation=orientation) self.friction = friction self.damping = damping self.effort = effort self.velocity = velocity + self.init_position = None + self.init_velocity = None + @property def limits(self): return self._limits @@ -304,66 +490,39 @@ class Joint(object): @property def position(self): - return self._position + return self.frame.position @position.setter def position(self, position): - if position is not None: - if isinstance(position, str): - position = [float(p) for p in position.split()] - position = np.asarray(position) - self._position = position + self.frame.position = position @property def orientation(self): - return self._orientation + return self.frame.orientation @orientation.setter def orientation(self, orientation): - if orientation is not None: - if isinstance(orientation, str): - orientation = [float(o) for o in orientation.split()] - if len(orientation) == 4: # quaternion - orientation = get_rpy_from_quaternion(orientation) - if len(orientation) == 3: # rpy - pass - orientation = np.asarray(orientation) - self._orientation = orientation + self.frame.orientation = orientation @property def rpy(self): - return self._orientation + return self.frame.rpy @property def quaternion(self): - return get_quaternion_from_rpy(self._orientation) + return self.frame.quaternion @property def rot(self): - return get_matrix_from_rpy(self.rpy) + return self.frame.quaternion @property def pose(self): - return self.position, self.orientation + return self.frame.pose @pose.setter def pose(self, pose): - if pose is not None: - if isinstance(pose, str): - pose = pose.split() - self.position = pose[:3] - self.orientation = pose[3:] - elif isinstance(pose, (tuple, list, np.ndarray)): - if len(pose) == 2: - self.position = pose[0] - self.orientation = pose[1] - elif len(pose) == 6: - self.position = pose[:3] - self.orientation = pose[3:] - else: - raise ValueError("Expecting the pose to be tuple, list or np.ndarray of length 2 or 6") - else: - raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray") + self.frame.pose = pose @property def friction(self): @@ -405,6 +564,44 @@ class Joint(object): velocity = float(velocity) self._velocity = velocity + @property + def init_position(self): + return self._init_position + + @init_position.setter + def init_position(self, position): + if position is not None: + if isinstance(position, str): + position = np.asarray([float(s) for s in position.split()]) + if len(position) == 1: + position = position[0] + elif isinstance(position, (tuple, list, np.ndarray)): + position = np.asarray([float(s) for s in position]) + if len(position) == 1: + position = position[0] + elif not isinstance(position, (float, int)): + raise TypeError("Expecting the init_position to be a float, int, list, tuple or np.ndarray") + self._init_position = position + + @property + def init_velocity(self): + return self._init_velocity + + @init_velocity.setter + def init_velocity(self, velocity): + if velocity is not None: + if isinstance(velocity, str): + velocity = np.asarray([float(s) for s in velocity.split()]) + if len(velocity) == 1: + velocity = velocity[0] + elif isinstance(velocity, (tuple, list, np.ndarray)): + velocity = np.asarray([float(s) for s in velocity]) + if len(velocity) == 1: + velocity = velocity[0] + elif not isinstance(velocity, (float, int)): + raise TypeError("Expecting the init_velocity to be a float, int, list, tuple or np.ndarray") + self._init_velocity = velocity + class Inertia(object): r"""Inertia data structure""" @@ -459,7 +656,7 @@ class Inertia(object): @ixx.setter def ixx(self, ixx): - if ixx is None: + if ixx is not None: ixx = float(ixx) self._ixx = ixx @@ -529,8 +726,7 @@ class Inertial(object): """ self.mass = mass self.inertia = inertia - self.position = position - self.orientation = orientation + self.frame = Frame(position=position, orientation=orientation) @property def mass(self): @@ -603,70 +799,49 @@ class Inertial(object): @property def position(self): - return self._position + return self.frame.position @position.setter def position(self, position): - if position is not None: - if isinstance(position, str): - position = [float(p) for p in position.split()] - position = np.asarray(position) - self._position = position + self.frame.position = position @property def orientation(self): - return self._orientation + return self.frame.orientation @orientation.setter def orientation(self, orientation): - if orientation is not None: - if isinstance(orientation, str): - orientation = [float(o) for o in orientation.split()] - if len(orientation) == 4: # quaternion - orientation = get_rpy_from_quaternion(orientation) - if len(orientation) == 3: # rpy - pass - orientation = np.asarray(orientation) - self._orientation = orientation + self.frame.orientation = orientation @property def rpy(self): - return self._orientation + return self.frame.rpy @property def quaternion(self): - return get_quaternion_from_rpy(self._orientation) + return self.frame.quaternion @property def rot(self): - return get_matrix_from_rpy(self.rpy) + return self.frame.quaternion @property def pose(self): - return self.position, self.orientation + return self.frame.pose @pose.setter def pose(self, pose): - if pose is not None: - if isinstance(pose, str): - pose = pose.split() - self.position = pose[:3] - self.orientation = pose[3:] - elif isinstance(pose, (tuple, list, np.ndarray)): - if len(pose) == 2: - self.position = pose[0] - self.orientation = pose[1] - elif len(pose) == 6: - self.position = pose[:3] - self.orientation = pose[3:] - else: - raise ValueError("Expecting the pose to be tuple, list or np.ndarray of length 2 or 6") - else: - raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray") + self.frame.pose = pose class Geometry(object): # Shape - """Geometry: plane, sphere, box, mesh, cylinder, ellipsoid, capsule, heightmap, etc.""" + """Geometry: plane, sphere, box, mesh, cylinder, ellipsoid, capsule, cone, heightmap, etc. + + - URDF: box, cylinder, mesh, sphere + - SDF: box, cylinder, heightmap, image, mesh, plane, polyline, sphere + - Skel: box, capsule, cone, cylinder, ellipsoid, mesh, multi_sphere, sphere + - MuJoCo: box, capsule, cylinder, ellipsoid, hfield (=height field), mesh, plane, sphere + """ def __init__(self, dtype=None, size=None, filename=None): self.dtype = dtype @@ -706,14 +881,11 @@ Shape = Geometry class Visual(object): r"""visual parameters for body.""" - def __init__(self, name=None, dtype=None, size=None, color=None, filename=None, position=None, orientation=None, - material=None): + def __init__(self, name=None, dtype=None, size=None, color=None, filename=None, position=None, orientation=None): self.name = name self.geometry = Geometry(dtype=dtype, size=size, filename=filename) - self.color = color - self.position = position - self.orientation = orientation - self.material = material + self.frame = Frame(position=position, orientation=orientation) + self.material = Material(color=color) @property def dtype(self): @@ -741,18 +913,11 @@ class Visual(object): @property def color(self): - return self._color + return self.material.color @color.setter def color(self, color): - if color is not None: - if isinstance(color, str): # e.g. '0.5 0.1 1. 1.' - color = (float(c) for c in color.split()) - if not isinstance(color, (list, tuple)): - raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float") - else: - color = tuple(color) - self._color = color + self.material.color = color @property def format(self): @@ -762,66 +927,39 @@ class Visual(object): @property def position(self): - return self._position + return self.frame.position @position.setter def position(self, position): - if position is not None: - if isinstance(position, str): - position = [float(p) for p in position.split()] - position = np.asarray(position) - self._position = position + self.frame.position = position @property def orientation(self): - return self._orientation + return self.frame.orientation @orientation.setter def orientation(self, orientation): - if orientation is not None: - if isinstance(orientation, str): - orientation = [float(o) for o in orientation.split()] - if len(orientation) == 4: # quaternion - orientation = get_rpy_from_quaternion(orientation) - if len(orientation) == 3: # rpy - pass - orientation = np.asarray(orientation) - self._orientation = orientation + self.frame.orientation = orientation @property def rpy(self): - return self._orientation + return self.frame.rpy @property def quaternion(self): - return get_quaternion_from_rpy(self._orientation) + return self.frame.quaternion @property def rot(self): - return get_matrix_from_rpy(self.rpy) + return self.frame.quaternion @property def pose(self): - return self.position, self.orientation + return self.frame.pose @pose.setter def pose(self, pose): - if pose is not None: - if isinstance(pose, str): - pose = pose.split() - self.position = pose[:3] - self.orientation = pose[3:] - elif isinstance(pose, (tuple, list, np.ndarray)): - if len(pose) == 2: - self.position = pose[0] - self.orientation = pose[1] - elif len(pose) == 6: - self.position = pose[:3] - self.orientation = pose[3:] - else: - raise ValueError("Expecting the pose to be tuple, list or np.ndarray of length 2 or 6") - else: - raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray") + self.frame.pose = pose class Collision(object): @@ -830,8 +968,7 @@ class Collision(object): def __init__(self, name=None, dtype=None, size=None, filename=None, position=None, orientation=None): self.name = name self.geometry = Geometry(dtype=dtype, size=size, filename=filename) - self.position = position - self.orientation = orientation + self.frame = Frame(position=position, orientation=orientation) @property def dtype(self): @@ -864,82 +1001,63 @@ class Collision(object): @property def position(self): - return self._position + return self.frame.position @position.setter def position(self, position): - if position is not None: - if isinstance(position, str): - position = [float(p) for p in position.split()] - position = np.asarray(position) - self._position = position + self.frame.position = position @property def orientation(self): - return self._orientation + return self.frame.orientation @orientation.setter def orientation(self, orientation): - if orientation is not None: - if isinstance(orientation, str): - orientation = [float(o) for o in orientation.split()] - if len(orientation) == 4: # quaternion - orientation = get_rpy_from_quaternion(orientation) - if len(orientation) == 3: # rpy - pass - orientation = np.asarray(orientation) - self._orientation = orientation + self.frame.orientation = orientation @property def rpy(self): - return self._orientation + return self.frame.rpy @property def quaternion(self): - return get_quaternion_from_rpy(self._orientation) + return self.frame.quaternion @property def rot(self): - return get_matrix_from_rpy(self.rpy) + return self.frame.quaternion @property def pose(self): - return self.position, self.orientation + return self.frame.pose @pose.setter def pose(self, pose): - if pose is not None: - if isinstance(pose, str): - pose = pose.split() - self.position = pose[:3] - self.orientation = pose[3:] - elif isinstance(pose, (tuple, list, np.ndarray)): - if len(pose) == 2: - self.position = pose[0] - self.orientation = pose[1] - elif len(pose) == 6: - self.position = pose[:3] - self.orientation = pose[3:] - else: - raise ValueError("Expecting the pose to be tuple, list or np.ndarray of length 2 or 6") - else: - raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray") + self.frame.pose = pose class Material(object): - r"""Material info.""" + r"""Material info. + + Type of colors: + - ambient: color of an object when no lights are pointing at it. + - diffuse: color of an object under a pure white light. + - specular: color and intensity of a highlight from a specular reflection (higher values make an object more shiny). + - emissive: color where the light appears to being emitted from the object. + """ def __init__(self, name=None, color=None, texture=None): self.name = name - self.color = color + self.color = color # ambient color self.texture = texture - @property - def color(self): - return self._color + # RGBA color + self.diffuse = None + self.specular = None + self.emissive = None - @color.setter - def color(self, color): + @staticmethod + def _check_color(color): if color is not None: if isinstance(color, str): # e.g. '0.5 0.1 1. 1.' color = (float(c) for c in color.split()) @@ -947,7 +1065,15 @@ class Material(object): raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float") else: color = tuple(color) - self._color = color + return color + + @property + def color(self): + return self._color + + @color.setter + def color(self, color): + self._color = self._check_color(color) @property def rgb(self): @@ -963,10 +1089,59 @@ class Material(object): return tuple(self.color) + (1.,) return tuple(self.color) + @property + def diffuse(self): + return self._diffuse + + @diffuse.setter + def diffuse(self, diffuse): + if diffuse is not None: + if isinstance(diffuse, str): # e.g. '0.5 0.1 1. 1.' + diffuse = (float(c) for c in diffuse.split()) + if not isinstance(diffuse, (list, tuple)): + raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float") + else: + diffuse = tuple(diffuse) + self._diffuse = diffuse + + @property + def specular(self): + return self._specular + + @specular.setter + def specular(self, specular): + if specular is not None: + if isinstance(specular, str): # e.g. '0.5 0.1 1. 1.' + specular = (float(c) for c in specular.split()) + if not isinstance(specular, (list, tuple)): + raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float") + else: + specular = tuple(specular) + self._specular = specular + + @property + def emissive(self): + return self._emissive + + @emissive.setter + def emissive(self, emissive): + if emissive is not None: + if isinstance(emissive, str): # e.g. '0.5 0.1 1. 1.' + emissive = (float(c) for c in emissive.split()) + if not isinstance(emissive, (list, tuple)): + raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float") + else: + emissive = tuple(emissive) + self._emissive = emissive + class Sensor(object): pass +class Actuator(object): # Motor + pass + + class Heightmap(object): pass diff --git a/pyrobolearn/utils/parsers/robots/mujoco_parser.py b/pyrobolearn/utils/parsers/robots/mujoco_parser.py index 20157f3..c638fb0 100644 --- a/pyrobolearn/utils/parsers/robots/mujoco_parser.py +++ b/pyrobolearn/utils/parsers/robots/mujoco_parser.py @@ -1,12 +1,31 @@ #!/usr/bin/env python -"""Define the MuJoCo parser. +"""Define the MuJoCo parser/generator. + +Notes: + - MuJoCo only accepts STL meshes + - MuJoCo can load PNG files for textures and heightmap. + +References: + - MuJoCo overview: http://www.mujoco.org/book/index.html + - MuJoCo XML format: http://www.mujoco.org/book/XMLreference.html """ # import XML parser import xml.etree.ElementTree as ET +# import mesh converter (from .obj to .stl) +try: + import pymesh # rapid prototyping platform focused on geometry processing + # doc: https://pymesh.readthedocs.io/en/latest/user_guide.html + + import pyassimp # library to import and export various 3d-model-formats + # doc: http://www.assimp.org/index.php + # github: https://github.com/assimp/assimp +except ImportError as e: + raise ImportError(str(e) + "\nTry to install pymesh pyassimp: `pip install pymesh pyassimp`") + from pyrobolearn.utils.parsers.robots.world_parser import WorldParser -from pyrobolearn.utils.parsers.robots.data_structures import Tree, World +from pyrobolearn.utils.parsers.robots.data_structures import * __author__ = "Brian Delhaisse" @@ -30,6 +49,12 @@ class MuJoCoParser(WorldParser): filename (str, None): path to the MuJoCo XML file. """ super().__init__(filename) + self.simulator = None + self.assets = OrderedDict() + self.compiler = dict() # set options for the built-in parser and compiler + self.options = dict() # simulation options + self.defaults = dict() # default values for the attributes when they are not specified + self.assets = dict() # assets (textures, meshes, etc) def parse(self, filename): """ @@ -49,14 +74,71 @@ class MuJoCoParser(WorldParser): raise RuntimeError("Expecting the first XML tag to be 'mujoco' but found instead: {}".format(root.tag)) # build the world - world = World() + world = World(name=root.attrib.get('model', 'world')) + + # check compiler + compiler_tag = root.find('compiler') # TODO: check other + if compiler_tag is not None: + + def update_compiler(attributes): + for attribute in attributes: + attrib = compiler_tag.attrib.get(attribute) + if attrib is not None: + self.compiler[attribute] = attrib + + coordinate = compiler_tag.attrib.get('coordinate') + if coordinate == 'global': + raise NotImplementedError("Currently, we only support local coordinate frames.") + + update_compiler(['coordinate', 'angle', 'meshdir', 'texturedir', 'eulerseq', 'discardvisual', + 'convexhull', 'inertiafromgeom', 'fitaabb', 'fusestatic']) # check default (this is the default configuration when they are not specified) default_tag = root.find('default') if default_tag is not None: - pass - # check physics + def update_default(tag, attributes): + tag = default_tag.find(tag) + if tag is not None: + self.defaults[tag] = {} + for attribute in attributes: + item = tag.attrib.get(attribute) + if item is not None: + self.defaults['tag'][attribute] = item + + update_default('mesh', ['scale']) + update_default('material', ['texture', 'emission', 'specular', 'shininess', 'reflectance', 'rgba']) + update_default('joint', ['type', 'pos', 'axis', 'limited', 'range', 'springdamper', 'stiffness', + 'damping', 'frictionloss', 'armature', 'margin', 'ref', 'springref']) + update_default('geom', ['type', 'contype', 'conaffinity', 'condim', 'size', 'material', 'rgba', + 'friction', 'mass', 'density', 'margin', 'fromto', 'pos', 'quat', 'axisangle', + 'xyaxes', 'zaxis', 'euler', 'hfield', 'mesh']) + update_default('site', ['type', 'material', 'rgba', 'size', 'fromto', 'pos', 'quat', 'axisangle', + 'xyaxes', 'zaxis', 'euler']) + update_default('camera', ['mode', 'target', 'fovy', 'ipd', 'pos', 'quat', 'axisangle', 'xyaxes', 'zaxis', + 'euler']) + update_default('light', ['mode', 'target', 'directional', 'castshadow', 'active', 'pos', 'dir', + 'attenuation', 'cutoff', 'exponent', 'ambient', 'diffuse', 'specular']) + update_default('pair', ['condim', 'friction', 'margin', 'gap']) + update_default('equality', ['active']) + # update_default('tendon', ['']) + update_default('general', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange', + 'gear', 'cranklength', 'dyntype', 'gaintype', 'biastype', 'dynprm', 'gainprm', + 'biasprm']) + # name, class, joint, jointinparent, site, tendon, slidersite, cranksite + update_default('motor', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange', 'gear', + 'cranklength']) + update_default('position', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange', + 'gear', 'cranklength', 'kp']) + update_default('velocity', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange', + 'gear', 'cranklength', 'kv']) + update_default('cylinder', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange', + 'gear', 'cranklength', 'timeconst', 'area', 'diameter', 'bias']) + update_default('muscle', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange', 'gear', + 'cranklength', 'timeconst', 'range', 'force', 'scale', 'lmin', 'lmax', 'vmax', + 'fpmax', 'fvmax']) + + # check options # check assets asset_tag = root.find('asset') @@ -66,7 +148,24 @@ class MuJoCoParser(WorldParser): # check world body worldbody_tag = root.find('worldbody') if worldbody_tag is not None: - pass + + # light + for i, light_tag in enumerate(worldbody_tag.findall('light')): + attrib = light_tag.attrib + light = Light(name=attrib.get('name', 'light_' + str(i)), cast_shadows=attrib.get('castshadow'), + position=attrib.get('pos'), direction=attrib.get('dir'), ambient=attrib.get('ambient'), + diffuse=attrib.get('diffuse'), specular=attrib.get('specular')) + + world.lights[light.name] = light + + # check each multi-body + for i, body_tag in enumerate(worldbody_tag.findall('body')): + # create tree + tree = Tree(name=body_tag.attrib.get('name', 'prl_multibody_' + str(i))) + + # check recursively body + body = self._check_body(tree, body_tag, idx=i) + world.trees[tree.name] = tree # check contact @@ -79,18 +178,62 @@ class MuJoCoParser(WorldParser): # set the world self.world = world - def _check_body(self, body_tag, idx): + def _check_model(self, model_tag, idx): """ - Return Body instance from a . + Return the Tree instance from a . Args: + model_tag (ET.Element): model XML element + idx (int): model index. + + Returns: + Tree: tree data structure containing the model. + """ + # create tree + tree = Tree(name=model_tag.attrib.get('name')) + + # check bodies/links + for i, link_tag in enumerate(model_tag.findall('link')): + body = self._check_body(link_tag, idx=i) + # add body to tree + tree.bodies[body.name] = body + + # check joints + for i, joint_tag in enumerate(root.findall('joint')): + # get joint instance from tag + joint = self._check_joint(joint_tag, idx=i) + + # add joint in trees + tree.joints[joint.name] = joint + + # add joint in parent body + parent_body = tree.bodies[joint.parent] + parent_body.joints[joint.name] = joint + + return tree + + def _check_body(self, tree, body_tag, idx): + """ + Construct recursively the given tree, and return Body instance from a . + + Args: + tree (Tree): tree data structure containing the model. body_tag (ET.Element): body XML element. idx (int): link index. Returns: Body: body data structure. """ - pass + # create body + body = Body(body_id=idx, name=body_tag.attrib.get('name', 'prl_body_' + str(idx))) + + # check geom + + # check joints + + # check bodies + + # check include def _check_joint(self, joint_tag, idx): """ @@ -105,15 +248,32 @@ class MuJoCoParser(WorldParser): """ pass - def generate(self, tree=None): + def generate(self, world=None): """ - Generate the XML tree from the `Tree` data structure. + Generate the XML world from the `World` data structure. Args: - tree (Tree): Tree data structure. + world (World): world data structure. Returns: ET.Element: root element in the XML file. """ - pass + if world is None: + world = self.world + # create root element + root = ET.Element('mujoco', attrib={'model': world.name}) + + # create compiler + + # create asset + + # create world + worldbody_tag = ET.SubElement(root, 'worldbody') # name = 'world' + + # create models + for tree in world.trees: + + # create bodies + for i, body in enumerate(tree.bodies): + pass diff --git a/pyrobolearn/utils/parsers/robots/proto_parser.py b/pyrobolearn/utils/parsers/robots/proto_parser.py index fadd992..6148ad4 100644 --- a/pyrobolearn/utils/parsers/robots/proto_parser.py +++ b/pyrobolearn/utils/parsers/robots/proto_parser.py @@ -1,5 +1,5 @@ #!/usr/bin/env python -"""Define the Proto parser. +"""Define the Proto parser/generator. Proto files are notably used in Webots. """ diff --git a/pyrobolearn/utils/parsers/robots/sdf_parser.py b/pyrobolearn/utils/parsers/robots/sdf_parser.py index 2719544..a1f9684 100644 --- a/pyrobolearn/utils/parsers/robots/sdf_parser.py +++ b/pyrobolearn/utils/parsers/robots/sdf_parser.py @@ -1,7 +1,10 @@ #!/usr/bin/env python -"""Define the SDF parser. +"""Define the SDF parser/generator. SDF files are notably used in Gazebo, and Bullet. + +References: + - SDF file format: http://sdformat.org/ """ # import XML parser @@ -32,7 +35,6 @@ class SDFParser(WorldParser): filename (str, None): path to the SDF file. """ super().__init__(filename) - self.worlds = [] def parse(self, filename): """ @@ -56,8 +58,26 @@ class SDFParser(WorldParser): # build the world world = World(name=world_tag.attrib.get('name', 'world_' + str(i))) + # check light + for l, light_tag in enumerate(world_tag.findall('light')): + attrib = light_tag.attrib + light = Light(name=attrib.get('name', 'light_' + str(l)), dtype=attrib.get('type')) + + for tag in ['cast_shadows', 'diffuse', 'specular', 'direction']: + item_tag = light_tag.find(tag) + if item_tag is not None: + if hasattr(light, tag): + setattr(light, tag, item_tag.text) + + pose_tag = light_tag.find('pose') + if pose_tag is not None: + light.pose = pose_tag.text + + # add the light in the world + world.lights[light.name] = light + # check model - for idx, model_tag in enumerate(root.findall('model')): + for idx, model_tag in enumerate(world_tag.findall('model')): tree = self._check_model(model_tag, idx=idx) world.trees[tree.name] = tree @@ -76,6 +96,9 @@ class SDFParser(WorldParser): if len(models) > 0: self.worlds.append(world) + # set the first world + self.world = self.worlds[0] + def _check_model(self, model_tag, idx): """ Return the Tree instance from a . @@ -88,7 +111,7 @@ class SDFParser(WorldParser): Tree: tree data structure containing the model. """ # create tree - tree = Tree(name=model_tag.attrib.get('name')) + tree = Tree(name=model_tag.attrib.get('name', 'model_' + str(idx))) # check bodies/links for i, link_tag in enumerate(model_tag.findall('link')): @@ -97,7 +120,7 @@ class SDFParser(WorldParser): tree.bodies[body.name] = body # check joints - for i, joint_tag in enumerate(root.findall('joint')): + for i, joint_tag in enumerate(model_tag.findall('joint')): # get joint instance from tag joint = self._check_joint(joint_tag, idx=i) @@ -203,7 +226,7 @@ class SDFParser(WorldParser): visual.filename = uri_tag.text visual.size = scale_tag.text - # material + # material # TODO # material = visual.find('material') # if material is not None: # name = material.attrib.get('name') @@ -329,6 +352,11 @@ class SDFParser(WorldParser): if lower_limit_tag is not None and upper_limit_tag is not None: # TODO: check if we can have one limit joint.limits = [lower_limit_tag.text, upper_limit_tag.text] + # init_position + init_pos_tag = axis_tag.find('init_position') + if init_pos_tag is not None: + joint.init_position = init_pos_tag.text + return joint def generate(self, world=None): @@ -351,20 +379,221 @@ class SDFParser(WorldParser): name = world.name if world.name is not None else 'default' world_tag = ET.SubElement(root, 'world', attrib={'name': name}) + def set_pose(parent_tag, item): + if item.position is None and item.orientation is None: + return None + position = [0., 0., 0.] if item.position is None else item.position + orientation = [0., 0., 0.] if item.orientation is None else item.orientation + pose = np.concatenate((position, orientation)) + pose_tag = ET.SubElement(parent_tag, 'pose') + pose_tag.text = str(pose)[1:-1] + + def set_geometry(parent_tag, item): + if item.geometry.dtype is not None: + geometry = item.geometry + dtype = geometry.dtype + geometry_tag = ET.SubElement(parent_tag, 'geometry') + + if dtype in {'box', 'sphere', 'cylinder', 'mesh'}: + dtype_tag = ET.SubElement(geometry_tag, dtype) + if dtype == 'box': + size_tag = ET.SubElement(dtype_tag, 'size') + size_tag.text = str(np.asarray(geometry.size))[1:-1] + elif dtype == 'sphere': + radius_tag = ET.SubElement(dtype_tag, 'radius') + radius_tag.text = str(geometry.size) + elif dtype == 'cylinder': + radius_tag = ET.SubElement(dtype_tag, 'radius') + length_tag = ET.SubElement(dtype_tag, 'length') + radius_tag.text = str(geometry.size[0]) + length_tag.text = str(geometry.size[1]) + elif dtype == 'mesh': + uri_tag = ET.SubElement(dtype_tag, 'uri') + uri_tag.text = geometry.filename + if geometry.size is not None: + scale_tag = ET.SubElement(dtype_tag, 'scale') + scale_tag.text = str(np.asarray())[1:-1] + # elif dtype == 'plane': + # pass + # elif dtype == 'heightmap': + # pass + # create models for tree in world.trees: model_tag = ET.SubElement(world_tag, 'model', attrib={'name': tree.name}) - if tree.position is not None or tree.orientation is not None: - pose_tag = ET.SubElement(model_tag, 'pose') - pose_tag.text = str(np.asarray(tree.pose))[1:-1] + + # pose + set_pose(model_tag, tree) # create links - for body in tree.bodies: # TODO - pass + for i, body in enumerate(tree.bodies): + link_tag = ET.SubElement(model_tag, 'link', attrib={'name': body.name}) + + # create inertial + if body.inertial is not None: + inertial = body.inertial + inertial_tag = ET.SubElement(link_tag, 'inertial') + + # pose + set_pose(inertial_tag, inertial) + + # mass + if inertial.mass is not None: + mass_tag = ET.SubElement(inertial_tag, 'mass') + mass_tag.text = str(inertial.mass) + + # inertia + if inertial.inertia is not None: + inertia = inertial.inertia + inertia_tag = ET.SubElement(inertial_tag, 'inertia') + + if inertia.ixx is not None: + ixx_tag = ET.SubElement(inertia_tag, 'ixx') + ixx_tag.text = str(inertia.ixx) + if inertia.ixy is not None: + ixy_tag = ET.SubElement(inertia_tag, 'ixy') + ixy_tag.text = str(inertia.ixy) + if inertia.ixz is not None: + ixz_tag = ET.SubElement(inertia_tag, 'ixz') + ixz_tag.text = str(inertia.ixz) + if inertia.iyy is not None: + iyy_tag = ET.SubElement(inertia_tag, 'iyy') + iyy_tag.text = str(inertia.iyy) + if inertia.iyz is not None: + iyz_tag = ET.SubElement(inertia_tag, 'iyz') + iyz_tag.text = str(inertia.iyz) + if inertia.izz is not None: + izz_tag = ET.SubElement(inertia_tag, 'izz') + izz_tag.text = str(inertia.izz) + + # create visual + if body.visual is not None: + visual = body.visual + name = 'visual_' + str(i) if visual.name is None else visual.name + visual_tag = ET.SubElement(link_tag, 'visual', attrib={'name': name}) + + # pose + set_pose(visual_tag, visual) + + # material + if visual.material is not None: + material = visual.material + material_tag = ET.SubElement(visual_tag, 'material') + + if material.color is not None: + ambient_tag = ET.SubElement(material_tag, 'ambient') + ambient_tag.text = str(np.asarray(material.rgba))[1:-1] + + if material.diffuse is not None: + diffuse_tag = ET.SubElement(material_tag, 'diffuse') + diffuse_tag.text = str(np.asarray(material.diffuse))[1:-1] + + if material.specular is not None: + specular_tag = ET.SubElement(material_tag, 'specular') + specular_tag.text = str(np.asarray(material.specular))[1:-1] + + if material.emissive is not None: + emissive_tag = ET.SubElement(material_tag, 'emissive') + emissive_tag.text = str(np.asarray(material.emissive))[1:-1] + + if material.texture is not None: + script_tag = ET.SubElement(material_tag, 'script') + name = 'material_' + str(i) if material.name is None else material.name + name_tag = ET.SubElement(script_tag, 'name') + name_tag.text = name + + # create Ogre script + with open(name + '.material', "w") as f: + s = "material {}" \ + "\n{" \ + "\n\ttechnique" \ + "\n\t{" \ + "\n\t\tpass" \ + "\n\t\t{" \ + "\n\t\t\ttexture_unit" \ + "\n\t\t\t{" \ + "\n\t\t\t\ttexture {}" \ + "\n\t\t\t}" \ + "\n\t\t}" \ + "\n\t}" \ + "\n}".format(name, material.texture) + f.write(s) + + uri_tag = ET.SubElement(script_tag, 'uri') + uri_tag.text = "file://" + name + '.material' + + # geometry + set_geometry(visual_tag, visual) + + # create collision + if body.collision is not None: + collision = body.collision + name = 'collision_' + str(i) if collision.name is None else collision.name + collision_tag = ET.SubElement(link_tag, 'collision', attrib={'name': name}) + + # pose + set_pose(collision_tag, collision) + + # geometry + set_geometry(collision_tag, collision) # create joints - for joint in tree.joints: # TODO - pass + for i, joint in enumerate(tree.joints): + joint_tag = ET.SubElement(model_tag, 'joint', attrib={'name': joint.name, 'type': joint.dtype}) + parent_tag = ET.SubElement(joint_tag, 'parent') + parent_tag.text = joint.parent + child_tag = ET.SubElement(joint_tag, 'child') + child_tag.text = joint.child + + # pose + set_pose(joint_tag, joint) + + # axis + axis_tag = None + if joint.axis is not None: + axis_tag = ET.SubElement(joint_tag, 'axis') + xyz_tag = ET.SubElement(axis_tag, 'xyz') + xyz_tag.text = str(np.asarray(joint.axis))[1:-1] + + # dynamics + if joint.damping is not None or joint.friction is not None: + if axis_tag is None: + axis_tag = ET.SubElement(joint_tag, 'axis') + dynamics_tag = ET.SubElement(axis_tag, 'dynamics') + + if joint.friction is not None: + friction_tag = ET.SubElement(dynamics_tag, 'friction') + friction_tag.text = str(joint.friction) + if joint.damping is not None: + damping_tag = ET.SubElement(dynamics_tag, 'damping') + damping_tag.text = str(joint.damping) + + # limits + if joint.limits is not None or joint.effort is not None or joint.velocity is not None: + if axis_tag is None: + axis_tag = ET.SubElement(joint_tag, 'axis') + limit_tag = ET.SubElement(axis_tag, 'limit') + + if joint.limits is not None: + lower_tag = ET.SubElement(limit_tag, 'lower') + upper_tag = ET.SubElement(limit_tag, 'upper') + lower_tag.text = str(joint.limits[0]) + upper_tag.text = str(joint.limits[1]) + + if joint.velocity is not None: + velocity_tag = ET.SubElement(limit_tag, 'velocity') + velocity_tag.text = str(joint.velocity) + + if joint.effort is not None: + effort_tag = ET.SubElement(limit_tag, 'effort') + effort_tag.text = str(joint.effort) + + # init_position + if isinstance(joint.init_position, (float, int)): + if axis_tag is None: + axis_tag = ET.SubElement(joint_tag, 'axis') + initial_pos_tag = ET.SubElement(axis_tag, 'initial_position') + initial_pos_tag.text = str(joint.init_position) # return root XML element return root diff --git a/pyrobolearn/utils/parsers/robots/skel_parser.py b/pyrobolearn/utils/parsers/robots/skel_parser.py new file mode 100644 index 0000000..eba2869 --- /dev/null +++ b/pyrobolearn/utils/parsers/robots/skel_parser.py @@ -0,0 +1,554 @@ +#!/usr/bin/env python +"""Define the Skel parser/generator. + +Skel files are notably used in Dart. + +References: + - SKEL file format: https://dartsim.github.io/skel_file_format.html + - Note: the specification is incomplete on the above website, check the examples in the next link. + - Examples of Skeleton: https://github.com/dartsim/dart/tree/master/data/skel +""" + +# import XML parser +import xml.etree.ElementTree as ET + +from pyrobolearn.utils.parsers.robots.world_parser import WorldParser +from pyrobolearn.utils.parsers.robots.data_structures import * + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class SkelParser(WorldParser): + r"""Skel Parser and Generator""" + + def __init__(self, filename=None): + """ + Initialize the Skel parser. + + Args: + filename (str, None): path to the Skel file. + """ + super().__init__(filename) + self.worlds = [] + + def parse(self, filename): + """ + Load and parse the given Skel file. + + Args: + filename (str): path to the Skel file. + """ + # load and parse the XML file + tree_xml = ET.parse(filename) + + # get the root + root = tree_xml.getroot() + + # check that the root is + if root.tag != 'skel': + raise RuntimeError("Expecting the first XML tag to be 'skel' but found instead: {}".format(root.tag)) + + # check world(s) + for i, world_tag in enumerate(root.findall('world')): + # build the world + world = World(name=world_tag.attrib.get('name', 'world_' + str(i))) + + # check physics + physics_tag = world_tag.find('physics') + physics = None + if physics_tag is not None: + physics = Physics() + timestep_tag = physics_tag.find('time_step') + if timestep_tag is not None: + physics.timestep = timestep_tag.text + gravity_tag = physics_tag.find('gravity') + if gravity_tag is not None: + physics.gravity = gravity_tag.text # TODO: check the convention that DART uses; g = (0, -9.81, 0)? + # TODO: collision collector + + if physics is not None: + world.physics = physics + + # check skeleton + for idx, skeleton_tag in enumerate(world_tag.findall('skeleton')): + tree = self._check_skeleton(skeleton_tag, idx=idx) + world.trees[tree.name] = tree + + # append the world to the list of worlds + self.worlds.append(world) + + # set the first world + self.world = self.worlds[0] + + def _check_skeleton(self, skeleton_tag, idx): + """ + Return the Tree instance from a . + + Args: + skeleton_tag (ET.Element): skeleton XML element + idx (int): skeleton index. + + Returns: + Tree: tree data structure containing the skeleton. + """ + # create tree + tree = Tree(name=skeleton_tag.attrib.get('name', 'skeleton_' + str(idx))) + + # check bodies/links + for i, body_tag in enumerate(skeleton_tag.findall('body')): + body = self._check_body(body_tag, idx=i) + # add body to tree + tree.bodies[body.name] = body + + # check joints + for i, joint_tag in enumerate(skeleton_tag.findall('joint')): + # get joint instance from tag + joint = self._check_joint(joint_tag, idx=i) + + # add joint in trees + tree.joints[joint.name] = joint + + # add joint in parent body + parent_body = tree.bodies[joint.parent] + parent_body.joints[joint.name] = joint + + return tree + + @staticmethod + def _check_body(body_tag, idx): + """ + Return Body instance from a . + + Args: + body_tag (ET.Element): body XML element. + idx (int): body index. + + Returns: + Body: body data structure. + """ + # create body/link + body = Body(body_id=idx, name=body_tag.attrib.get('name', 'body_' + str(idx))) + + # check tag + inertial_tag = body_tag.find('inertia') + if inertial_tag is not None: + inertial = Inertial() + + # transformation + pose_tag = inertial_tag.find('transformation') + if pose_tag is not None: + inertial.pose = pose_tag.text + + # offset + offset_tag = inertial_tag.find('offset') + if offset_tag is not None: + inertial.position = offset_tag.text + + # mass + mass_tag = inertial_tag.find('mass') + if mass_tag is not None: + inertial.mass = mass_tag.text + + # moment_of_inertia + inertia_tag = inertial_tag.find('moment_of_inertia') + if inertia_tag is not None: + ixx = inertia_tag.find('ixx') + if ixx is not None: + ixx = ixx.text + ixy = inertia_tag.find('ixy') + if ixy is not None: + ixy = ixy.text + ixz = inertia_tag.find('ixz') + if ixz is not None: + ixz = ixz.text + iyy = inertia_tag.find('iyy') + if iyy is not None: + iyy = iyy.text + iyz = inertia_tag.find('iyz') + if iyz is not None: + iyz = iyz.text + izz = inertia_tag.find('izz') + if izz is not None: + izz = izz.text + inertial.inertia = {'ixx': ixx, 'ixy': ixy, 'ixz': ixz, 'iyy': iyy, 'iyz': iyz, 'izz': izz} + + # set inertial to body + body.inertial = inertial + + # check tag + visual_tag = body_tag.find('visualization_shape') + if visual_tag is not None: + visual = Visual() + + # name + visual.name = visual_tag.attrib.get('name') + + # transformation + pose_tag = visual_tag.find('transformation') + if pose_tag is not None: + visual.pose = pose_tag.text + + # geometry + geometry_tag = visual_tag.find('geometry') + if geometry_tag is not None: + for geometry_type in ['box', 'mesh', 'cylinder', 'sphere', 'plane', 'heightmap']: # polyline, image + geometry_type_tag = geometry_tag.find(geometry_type) + if geometry_type_tag is not None: + dtype = geometry_type + visual.dtype = dtype + if dtype == 'box': + size_tag = geometry_type_tag.find('size') + visual.size = size_tag.text + elif dtype == 'sphere': + radius_tag = geometry_type_tag.find('radius') + visual.size = radius_tag.text + elif dtype == 'cylinder': + radius_tag = geometry_type_tag.find('radius') + length_tag = geometry_type_tag.find('length') + visual.size = (radius_tag.text, length_tag.text) + elif dtype == 'mesh': + uri_tag = geometry_type_tag.find('uri') + scale_tag = geometry_type_tag.find('scale') + visual.filename = uri_tag.text + visual.size = scale_tag.text + + # set visual to body + body.visual = visual + + # check tag + collision_tag = body_tag.find('collision_shape') + if collision_tag is not None: + collision = Collision() + + # name + # collision.name = collision_tag.attrib.get('name') + + # origin + pose_tag = collision_tag.find('transformation') + if pose_tag is not None: + collision.pose = pose_tag.text + + # geometry + geometry_tag = collision_tag.find('geometry') + if geometry_tag is not None: + geometry_types = ['box', 'capsule', 'cone', 'cylinder', 'ellipsoid', 'mesh', 'sphere'] # multi_sphere + for geometry_type in geometry_types: + geometry_type_tag = geometry_tag.find(geometry_type) + if geometry_type_tag is not None: + dtype = geometry_type + collision.dtype = dtype + if dtype == 'box' or dtype == 'ellipsoid': + size_tag = geometry_type_tag.find('size') + collision.size = size_tag.text + elif dtype == 'sphere': + radius_tag = geometry_type_tag.find('radius') + collision.size = radius_tag.text + elif dtype == 'cylinder' or dtype == 'capsule' or dtype == 'cone': + radius_tag = geometry_type_tag.find('radius') + height_tag = geometry_type_tag.find('height') + collision.size = (radius_tag.text, height_tag.text) + elif dtype == 'mesh': + filename_tag = geometry_type_tag.find('file_name') + scale_tag = geometry_type_tag.find('scale') + collision.filename = filename_tag.text + collision.size = scale_tag.text + + # set collision to body + body.collision = collision + + # return the body instance + return body + + @staticmethod + def _check_joint(joint_tag, idx): + """ + Return Joint instance from a tag. + + Args: + joint_tag (ET.Element): joint XML element. + idx (int): joint index. + + Returns: + Joint: joint data structure. + """ + attrib = joint_tag.attrib + joint = Joint(joint_id=idx, name=attrib.get('name', 'joint_' + str(idx)), dtype=attrib['type']) + + # add parent and child body/link + parent_tag = joint_tag.find('parent') + if parent_tag is None: + raise RuntimeError("Expecting the joint '" + joint.name + "' to have a parent link/body") + joint.parent = parent_tag.text + + child_tag = joint_tag.find('child') + if child_tag is None: + raise RuntimeError("Expecting the joint '" + joint.name + "' to have a child link/body") + joint.child = child_tag.text + + # transformation + pose_tag = joint_tag.find('transformation') + if pose_tag is not None: + joint.pose = pose_tag.text + + # axis + axis_tag = joint_tag.find('axis') + if axis_tag is not None: + axis_xyz_tag = axis_tag.find('xyz') + if axis_xyz_tag is not None: + joint.axis = axis_xyz_tag.text + + # dynamics + dynamics_tag = axis_tag.find('dynamics') + if dynamics_tag is not None: + damping_tag = dynamics_tag.find('damping') + + # damping + if damping_tag is not None: + joint.damping = damping_tag.text + + # friction + friction_tag = dynamics_tag.find('friction') + if friction_tag is not None: + joint.friction = friction_tag.text + + # limits + limits_tag = axis_tag.find('limits') + if limits_tag is not None: + effort_tag = limits_tag.find('effort') + if effort_tag is not None: + joint.effort = effort_tag.text + + velocity_tag = limits_tag.find('velocity') + if velocity_tag is not None: + joint.velocity = velocity_tag.text + + lower_limit_tag = limits_tag.find('lower') + upper_limit_tag = limits_tag.find('upper') + if lower_limit_tag is not None and upper_limit_tag is not None: # TODO: check if we can have one limit + joint.limits = [lower_limit_tag.text, upper_limit_tag.text] + + # init_pos + init_pos_tag = joint_tag.find('init_pos') + if init_pos_tag is not None: + joint.init_position = init_pos_tag.text + + # init_vel + init_vel_tag = joint_tag.find('init_vel') + if init_vel_tag is not None: + joint.init_velocity = init_vel_tag.text + + return joint + + def generate(self, world=None): + """ + Generate the XML world from the `World` data structure. + + Args: + world (World, Tree): world / tree data structure. + + Returns: + ET.Element: root element in the XML file. + """ + if world is None: + world = self.worlds[0] + + # create root element + root = ET.Element('skel', attrib={'version': '1.0'}) + + # create world tag + name = world.name if world.name is not None else 'world' + world_tag = ET.SubElement(root, 'world', attrib={'name': name}) + + def set_transformation(parent_tag, item): + if item.position is None and item.orientation is None: + return None + position = [0., 0., 0.] if item.position is None else item.position + orientation = [0., 0., 0.] if item.orientation is None else item.orientation + pose = np.concatenate((position, orientation)) + pose_tag = ET.SubElement(parent_tag, 'transformation') + pose_tag.text = str(pose)[1:-1] + + def set_geometry(parent_tag, item): + if item.geometry.dtype is not None: + geometry = item.geometry + dtype = geometry.dtype + geometry_tag = ET.SubElement(parent_tag, 'geometry') + + if dtype in {'box', 'capsule', 'cone', 'cylinder', 'ellipsoid', 'mesh', 'sphere'}: + dtype_tag = ET.SubElement(geometry_tag, dtype) + if dtype == 'box' or dtype == 'ellipsoid': + size_tag = ET.SubElement(dtype_tag, 'size') + size_tag.text = str(np.asarray(geometry.size))[1:-1] + elif dtype == 'sphere': + radius_tag = ET.SubElement(dtype_tag, 'radius') + radius_tag.text = str(geometry.size) + elif dtype == 'cylinder' or dtype == 'capsule' or dtype == 'cone': + radius_tag = ET.SubElement(dtype_tag, 'radius') + height_tag = ET.SubElement(dtype_tag, 'height') + radius_tag.text = str(geometry.size[0]) + height_tag.text = str(geometry.size[1]) + elif dtype == 'mesh': + filename_tag = ET.SubElement(dtype_tag, 'file_name') + filename_tag.text = geometry.filename + if geometry.size is not None: + scale_tag = ET.SubElement(dtype_tag, 'scale') + scale_tag.text = str(np.asarray())[1:-1] + # elif dtype == 'plane': + # pass + # elif dtype == 'heightmap': + # pass + + # create skeletons + for tree in world.trees: + skeleton_tag = ET.SubElement(world_tag, 'skeleton', attrib={'name': tree.name}) + + # transformation + set_transformation(skeleton_tag, tree) + + # create bodies/links + for i, body in enumerate(tree.bodies): + body_tag = ET.SubElement(skeleton_tag, 'body', attrib={'name': body.name}) + + # create inertial + if body.inertial is not None: + inertial = body.inertial + inertial_tag = ET.SubElement(body_tag, 'inertia') + + # offset + if inertial.position is not None: + if inertial.orientation is not None: + set_transformation(inertial_tag, inertial) + else: + offset_tag = ET.SubElement(inertial_tag, 'offset') + offset_tag.text = str(np.asarray(inertial.position))[1:-1] + + # mass + if inertial.mass is not None: + mass_tag = ET.SubElement(inertial_tag, 'mass') + mass_tag.text = str(inertial.mass) + + # inertia + if inertial.inertia is not None: + inertia = inertial.inertia + inertia_tag = ET.SubElement(inertial_tag, 'moment_of_inertia') + + if inertia.ixx is not None: + ixx_tag = ET.SubElement(inertia_tag, 'ixx') + ixx_tag.text = str(inertia.ixx) + if inertia.ixy is not None: + ixy_tag = ET.SubElement(inertia_tag, 'ixy') + ixy_tag.text = str(inertia.ixy) + if inertia.ixz is not None: + ixz_tag = ET.SubElement(inertia_tag, 'ixz') + ixz_tag.text = str(inertia.ixz) + if inertia.iyy is not None: + iyy_tag = ET.SubElement(inertia_tag, 'iyy') + iyy_tag.text = str(inertia.iyy) + if inertia.iyz is not None: + iyz_tag = ET.SubElement(inertia_tag, 'iyz') + iyz_tag.text = str(inertia.iyz) + if inertia.izz is not None: + izz_tag = ET.SubElement(inertia_tag, 'izz') + izz_tag.text = str(inertia.izz) + + # create visual + if body.visual is not None: + visual = body.visual + # name = 'visual_' + str(i) if visual.name is None else visual.name + visual_tag = ET.SubElement(body_tag, 'visualization_shape') # attrib={'name': name}) + + # transformation + set_transformation(visual_tag, visual) + + # geometry + set_geometry(visual_tag, visual) + + # create collision + if body.collision is not None: + collision = body.collision + # name = 'collision_' + str(i) if collision.name is None else collision.name + collision_tag = ET.SubElement(body_tag, 'collision_shape') # , attrib={'name': name}) + + # transformation + set_transformation(collision_tag, collision) + + # geometry + set_geometry(collision_tag, collision) + + # create joints + for i, joint in enumerate(tree.joints): + joint_tag = ET.SubElement(skeleton_tag, 'joint', attrib={'name': joint.name, 'type': joint.dtype}) + parent_tag = ET.SubElement(joint_tag, 'parent') + parent_tag.text = joint.parent + child_tag = ET.SubElement(joint_tag, 'child') + child_tag.text = joint.child + + # transformation + set_transformation(joint_tag, joint) + + # axis + axis_tag = None + if joint.axis is not None: + axis_tag = ET.SubElement(joint_tag, 'axis') + xyz_tag = ET.SubElement(axis_tag, 'xyz') + xyz_tag.text = str(np.asarray(joint.axis))[1:-1] + + # dynamics + if joint.damping is not None or joint.friction is not None: + if axis_tag is None: + axis_tag = ET.SubElement(joint_tag, 'axis') + dynamics_tag = ET.SubElement(axis_tag, 'dynamics') + + if joint.friction is not None: + friction_tag = ET.SubElement(dynamics_tag, 'friction') + friction_tag.text = str(joint.friction) + if joint.damping is not None: + damping_tag = ET.SubElement(dynamics_tag, 'damping') + damping_tag.text = str(joint.damping) + + # limits + if joint.limits is not None or joint.effort is not None or joint.velocity is not None: + if axis_tag is None: + axis_tag = ET.SubElement(joint_tag, 'axis') + limit_tag = ET.SubElement(axis_tag, 'limit') + + if joint.limits is not None: + lower_tag = ET.SubElement(limit_tag, 'lower') + upper_tag = ET.SubElement(limit_tag, 'upper') + lower_tag.text = str(joint.limits[0]) + upper_tag.text = str(joint.limits[1]) + + if joint.velocity is not None: + velocity_tag = ET.SubElement(limit_tag, 'velocity') + velocity_tag.text = str(joint.velocity) + + if joint.effort is not None: + effort_tag = ET.SubElement(limit_tag, 'effort') + effort_tag.text = str(joint.effort) + + # init_pos + if joint.init_position is not None: + init_pos_tag = ET.SubElement(joint_tag, 'init_pos') + if isinstance(joint.init_position, (float, int)): + init_pos_tag.text = str(joint.init_position) + else: + init_pos_tag.text = str(np.asarray(joint.init_position))[1:-1] + + # init_vel + if joint.init_velocity is not None: + init_vel_tag = ET.SubElement(joint_tag, 'init_vel') + if isinstance(joint.init_velocity, (float, int)): + init_vel_tag.text = str(joint.init_velocity) + else: + init_vel_tag.text = str(np.asarray(joint.init_velocity))[1:-1] + + # return root XML element + return root diff --git a/pyrobolearn/utils/parsers/robots/urdf_parser.py b/pyrobolearn/utils/parsers/robots/urdf_parser.py index 3d0f1ea..93ddf18 100644 --- a/pyrobolearn/utils/parsers/robots/urdf_parser.py +++ b/pyrobolearn/utils/parsers/robots/urdf_parser.py @@ -1,7 +1,11 @@ #!/usr/bin/env python -"""Define the URDF parser. +"""Define the URDF parser/generator. URDF files are notably used in ROS, Gazebo, Bullet, Dart, and MuJoCo. + +References: + - URDF XML specifications: http://wiki.ros.org/urdf/XML + - Tutorial: Using a URDF in Gazebo: http://gazebosim.org/tutorials/?tut=ros_urdf """ # import XML parser @@ -148,7 +152,7 @@ class URDFParser(RobotParser): # geometry geometry_tag = visual_tag.find('geometry') if geometry_tag is not None: - for geometry_type in ['box', 'mesh', 'cylinder', 'sphere']: + for geometry_type in {'box', 'mesh', 'cylinder', 'sphere'}: geometry_type_tag = geometry_tag.find(geometry_type) if geometry_type_tag is not None: dtype = geometry_type @@ -200,7 +204,7 @@ class URDFParser(RobotParser): # geometry geometry_tag = collision_tag.find('geometry') if geometry_tag is not None: - for geometry_type in ['box', 'mesh', 'cylinder', 'sphere']: + for geometry_type in {'box', 'mesh', 'cylinder', 'sphere'}: geometry_type_tag = geometry_tag.find(geometry_type) if geometry_type_tag is not None: dtype = geometry_type diff --git a/pyrobolearn/utils/parsers/robots/world_parser.py b/pyrobolearn/utils/parsers/robots/world_parser.py index fc5ca94..1a107ad 100644 --- a/pyrobolearn/utils/parsers/robots/world_parser.py +++ b/pyrobolearn/utils/parsers/robots/world_parser.py @@ -30,6 +30,7 @@ class WorldParser(object): """ self.root = None self.world = None + self.worlds = [] self.filename = filename if filename is not None: self.parse(filename)