update doc for robots + add grippers

pyrobolearn/__init__.py

@@ -5,6 +5,7 @@ name = "pyrobolearn"
 
 import sys
 import signal
+from itertools import count
 
 # logging
 import logging
@@ -82,6 +83,9 @@ from . import algos
 
 # import experiments
 
+# import priority tasks
+from . import priorities
+
 
 # Meta-information about the package
 __author__ = "Brian Delhaisse"

pyrobolearn/robots/README.md

@@ -40,7 +40,7 @@ The folder contains different kind of robots including manipulators, legged robo
 - [E-puck](https://github.com/gctronic/epuck_driver_cpp)
 - [F10 racecar](https://github.com/erwincoumans/pybullet_robots/tree/master/data/f10_racecar)
 - [Fetch](https://github.com/fetchrobotics/fetch_ros)
-- [Franka Emika](https://github.com/frankaemika/franka_ros)
+- [Franka Emika Panda](https://github.com/frankaemika/franka_ros)
 - [Half Cheetah](https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf)
 - [Hopper](https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf)
 - [Hubo](https://github.com/robEllenberg/hubo-urdf)

pyrobolearn/robots/__init__.py

@@ -3,6 +3,7 @@
 import os
 import importlib
 import inspect
+import re
 
 # General robot class
 from .base import Body, MovableBody, ControllableBody
@@ -62,23 +63,24 @@ from .rhex import Rhex
 
 # Manipulators
 from .rrbot import RRBot
-from .wam import WAM
+from .wam import WAM, BarrettHand
 from .kuka_lwr import KukaLWR
 from .kuka_iiwa import KukaIIWA
-from .jaco import Jaco
-from .franka import Franka
+from .jaco import Jaco, JacoGripper
+from .franka import Franka, FrankaGripper
 from .sawyer import Sawyer
 from .edo import Edo
 from .kr5 import KR5
 from .manipulator2d import Manipulator2D
 
 # Bi-Manipulators
-from .baxter import Baxter
+from .baxter import Baxter, BaxterGripper
 
 # Hands
 from .allegrohand import AllegroHand
 from .softhand import SoftHand
 from .shadowhand import ShadowHand
+from .schunk_hand import SchunkHand
 
 # Wheeled
 from .epuck import Epuck
@@ -87,11 +89,11 @@ from .mkz import MKZ
 from .husky import Husky
 
 # Wheeled + (single) manipulator
-from .fetch import Fetch
+from .fetch import Fetch, FetchGripper
 
 # Wheeled + Bi-manipulators
 from .pepper import Pepper
-from .pr2 import PR2
+from .pr2 import PR2, PR2Gripper
 
 # Wheeled + Quadruped + Bi-manipulators
 from .centauro import Centauro
@@ -114,7 +116,7 @@ from .cartpole import CartPole
 from .cubli import Cubli
 from .sea_snake import SEASnake
 from .bb8 import BB8
-from .youbot import Youbot, YoubotBase, KukaYoubotArm, YoubotDualArm
+from .youbot import Youbot, YoubotBase, KukaYoubotArm, YoubotDualArm, YoubotGripper
 from .ballbot import Ballbot
 
 # Robots #
@@ -125,7 +127,7 @@ implemented_robots = set([f[:-3] for f in os.listdir(path) if os.path.isfile(os.
                           and f.endswith('.py')])
 # remove few items from the set
 for s in ['__init__', 'actuators', 'sensors', 'legged_robot', 'manipulator', 'wheeled_robot', 'uav', 'usv',
-          'uuv', 'hand']:
+          'uuv', 'hand', 'gripper']:
     if s in implemented_robots:
         implemented_robots.remove(s)
 
@@ -134,14 +136,31 @@ implemented_robots = list(implemented_robots)
 # TODO: fix problem with icub
 implemented_robots.remove('icub')
 
+print(len(implemented_robots), implemented_robots)
+
 
 # create dictionary that maps robot names to robot classes
 robot_names_to_classes = {}
 for robot_name in implemented_robots:
     module = importlib.import_module('pyrobolearn.robots.' + robot_name)  # 'robots.'+robot)
     # robot_class = getattr(module, robot.capitalize())
-    for name, obj in inspect.getmembers(module):
+    for name, cls in inspect.getmembers(module):
         # check if it is a class, and the names match
-        if inspect.isclass(obj) and name.lower() == ''.join(robot_name.split('_')):
-            robot_names_to_classes[robot_name] = obj
-            break
+        if inspect.isclass(cls) and issubclass(cls, Robot):
+            if name.lower() == ''.join(robot_name.split('_')):
+                robot_names_to_classes[robot_name] = cls
+            else:
+                name_list = re.findall('[0-9]*[A-Z]+[0-9]*[a-z]*', name)
+                name = '_'.join([n.lower() for n in name_list])
+                # TODO: improve regex
+                if name == 'wamgripper':
+                    name = 'wam_gripper'
+                elif name == 'uuvrobot':
+                    name = 'uuv_robot'
+                elif name == 'usvrobot':
+                    name = 'usv_robot'
+                robot_names_to_classes[name] = cls
+
+implemented_robots = set(list(robot_names_to_classes.keys()))
+
+print(len(implemented_robots), implemented_robots)

pyrobolearn/robots/aibo.py

@@ -23,16 +23,22 @@ class Aibo(QuadrupedRobot):
     WARNINGS: THE INERTIA MATRICES AND THE POSITION OF COLLISIONS MESHES IN THE URDF NEED TO BE CORRECTED!!
 
     References:
-        [1] https://github.com/dkotfis/aibo_ros
+        - [1] https://github.com/dkotfis/aibo_ros
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.02),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.02), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/aibo/aibo.urdf'):
+        """
+        Initialize the Aibo robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.02)

pyrobolearn/robots/allegrohand.py

@@ -20,17 +20,22 @@ class AllegroHand(Hand):
     r"""Allegro Hand
 
     References:
-        [1] http://www.simlab.co.kr/Allegro-Hand.htm
-        [2] https://github.com/simlabrobotics/allegro_hand_ros
+        - [1] http://www.simlab.co.kr/Allegro-Hand.htm
+        - [2] https://github.com/simlabrobotics/allegro_hand_ros
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 scale=1.,
-                 left=False,
-                 fixed_base=True):
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), scale=1., fixed_base=True):
+        # left=False
+        """
+        Initialize the Allegro hand.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the hand base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the hand.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)

pyrobolearn/robots/ant.py

@@ -19,15 +19,10 @@ class Ant(QuadrupedRobot):
     r"""Ant Mujoco Model
 
     References:
-        [1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
+        - [1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.2),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.2), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/mjcfs/ant.xml'):
         # check parameters
         if position is None:

pyrobolearn/robots/anymal.py

@@ -24,19 +24,25 @@ class ANYmal(QuadrupedRobot):
     ANYmal is equipped with 12 SEAs'
 
     References:
-        [1] "ANYmal - a Highly Mobile and Dynamic Quadrupedal Robot", Hutter et al., 2016
-        [2] "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019
-        [3] www.rsl.ethz.ch/robots-media/anymal.html
-        [4] www.anybotics.com/anymal
+        - [1] "ANYmal - a Highly Mobile and Dynamic Quadrupedal Robot", Hutter et al., 2016
+        - [2] "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019
+        - [3] www.rsl.ethz.ch/robots-media/anymal.html
+        - [4] www.anybotics.com/anymal
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, .6),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, .6), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/anymal/anymal.urdf'):
+        """
+        Initialize the ANYmal robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.6)

pyrobolearn/robots/atlas.py

@@ -22,17 +22,23 @@ class Atlas(BipedRobot, BiManipulator):
     Atlas robot developed by Boston Dynamics.
 
     References:
-        [1] Boston Dynamics: https://www.bostondynamics.com/atlas
-        [2] URDF: https://github.com/openai/roboschool/tree/master/roboschool/models_robot/atlas_description
+        - [1] Boston Dynamics: https://www.bostondynamics.com/atlas
+        - [2] URDF: https://github.com/openai/roboschool/tree/master/roboschool/models_robot/atlas_description
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 1.),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 1.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/atlas/atlas_v4_with_multisense.urdf'):
+        """
+        Initialize the Atlas robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 1.)

pyrobolearn/robots/ballbot.py

@@ -17,15 +17,26 @@ __status__ = "Development"
 
 # TODO inertia are not corrects
 class Ballbot(Robot):
-    r"""BB8 robot
+    r"""Ballbot robot
 
     References:
-        [1] https://github.com/eborghi10/BB-8-ROS
-        [2] http://www.theconstructsim.com/bb-8-gazebo-model/
+        - [1] https://github.com/eborghi10/BB-8-ROS
+        - [2] http://www.theconstructsim.com/bb-8-gazebo-model/
     """
 
     def __init__(self, simulator, position=(0, 0, 0.), orientation=(0, 0, 0, 1), fixed_base=False,
                  scale=1., urdf=os.path.dirname(__file__) + '/urdfs/ballbot/ballbot.urdf'):
+        """
+        Initialize the Ballbot robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)

pyrobolearn/robots/baxter.py

@@ -5,6 +5,8 @@
 import os
 
 from pyrobolearn.robots.manipulator import BiManipulator
+from pyrobolearn.robots.gripper import ParallelGripper
+
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -21,17 +23,23 @@ class Baxter(BiManipulator):
     Baxter robot built by Rethink Robotics.
 
     References:
-        [1] Rethink Robotics: https://www.rethinkrobotics.com/
-        [2] https://github.com/RethinkRobotics/baxter_common
+        - [1] Rethink Robotics: https://www.rethinkrobotics.com/
+        - [2] https://github.com/RethinkRobotics/baxter_common
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.95),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.95), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/baxter/baxter.urdf'):
+        """
+        Initialize the Baxter robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.95)
@@ -58,6 +66,43 @@ class Baxter(BiManipulator):
         self.hands = [self.get_link_ids(link) for link in ['left_gripper', 'right_gripper'] if link in self.link_names]
 
 
+class BaxterGripper(ParallelGripper):
+    r"""Baxter Gripper
+
+    Baxter robot built by Rethink Robotics.
+
+    References:
+        - [1] Rethink Robotics: https://www.rethinkrobotics.com/
+        - [2] https://github.com/RethinkRobotics/baxter_common
+    """
+
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/baxter/baxter_gripper.urdf'):
+        """
+        Initialize the Baxter gripper.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the gripper will be fixed in the world.
+            scale (float): scaling factor that is used to scale the gripper.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
+        # 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 = True
+
+        super(BaxterGripper, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'baxter_gripper'
+
+
 if __name__ == "__main__":
     from itertools import count
     from pyrobolearn.simulators import BulletSim

pyrobolearn/robots/bb8.py

@@ -19,12 +19,23 @@ class BB8(Robot):
     r"""BB8 robot
 
     References:
-        [1] https://github.com/eborghi10/BB-8-ROS
-        [2] http://www.theconstructsim.com/bb-8-gazebo-model/
+        - [1] https://github.com/eborghi10/BB-8-ROS
+        - [2] http://www.theconstructsim.com/bb-8-gazebo-model/
     """
 
     def __init__(self, simulator, position=(0, 0, 0.4), orientation=(0, 0, 0, 1), fixed_base=False,
                  scale=1., urdf=os.path.dirname(__file__) + '/urdfs/bb8/bb8.urdf'):
+        """
+        Initialize the BB8 robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.4)

pyrobolearn/robots/blackbird.py

@@ -23,17 +23,23 @@ class Blackbird(BipedRobot):
     roughly 15 kg" [1].
 
     References:
-        [1] https://hackaday.io/project/160882-blackbird-bipedal-robot
-        [2] https://github.com/G-Levine
+        - [1] https://hackaday.io/project/160882-blackbird-bipedal-robot
+        - [2] https://github.com/G-Levine
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 1.2),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 1.2), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/blackbird/blackbird_biped.urdf'):
+        """
+        Initialize the Blackbird robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
 
         self.height = 1.2
         self.base_height = self.height

pyrobolearn/robots/cartpole.py

@@ -27,23 +27,30 @@ class CartPole(Robot):
     The number of links for the pendulum can be specified during runtime.
 
     References:
-        [1] "Reinforcement Learning: an Introduction", Barto and Sutton, 1998
-        [2] Cartpole bullet environment:
+        - [1] "Reinforcement Learning: an Introduction", Barto and Sutton, 1998
+        - [2] Cartpole bullet environment:
             github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py
-        [3] "PyDy Tutorial: Human Standing": https://github.com/pydy/pydy-tutorial-human-standing
-        [4] "Dynamics with Python balancing the five link pendulum": http://www.moorepants.info/blog/npendulum.html
+        - [3] "PyDy Tutorial: Human Standing": https://github.com/pydy/pydy-tutorial-human-standing
+        - [4] "Dynamics with Python balancing the five link pendulum": http://www.moorepants.info/blog/npendulum.html
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 scale=1.,
-                 fixed_base=True,
-                 urdf=os.path.join(pybullet_data.getDataPath(), "cartpole.urdf"),
-                 num_links=1,
-                 inverted_pole=False,
-                 pole_mass=1):   # pole_mass=10
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), scale=1., fixed_base=True,
+                 num_links=1, inverted_pole=False, pole_mass=1,
+                 urdf=os.path.join(pybullet_data.getDataPath(), "cartpole.urdf")):  # pole_mass=10
+        """
+        Initialize the Cartpole robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            num_links (int): number of links / poles connected to each other.
+            inverted_pole (bool): if we should start with inverted poles, or not.
+            pole_mass (float): mass of each link/pole.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)

pyrobolearn/robots/cassie.py

@@ -22,22 +22,28 @@ class Cassie(BipedRobot):
     This class describes the cassie robot developed by Agility Robotics.
 
     References:
-        [1] http://www.agilityrobotics.com/sims/
-        [2] "Feedback Control For Cassie With Deep Reinforcement Learning", Xie et al., 2018
-            https://arxiv.org/abs/1803.05580
-        [3] https://github.com/agilityrobotics/cassie-gazebo-sim
-        [4] https://github.com/UMich-BipedLab/Cassie_Model
-        [5] https://github.com/UMich-BipedLab/cassie_description
-        [6] https://github.com/erwincoumans/pybullet_robots/tree/master/data/cassie
+        - [1] http://www.agilityrobotics.com/sims/
+        - [2] "Feedback Control For Cassie With Deep Reinforcement Learning", Xie et al., 2018
+              https://arxiv.org/abs/1803.05580
+        - [3] https://github.com/agilityrobotics/cassie-gazebo-sim
+        - [4] https://github.com/UMich-BipedLab/Cassie_Model
+        - [5] https://github.com/UMich-BipedLab/cassie_description
+        - [6] https://github.com/erwincoumans/pybullet_robots/tree/master/data/cassie
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, .8),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, .8), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/cassie/cassie.urdf'):
+        """
+        Initialize the Cassie robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.8)

pyrobolearn/robots/centauro.py

@@ -22,19 +22,25 @@ class Centauro(WheeledRobot, QuadrupedRobot, BiManipulator):
     r"""Centauro robot
 
     References:
-        [1] https://github.com/ADVRHumanoids/centauro-simulator
+        - [1] https://github.com/ADVRHumanoids/centauro-simulator
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 1.),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 1.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/centauro/centauro_stick.urdf'
                  # centauro_stick.urdf, centauro_soft_hand.urdf, centauro_heri.urdf,
                  # centauro_schunk_handL.urdf, centauro_schunk_hand.urdf
                  ):
+        """
+        Initialize the Centauro robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 1.)

pyrobolearn/robots/cogimon.py

@@ -20,18 +20,24 @@ class Cogimon(BipedRobot, BiManipulator):
     r"""Cogimon humanoid robot.
 
     References:
-        [1] https://github.com/ADVRHumanoids/iit-cogimon-ros-pkg
+        - [1] https://github.com/ADVRHumanoids/iit-cogimon-ros-pkg
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 1.),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
-                 urdf=os.path.dirname(__file__) + '/urdfs/cogimon/cogimon.urdf',
-                 lower_body=False):  # cogimon_lower_body.urdf
+    def __init__(self, simulator, position=(0, 0, 1.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 lower_body=False, urdf=os.path.dirname(__file__) + '/urdfs/cogimon/cogimon.urdf'):
+        # cogimon_lower_body.urdf
+        """
+        Initialize the Cogimon robot.
 
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            lower_body (bool): if we should only use the lower body of the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 1.)

pyrobolearn/robots/coman.py

@@ -21,16 +21,22 @@ class Coman(BipedRobot, BiManipulator):
     r"""Coman robot
 
     References:
-        [1] https://github.com/ADVRHumanoids/iit-coman-ros-pkg
+        - [1] https://github.com/ADVRHumanoids/iit-coman-ros-pkg
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.5),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.5), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/coman/coman.urdf'):
+        """
+        Initialize the Coman robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.5)

pyrobolearn/robots/crab.py

@@ -19,17 +19,23 @@ class Crab(HexapodRobot):
     r"""Crab Hexapod robot
 
     References:
-        [1] http://wiki.ros.org/Robots/HexapodRobot
-        [2] https://github.com/tuuzdu/crab_project
+        - [1] http://wiki.ros.org/Robots/HexapodRobot
+        - [2] https://github.com/tuuzdu/crab_project
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.12),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.12), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/crab/crab.urdf'):
+        """
+        Initialize the Crab robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.12)

pyrobolearn/robots/cubli.py

@@ -7,6 +7,7 @@ import os
 from pyrobolearn.robots.robot import Robot
 from pyrobolearn.utils.transformation import get_rpy_from_quaternion
 
+
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __license__ = "GNU GPLv3"
@@ -20,24 +21,31 @@ class Cubli(Robot):
     r"""Cubli robot
 
     References:
-        [1] "The Cubli: a Cube that can Jump Up and Balance", Gajamohan et al., 2012
-        [2] "The Cubli: a Reaction Wheel Based 3D Inverted Pendulum", Gajamohan et al., 2013
-        [3] "Nonlinear Analysis and Control of a Reaction Wheel-based 3D Inverted Pendulum", Muehlebach et al., 2017
-        [4] "Balancing Control of a Cubical Robot Balancing on its Corner", Chen et al., 2018
+        - [1] "The Cubli: a Cube that can Jump Up and Balance", Gajamohan et al., 2012
+        - [2] "The Cubli: a Reaction Wheel Based 3D Inverted Pendulum", Gajamohan et al., 2013
+        - [3] "Nonlinear Analysis and Control of a Reaction Wheel-based 3D Inverted Pendulum", Muehlebach et al., 2017
+        - [4] "Balancing Control of a Cubical Robot Balancing on its Corner", Chen et al., 2018
+        - [5] Cubli Robot: https://github.com/xinsongyan/cubli
 
     Also, check about M-Blocks:
-        [1] "M-Blocks: Momentum-driven, Magnetic Modular Robots", Romanishin et al., 2013
-        [2] "3D M-Blocks: Self-reconfiguring Robots Capable of Locomtion via Pivoting in 3 Dimensions", Romanishin
-            et al., 2015
+        - [1] "M-Blocks: Momentum-driven, Magnetic Modular Robots", Romanishin et al., 2013
+        - [2] "3D M-Blocks: Self-reconfiguring Robots Capable of Locomotion via Pivoting in 3 Dimensions", Romanishin
+              et al., 2015
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.5),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.5), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/cubli/cubli.urdf'):
+        """
+        Initialize the Cubli robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.5)

pyrobolearn/robots/darwin.py

@@ -21,16 +21,22 @@ class Darwin(BipedRobot, BiManipulator):
     r"""Darwin robot
 
     References:
-        [1] https://github.com/HumaRobotics/darwin_description
+        - [1] https://github.com/HumaRobotics/darwin_description
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.34),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.34), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/darwin/darwin.urdf'):
+        """
+        Initialize the Darwin robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.34)

pyrobolearn/robots/ecaa9.py

@@ -64,16 +64,27 @@ class ECAA9(UUVRobot):
     For a submarine, the coefficient is approximately around 0.04 (see [3]).
 
     References:
-        [1] https://www.ecagroup.com/en/solutions/a9-s-auv-autonomous-underwater-vehicle
-        [2] UUV Simulator: https://uuvsimulator.github.io/
-        [3] Aerodynamics (Nasa - check for equation): https://www.grc.nasa.gov/www/k-12/airplane/short.html
-        [4] https://s2.smu.edu/propulsion/Pages/navigation.htm
-        [5] Introduction to Ocean Waves: http://pordlabs.ucsd.edu/rsalmon/111.textbook.pdf
-        [6] https://fenicsproject.org/
+        - [1] https://www.ecagroup.com/en/solutions/a9-s-auv-autonomous-underwater-vehicle
+        - [2] UUV Simulator: https://uuvsimulator.github.io/
+        - [3] Aerodynamics (Nasa - check for equation): https://www.grc.nasa.gov/www/k-12/airplane/short.html
+        - [4] https://s2.smu.edu/propulsion/Pages/navigation.htm
+        - [5] Introduction to Ocean Waves: http://pordlabs.ucsd.edu/rsalmon/111.textbook.pdf
+        - [6] https://fenicsproject.org/
     """
 
-    def __init__(self, simulator, position=(0, 0, 1.), orientation=(0, 0, 0, 1), fixed_base=False, scaling=1.,
-                 urdf_path=os.path.dirname(__file__) + '/urdfs/ecaa9/eca_a9.urdf'):
+    def __init__(self, simulator, position=(0, 0, 1.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/ecaa9/eca_a9.urdf'):
+        """
+        Initialize the ECAA9 vehicle.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the vehicle base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the vehicle.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 1.)
@@ -84,10 +95,10 @@ class ECAA9(UUVRobot):
         if fixed_base is None:
             fixed_base = False
 
-        super(ECAA9, self).__init__(simulator, urdf_path, position, orientation, fixed_base, scaling)
+        super(ECAA9, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
         self.name = 'eca_a9'
 
-        self.volume = 0.0679998770412 * scaling**3  # from urdf
+        self.volume = 0.0679998770412 * scale ** 3  # from urdf
         self.sea_water_density = 1027
         self.center_buoyancy = np.array([0.000106, 0., 0.6])  # from urdf
 

pyrobolearn/robots/edo.py

@@ -21,18 +21,24 @@ class Edo(Manipulator):
     E.Do robot developed by Comau.
 
     References:
-        [1] e.Do: https://edo.cloud/
-        [2] Comau: https://www.comau.com/EN/our-competences/robotics/eDO
-        [3] Github: https://github.com/Comau/eDO_description
+        - [1] e.Do: https://edo.cloud/
+        - [2] Comau: https://www.comau.com/EN/our-competences/robotics/eDO
+        - [3] Github: https://github.com/Comau/eDO_description
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=True,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=True, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/edo/edo.urdf'):
+        """
+        Initialize the E.Do robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)

pyrobolearn/robots/epuck.py

@@ -20,18 +20,24 @@ class Epuck(DifferentialWheeledRobot):
     r"""Epuck robot
 
     References:
-        [1] http://www.e-puck.org/
-        [2] http://www.gctronic.com/doc/index.php/E-Puck
-        [3] https://github.com/gctronic/epuck_driver_cpp
+        - [1] http://www.e-puck.org/
+        - [2] http://www.gctronic.com/doc/index.php/E-Puck
+        - [3] https://github.com/gctronic/epuck_driver_cpp
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/epuck/epuck.urdf'):
+        """
+        Initialize the E-puck robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0)

pyrobolearn/robots/f10_racecar.py

@@ -20,16 +20,22 @@ class F10Racecar(AckermannWheeledRobot):
     r"""F10Racecar robot
 
     References:
-        [1] https://github.com/erwincoumans/pybullet_robots/tree/master/data/f10_racecar
+        - [1] https://github.com/erwincoumans/pybullet_robots/tree/master/data/f10_racecar
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, .1),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, .1), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/f10_racecar/racecar.urdf'):  # racecar_differential.urdf
+        """
+        Initialize the F10 racecar.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the car base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the car.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.1)

pyrobolearn/robots/fetch.py

@@ -6,6 +6,8 @@ import os
 
 from pyrobolearn.robots.wheeled_robot import WheeledRobot
 from pyrobolearn.robots.manipulator import Manipulator
+from pyrobolearn.robots.gripper import ParallelGripper
+
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -20,17 +22,23 @@ class Fetch(WheeledRobot, Manipulator):
     r"""Fetch robot
 
     References:
-        [1] Fetch robotics: https://fetchrobotics.com/
-        [2] Fetch description: https://github.com/fetchrobotics/fetch_ros
+        - [1] Fetch robotics: https://fetchrobotics.com/
+        - [2] Fetch description: https://github.com/fetchrobotics/fetch_ros
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.1),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.1), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/fetch/fetch.urdf'):
+        """
+        Initialize the Fetch manipulator.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.1)
@@ -45,6 +53,41 @@ class Fetch(WheeledRobot, Manipulator):
         self.name = 'fetch'
 
 
+class FetchGripper(ParallelGripper):
+    r"""Fetch Gripper
+
+    References:
+        - [1] Fetch robotics: https://fetchrobotics.com/
+        - [2] Fetch description: https://github.com/fetchrobotics/fetch_ros
+    """
+
+    def __init__(self, simulator, position=(0, 0, 0.16), orientation=(0, -0.707, 0, 0.707), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/fetch/fetch_gripper.urdf'):
+        """
+        Initialize the Fetch gripper.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the gripper will be fixed in the world.
+            scale (float): scaling factor that is used to scale the gripper.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
+        # check parameters
+        if position is None:
+            position = (0., 0., 0.16)
+        if len(position) == 2:  # assume x, y are given
+            position = tuple(position) + (0.16,)
+        if orientation is None:
+            orientation = (0, -0.707, 0, 0.707)
+        if fixed_base is None:
+            fixed_base = True
+
+        super(FetchGripper, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'fetch_gripper'
+
+
 # Test
 if __name__ == "__main__":
     from itertools import count

pyrobolearn/robots/flappy.py

@@ -2,6 +2,8 @@
 """Provide the techpod platform.
 """
 
+# TODO: finish to implement this class
+
 import os
 import json
 import numpy as np
@@ -42,8 +44,8 @@ class Wing(object):
     Python code translated from C++ code provided in [1].
 
     References:
-        [1] https://github.com/purdue-biorobotics/flappy/blob/master/flappy/envs/Wing.cpp
-        [2] "Flappy Hummingbird: An Open Source Dynamic Simulation of Flapping Wing Robots and Animals", Fei et al.,
+        - [1] https://github.com/purdue-biorobotics/flappy/blob/master/flappy/envs/Wing.cpp
+        - [2] "Flappy Hummingbird: An Open Source Dynamic Simulation of Flapping Wing Robots and Animals", Fei et al.,
             2019
     """
 
@@ -306,16 +308,27 @@ class Flappy(FlappingWingUAV):
     described in the paper and code [2,3]. The gravity is carried out by pybullet.
 
     References:
-        [1] "Design Optimization and System Integration of Robotic Hummingbird", Zhang et al., 2017
-        [2] "Flappy Hummingbird: An Open Source Dynamic Simulation of Flapping Wing Robots and Animals", Fei et al.,
+        - [1] "Design Optimization and System Integration of Robotic Hummingbird", Zhang et al., 2017
+        - [2] "Flappy Hummingbird: An Open Source Dynamic Simulation of Flapping Wing Robots and Animals", Fei et al.,
             2019
-        [3] https://github.com/purdue-biorobotics/flappy
+        - [3] https://github.com/purdue-biorobotics/flappy
     """
 
-    def __init__(self, simulator, position=(0, 0, 0.5), orientation=(0, 0, 0, 1), fixed_base=False, scaling=1.,
+    def __init__(self, simulator, position=(0, 0, 0.5), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/flappy/flappy.urdf',
                  config=os.path.dirname(__file__) + '/urdfs/flappy/config/mav_config.json'):
-        super(Flappy, self).__init__(simulator, urdf, position, orientation, fixed_base)
+        """
+        Initialize the Flappy robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
+        super(Flappy, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
 
         with open(config) as f:
             config = json.load(f)[0]

pyrobolearn/robots/franka.py

@@ -5,6 +5,8 @@
 import os
 
 from pyrobolearn.robots.manipulator import Manipulator
+from pyrobolearn.robots.gripper import ParallelGripper
+
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -16,24 +18,31 @@ __status__ = "Development"
 
 
 class Franka(Manipulator):
-    r"""Franka Emika robot
+    r"""Franka Emika Panda robot
 
-    WARNING: CURRENTLY, THE INERTIAL TAGS ARE NOT SET IN THE URDF!!
+    Warnings: CURRENTLY, THE INERTIAL TAGS ARE NOT CORRECT IN THE URDF!! I INVENTED THEM MYSELF BASED ON THE VOLUME,
+    UNIFORM DENSITY, AND SUPPOSED MASS.
 
     References:
-        [1] Documentation: https://frankaemika.github.io/docs/index.html
-        [2] Overview: https://frankaemika.github.io/docs/overview.html
-        [3] C++ library: https://github.com/frankaemika/libfranka
-        [2] ROS integration: https://github.com/frankaemika/franka_ros
+        - [1] Documentation: https://frankaemika.github.io/docs/index.html
+        - [2] Overview: https://frankaemika.github.io/docs/overview.html
+        - [3] C++ library: https://github.com/frankaemika/libfranka
+        - [4] ROS integration: https://github.com/frankaemika/franka_ros
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 scale=1.,
-                 fixed_base=True,
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), scale=1., fixed_base=True,
                  urdf=os.path.dirname(__file__) + '/urdfs/franka/franka.urdf'):
+        """
+        Initialize the Franka Emika Panda manipulator.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)
@@ -50,6 +59,46 @@ class Franka(Manipulator):
         # self.disable_motor()
 
 
+class FrankaGripper(ParallelGripper):
+    r"""Franka Emika Panda gripper
+
+    Warnings: CURRENTLY, THE INERTIAL TAGS ARE NOT CORRECT IN THE URDF!! I INVENTED THEM MYSELF BASED ON THE VOLUME,
+    UNIFORM DENSITY, AND SUPPOSED MASS.
+
+    References:
+        - [1] Documentation: https://frankaemika.github.io/docs/index.html
+        - [2] Overview: https://frankaemika.github.io/docs/overview.html
+        - [3] C++ library: https://github.com/frankaemika/libfranka
+        - [4] ROS integration: https://github.com/frankaemika/franka_ros
+    """
+
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0., 0, 1.), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/franka/franka_gripper.urdf'):
+        """
+        Initialize the Franka Emika Panda gripper.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the gripper will be fixed in the world.
+            scale (float): scaling factor that is used to scale the gripper.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
+        # 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 = True
+
+        super(FrankaGripper, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'franka_gripper'
+
+
 # Test
 if __name__ == "__main__":
     from itertools import count

pyrobolearn/robots/gripper.py

@@ -0,0 +1,112 @@
+#!/usr/bin/env python
+"""Provide the Gripper abstract classes.
+"""
+
+from pyrobolearn.robots.robot import Robot
+
+__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 Gripper(Robot):
+    r"""Gripper end-effector
+    """
+
+    def __init__(self, simulator, urdf, position=(0, 0, 1.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.):
+        """
+        Initialize the gripper.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the URDF file.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the gripper will be fixed in the world.
+            scale (float): scaling factor that is used to scale the gripper.
+        """
+        super(Gripper, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+
+        self.fingers = []  # list of fingers where each finger is a list of links/joints
+
+    @property
+    def num_fingers(self):
+        """Return the number of fingers on the hand"""
+        return len(self.fingers)
+
+    def get_finger(self, finger_id=None):
+        """Return the list of joint/link ids for the specified finger"""
+        if finger_id:
+            return self.fingers[finger_id]
+        return self.fingers
+
+
+class ParallelGripper(Gripper):
+    r"""Parallel Gripper
+
+    When the fingers are closing toward each other, they remain parallel. Most of the time, these types of grippers
+    have 2 fingers. This type is also known as the slider type.
+    """
+
+    def __init__(self, simulator, urdf, position=(0, 0, 1.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.):
+        """
+        Initialize the gripper.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the URDF file.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the gripper will be fixed in the world.
+            scale (float): scaling factor that is used to scale the gripper.
+        """
+        super(ParallelGripper, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+
+
+class AngularGripper(Gripper):
+    r"""Angular Gripper
+
+    When the fingers are closing toward each other, each finger rotates around an axis. They can have 2, 3, or even
+    4 grippers. This type is also known as the lever type.
+    """
+
+    def __init__(self, simulator, urdf, position=(0, 0, 1.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.):
+        """
+        Initialize the gripper.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the URDF file.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the gripper will be fixed in the world.
+            scale (float): scaling factor that is used to scale the gripper.
+        """
+        super(AngularGripper, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+
+
+class VacuumGripper(Gripper):
+    r"""Vacuum Gripper
+
+    A vacuum gripper doesn't have any fingers but use suction in order to grasp different objects. Note that this
+    required the simulator to be able to simulate soft bodies and the interaction of these ones with rigid bodies.
+    """
+
+    def __init__(self, simulator, urdf, position=(0, 0, 1.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.):
+        """
+        Initialize the gripper.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the URDF file.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the gripper will be fixed in the world.
+            scale (float): scaling factor that is used to scale the gripper.
+        """
+        super(VacuumGripper, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)

pyrobolearn/robots/half_cheetah.py

@@ -19,15 +19,10 @@ class HalfCheetah(Robot):
     r"""Half Cheetah Mujoco Model
 
     References:
-        [1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
+        - [1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(-0.5, 0, 0.1),
-                 orientation=(0, 0.707, 0, 0.707),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(-0.5, 0, 0.1), orientation=(0, 0.707, 0, 0.707), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/mjcfs/half_cheetah.xml'):
         # check parameters
         if position is None:

pyrobolearn/robots/hand.py

@@ -18,13 +18,18 @@ class Hand(Robot):
     r"""Hand end-effector
     """
 
-    def __init__(self,
-                 simulator,
-                 urdf,
-                 position=(0, 0, 1.),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.):
+    def __init__(self, simulator, urdf, position=(0, 0, 1.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.):
+        """
+        Initialize the hand robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(Hand, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
 
         self.fingers = []  # list of fingers where each finger is a list of links/joints
@@ -46,13 +51,18 @@ class TwoHand(Hand):
 
     """
 
-    def __init__(self,
-                 simulator,
-                 urdf,
-                 position=(0, 0, 1.),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.):
+    def __init__(self, simulator, urdf, position=(0, 0, 1.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.):
+        """
+        Initialize the two hands robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(TwoHand, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
 
         self.left_fingers = []  # list of ids in self.fingers

pyrobolearn/robots/hopper.py

@@ -19,15 +19,10 @@ class Hopper(Robot):
     r"""Hopper Mujoco Model
 
     References:
-        [1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
+        - [1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(-0.5, 0, 0.1),
-                 orientation=(0, 0.707, 0, 0.707),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(-0.5, 0, 0.1), orientation=(0, 0.707, 0, 0.707), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/mjcfs/hopper.xml'):
         # check parameters
         if position is None:

pyrobolearn/robots/hubo.py

@@ -27,19 +27,25 @@ class Hubo(BipedRobot, BiManipulator, TwoHand):
     and 1 for the waist.
 
     References:
-        [1] ROS wiki: http://wiki.ros.org/Robots/HUBO
-        [2] Hubo Lab: hubolab.kaist.ac.kr
-        [3] Rainbow Robotics: http://www.rainbow-robotics.com/new/
-        [4] URDF: https://github.com/robEllenberg/hubo-urdf
+        - [1] ROS wiki: http://wiki.ros.org/Robots/HUBO
+        - [2] Hubo Lab: hubolab.kaist.ac.kr
+        - [3] Rainbow Robotics: http://www.rainbow-robotics.com/new/
+        - [4] URDF: https://github.com/robEllenberg/hubo-urdf
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 1),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 1), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/hubo/hubo.urdf'):
+        """
+        Initialize the Hubo robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 1.)

pyrobolearn/robots/humanoid.py

@@ -20,15 +20,10 @@ class Humanoid(BipedRobot, BiManipulator):
     r"""Humanoid Mujoco Model
 
     References:
-        [1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
+        - [1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(-0.5, 0, 1.),
-                 orientation=(0, 0.707, 0, 0.707),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(-0.5, 0, 1.), orientation=(0, 0.707, 0, 0.707), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/mjcfs/humanoid.xml'):  # humanoid_symmetric.xml
         # check parameters
         if position is None:

pyrobolearn/robots/husky.py

@@ -22,18 +22,24 @@ class Husky(DifferentialWheeledRobot):
     Husky robot from Clearpath Robotics [1].
 
     References:
-        [1] Clearpath Robotics: https://www.clearpathrobotics.com/husky-unmanned-ground-vehicle-robot/
-        [2] ROS wiki: http://wiki.ros.org/Robots/Husky
-        [3] Github: https://github.com/husky/husky
+        - [1] Clearpath Robotics: https://www.clearpathrobotics.com/husky-unmanned-ground-vehicle-robot/
+        - [2] ROS wiki: http://wiki.ros.org/Robots/Husky
+        - [3] Github: https://github.com/husky/husky
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, .14),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, .14), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/husky/husky.urdf'):
+        """
+        Initialize the Husky robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.14)

pyrobolearn/robots/hyq.py

@@ -21,17 +21,23 @@ class HyQ(QuadrupedRobot):
     HyQ robot created by IIT.
 
     References:
-        [1] https://dls.iit.it/robots/hyq-robot
-        [2] https://github.com/iit-DLSLab/hyq-description
+        - [1] https://dls.iit.it/robots/hyq-robot
+        - [2] https://github.com/iit-DLSLab/hyq-description
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, .9),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, .9), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/hyq/hyq.urdf'):
+        """
+        Initialize the HyQ robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.9)

pyrobolearn/robots/hyq2max.py

@@ -22,20 +22,26 @@ class HyQ2Max(QuadrupedRobot):
     HyQ2Max robot created by IIT.
 
     References:
-        [1] "Design of the Hydraulically-Actuated,Torque-Controlled Quadruped Robot HyQ2Max", Semini et al., 2016
-        [2] https://dls.iit.it/robots/hyq2max
-        [3] https://github.com/iit-DLSLab/hyq2max-description
+        - [1] "Design of the Hydraulically-Actuated,Torque-Controlled Quadruped Robot HyQ2Max", Semini et al., 2016
+        - [2] https://dls.iit.it/robots/hyq2max
+        - [3] https://github.com/iit-DLSLab/hyq2max-description
     """
 
     default_height = 0.8
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.8),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.8), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/hyq2max/hyq2max.urdf'):
+        """
+        Initialize the HyQ2Max robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.8)

pyrobolearn/robots/icub.py

@@ -2,6 +2,8 @@
 """Provide the ICub robotic platform.
 """
 
+# TODO: fix bug
+
 import os
 
 from pyrobolearn.robots.legged_robot import BipedRobot
@@ -20,18 +22,24 @@ class ICub(BipedRobot, BiManipulator):
     r"""ICub robot
 
     References:
-        [1] http://www.icub.org/
-        [2] https://github.com/robotology-playground/icub-models
-        [3] https://github.com/robotology-playground/icub-model-generator
+        - [1] http://www.icub.org/
+        - [2] https://github.com/robotology-playground/icub-models
+        - [3] https://github.com/robotology-playground/icub-model-generator
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.7),
-                 orientation=(0, 0, 1, 0),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.7), orientation=(0, 0, 1, 0), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/icub/icub-v2.5+.urdf'):
+        """
+        Initialize the ICub robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.7)

pyrobolearn/robots/jaco.py

@@ -5,6 +5,8 @@
 import os
 
 from pyrobolearn.robots.manipulator import Manipulator
+from pyrobolearn.robots.gripper import AngularGripper
+
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -19,18 +21,24 @@ class Jaco(Manipulator):
     r"""Jaco (manipulator) robot
 
     References:
-        [1] https://github.com/JenniferBuehler/jaco-arm-pkgs
-        [2] https://github.com/Kinovarobotics/kinova-ros
-        [3] https://github.com/RIVeR-Lab/wpi_jaco
+        - [1] https://github.com/JenniferBuehler/jaco-arm-pkgs
+        - [2] https://github.com/Kinovarobotics/kinova-ros
+        - [3] https://github.com/RIVeR-Lab/wpi_jaco
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=True,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=True, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/jaco/jaco.urdf'):
+        """
+        Initialize the Jaco manipulator.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)
@@ -45,6 +53,42 @@ class Jaco(Manipulator):
         self.name = 'jaco'
 
 
+class JacoGripper(AngularGripper):
+    r"""Jaco Gripper
+
+    References:
+        - [1] https://github.com/JenniferBuehler/jaco-arm-pkgs
+        - [2] https://github.com/Kinovarobotics/kinova-ros
+        - [3] https://github.com/RIVeR-Lab/wpi_jaco
+    """
+
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0.707, 0, 0.707), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/jaco/jaco_gripper.urdf'):
+        """
+        Initialize the Jaco gripper.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the gripper will be fixed in the world.
+            scale (float): scaling factor that is used to scale the gripper.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
+        # 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.707, 0, 0.707)
+        if fixed_base is None:
+            fixed_base = True
+
+        super(JacoGripper, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'jaco_gripper'
+
+
 # Test
 if __name__ == "__main__":
     from itertools import count

pyrobolearn/robots/kilobot.py

@@ -31,22 +31,28 @@ class Kilobot(Robot):
     - linear resonant actuator (LRA) [5.2]
 
     References:
-        [1] "Kilobot: a Low Cost Scalable Robot System for Collective Behaviors", Rubenstein et al., 2012
-        [2] "Programmable self-assembly in a thousand-robot swarm", Rubenstein et al., 2014
-        [3] Harvard's Self-Organizing Systems Research Group: https://ssr.seas.harvard.edu/kilobots
-        [4] K-Team Corporation: https://www.k-team.com/mobile-robotics-products/kilobot
-        [5] Precision Micro drives: https://www.precisionmicrodrives.com/
+        - [1] "Kilobot: a Low Cost Scalable Robot System for Collective Behaviors", Rubenstein et al., 2012
+        - [2] "Programmable self-assembly in a thousand-robot swarm", Rubenstein et al., 2014
+        - [3] Harvard's Self-Organizing Systems Research Group: https://ssr.seas.harvard.edu/kilobots
+        - [4] K-Team Corporation: https://www.k-team.com/mobile-robotics-products/kilobot
+        - [5] Precision Micro drives: https://www.precisionmicrodrives.com/
             - ERM: https://www.precisionmicrodrives.com/vibration-motors/
             - LRA: https://www.precisionmicrodrives.com/vibration-motors/linear-resonant-actuators-lras/
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/kilobot/kilobot.urdf'):  # TODO: finish URDF
+        """
+        Initialize the Kilobot robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0)

pyrobolearn/robots/kr5.py

@@ -21,18 +21,24 @@ class KR5(Manipulator):
     Payload of 5.00kg and a reach of 650mm or 850mm.
 
     References:
-        [1] Kuka robotics: https://www.kuka.com/en-de
-        [2] https://github.com/a-price/KR5sixxR650WP_description
-        [3] https://github.com/ros-industrial/kuka_experimental
+        - [1] Kuka robotics: https://www.kuka.com/en-de
+        - [2] https://github.com/a-price/KR5sixxR650WP_description
+        - [3] https://github.com/ros-industrial/kuka_experimental
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=True,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=True, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/kuka/kr5/kr5.urdf'):
+        """
+        Initialize the KR5 manipulator.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)

pyrobolearn/robots/kuka_iiwa.py

@@ -22,18 +22,24 @@ class KukaIIWA(Manipulator):
     end-effector. Payload of 14kg and a range of 820mm.
 
     References:
-        [1] Kuka robotics: https://www.kuka.com/en-de
-        [2] https://github.com/IFL-CAMP/iiwa_stack
-        [3] https://github.com/bulletphysics/bullet3/tree/master/data/kuka_iiwa
+        - [1] Kuka robotics: https://www.kuka.com/en-de
+        - [2] https://github.com/IFL-CAMP/iiwa_stack
+        - [3] https://github.com/bulletphysics/bullet3/tree/master/data/kuka_iiwa
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 scale=1.,
-                 fixed_base=True,
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), scale=1., fixed_base=True,
                  urdf=os.path.dirname(__file__) + '/urdfs/kuka/kuka_iiwa/iiwa14.urdf'):
+        """
+        Initialize the Kuka IIWA manipulator.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)

pyrobolearn/robots/kuka_lwr.py

@@ -22,18 +22,24 @@ class KukaLWR(Manipulator):
     Payload of 7kg and a range of 790mm.
 
     References:
-        [1] Kuka robotics: https://www.kuka.com/en-de
-        [2] https://github.com/CentroEPiaggio/kuka-lwr
-        [3] https://github.com/bulletphysics/bullet3/tree/master/data/kuka_lwr
+        - [1] Kuka robotics: https://www.kuka.com/en-de
+        - [2] https://github.com/CentroEPiaggio/kuka-lwr
+        - [3] https://github.com/bulletphysics/bullet3/tree/master/data/kuka_lwr
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=True,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=True, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/kuka/kuka_lwr/kuka.urdf'):
+        """
+        Initialize the Kuka LWR robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)

pyrobolearn/robots/laikago.py

@@ -20,17 +20,23 @@ class Laikago(QuadrupedRobot):
     r"""Laikago robot
 
     References:
-        [1] Laikago: http://www.unitree.cc/e/action/ShowInfo.php?classid=6&id=1
-        [2] https://github.com/erwincoumans/pybullet_robots/tree/master/data/laikago
+        - [1] Laikago: http://www.unitree.cc/e/action/ShowInfo.php?classid=6&id=1
+        - [2] https://github.com/erwincoumans/pybullet_robots/tree/master/data/laikago
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, .5),
-                 orientation=(0.5, 0.5, 0.5, 0.5),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, .5), orientation=(0.5, 0.5, 0.5, 0.5), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/laikago/laikago.urdf'):
+        """
+        Initialize the Laikago robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.5)

pyrobolearn/robots/legged_robot.py

@@ -30,6 +30,18 @@ class LeggedRobot(Robot):
 
     def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1.,
                  foot_frictions=None):
+        """
+        Initialize the Legged robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            foot_frictions (float, list of float): foot friction value(s).
+        """
         super(LeggedRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scale=scale)
 
         # leg and feet ids
@@ -174,7 +186,7 @@ class LeggedRobot(Robot):
         References:
             - [1] "Postural Stability of Biped Robots and Foot-Rotation Index (FRI) Point", Goswami, 1999
             - [2] "Ground Reference Points in Legged Locomotion: Definitions, Biological Trajectories and Control
-            Implications", Popovic et al., 2005
+              Implications", Popovic et al., 2005
         """
         if floor_id is not None:
             # get contact points between the robot's links and the floor
@@ -255,7 +267,7 @@ class LeggedRobot(Robot):
 
         References:
             - [1] "Ground Reference Points in Legged Locomotion: Definitions, Biological Trajectories and Control
-            Implications", Popovic et al., 2005
+              Implications", Popovic et al., 2005
             - [2] "Biped Walking Pattern Generation by using Preview Control of ZMP", Kajita et al., 2003
             - [3] "Exploiting Angular Momentum to Enhance Bipedal Center-of-Mass Control", Hofmann et al., 2009
         """
@@ -329,7 +341,7 @@ class LeggedRobot(Robot):
         References:
             - [1] "Postural Stability of Biped Robots and the Foot-Rotation Indicator (FRI) Point", Goswami, 1999
             - [2] "Ground Reference Points in Legged Locomotion: Definitions, Biological Trajectories and Control
-            Implications", Popovic et al., 2005
+              Implications", Popovic et al., 2005
         """
         raise NotImplementedError
 
@@ -363,7 +375,7 @@ class LeggedRobot(Robot):
 
         References:
             - [1] "Ground Reference Points in Legged Locomotion: Definitions, Biological Trajectories and Control
-            Implications", Popovic et al., 2005
+              Implications", Popovic et al., 2005
         """
         # update the CoM
         if update_com:
@@ -730,6 +742,17 @@ class BipedRobot(LeggedRobot):
     """
 
     def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1.):
+        """
+        Initialize the Biped robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(BipedRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
 
         self.left_leg_id = 0
@@ -767,6 +790,17 @@ class QuadrupedRobot(LeggedRobot):
     """
 
     def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1.):
+        """
+        Initialize the Quadruped robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(QuadrupedRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
 
         self.left_front_leg_id = 0
@@ -826,6 +860,17 @@ class HexapodRobot(LeggedRobot):
     """
 
     def __init__(self, simulator, urdf, position, orientation=None, fixed_base=False, scale=1.):
+        """
+        Initialize the hexapod robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(HexapodRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
 
         self.left_front_leg_id = 0

pyrobolearn/robots/littledog.py

@@ -20,18 +20,24 @@ class LittleDog(QuadrupedRobot):
     r"""Little Dog
 
     References:
-        [1] "The LittleDog Robot", Murphy et al., 2010
+        - [1] "The LittleDog Robot", Murphy et al., 2010
             https://journals.sagepub.com/doi/abs/10.1177/0278364910387457?journalCode=ijra
-        [2] https://github.com/RobotLocomotion/LittleDog
+        - [2] https://github.com/RobotLocomotion/LittleDog
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.2),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.2), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/littledog/littleDog.urdf'):
+        """
+        Initialize the LittleDog robots.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.2)

pyrobolearn/robots/manipulator.py

@@ -20,13 +20,18 @@ class Manipulator(Robot):
     Manipulator robots are robots that use some of its end-effectors to manipulate objects in its environment.
     """
 
-    def __init__(self,
-                 simulator,
-                 urdf,
-                 position=(0, 0, 0.),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.):
+    def __init__(self, simulator, urdf, position=(0, 0, 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.):
+        """
+        Initialize the manipulator.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(Manipulator, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
 
         self.arms = []  # list of arms where an arm is a list of links
@@ -123,8 +128,18 @@ class BiManipulator(Manipulator):
     Bi-manipulators are robots that have two manipulators to manipulate objects in the environment.
     """
 
-    def __init__(self, simulator, urdf, position=(0, 0, 1.5), orientation=(0, 0, 0, 1), fixed_base=False,
-                 scale=1.):
+    def __init__(self, simulator, urdf, position=(0, 0, 1.5), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.):
+        """
+        Initialize the bi-manipulator.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(BiManipulator, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
 
         self.left_arm_id = 0

pyrobolearn/robots/manipulator2d.py

@@ -20,16 +20,22 @@ class Manipulator2D(Manipulator):
     r"""2D manipulator robot
 
     References:
-        [1] https://github.com/domingoesteban/robolearn_robots_ros
+        - [1] https://github.com/domingoesteban/robolearn_robots_ros
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/manipulator2d/manipulator2d.urdf'):
+        """
+        Initialize the 2D manipulator.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)

pyrobolearn/robots/minitaur.py

@@ -23,20 +23,26 @@ class Minitaur(QuadrupedRobot):
     Minitaur robot from Ghost Robotics (https://www.ghostrobotics.io/)
 
     References:
-        [1] "Design Principles for a Family of Direct-Drive Legged Robots", Kenneally et al., 2016
-        [2] pybullet/gym/pybullet_envs/bullet/minitaur.py
-        [3] https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_envs/bullet/minitaur.py
+        - [1] "Design Principles for a Family of Direct-Drive Legged Robots", Kenneally et al., 2016
+        - [2] pybullet/gym/pybullet_envs/bullet/minitaur.py
+        - [3] https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_envs/bullet/minitaur.py
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, .3),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
-                 couple_legs=True,
-                 foot_friction=1.,
-                 urdf=os.path.dirname(__file__) + '/urdfs/minitaur/minitaur.urdf'):
+    def __init__(self, simulator, position=(0, 0, .3), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 couple_legs=True, foot_friction=1., urdf=os.path.dirname(__file__) + '/urdfs/minitaur/minitaur.urdf'):
+        """
+        Initialize the Minitaur robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            couple_legs (bool): if True, it will couple the legs by setting a constraint between two legs.
+            foot_friction (float): foot friction value.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.3)

pyrobolearn/robots/mkz.py

@@ -22,18 +22,24 @@ class MKZ(AckermannWheeledRobot):
     Drive-by-wire interface to the Dataspeed Inc. Lincoln MKZ DBW kit.
 
     References:
-        [1] Dataspeed Inc.: https://www.dataspeedinc.com/
-        [2] ROS wiki: http://wiki.ros.org/dbw_mkz
-        [3] Bitbucket: https://bitbucket.org/DataspeedInc/dbw_mkz_ros
+        - [1] Dataspeed Inc.: https://www.dataspeedinc.com/
+        - [2] ROS wiki: http://wiki.ros.org/dbw_mkz
+        - [3] Bitbucket: https://bitbucket.org/DataspeedInc/dbw_mkz_ros
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, .4),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, .4), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/mkz/mkz.urdf'):
+        """
+        Initialize the MKZ car.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.4)

pyrobolearn/robots/morphex.py

@@ -19,17 +19,23 @@ class Morphex(HexapodRobot):
     r"""Morphex Hexapod robot
 
     References:
-        [1] http://zentasrobots.com/
-        [2] https://gist.github.com/lanius/cb8b5e0ede9ff3b2b2c1bc68b95066fb
+        - [1] http://zentasrobots.com/
+        - [2] https://gist.github.com/lanius/cb8b5e0ede9ff3b2b2c1bc68b95066fb
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.2),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.2), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/morphex/morphex.urdf'):
+        """
+        Initialize the Morphex robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.2)

pyrobolearn/robots/nao.py

@@ -20,15 +20,24 @@ __status__ = "Development"
 class Nao(BipedRobot, BiManipulator, TwoHand):
     r"""Nao robot
 
+    References:
+        - [1] https://github.com/ros-naoqi/nao_robot
+        - [2] https://github.com/ros-naoqi/nao_meshes
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.35),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.35), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/nao/nao_v40.urdf'):
+        """
+        Initialize the Nao robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.35)

pyrobolearn/robots/opendog.py

@@ -19,17 +19,23 @@ class OpenDog(QuadrupedRobot):
     r""" OpenDog robot
 
     References:
-        [1] https://github.com/XRobots/openDog
-        [2] https://github.com/wiccopruebas/opendog_project
+        - [1] https://github.com/XRobots/openDog
+        - [2] https://github.com/wiccopruebas/opendog_project
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, .6),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, .6), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/opendog/opendog.urdf'):
+        """
+        Initialize the OpenDog robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.6)

pyrobolearn/robots/pepper.py

@@ -26,16 +26,25 @@ class Pepper(WheeledRobot, BiManipulator):
     for these joints are probably not correct.
 
     For more information:
-        [1] http://doc.aldebaran.com/2-0/home_juliette.html
+        - [1] http://doc.aldebaran.com/2-0/home_juliette.html
+
+    References:
+        - [2] https://github.com/ros-naoqi/pepper_robot
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.9),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.9), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/pepper/pepper.urdf'):
+        """
+        Initialize the Pepper robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.9)

pyrobolearn/robots/phantomx.py

@@ -19,17 +19,23 @@ class PhantomX(HexapodRobot):
     r"""Phantom X Hexapod robot
 
     References:
-        [1] https://www.trossenrobotics.com/phantomx-ax-hexapod.aspx
-        [2] https://github.com/HumaRobotics/phantomx_description
+        - [1] https://www.trossenrobotics.com/phantomx-ax-hexapod.aspx
+        - [2] https://github.com/HumaRobotics/phantomx_description
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.2),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.2), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/phantomx/phantomx.urdf'):
+        """
+        Initialize the PhantomX robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.2)

pyrobolearn/robots/pleurobot.py

@@ -21,12 +21,23 @@ class Pleurobot(QuadrupedRobot, UUVRobot, USVRobot):
     r"""Pleurobot Salamander robot
 
     References:
-        [1] https://github.com/KM-RoBoTa/pleurobot_ros_pkg
-        [2] https://biorob.epfl.ch/pleurobot
+        - [1] https://github.com/KM-RoBoTa/pleurobot_ros_pkg
+        - [2] https://biorob.epfl.ch/pleurobot
     """
 
     def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/pleurobot/pleurobot.urdf'):
+        """
+        Initialize the Pleurobot robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)

pyrobolearn/robots/pr2.py

@@ -6,6 +6,8 @@ import os
 
 from pyrobolearn.robots.wheeled_robot import WheeledRobot
 from pyrobolearn.robots.manipulator import BiManipulator
+from pyrobolearn.robots.gripper import AngularGripper
+
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -20,17 +22,23 @@ class PR2(WheeledRobot, BiManipulator):
     r"""PR2 robot
 
     References:
-        [1] http://www.willowgarage.com/pages/pr2/overview
-        [2] https://github.com/pr2/pr2_common
+        - [1] http://www.willowgarage.com/pages/pr2/overview
+        - [2] https://github.com/pr2/pr2_common
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/pr2/pr2.urdf'):
+        """
+        Initialize the PR2 robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)
@@ -45,6 +53,41 @@ class PR2(WheeledRobot, BiManipulator):
         self.name = 'pr2'
 
 
+class PR2Gripper(AngularGripper):
+    r"""PR2 Gripper
+
+    References:
+        - [1] http://www.willowgarage.com/pages/pr2/overview
+        - [2] https://github.com/pr2/pr2_common
+    """
+
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, -0.707, 0, 0.707), fixed_base=True, scale=1.,
+                 urdf=os.path.dirname(os.path.abspath(__file__)) + '/urdfs/pr2/pr2_gripper.urdf'):
+        """
+        Initialize the PR2 Gripper.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the gripper will be fixed in the world.
+            scale (float): scaling factor that is used to scale the gripper.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
+        # 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.707, 0, 0.707)
+        if fixed_base is None:
+            fixed_base = True
+
+        super(PR2Gripper, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'pr2_gripper'
+
+
 # Test
 if __name__ == "__main__":
     from itertools import count

pyrobolearn/robots/quadcopter.py

@@ -23,7 +23,8 @@ __status__ = "Development"
 class Quadcopter(RotaryWingUAV):
     r"""Quadcopter
 
-    WARNING: Currently, in pybullet there is no air, so we simulate the thrust force.
+    WARNING: If the given simulator can not simulate air dynamics, then we compute and simulate the thrust force.
+    For instance, in pybullet there is no air, so we simulate the thrust force with that simulator.
 
     Based on momentum theory, we can calculate the thrust [5,6,7] to be:
 
@@ -53,27 +54,33 @@ class Quadcopter(RotaryWingUAV):
     * the air density :math:`\rho` is :math:`1.225kg/m^3` at sea level and at :math:`15C`.
 
     References:
-        [1] https://www.wilselby.com/research/ros-integration/
-        [2] https://github.com/wilselby/ROS_quadrotor_simulator
-        [3] https://github.com/prfraanje/quadcopter_sim
-        [4] https://github.com/ethz-asl/rotors_simulator
+        - [1] https://www.wilselby.com/research/ros-integration/
+        - [2] https://github.com/wilselby/ROS_quadrotor_simulator
+        - [3] https://github.com/prfraanje/quadcopter_sim
+        - [4] https://github.com/ethz-asl/rotors_simulator
 
-        [5] "Propeller Thrust" (NASA): https://www.grc.nasa.gov/WWW/K-12/airplane/propth.html
-        [6] "Static thrust calculation": https://quadcopterproject.wordpress.com/static-thrust-calculation/
-        [7] "Propeller Static & Dynamic Thrust Calculation":
+        - [5] "Propeller Thrust" (NASA): https://www.grc.nasa.gov/WWW/K-12/airplane/propth.html
+        - [6] "Static thrust calculation": https://quadcopterproject.wordpress.com/static-thrust-calculation/
+        - [7] "Propeller Static & Dynamic Thrust Calculation":
             https://www.electricrcaircraftguy.com/2013/09/propeller-static-dynamic-thrust-equation.html
             https://www.electricrcaircraftguy.com/2014/04/propeller-static-dynamic-thrust-equation-background.html
-        [8] "Flying Principle of a Quadrotor" (from the course "Autonomous Navigation for Flying Robots" on EdX),
+        - [8] "Flying Principle of a Quadrotor" (from the course "Autonomous Navigation for Flying Robots" on EdX),
             Jurgen, https://jsturm.de/publications/data/lecture_1_part_3.pdf
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.2),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.2), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/quadcopter/quadcopter.urdf'):
+        """
+        Initialize the quadcopter robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.2)
@@ -167,25 +174,26 @@ class Quadcopter(RotaryWingUAV):
         # call parent method
         super(Quadcopter, self).set_joint_velocities(velocities, joint_ids, forces, max_velocity)
 
-        # calculate thrust force of the given joints, and apply it on the link
-        for jnt, d, v in zip(joint_ids, self.propeller_directions, velocities):
-            if max_velocity and v > self.max_velocity:
-                v = self.max_velocity
+        # if the simulator can not simulate air, calculate thrust force of the given joints, and apply it on the link
+        if not self.simulator.simulate_gas_dynamics():
+            for jnt, d, v in zip(joint_ids, self.propeller_directions, velocities):
+                if max_velocity and v > self.max_velocity:
+                    v = self.max_velocity
 
-            # compute propeller speed v0
-            state = self.sim.get_link_state(self.id, jnt, compute_velocity=True)  # , compute_forward_kinematics=True)
-            R = get_matrix_from_quaternion(state[1])
-            linear_velocity = np.array(state[-2])
-            propeller_up_vec = R.dot(np.array([0., 0., 1.]))
-            v0 = linear_velocity.dot(propeller_up_vec)
-            # v0 = 0    # static thrust
+                # compute propeller speed v0
+                state = self.sim.get_link_state(self.id, jnt, compute_velocity=True)  # , compute_forward_kinematics=True)
+                R = get_matrix_from_quaternion(state[1])
+                linear_velocity = np.array(state[-2])
+                propeller_up_vec = R.dot(np.array([0., 0., 1.]))
+                v0 = linear_velocity.dot(propeller_up_vec)
+                # v0 = 0    # static thrust
 
-            # compute thrust
-            f = self.calculate_thrust_force(v * d, self.area, self.propeller_pitch, v0)
-            # f = self.mass * self.gravity / 4.
+                # compute thrust
+                f = self.calculate_thrust_force(v * d, self.area, self.propeller_pitch, v0)
+                # f = self.mass * self.gravity / 4.
 
-            # apply force in the simulation
-            self.apply_external_force(force=[0, 0, f], link_id=jnt, position=(0., 0., 0.))
+                # apply force in the simulation
+                self.apply_external_force(force=[0, 0, f], link_id=jnt, position=(0., 0., 0.))
 
     def get_stationary_joint_velocity(self):
         fg = self.mass * self.gravity / 4.

pyrobolearn/robots/rhex.py

@@ -24,18 +24,24 @@ class Rhex(HexapodRobot):
     It was created by researchers at the University of Michigan and McGill University.
 
     References:
-        [1] https://robots.ieee.org/robots/rhex/
-        [2] https://www.rhex.web.tr/
-        [3] https://github.com/grafoteka/rhex
+        - [1] https://robots.ieee.org/robots/rhex/
+        - [2] https://www.rhex.web.tr/
+        - [3] https://github.com/grafoteka/rhex
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.12),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.12), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/rhex/rhex.urdf'):
+        """
+        Initialize the Rhex robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.12)

pyrobolearn/robots/robot.py

@@ -1505,6 +1505,48 @@ class Robot(ControllableBody):
         pass
 
     def get_link_accelerations(self, link_ids=None):
+        r"""
+
+        Args:
+            link_ids:
+
+        Returns:
+
+        """
+        pass
+
+    def get_link_world_accelerations(self, link_ids=None, flatten=True):
+        r"""
+        Return the linear and angular accelerations (expressed in the Cartesian world space coordinates) for the given
+        link(s).
+
+        The acceleration of a link can be computed in a recursive form:
+
+        .. math:: a_i = a_{i-1} + s
+
+        or from the base to the link:
+
+        .. math::
+
+        or based on the Jacobian:
+
+        .. math:: a = \frac{d}{dt} v = \frac{d}{dt} J(q) \dot{q} = J(q) \ddot{q} + \dot{J}(q) \dot{q},
+
+        where :math:`J(q)` is the link Jacobian at the current configuration (i.e. joint positions) :math:`q`,
+        :math:`\dot{q}` and :math:`\ddot{q}` are the current joint velocities and accelerations respectively, and
+        :math:`\dot{J}(q)` is the time derivative of the link Jacobian.
+
+        Args:
+            link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the linear and angular
+                velocities of all links associated to actuated joints.
+            flatten (bool): if True, it will return a 1D array instead of a 2D array
+
+        Returns:
+            if 1 link:
+                np.array[6]: linear and angular velocity of the link in the Cartesian world space
+            if multiple links:
+                np.array[Nx6], np.array[N,6]: linear and angular velocity of each link
+        """
         pass
 
     def get_link_contacts(self, link_ids):

pyrobolearn/robots/rrbot.py

@@ -23,13 +23,19 @@ class RRBot(Manipulator):
     for these joints are probably not correct.
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=True,
-                 scale=1,
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=True, scale=1,
                  urdf=os.path.dirname(__file__) + '/urdfs/rrbot/rrbot.urdf'):
+        """
+        Initialize the RRBot robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)

pyrobolearn/robots/sawyer.py

@@ -22,17 +22,23 @@ class Sawyer(Manipulator, WheeledRobot):
     Sawyer robot built by Rethink Robotics.
 
     References:
-        [1] Rethink Robotics
-        [2] https://github.com/RethinkRobotics/sawyer_robot
+        - [1] Rethink Robotics
+        - [2] https://github.com/RethinkRobotics/sawyer_robot
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.92),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=True,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.92), orientation=(0, 0, 0, 1), fixed_base=True, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/sawyer/sawyer.urdf'):
+        """
+        Initialize the Sawyer robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.92)

pyrobolearn/robots/schunk_hand.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python
+"""Provide the Schunk hand robotic platform.
+"""
+
+import os
+
+from pyrobolearn.robots.hand import Hand
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class SchunkHand(Hand):
+    r"""Schunk Hand
+
+    References:
+        - [1] https://github.com/fzi-forschungszentrum-informatik/schunk_svh_driver
+        - [2] https://github.com/ADVRHumanoids/centauro-simulator
+    """
+
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=True, scale=1.):
+        # left=False
+        """
+        Initialize the Schunk hand.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the hand base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the hand.
+        """
+        # 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 = True
+
+        # if left:
+        #     urdf_path = '../robots/urdfs/centauro/schunk_left_hand.urdf'
+        # else:
+        urdf_path = os.path.dirname(__file__) + '/urdfs/centauro/schunk_hand.urdf'
+
+        super(SchunkHand, self).__init__(simulator, urdf_path, position, orientation, fixed_base, scale)
+        self.name = 'schunk_hand'
+
+
+# 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
+    right_hand = SchunkHand(sim)
+
+    # print information about the robot
+    right_hand.print_info()
+    # H = right_hand.get_mass_matrix()
+    # print("Inertia matrix: H(q) = {}".format(H))
+
+    # Position control using sliders
+    right_hand.add_joint_slider()
+
+    for i in count():
+        right_hand.update_joint_slider()
+        # right_hand.set_joint_positions([0.] * right_hand.num_dofs)
+
+        # step in simulation
+        world.step(sleep_dt=1./240)

pyrobolearn/robots/sea_hexapod.py

@@ -19,16 +19,22 @@ class SEAHexapod(HexapodRobot):
     r"""SEA Hexapod robot (from CMU Biorobotics Lab)
 
     References:
-        [1] https://github.com/alexansari101/snake_ws
+        - [1] https://github.com/alexansari101/snake_ws
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.15),
-                 orientation=(0, 0, 0.707, 0.707),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.15), orientation=(0, 0, 0.707, 0.707), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/cmu_sea/hexapod.urdf'):
+        """
+        Initialize the SEA Hexapod robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.15)

pyrobolearn/robots/sea_snake.py

@@ -19,16 +19,22 @@ class SEASnake(Robot):
     r"""SEA snake robot (from CMU Biorobotics Lab)
 
     References:
-        [1] https://github.com/alexansari101/snake_ws
+        - [1] https://github.com/alexansari101/snake_ws
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(-0.5, 0, 0.1),
-                 orientation=(0, 0.707, 0, 0.707),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(-0.5, 0, 0.1), orientation=(0, 0.707, 0, 0.707), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/cmu_sea/snake.urdf'):
+        """
+        Initialize the SEA snake robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (-0.5, 0., 0.1)

pyrobolearn/robots/shadowhand.py

@@ -19,18 +19,24 @@ class ShadowHand(Hand):
     r"""Shadow Hand
 
     References:
-        [1] https://www.shadowrobot.com/products/dexterous-hand/
-        [2] Shadow hand description: https://github.com/shadow-robot/sr_common
-        [3] Documentation: https://dexterous-hand.readthedocs.io/en/latest/index.html
+        - [1] https://www.shadowrobot.com/products/dexterous-hand/
+        - [2] Shadow hand description: https://github.com/shadow-robot/sr_common
+        - [3] Documentation: https://dexterous-hand.readthedocs.io/en/latest/index.html
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0.707, 0.707),
-                 scale=1.,
-                 left=True,
-                 fixed_base=True):
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0.707, 0.707), fixed_base=True, scale=1.,
+                 left=True):
+        """
+        Initialize the Shadow hand robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            left (bool): if we should create a left hand, or right hand.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)

pyrobolearn/robots/softhand.py

@@ -20,17 +20,22 @@ class SoftHand(Hand):
     r"""Pisa-IIT Soft Hand
 
     References:
-        [1] https://github.com/CentroEPiaggio/pisa-iit-soft-hand
-        [2] "Adaptive Synergies for the Design and Control of the Pisa/IIT SoftHand", Catalano et al., 2014
+        - [1] https://github.com/CentroEPiaggio/pisa-iit-soft-hand
+        - [2] "Adaptive Synergies for the Design and Control of the Pisa/IIT SoftHand", Catalano et al., 2014
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 scale=1.,
-                 left=True,
-                 fixed_base=True):
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=True, scale=1., left=True):
+        """
+        Initialize the Soft hand robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            left (bool): if we should create a left hand, or right hand.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)

pyrobolearn/robots/swimmer.py

@@ -19,15 +19,10 @@ class Swimmer(Robot):
     r"""Swimmer Mujoco Model
 
     References:
-        [1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
+        - [1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(-0.5, 0, 0.1),
-                 orientation=(0, 0.707, 0, 0.707),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(-0.5, 0, 0.1), orientation=(0, 0.707, 0, 0.707), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/mjcfs/swimmer.xml'):
         # check parameters
         if position is None:

pyrobolearn/robots/techpod.py

@@ -29,18 +29,24 @@ class Techpod(FixedWingUAV):
     is carried out by pybullet.
 
     References:
-        [1] https://github.com/ethz-asl/rotors_simulator
-        [2] "Theory of flight": web.mit.edu/16.00/www/aec/flight.html
-        [3] "NASA: Guided Tours of the BGA": www.grc.nasa.gov/WWW/k-12/airplane/guided.html
+        - [1] https://github.com/ethz-asl/rotors_simulator
+        - [2] "Theory of flight": web.mit.edu/16.00/www/aec/flight.html
+        - [3] "NASA: Guided Tours of the BGA": www.grc.nasa.gov/WWW/k-12/airplane/guided.html
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.5),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scaling=1.,
+    def __init__(self, simulator, position=(0, 0, 0.5), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/techpod/techpod.urdf'):
+        """
+        Initialize the Techpod robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.5)
@@ -51,7 +57,7 @@ class Techpod(FixedWingUAV):
         if fixed_base is None:
             fixed_base = False
 
-        super(Techpod, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling)
+        super(Techpod, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
         self.name = 'techpod'
 
         # info

pyrobolearn/robots/uav.py

@@ -21,6 +21,17 @@ class UAVRobot(Robot):
     """
 
     def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1.):
+        """
+        Initialize the UAV.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(UAVRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
 
         self.propellers = []  # list of propellers id
@@ -43,6 +54,17 @@ class FixedWingUAV(UAVRobot):
     """
 
     def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1.):
+        """
+        Initialize the Fixed wing UAV.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(FixedWingUAV, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
 
 
@@ -52,6 +74,17 @@ class RotaryWingUAV(UAVRobot):
     """
 
     def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1.):
+        """
+        Initialize the rotary UAV.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(RotaryWingUAV, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
 
     def hover(self):
@@ -94,4 +127,15 @@ class FlappingWingUAV(UAVRobot):
     """
 
     def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1.):
+        """
+        Initialize the flapping UAV.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(FlappingWingUAV, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)

pyrobolearn/robots/urdfs/allegrohand/allegro_right_hand.urdf

@@ -22,7 +22,7 @@ and one fixed link will be added for the tip. -->
   <!-- ============================================================================= -->
   <!-- ============================================================================= -->
   <!-- BASE -->
-  <link name="box_link">
+  <!-- <link name="box_link">
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
@@ -47,7 +47,7 @@ and one fixed link will be added for the tip. -->
     <parent link="box_link"/>
     <child link="base_link"/>
     <origin xyz="0 0 0.145"/>
-  </joint>
+  </joint> -->
 
   <link name="base_link">
     <visual>

pyrobolearn/robots/urdfs/baxter/baxter_gripper.urdf

@@ -0,0 +1,326 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from /home/brian/catkin_baxter/src/baxter_common/baxter_description/urdf/baxter.urdf.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="baxter_gripper" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  
+  <link name="right_wrist">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/wrist/W2.STL"/>
+      </geometry>
+      <material name="lightgrey">
+        <color rgba=".1 .1 .1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.06"/>
+      <geometry>
+        <cylinder length="0.0165" radius="0.06"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.00198 0.00125 0.01855"/>
+      <mass value="0.35093"/>
+      <inertia ixx="0.00025289155" ixy="0.00000575311" ixz="-0.00000159345" iyy="0.0002688601" iyz="-0.00000519818" izz="0.0003074118"/>
+    </inertial>
+  </link>
+  <link name="right_hand">
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.0232"/>
+      <geometry>
+        <cylinder length="0.0464" radius="0.04"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.01093 0.00042 -0.01532"/>
+      <mass value="0.19125"/>
+      <inertia ixx="0.00017588" ixy="0.00000147073" ixz="0.0000243633" iyy="0.00021166377" iyz="0.00000172689" izz="0.00023745397"/>
+    </inertial>
+  </link>
+  <link name="right_hand_camera">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.01" radius="0.02"/>
+      </geometry>
+      <material name="blue">
+        <color rgba="0 0 1 0.8"/>
+      </material>
+    </visual>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.0001"/>
+      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
+    </inertial>
+  </link>
+  <link name="right_hand_camera_axis">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.0001"/>
+      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
+    </inertial>
+  </link>
+  <link name="right_hand_range">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.005 .02 .005"/>
+      </geometry>
+      <material name="blue">
+        <color rgba="0 0 1 0.8"/>
+      </material>
+    </visual>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.0001"/>
+      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
+    </inertial>
+  </link>
+  <link name="right_hand_accelerometer">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+      <material name="black">
+        <color rgba="0 0 0 1"/>
+      </material>
+    </visual>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
+      <mass value="0.0001"/>
+      <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
+    </inertial>
+  </link>
+
+  <joint name="right_hand" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.11355"/>
+    <axis xyz="0 0 1"/>
+    <parent link="right_wrist"/>
+    <child link="right_hand"/>
+  </joint>
+  <joint name="right_hand_camera" type="fixed">
+    <origin rpy="0 0 -1.57079633" xyz="0.03825 0.012 0.015355"/>
+    <parent link="right_hand"/>
+    <child link="right_hand_camera"/>
+  </joint>
+  <joint name="right_hand_camera_axis" type="fixed">
+    <origin rpy="0 0 0" xyz="0.03825 0.012 0.015355"/>
+    <parent link="right_hand"/>
+    <child link="right_hand_camera_axis"/>
+  </joint>
+  <joint name="right_hand_range" type="fixed">
+    <origin rpy="0 -1.57079632679 -1.57079632679" xyz="0.032 -0.020245 0.0288"/>
+    <parent link="right_hand"/>
+    <child link="right_hand_range"/>
+  </joint>
+  <joint name="right_hand_accelerometer" type="fixed">
+    <origin rpy="0 0 0" xyz="0.00198 0.000133 -0.0146"/>
+    <parent link="right_hand"/>
+    <child link="right_hand_accelerometer"/>
+  </joint>
+
+  <!-- Base of end effector -->
+  <link name="right_gripper_base">
+    <visual>
+      <origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/electric_gripper/electric_gripper_base.STL"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="-1.57079632679 3.14159265359 0" xyz="0.0 0.0 0"/>
+      <geometry>
+        <cylinder length="0.1" radius="0.029"/>
+      </geometry>
+      <material name="darkred">
+        <color rgba=".5 .1 .1 1"/>
+      </material>
+    </collision>
+    <inertial>
+      <origin rpy="-1.57079632679 3.14159265359 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.3"/>
+      <inertia ixx="2e-08" ixy="0" ixz="0" iyy="3e-08" iyz="0" izz="2e-08"/>
+    </inertial>
+  </link>
+  <link name="r_gripper_l_finger">
+    <visual>
+      <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/electric_gripper/fingers/extended_narrow.STL"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -3.14159265359" xyz="0 0.01725 0.0615"/>
+      <geometry>
+        <box size="0.01 0.0135 0.1127"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 -3.14159265359" xyz="-0.005 -0.003 0.0083"/>
+      <geometry>
+        <box size="0.01 0.05 0.017"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
+      <mass value="0.02"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+    </inertial>
+  </link>
+  <gazebo reference="r_gripper_l_finger">
+    <mu1>1000</mu1>
+    <mu2>1000</mu2>
+    <fdir1>0.0 0.0 1.0</fdir1>
+    <kp>1e5</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <link name="r_gripper_l_finger_tip">
+    <visual>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/electric_gripper/fingers/half_round_tip.STL"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14159265359" xyz="0 -0.0045 -0.015"/>
+      <geometry>
+        <cylinder length="0.037" radius="0.008"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <mass value="0.01"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+    </inertial>
+  </link>
+  <joint name="r_gripper_l_finger_tip_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 0.01725 0.1127"/>
+    <parent link="r_gripper_l_finger"/>
+    <child link="r_gripper_l_finger_tip"/>
+  </joint>
+  <link name="r_gripper_r_finger">
+    <visual>
+      <origin rpy="0 0 -0.0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/electric_gripper/fingers/extended_narrow.STL"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.0" xyz="0 -0.01725 0.0615"/>
+      <geometry>
+        <box size="0.01 0.0135 0.1127"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 -0.0" xyz="0.005 0.003 0.0083"/>
+      <geometry>
+        <box size="0.01 0.05 0.017"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 -0.0" xyz="0 0 0"/>
+      <mass value="0.02"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+    </inertial>
+  </link>
+  <gazebo reference="r_gripper_r_finger">
+    <mu1>1000</mu1>
+    <mu2>1000</mu2>
+    <fdir1>0.0 0.0 1.0</fdir1>
+    <kp>1e5</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <link name="r_gripper_r_finger_tip">
+    <visual>
+      <origin rpy="0 0 0.0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/electric_gripper/fingers/half_round_tip.STL"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0.0" xyz="0 0.0045 -0.015"/>
+      <geometry>
+        <cylinder length="0.037" radius="0.008"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0.0" xyz="0 0 0"/>
+      <mass value="0.01"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+    </inertial>
+  </link>
+  <joint name="r_gripper_r_finger_tip_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 -0.01725 0.1127"/>
+    <parent link="r_gripper_r_finger"/>
+    <child link="r_gripper_r_finger_tip"/>
+  </joint>
+  <!-- Electric Gripper Base Joint -->
+  <joint name="right_gripper_base" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.025"/>
+    <parent link="right_hand"/>
+    <child link="right_gripper_base"/>
+  </joint>
+  <!-- Electric Gripper Tip joint -->
+  <joint name="right_endpoint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.1327"/>
+    <parent link="right_gripper_base"/>
+    <child link="right_gripper"/>
+  </joint>
+  <!-- Electric Gripper Tip link -->
+  <link name="right_gripper">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.0001"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0.0"/>
+    </inertial>
+  </link>
+  <joint name="r_gripper_l_finger_joint" type="prismatic">
+    <origin rpy="0 0 0" xyz="0.0 -0.0015 0.02"/>
+    <axis xyz="0 1 0"/>
+    <parent link="right_gripper_base"/>
+    <child link="r_gripper_l_finger"/>
+    <limit effort="20.0" lower="0.0" upper="0.020833" velocity="5.0"/>
+    <dynamics damping="0.7" friction="0.0"/>
+  </joint>
+  <joint name="r_gripper_r_finger_joint" type="prismatic">
+    <origin rpy="0 0 0" xyz="0.0 0.0015 0.02"/>
+    <axis xyz="0 1 0"/>
+    <parent link="right_gripper_base"/>
+    <child link="r_gripper_r_finger"/>
+    <limit effort="20.0" lower="-0.020833" upper="0.0" velocity="5.0"/>
+    <mimic joint="r_gripper_l_finger_joint" multiplier="-1.0"/>
+    <dynamics damping="0.7" friction="0.0"/>
+  </joint>
+  <transmission name="gripper_right1">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="r_gripper_r_finger_joint">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="gripper_r1_motor1">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="gripper_right2">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="r_gripper_l_finger_joint">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="gripper_r1_motor2">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <gazebo reference="r_gripper_r_finger_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="r_gripper_l_finger_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+
+</robot>
+

pyrobolearn/robots/urdfs/fetch/fetch_gripper.urdf

@@ -0,0 +1,81 @@
+<?xml version="1.0" ?>
+<robot name="fetch_gripper">
+  
+  <link name="gripper_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0900 -0.0001 -0.0017" />
+      <mass value="1.5175" />
+      <inertia ixx="0.0013" ixy="-0.0" ixz="0.0" iyy="0.0019" iyz="-0.0" izz="0.0024" />
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/gripper_link.dae" />
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/gripper_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="r_gripper_finger_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.01 0 0" />
+      <mass value="0.0798" />
+      <inertia ixx="0.002" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0.101425 0" />
+      <geometry>
+        <mesh filename="meshes/r_gripper_finger_link.STL" />
+      </geometry>
+      <material name="a">
+        <color rgba="0.356 0.361 0.376 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0.101425 0" />
+      <geometry>
+        <mesh filename="meshes/r_gripper_finger_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="r_gripper_finger_joint" type="prismatic">
+    <origin rpy="0 0 0" xyz="0 0.015425 0" />
+    <parent link="gripper_link" />
+    <child link="r_gripper_finger_link" />
+    <axis xyz="0 1 0" />
+  <limit effort="60" lower="0.0" upper="0.05" velocity="0.05" /><dynamics damping="100.0" /></joint>
+  <link name="l_gripper_finger_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.01 0 0" />
+      <mass value="0.0798" />
+      <inertia ixx="0.002" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 -0.101425 0" />
+      <geometry>
+        <mesh filename="meshes/l_gripper_finger_link.STL" />
+      </geometry>
+      <material name="a">
+        <color rgba="0.356 0.361 0.376 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 -0.101425 0" />
+      <geometry>
+        <mesh filename="meshes/l_gripper_finger_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="l_gripper_finger_joint" type="prismatic">
+    <origin rpy="0 0 0" xyz="0 -0.015425 0" />
+    <parent link="gripper_link" />
+    <child link="l_gripper_finger_link" />
+    <axis xyz="0 -1 0" />
+  <limit effort="60" lower="0.0" upper="0.05" velocity="0.05" /><dynamics damping="100.0" />
+  </joint>
+
+</robot>

pyrobolearn/robots/urdfs/franka/franka_gripper.urdf

@@ -0,0 +1,151 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from panda_arm_hand.urdf.xacro      | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <material name="black">
+    <color rgba="0.0 0.0 0.0 1"/>
+  </material>
+  <material name="white">
+    <color rgba="0.9 0.9 0.9 1"/>
+  </material>
+  <material name="dark_gray">
+    <color rgba="0.3 0.3 0.3 1"/>
+  </material>
+  <material name="gray">
+    <color rgba="0.5 0.5 0.5 1"/>
+  </material>
+  <material name="light_gray">
+    <color rgba="0.7 0.7 0.7 1"/>
+  </material>
+  <material name="pure_red">
+    <color rgba="1.0 0.0 0.0 1"/>
+  </material>
+  <material name="red">
+    <color rgba="0.5 0.1 0.2 1"/>
+  </material>
+  <material name="pure_green">
+    <color rgba="0.0 1.0 0.0 1"/>
+  </material>
+  <material name="pure_blue">
+    <color rgba="0.0 0.0 1.0 1"/>
+  </material>
+  <material name="blue">
+    <color rgba="0.0 0.2 0.3 1"/>
+  </material>
+
+  <link name="panda_link7">
+    <inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
+      <mass value="1"/> <!-- volume = 0.000454 -->
+      <origin xyz="0.010969 0.010797 0.065041"/>
+      <!--inertia ixx="0.000001" ixy="0.000000" ixz="0.000000" iyy="0.000001" iyz="0.000000" izz="0.000001"/-->
+      <inertia ixx="0.00220264" ixy="0.000000" ixz="0.000000" iyy="0.00220264" iyz="0.000000" izz="0.00220264"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+    </inertial>
+    <visual>
+      <material name="white"/>
+      <geometry>
+        <mesh filename="meshes/visual/link7.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/link7.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="panda_link8">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+  </link> <!-- in pybullet, the inertia need to be specified even for dummy links, otherwise it will put an identity inertia and a mass of 1 by default -->
+  <joint name="panda_joint8" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.107"/>
+    <parent link="panda_link7"/>
+    <child link="panda_link8"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <joint name="panda_hand_joint" type="fixed">
+    <parent link="panda_link8"/>
+    <child link="panda_hand"/>
+    <origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
+  </joint>
+  <link name="panda_hand">
+    <inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
+      <mass value="1"/> <!-- volume = 0.000488 -->
+      <origin xyz="-0.000007 0.001524 0.027591"/>
+      <!--inertia ixx="0.000002" ixy="0.000000" ixz="0.000000" iyy="0.0000003" iyz="0.000000" izz="0.000002"/-->
+      <inertia ixx="0.00409836" ixy="0.000000" ixz="0.000000" iyy="0.00061475" iyz="0.000000" izz="0.00409836"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+    </inertial>
+    <visual>
+      <material name="white"/>
+      <geometry>
+        <mesh filename="meshes/visual/hand.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/hand.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="panda_leftfinger">
+    <inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
+      <mass value="1"/> <!-- volume = 0.000011 -->
+      <origin xyz="0.000002 0.014564 0.022794"/>
+      <!--inertia ixx="0.00000000338" ixy="0.000000" ixz="0.000000" iyy="0.000000000332" iyz="0.00000000060" izz="0.00000000078"/-->
+      <inertia ixx="0.00030727" ixy="0.000000" ixz="0.000000" iyy="0.00003018" iyz="0.00005455" izz="0.00007091"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+    </inertial>
+    <visual>
+      <material name="light_gray"/>
+      <geometry>
+        <mesh filename="meshes/visual/finger.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/collision/finger.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="panda_rightfinger">
+    <inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
+      <mass value="1"/> <!-- volume = 0.000011 -->
+      <origin xyz="0.000002 0.014564 0.022794"/>
+      <!--inertia ixx="0.00000000338" ixy="0.000000" ixz="0.000000" iyy="0.000000000332" iyz="0.00000000060" izz="0.00000000078"/-->
+      <inertia ixx="0.00030727" ixy="0.000000" ixz="0.000000" iyy="0.00003018" iyz="0.00005455" izz="0.00007091"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+    </inertial>
+    <visual>
+      <material name="light_gray"/>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/visual/finger.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/collision/finger.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="panda_finger_joint1" type="prismatic">
+    <parent link="panda_hand"/>
+    <child link="panda_leftfinger"/>
+    <origin rpy="0 0 0" xyz="0 0 0.0584"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
+  </joint>
+  <joint name="panda_finger_joint2" type="prismatic">
+    <parent link="panda_hand"/>
+    <child link="panda_rightfinger"/>
+    <origin rpy="0 0 0" xyz="0 0 0.0584"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
+    <mimic joint="panda_finger_joint1"/>
+  </joint>
+</robot>
+

pyrobolearn/robots/urdfs/jaco/jaco_gripper.urdf

@@ -0,0 +1,631 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from jaco_robot.urdf.xacro          | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="jaco_gripper" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
+  <!--link name="robot_root">
+  </link>
+  <!-- fake cylinder which is actually a box -->
+  <!--xacro:macro name="cyl_inertia" params="mass r h ">
+      <mass value="${mass}"/>
+      <inertia
+      ixx="${mass / 12.0 * (r*r + h*h)}" ixy="0.0" ixz="0.0"
+      iyy="${mass / 12.0 * (h*h + r*r)}" iyz="0.0"
+      izz="${mass / 12.0 * (r*r + r*r)}"/>
+  </xacro:macro-->
+  <!-- ***************** MODEL CORRECTIONS ************************* -->
+  <!-- *************************************************************** -->
+  <!-- from measurements, pitch is -0.2293 -->
+  <!-- MODEL_FIX transforms from wrist to finger mounts -->
+  <!--<xacro:property name="f_thb_m_xyz" value=" 0.037  0.001  0.106" />
+  <xacro:property name="f_idx_m_xyz" value="-0.034  0.022 0.106" />
+  <xacro:property name="f_pnk_m_xyz" value="-0.034 -0.022 0.106" />
+  <xacro:property name="f_thb_m_rpy" value="0 -0.2658 ${M_PI-0.226892}" /> 
+  <xacro:property name="f_idx_m_rpy" value="0 -0.2293 -0.191986" />
+  <xacro:property name="f_pnk_m_rpy" value="0 -0.2293 0.191986" />-->
+  <!--joint name="jaco_arm_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="robot_root"/>
+    <child link="jaco_0_baseA"/>
+  </joint>
+  <!-- for some reason, material only applies if full name specified...
+       these only seem to work in older gazebo versions -->
+  <gazebo reference="jaco_ring_">
+    <material>Gazebo/Grey</material>
+    <implicitSpringDamper>1</implicitSpringDamper>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="_limb">
+    <material>Gazebo/FlatBlack</material>
+    <implicitSpringDamper>1</implicitSpringDamper>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="_finger">
+    <material>Gazebo/Grey</material>
+    <implicitSpringDamper>1</implicitSpringDamper>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <!-- for some reason, material only applies if full name specified -->
+  <gazebo reference="jaco_0_baseA">
+    <material>Gazebo/White</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_0_baseB_limb">
+    <material>Gazebo/FlatBlack</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_ring_1">
+    <material>Gazebo/Grey</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_ring_2">
+    <material>Gazebo/Grey</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_ring_3">
+    <material>Gazebo/Grey</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_ring_4">
+    <material>Gazebo/Grey</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_ring_5">
+    <material>Gazebo/Grey</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_ring_6">
+    <material>Gazebo/Grey</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_1_shoulder_limb">
+    <material>Gazebo/FlatBlack</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_2_upperarm_limb">
+    <material>Gazebo/FlatBlack</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_3_forearm_limb">
+    <material>Gazebo/FlatBlack</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_4_upperwrist_limb">
+    <material>Gazebo/FlatBlack</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_5_lowerwrist_limb">
+    <material>Gazebo/FlatBlack</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_6_hand_limb">
+    <material>Gazebo/FlatBlack</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_fingers_base_link">
+    <material>Gazebo/FlatBlack</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_7_finger_mount_index">
+    <material>Gazebo/FlatBlack</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_7_finger_mount_thumb">
+    <material>Gazebo/FlatBlack</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_7_finger_mount_pinkie">
+    <material>Gazebo/FlatBlack</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_8_finger_thumb">
+    <material>Gazebo/Grey</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_8_finger_index">
+    <material>Gazebo/Grey</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_8_finger_pinkie">
+    <material>Gazebo/Grey</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_9_finger_thumb_tip">
+    <material>Gazebo/Grey</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_9_finger_index_tip">
+    <material>Gazebo/Grey</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_9_finger_pinkie_tip">
+    <material>Gazebo/Grey</material>
+    <mu1>100000</mu1>
+    <mu2>100000</mu2>
+    <kp>100000000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+  <gazebo reference="jaco_finger_joint_0">
+    <provideFeedback value="true"/>
+  </gazebo>
+  <gazebo reference="jaco_finger_joint_2">
+    <provideFeedback value="true"/>
+  </gazebo>
+  <gazebo reference="jaco_finger_joint_4">
+    <provideFeedback value="true"/>
+  </gazebo>
+
+  <!-- ...........................        -->
+  <link name="jaco_6_hand_limb">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0555917045329114 -7.25586546752172E-05 -0.00317411883668722"/>
+      <mass value="0.2"/>
+      <inertia ixx="0.0007852635256" ixy="0" ixz="0" iyy="0.0007852635256" iyz="0" izz="0.00121"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/6_hand_limb.STL" scale="1.0 1.0 1.0"/>
+      </geometry>
+      <material name="black">
+        <color rgba="0.1 0.1 0.1 1"/>
+      </material>
+    </visual>
+    <!--     <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.10 0.10 0.10 1" />
+      </material>
+    </visual>
+-->
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/6_hand_limb.STL" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="jaco_fingers_base_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.001"/>
+      <inertia ixx="3.33332e-08" ixy="0" ixz="0" iyy="3.33332e-08" iyz="0" izz="5e-08"/>
+    </inertial>
+  </link>
+  <!-- this joint is only to align the coordinate system with the kinova specification frame -->
+  <joint name="jaco_fingers_base_joint" type="fixed">
+    <origin rpy="-1.57079632679 3.14159265359 1.57079632679" xyz="0 0 0"/>
+    <parent link="jaco_6_hand_limb"/>
+    <child link="jaco_fingers_base_link"/>
+  </joint>
+  <!-- ...........................  -->
+  <!--      FINGERS          -->
+  <!-- ...........................  -->
+  <link name="jaco_7_finger_mount_index">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0.00706399246397285 -0.00176078508424846"/>
+      <mass value="0.006465"/>
+      <inertia ixx="7.8441686232e-07" ixy="0" ixz="0" iyy="7.8441686232e-07" iyz="0" izz="1.04733e-06"/>
+    </inertial>
+    <visual>
+      <origin rpy="1.57079632679 0 1.57079632679" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/7_finger_mount_index.STL"/>
+      </geometry>
+      <material name="grey">
+        <color rgba="0.5 0.5 0.5 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.57079632679 0 1.57079632679" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/7_finger_mount_index.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="jaco_finger_mount_index_fixed" type="fixed">
+    <!--- remark 2014: damping greater than 0.1 (0.2) lead to model break downs when testing it bit by bit -->
+    <!--- remark jan 2016: damping and friction higher than 0.005 required very high forces in order for the fingers
+              to move at all, and then they mooved only slowly. -->
+    <!--dynamics damping="0.1" friction="1.0"/-->
+    <dynamics damping="0.005" friction="0.005"/>
+    <origin rpy="0 -0.2293 -0.191986" xyz="-0.034  0.022 0.095"/>
+    <parent link="jaco_fingers_base_link"/>
+    <child link="jaco_7_finger_mount_index"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- ...........................        -->
+  <link name="jaco_8_finger_index">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0181403689626489 0 0.00272470933850985"/>
+      <mass value="0.01845"/>
+      <inertia ixx="6.37560262249e-06" ixy="0" ixz="0" iyy="6.37560262249e-06" iyz="0" izz="1.68125625e-06"/>
+    </inertial>
+    <!--     <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.10 0.10 0.10 1" />
+      </material>
+    </visual>
+-->
+    <visual>
+      <origin rpy="3.14159265359 1.1780972445 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/8_finger_index.STL"/>
+      </geometry>
+      <material name="grey">
+        <color rgba="0.50 0.50 0.50 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="3.14159265359 1.1780972445 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/8_finger_index.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="jaco_finger_joint_0" type="revolute">
+    <!--- remark 2014: damping greater than 0.1 (0.2) lead to model break downs when testing it bit by bit -->
+    <!--- remark jan 2016: damping and friction higher than 0.005 required very high forces in order for the fingers
+              to move at all, and then they mooved only slowly. -->
+    <!--dynamics damping="0.1" friction="1.0"/-->
+    <dynamics damping="0.005" friction="0.005"/>
+    <origin rpy="0 -0.34 -0.191986" xyz="-0.0276  0.0204 0.11"/>
+    <parent link="jaco_fingers_base_link"/>
+    <child link="jaco_8_finger_index"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="1.5" lower="-0.0045" upper="1.05" velocity="0.2"/>
+  </joint>
+  <!-- ...........................        -->
+  <link name="jaco_9_finger_index_tip">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.01340812509202 -4.52693266978291E-07 0.0118561361126153"/>
+      <mass value="0.0122"/>
+      <inertia ixx="2.6850059266e-06" ixy="0" ixz="0" iyy="2.6850059266e-06" iyz="0" izz="8.784e-07"/>
+    </inertial>
+    <!--     <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.10 0.10 0.10 1" />
+      </material>
+    </visual>
+-->
+    <visual>
+      <origin rpy="0 1.57079632679 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/9_finger_index_tip.STL" scale="1 1 1"/>
+      </geometry>
+      <material name="grey">
+        <color rgba="0.5 0.5 0.5 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 1.57079632679 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/9_finger_index_tip.STL" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="jaco_finger_joint_1" type="fixed">
+    <!--- remark 2014: damping greater than 0.1 (0.2) lead to model break downs when testing it bit by bit -->
+    <!--- remark jan 2016: damping and friction higher than 0.005 required very high forces in order for the fingers
+              to move at all, and then they mooved only slowly. -->
+    <!--dynamics damping="0.1" friction="1.0"/-->
+    <dynamics damping="0.005" friction="0.005"/>
+    <origin rpy="0 0 0" xyz="0 0 0.0441"/>
+    <parent link="jaco_8_finger_index"/>
+    <child link="jaco_9_finger_index_tip"/>
+  </joint>
+  <!-- ...........................        -->
+  <link name="jaco_7_finger_mount_thumb">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-2.65251193937339E-07 0.00706398229186052 -0.00176078674955921"/>
+      <mass value="0.006465"/>
+      <inertia ixx="7.8441686232e-07" ixy="0" ixz="0" iyy="7.8441686232e-07" iyz="0" izz="1.04733e-06"/>
+    </inertial>
+    <visual>
+      <origin rpy="1.57079632679 0 1.57079632679" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/7_finger_mount_thumb.STL"/>
+      </geometry>
+      <material name="grey">
+        <color rgba="0.50 0.50 0.50 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.57079632679 0 1.57079632679" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/7_finger_mount_thumb.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="jaco_finger_mount_thumb_fixed" type="fixed">
+    <!--- remark 2014: damping greater than 0.1 (0.2) lead to model break downs when testing it bit by bit -->
+    <!--- remark jan 2016: damping and friction higher than 0.005 required very high forces in order for the fingers
+              to move at all, and then they mooved only slowly. -->
+    <!--dynamics damping="0.1" friction="1.0"/-->
+    <dynamics damping="0.005" friction="0.005"/>
+    <origin rpy="0 -0.2658 2.91470065359" xyz=" 0.037  0.001  0.095"/>
+    <parent link="jaco_fingers_base_link"/>
+    <child link="jaco_7_finger_mount_thumb"/>
+  </joint>
+  <!-- ...........................        -->
+  <link name="jaco_8_finger_thumb">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0181403317517619 -3.13818432741766E-07 0.00272473343106584"/>
+      <mass value="0.0184"/>
+      <inertia ixx="6.3583245666e-06" ixy="0" ixz="0" iyy="6.3583245666e-06" iyz="0" izz="1.6767e-06"/>
+    </inertial>
+    <!--     <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.10 0.10 0.10 1" />
+      </material>
+    </visual>
+-->
+    <visual>
+      <origin rpy="3.14159265359 1.1780972445 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/8_finger_thumb.STL"/>
+      </geometry>
+      <material name="grey">
+        <color rgba="0.5 0.5 0.5 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="3.14159265359 1.1780972445 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/8_finger_thumb.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="jaco_finger_joint_2" type="revolute">
+    <!--- remark 2014: damping greater than 0.1 (0.2) lead to model break downs when testing it bit by bit -->
+    <!--- remark jan 2016: damping and friction higher than 0.005 required very high forces in order for the fingers
+              to move at all, and then they mooved only slowly. -->
+    <!--dynamics damping="0.1" friction="1.0"/-->
+    <dynamics damping="0.005" friction="0.005"/>
+    <origin rpy="0 -0.37 2.91470065359" xyz=" 0.0319  0.003  0.11"/>
+    <parent link="jaco_fingers_base_link"/>
+    <child link="jaco_8_finger_thumb"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="1.5" lower="-0.0045" upper="1.05" velocity="0.2"/>
+  </joint>
+  <!-- ...........................        -->
+  <link name="jaco_9_finger_thumb_tip">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0134080639046228 -4.56622786070351E-07 0.0118561154422042"/>
+      <mass value="0.0122"/>
+      <inertia ixx="2.6850059266e-06" ixy="0" ixz="0" iyy="2.6850059266e-06" iyz="0" izz="8.784e-07"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57079632679 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/9_finger_thumb_tip.STL" scale="1 1 1"/>
+      </geometry>
+      <material name="grey">
+        <color rgba="0.5 0.5 0.5 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 1.57079632679 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/9_finger_thumb_tip.STL" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="jaco_finger_joint_3" type="fixed">
+    <!--- remark 2014: damping greater than 0.1 (0.2) lead to model break downs when testing it bit by bit -->
+    <!--- remark jan 2016: damping and friction higher than 0.005 required very high forces in order for the fingers
+              to move at all, and then they mooved only slowly. -->
+    <!--dynamics damping="0.1" friction="1.0"/-->
+    <dynamics damping="0.005" friction="0.005"/>
+    <origin rpy="0 0 0" xyz="0 0 0.0441"/>
+    <parent link="jaco_8_finger_thumb"/>
+    <child link="jaco_9_finger_thumb_tip"/>
+  </joint>
+  <!-- ...........................        -->
+  <link name="jaco_7_finger_mount_pinkie">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0.00706399455294665 -0.00176077690193762"/>
+      <mass value="0.00646"/>
+      <inertia ixx="7.8381019808e-07" ixy="0" ixz="0" iyy="7.8381019808e-07" iyz="0" izz="1.04652e-06"/>
+    </inertial>
+    <visual>
+      <origin rpy="1.57079632679 0 1.57079632679" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/7_finger_mount_pinkie.STL"/>
+      </geometry>
+      <material name="grey">
+        <color rgba="0.5 0.5 0.5 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="1.57079632679 0 1.57079632679" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/7_finger_mount_pinkie.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="jaco_finger_mount_pinkie_fixed" type="fixed">
+    <!--- remark 2014: damping greater than 0.1 (0.2) lead to model break downs when testing it bit by bit -->
+    <!--- remark jan 2016: damping and friction higher than 0.005 required very high forces in order for the fingers
+              to move at all, and then they mooved only slowly. -->
+    <!--dynamics damping="0.1" friction="1.0"/-->
+    <dynamics damping="0.005" friction="0.005"/>
+    <origin rpy="0 -0.2293 0.191986" xyz="-0.034 -0.022 0.095"/>
+    <parent link="jaco_fingers_base_link"/>
+    <child link="jaco_7_finger_mount_pinkie"/>
+  </joint>
+  <!-- ...........................        -->
+  <link name="jaco_8_finger_pinkie">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0181404164708055 -2.87009015777565E-07 0.00272467774606585"/>
+      <mass value="0.01845"/>
+      <inertia ixx="6.37560262249e-06" ixy="0" ixz="0" iyy="6.37560262249e-06" iyz="0" izz="1.68125625e-06"/>
+    </inertial>
+    <!--     <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.10 0.10 0.10 1" />
+      </material>
+    </visual>
+-->
+    <visual>
+      <origin rpy="3.14159265359 1.1780972445 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/8_finger_pinkie.STL"/>
+      </geometry>
+      <material name="grey">
+        <color rgba="0.5 0.5 0.5 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="3.14159265359 1.1780972445 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/8_finger_pinkie.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="jaco_finger_joint_4" type="revolute">
+    <!--- remark 2014: damping greater than 0.1 (0.2) lead to model break downs when testing it bit by bit -->
+    <!--- remark jan 2016: damping and friction higher than 0.005 required very high forces in order for the fingers
+              to move at all, and then they mooved only slowly. -->
+    <!--dynamics damping="0.1" friction="1.0"/-->
+    <dynamics damping="0.005" friction="0.005"/>
+    <origin rpy="0 -0.34 0.191986" xyz="-0.0276 -0.0204 0.11"/>
+    <parent link="jaco_fingers_base_link"/>
+    <child link="jaco_8_finger_pinkie"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="1.5" lower="-0.0045" upper="1.05" velocity="0.2"/>
+  </joint>
+  <!-- ...........................        -->
+  <link name="jaco_9_finger_pinkie_tip">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0134 0 0.01185"/>
+      <mass value="0.0122"/>
+      <inertia ixx="2.6850059266e-06" ixy="0" ixz="0" iyy="2.6850059266e-06" iyz="0" izz="8.784e-07"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.57079632679 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/9_finger_pinkie_tip.STL" scale="1 1 1"/>
+      </geometry>
+      <material name="grey">
+        <color rgba="0.5 0.5 0.5 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 1.57079632679 3.14159265359" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/9_finger_pinkie_tip.STL" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="jaco_finger_joint_5" type="fixed">
+    <!--- remark 2014: damping greater than 0.1 (0.2) lead to model break downs when testing it bit by bit -->
+    <!--- remark jan 2016: damping and friction higher than 0.005 required very high forces in order for the fingers
+              to move at all, and then they mooved only slowly. -->
+    <!--dynamics damping="0.1" friction="1.0"/-->
+    <dynamics damping="0.005" friction="0.005"/>
+    <origin rpy="0 0 0" xyz="0 0 0.0441"/>
+    <parent link="jaco_8_finger_pinkie"/>
+    <child link="jaco_9_finger_pinkie_tip"/>
+  </joint>
+</robot>
+

pyrobolearn/robots/urdfs/pr2/pr2_gripper.urdf

@@ -0,0 +1,683 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from ./robots/pr2.urdf.xacro        | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="pr2" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://ros.org/wiki/xacro" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#slider" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
+  <!--TODO Define and give source-->
+  <!-- ============================   Shoulder   ============================ -->
+  <!-- ============================   Upper Arm   ============================ -->
+  <!-- ============================   Forearm   ============================ -->
+  <!-- DATA SOURCES -->
+  <!-- all link offsets, CG, limits are obtained from Function Engineering spreadsheet 090224_link_data.xls unless stated otherwise -->
+  <!-- all link geometry sizes are obtained from Function provided CAD model unless stated otherwise -->
+  <!-- all simplified collision geometry are hand approximated from CAD model, sometimes from respective bounding boxes -->
+  <!-- This is the 'effective' wheel radius. Wheel radius for uncompressed wheel is 0.079.  mp 20080801 -->
+  <gazebo>
+    <plugin filename="libgazebo_ros_controller_manager.so" name="gazebo_ros_controller_manager">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>1000.0</updateRate>
+    </plugin>
+    <plugin filename="libgazebo_ros_power_monitor.so" name="gazebo_ros_power_monitor_controller">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>1.0</updateRate>
+      <timeout>5</timeout>
+      <powerStateTopic>power_state</powerStateTopic>
+      <powerStateRate>10.0</powerStateRate>
+      <fullChargeCapacity>87.78</fullChargeCapacity>
+      <dischargeRate>-474</dischargeRate>
+      <chargeRate>525</chargeRate>
+      <dischargeVoltage>15.52</dischargeVoltage>
+      <chargeVoltage>16.41</chargeVoltage>
+    </plugin>
+  </gazebo>
+  <material name="Blue">
+    <color rgba="0.0 0.0 0.8 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.0 0.8 0.0 1.0"/>
+  </material>
+  <material name="Grey">
+    <color rgba="0.7 0.7 0.7 1.0"/>
+  </material>
+  <material name="Grey2">
+    <color rgba="0.9 0.9 0.9 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.8 0.0 0.0 1.0"/>
+  </material>
+  <material name="White">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  <material name="Black">
+    <color rgba="0.1 0.1 0.1 1.0"/>
+  </material>
+  <material name="LightGrey">
+    <color rgba="0.6 0.6 0.6 1.0"/>
+  </material>
+  <material name="Caster">
+    <texture filename="materials/textures/pr2_caster_texture.png"/>
+  </material>
+  <material name="Wheel_l">
+    <texture filename="materials/textures/pr2_wheel_left.png"/>
+  </material>
+  <material name="Wheel_r">
+    <texture filename="materials/textures/pr2_wheel_right.png"/>
+  </material>
+  <material name="RollLinks">
+    <texture filename="materials/textures/pr2_wheel_left.png"/>
+  </material>
+  <!-- Now we can start using the macros included above to define the actual PR2 -->
+  
+  <!-- Wrist flex -->
+  <link name="r_wrist_roll_link">
+    <inertial>
+      <!-- dummy masses, to be removed.  wrist roll masses are on "gripper_palm" -->
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/forearm_v0/wrist_roll.stl"/>
+      </geometry>
+      <material name="RollLinks"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/forearm_v0/wrist_roll_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="r_forearm_link">
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <!-- Wrist flex -->
+  <gazebo reference="r_wrist_flex_link">
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="r_wrist_flex_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <!-- Wrist roll -->
+  <gazebo reference="r_wrist_roll_link">
+    <turnGravityOff>true</turnGravityOff>
+    <material value="PR2/RollLinks"/>
+  </gazebo>
+  <gazebo reference="r_wrist_roll_joint">
+    <fudgeFactor value="0.5"/>
+  </gazebo>
+  <!-- Wrist flex , Wrist roll -->
+  <transmission name="r_wrist_trans" type="pr2_mechanism_model/WristTransmission">
+    <rightActuator mechanicalReduction="60.1714285714" name="r_wrist_r_motor"/>
+    <leftActuator mechanicalReduction="60.1714285714" name="r_wrist_l_motor"/>
+    <flexJoint mechanicalReduction="-1.0" name="r_wrist_flex_joint"/>
+    <rollJoint mechanicalReduction="1.0" name="r_wrist_roll_joint"/>
+  </transmission>
+  <joint name="r_gripper_palm_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="r_wrist_roll_link"/>
+    <child link="r_gripper_palm_link"/>
+  </joint>
+  <link name="r_gripper_palm_link">
+    <inertial>
+      <mass value="0.58007"/>
+      <origin rpy="0 0 0" xyz="0.06623 0.00053 -0.00119"/>
+      <inertia ixx="0.00035223921" ixy="-0.00001580476" ixz="-0.00000091750" iyy="0.00067741312" iyz="-0.00000059554" izz="0.00086563316"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/gripper_v0/gripper_palm.stl"/> <!-- .obj"/-->
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/gripper_v0/gripper_palm.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="r_gripper_led_joint" type="fixed">
+    <!--  Need to check if we need a positive or negative Z term -->
+    <origin xyz="0.0513 0.0 .0244"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_led_frame"/>
+  </joint>
+  <link name="r_gripper_led_frame">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+  </link> <!-- in pybullet, the inertia need to be specified even for dummy links, otherwise it will put an identity inertia and a mass of 1 by default -->
+  <joint name="r_gripper_motor_accelerometer_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_motor_accelerometer_link"/>
+  </joint>
+  <link name="r_gripper_motor_accelerometer_link">
+    <inertial>
+      <mass value="0.001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="r_gripper_tool_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.18 0 0"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_tool_frame"/>
+  </joint>
+  <link name="r_gripper_tool_frame">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+  </link> <!-- in pybullet, the inertia need to be specified even for dummy links, otherwise it will put an identity inertia and a mass of 1 by default -->
+  <!-- actuated motor screw joint -->
+  <link name="r_gripper_motor_slider_link">
+    <inertial>
+      <mass value="0.01"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+    <!-- for debugging only
+      <visual>
+        <origin xyz="0 0 0" rpy="1.5708 0 0" />
+        <geometry>
+          <cylinder length="0.002" radius="0.025"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="1.5708 0 0" />
+        <geometry>
+          <cylinder length="0.002" radius="0.025"/>
+        </geometry>
+      </collision>
+      -->
+  </link>
+  <joint name="r_gripper_motor_slider_joint" type="prismatic">
+    <origin rpy="0 0 0" xyz="0.16828 0 0"/>
+    <axis xyz="1 0 0"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_motor_slider_link"/>
+    <limit effort="1000.0" lower="-0.1" upper="0.1" velocity="0.2"/>
+  </joint>
+  <link name="r_gripper_motor_screw_link">
+    <inertial>
+      <mass value="0.01"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+    </inertial>
+    <!-- for debugging only
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <box size="0.05 0.001 0.05" />
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <box size="0.05 0.001 0.05" />
+        </geometry>
+      </collision>
+      -->
+  </link>
+  <joint name="r_gripper_motor_screw_joint" type="continuous">
+    <origin rpy="0 0 0" xyz="0.0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <parent link="r_gripper_motor_slider_link"/>
+    <child link="r_gripper_motor_screw_link"/>
+    <dynamics damping="0.0001"/>
+  </joint>
+  <!-- Finger proximal digit -->
+  <joint name="r_gripper_l_finger_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <!-- limits on passive finger and finger top joints without
+         transmission are not enforced by safety controllers.
+         The lower/upper limits and are enforced in PR2 simulation and
+         effort and velocity limits are ignored. This is also needed because
+         these joints are declared revolute rather than continuous.-->
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.02"/>
+    <origin rpy="0 0 0" xyz="0.07691 0.01 0"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_l_finger_link"/>
+  </joint>
+  <link name="r_gripper_l_finger_link">
+    <inertial>
+      <mass value="0.17126"/>
+      <origin rpy="0 0 0" xyz="0.03598 0.01730 -0.00164"/>
+      <inertia ixx="0.00007756198" ixy="0.00000149095" ixz="-0.00000983385" iyy="0.00019708305" iyz="-0.00000306125" izz="0.00018105446"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/gripper_v0/l_finger.stl"/> <!-- .obj"/-->
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Finger proximal digit -->
+  <joint name="r_gripper_r_finger_joint" type="revolute">
+    <axis xyz="0 0 -1"/>
+    <origin rpy="0 0 0" xyz="0.07691 -0.01 0"/>
+    <!-- limits on passive finger and finger top joints without
+         transmission are not enforced by safety controllers.
+         The lower/upper limits and are enforced in PR2 simulation and
+         effort and velocity limits are ignored. This is also needed because
+         these joints are declared revolute rather than continuous.-->
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.02"/>
+    <mimic joint="r_gripper_l_finger_joint" multiplier="1" offset="0"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_r_finger_link"/>
+  </joint>
+  <link name="r_gripper_r_finger_link">
+    <inertial>
+      <mass value="0.17389"/>
+      <origin rpy="0 0 0" xyz="0.03576 -0.01736 -0.00095"/>
+      <inertia ixx="0.00007738410" ixy="-0.00000209309" ixz="-0.00000836228" iyy="0.00019847383" iyz="0.00000246110" izz="0.00018106988"/>
+    </inertial>
+    <visual>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/gripper_v0/l_finger.stl"/> <!-- .obj"/-->
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Finger tip -->
+  <joint name="r_gripper_l_finger_tip_joint" type="revolute">
+    <axis xyz="0 0 -1"/>
+    <origin rpy="0 0 0" xyz="0.09137 0.00495 0"/>
+    <!-- limits on passive finger and finger top joints without
+         transmission are not enforced by safety controllers.
+         The lower/upper limits and are enforced in PR2 simulation and
+         effort and velocity limits are ignored. This is also needed because
+         these joints are declared revolute rather than continuous.-->
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.001"/>
+    <mimic joint="r_gripper_l_finger_joint" multiplier="1" offset="0"/>
+    <parent link="r_gripper_l_finger_link"/>
+    <child link="r_gripper_l_finger_tip_link"/>
+  </joint>
+  <link name="r_gripper_l_finger_tip_link">
+    <inertial>
+      <mass value="0.04419"/>
+      <origin rpy="0 0 0" xyz="0.00423 0.00284 0.0"/>
+      <inertia ixx="0.00000837047" ixy="0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/gripper_v0/l_finger_tip.stl"/> <!-- .obj"/-->
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Finger tip -->
+  <joint name="r_gripper_r_finger_tip_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.09137 -0.00495 0"/>
+    <!-- limits on passive finger and finger top joints without
+         transmission are not enforced by safety controllers.
+         The lower/upper limits and are enforced in PR2 simulation and
+         effort and velocity limits are ignored. This is also needed because
+         these joints are declared revolute rather than continuous.-->
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.001"/>
+    <mimic joint="r_gripper_l_finger_joint" multiplier="1" offset="0"/>
+    <parent link="r_gripper_r_finger_link"/>
+    <child link="r_gripper_r_finger_tip_link"/>
+  </joint>
+  <link name="r_gripper_r_finger_tip_link">
+    <inertial>
+      <mass value="0.04419"/>
+      <origin rpy="0 0 0" xyz="0.00423 -0.00284 0.0"/>
+      <inertia ixx="0.00000837047" ixy="-0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
+    </inertial>
+    <visual>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/gripper_v0/l_finger_tip.stl"/> <!-- .obj"/-->
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Finger proximal digit -->
+  <gazebo reference="r_gripper_l_finger_link">
+    <turnGravityOff>true</turnGravityOff>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <!-- for "${prefix}_l_finger_joint"-->
+  </gazebo>
+  <gazebo reference="r_gripper_l_finger_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="10000000.0"/>
+    <fudgeFactor value="1.0"/>
+    <provideFeedback value="true"/>
+  </gazebo>
+  <!-- Finger proximal digit -->
+  <gazebo reference="r_gripper_r_finger_link">
+    <turnGravityOff>true</turnGravityOff>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="r_gripper_r_finger_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="10000000.0"/>
+    <fudgeFactor value="1.0"/>
+    <provideFeedback value="true"/>
+  </gazebo>
+  <!-- Finger tip -->
+  <gazebo reference="r_gripper_l_finger_tip_link">
+    <turnGravityOff>true</turnGravityOff>
+    <selfCollide>false</selfCollide>
+    <sensor name="r_gripper_l_finger_tip_contact_sensor" type="contact">
+      <update_rate>100.0</update_rate>
+      <contact>
+        <collision>r_gripper_l_finger_tip_link_collision</collision>
+      </contact>
+      <plugin filename="libgazebo_ros_bumper.so" name="r_gripper_l_finger_tip_gazebo_ros_bumper_controller">
+        <alwaysOn>true</alwaysOn>
+        <frameName>r_gripper_l_finger_tip_link</frameName>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_gripper_l_finger_tip_bumper</bumperTopicName>
+      </plugin>
+    </sensor>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="10000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="r_gripper_l_finger_tip_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="10000000.0"/>
+    <fudgeFactor value="1.0"/>
+    <provideFeedback value="true"/>
+  </gazebo>
+  <!-- Finger tip -->
+  <gazebo reference="r_gripper_r_finger_tip_link">
+    <turnGravityOff>true</turnGravityOff>
+    <selfCollide>false</selfCollide>
+    <sensor name="r_gripper_r_finger_tip_contact_sensor" type="contact">
+      <update_rate>100.0</update_rate>
+      <contact>
+        <collision>r_gripper_r_finger_tip_link_collision</collision>
+      </contact>
+      <plugin filename="libgazebo_ros_bumper.so" name="r_gripper_r_finger_tip_gazebo_ros_bumper_controller">
+        <alwaysOn>true</alwaysOn>
+        <frameName>r_gripper_r_finger_tip_link</frameName>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_gripper_r_finger_tip_bumper</bumperTopicName>
+      </plugin>
+    </sensor>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="10000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_p3d.so" name="p3d_r_gripper_l_finger_controller">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+      <bodyName>r_gripper_l_finger_link</bodyName>
+      <topicName>r_gripper_l_finger_pose_ground_truth</topicName>
+      <gaussianNoise>0.0</gaussianNoise>
+      <frameName>base_link</frameName>
+    </plugin>
+    <plugin filename="libgazebo_ros_f3d.so" name="f3d_r_gripper_l_finger_controller">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+      <bodyName>r_gripper_l_finger_link</bodyName>
+      <topicName>r_gripper_l_finger_force_ground_truth</topicName>
+    </plugin>
+  </gazebo>
+  <gazebo reference="r_gripper_r_finger_tip_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="10000000.0"/>
+    <fudgeFactor value="1.0"/>
+    <provideFeedback value="true"/>
+  </gazebo>
+  <!-- parallel link for simulating gripper constraints -->
+  <gazebo>
+    <link name="r_gripper_l_parallel_link">
+      <inertial>
+        <mass>0.17126</mass>
+        <inertia>
+          <ixx>7.7562e-05</ixx>
+          <ixy>1.49095e-06</ixy>
+          <ixz>-9.83385e-06</ixz>
+          <iyy>0.000197083</iyy>
+          <iyz>-3.06125e-06</iyz>
+          <izz>0.000181054</izz>
+        </inertia>
+        <pose>0.03598 0.0173 -0.00164 0 0 0</pose>
+      </inertial>
+      <pose>0.82991 -0.157 0.790675 0 -0 0</pose>
+      <gravity>false</gravity>
+    </link>
+    <link name="r_gripper_r_parallel_link">
+      <inertial>
+        <mass>0.17389</mass>
+        <inertia>
+          <ixx>7.73841e-05</ixx>
+          <ixy>-2.09309e-06</ixy>
+          <ixz>-8.36228e-06</ixz>
+          <iyy>0.000198474</iyy>
+          <iyz>2.4611e-06</iyz>
+          <izz>0.00018107</izz>
+        </inertia>
+        <pose>0.03576 -0.01736 -0.00095 0 0 0</pose>
+      </inertial>
+      <pose>0.82991 -0.219 0.790675 0 0 0</pose>
+      <gravity>false</gravity>
+    </link>
+  </gazebo>
+  <gazebo>
+    <joint name="r_gripper_r_screw_screw_joint" type="screw">
+      <child>r_gripper_motor_screw_link</child>
+      <parent>r_gripper_r_finger_tip_link</parent>
+      <thread_pitch>-3141.6</thread_pitch>
+      <axis>
+        <xyz>0 1 0</xyz>
+      </axis>
+    </joint>
+    <joint name="r_gripper_l_screw_screw_joint" type="screw">
+      <parent>r_gripper_l_finger_tip_link</parent>
+      <child>r_gripper_motor_screw_link</child>
+      <thread_pitch>3141.6</thread_pitch>
+      <axis>
+        <xyz>0 1 0</xyz>
+      </axis>
+    </joint>
+  </gazebo>
+  <gazebo>
+    <joint name="r_gripper_r_parallel_root_joint" type="revolute">
+      <parent>r_gripper_r_parallel_link</parent>
+      <child>r_gripper_palm_link</child>
+      <axis>
+        <xyz>0 0 -1</xyz>
+        <dynamics>
+          <damping>0.2</damping>
+        </dynamics>
+      </axis>
+      <pose>0.05891 -0.031 0 0 0 0</pose>
+    </joint>
+    <joint name="r_gripper_l_parallel_root_joint" type="revolute">
+      <parent>r_gripper_l_parallel_link</parent>
+      <child>r_gripper_palm_link</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <dynamics>
+          <damping>0.2</damping>
+        </dynamics>
+      </axis>
+      <pose>0.05891 0.031 0 0 0 0</pose>
+    </joint>
+    <joint name="r_gripper_r_parallel_tip_joint" type="revolute">
+      <parent>r_gripper_r_parallel_link</parent>
+      <child>r_gripper_r_finger_tip_link</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+      <pose>-0.018 -0.021 0 0 0 0</pose>
+    </joint>
+    <joint name="r_gripper_l_parallel_tip_joint" type="revolute">
+      <parent>r_gripper_l_parallel_link</parent>
+      <child>r_gripper_l_finger_tip_link</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+      <pose>-0.018 0.021 0 0 0 0</pose>
+    </joint>
+    <joint name="r_gripper_joint" type="prismatic">
+      <parent>r_gripper_r_finger_tip_link</parent>
+      <child>r_gripper_l_finger_tip_link</child>
+      <axis>
+        <xyz>0 1 0</xyz>
+      </axis>
+    </joint>
+  </gazebo>
+  <gazebo reference="r_gripper_motor_slider_link">
+    <turnGravityOff>true</turnGravityOff>
+    <material value="PR2/Red"/>
+  </gazebo>
+  <gazebo reference="r_gripper_motor_screw_link">
+    <turnGravityOff>true</turnGravityOff>
+    <material value="PR2/Red"/>
+  </gazebo>
+  <gazebo reference="r_gripper_l_parallel_link">
+    <turnGravityOff>true</turnGravityOff>
+    <material value="PR2/Red"/>
+  </gazebo>
+  <gazebo reference="r_gripper_r_parallel_link">
+    <turnGravityOff>true</turnGravityOff>
+    <material value="PR2/Red"/>
+  </gazebo>
+  <!-- fictitous joint that represents the gripper gap -->
+  <!-- effort is the linear force at the gripper gap
+         velocity limit is the linear velocity limit at the gripper gap
+         try and introduce a very stiff spring
+         The velocity limits are alpha tested.
+         The effort limits are somewhat inflated.
+         k_velocity was recently raised from 500.0 to 5000.0.  Tested on beta
+    -->
+  <joint name="r_gripper_joint" type="prismatic">
+    <parent link="r_gripper_r_finger_tip_link"/>
+    <child link="r_gripper_l_finger_tip_frame"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="10.0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.09" velocity="0.2"/>
+    <safety_controller k_position="20.0" k_velocity="5000.0" soft_lower_limit="-0.01" soft_upper_limit="0.088"/>
+  </joint>
+  <!-- This link is the same as the l_finger_tip_link,
+	 but because the urdf does not support graph structures,
+	 this link exists twice -->
+  <link name="r_gripper_l_finger_tip_frame">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+  </link> <!-- in pybullet, the inertia need to be specified even for dummy links, otherwise it will put an identity inertia and a mass of 1 by default -->
+  <gazebo reference="r_gripper_palm_link">
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_p3d.so" name="p3d_r_gripper_palm_controller">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+      <bodyName>r_gripper_palm_link</bodyName>
+      <topicName>r_gripper_palm_pose_ground_truth</topicName>
+      <xyzOffsets>0 0 0</xyzOffsets>
+      <rpyOffsets>0 0 0</rpyOffsets>
+      <gaussianNoise>0.0</gaussianNoise>
+      <frameName>map</frameName>
+    </plugin>
+    <!-- a formal implementation of grasp hack in gazebo with fixed joint -->
+    <gripper name="r_grasp_hack">
+      <grasp_check>
+        <attach_steps>20</attach_steps>
+        <detach_steps>40</detach_steps>
+        <min_contact_count>2</min_contact_count>
+      </grasp_check>
+      <gripper_link>r_gripper_r_finger_tip_link</gripper_link>
+      <gripper_link>r_gripper_l_finger_tip_link</gripper_link>
+      <palm_link>r_gripper_palm_link</palm_link>
+    </gripper>
+  </gazebo>
+  <!-- [lr]_gripper_joint is a fictitious joint, used by transmission for controller gap   -->
+  <!-- [lr]_gripper_joint is not attached to any link                                      -->
+  <!-- [lr]_gripper_joint position is the gap_size                                         -->
+  <!-- [lr]_gripper_joint velocity is the gap linear velocity                              -->
+  <!-- [lr]_gripper_joint effort   is the gap linear force                                 -->
+  <!-- Please refer to function engineering spreadsheet 090224_link_data.xls for           -->
+  <!-- the transmission function.                                                          -->
+  <!-- Please refer to mechanism_model/src/pr2_gripper_transmission.cpp for implementation.-->
+  <!-- gazebo_mimic_pid is for sim only.                                                   -->
+  <transmission name="r_gripper_trans" type="pr2_mechanism_model/PR2GripperTransmission">
+    <actuator name="r_gripper_motor"/>
+    <gap_joint L0="0.0375528" a="0.0683698" b="0.0433849" gear_ratio="40.095" h="0.0" mechanical_reduction="1.0" name="r_gripper_joint" phi0="0.518518122146" r="0.0915" screw_reduction="0.004" t0="-0.0001914" theta0="0.0628824676201"/>
+    <!-- if a gazebo joint exists as [l|r]_gripper_joint, use this tag to have
+           gripper transmission apply torque directly to prismatic joint
+           this should be the default behavior in diamondback, deprecating this flag -->
+    <use_simulated_gripper_joint/>
+    <!-- set passive joint angles so things look nice in rviz -->
+    <passive_joint name="r_gripper_l_finger_joint"/>
+    <passive_joint name="r_gripper_r_finger_joint"/>
+    <passive_joint name="r_gripper_r_finger_tip_joint"/>
+    <passive_joint name="r_gripper_l_finger_tip_joint"/>
+    <!-- screw joint to capture gripper "dynamics" -->
+    <simulated_actuated_joint name="r_gripper_motor_screw_joint" passive_actuated_joint="r_gripper_motor_slider_joint" simulated_reduction="314.16"/>
+  </transmission>
+
+</robot>
+

pyrobolearn/robots/urdfs/shadowhand/left_hand.urdf

@@ -31,12 +31,12 @@
     </plugin>
   </gazebo>
   <!-- hand with cylindrical base drifts in gazebo, attach it to the world -->
-  <link name="world"/>
+  <!--link name="world"/>
   <joint name="lh_world_joint" type="fixed">
     <parent link="world"/>
     <child link="lh_forearm"/>
     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
-  </joint>
+  </joint--> <!-- in pybullet, the inertia need to be specified even for dummy links, otherwise it will put an identity inertia and a mass of 1 by default. The base link should be removed if you want the robot to not have a fixed base! -->
   <link name="lh_forearm">
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0.09"/>

pyrobolearn/robots/urdfs/shadowhand/right_hand.urdf

@@ -31,12 +31,12 @@
     </plugin>
   </gazebo>
   <!-- hand with cylindrical base drifts in gazebo, attach it to the world -->
-  <link name="world"/>
+  <!--link name="world"/>
   <joint name="rh_world_joint" type="fixed">
     <parent link="world"/>
     <child link="rh_forearm"/>
     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
-  </joint>
+  </joint-->  <!-- in pybullet, the inertia need to be specified even for dummy links, otherwise it will put an identity inertia and a mass of 1 by default. The base link should be removed if you want the robot to not have a fixed base! -->
   <link name="rh_forearm">
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0.09"/>

pyrobolearn/robots/urdfs/softhand/left_hand.urdf

@@ -39,8 +39,8 @@
   <link name="left_hand_kuka_coupler_bottom">
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0"/>
-      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+      <mass value="1.e-6"/>
+      <inertia ixx="1.e-12" ixy="0.0" ixz="0.0" iyy="1.e-12" iyz="0.0" izz="1.e-12"/>
     </inertial> <!-- in pybullet, the inertia need to be specified even for dummy links, otherwise it will put an identity inertia and a mass of 1 by default -->
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0.0115"/>

pyrobolearn/robots/urdfs/softhand/right_hand.urdf

@@ -39,8 +39,8 @@
   <link name="right_hand_kuka_coupler_bottom">
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0"/>
-      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+      <mass value="1.e-6"/>
+      <inertia ixx="1.e-12" ixy="0.0" ixz="0.0" iyy="1.e-12" iyz="0.0" izz="1.e-12"/>
     </inertial> <!-- in pybullet, the inertia need to be specified even for dummy links, otherwise it will put an identity inertia and a mass of 1 by default -->
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0.0115"/>

pyrobolearn/robots/urdfs/wam/wam_gripper.urdf

@@ -0,0 +1,505 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from wam7_bhand.urdf.xacro          | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="wam" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!-- Materials -->
+  <material name="WAMGrey">
+    <color rgba="0.6 0.6 0.6 1.0"/>
+  </material>
+  <material name="black">
+    <color rgba="0.2 0.2 0.2 1"/>
+  </material>
+  <material name="white">
+    <color rgba="0.9 0.9 0.9 1"/>
+  </material>
+  <material name="dark_gray">
+    <color rgba="0.3 0.3 0.3 1"/>
+  </material>
+  <material name="gray">
+    <color rgba="0.5 0.5 0.5 1"/>
+  </material>
+  <material name="light_gray">
+    <color rgba="0.6 0.6 0.6 1"/>
+  </material>
+  <material name="pure_red">
+    <color rgba="1.0 0.0 0.0 1"/>
+  </material>
+  <material name="red">
+    <color rgba="0.5 0.1 0.2 1"/>
+  </material>
+  <material name="pure_green">
+    <color rgba="0.0 1.0 0.0 1"/>
+  </material>
+  <material name="pure_blue">
+    <color rgba="0.0 0.0 1.0 1"/>
+  </material>
+  <material name="blue">
+    <color rgba="0.0 0.2 0.8 1"/>
+  </material>
+  <!-- Root link is in the world frame-->
+  <!--link name="world">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+  </link> <!-- if you put mass/inertia to 0 for the base_link in pybullet then the robot will not be affected by gravity (i.e. it will float). Just removing the base_link fixes the problem. -->
+  <!--joint name="wam/wam_fixed_joint" type="fixed">
+    <parent link="world"/>
+    <child link="wam/base_link"/>
+    <origin rpy="0 0 0" xyz="0.0 0.0 1.0"/>
+  </joint-->
+  <link name="wam/wrist_palm_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.055"/>
+      <mass value="0.08286134"/>
+      <inertia ixx="0.00010859" ixy="0.00000000" ixz="-0.00000000" iyy="0.00020683" iyz="-0.0000000" izz="0.00010851"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.6 0.6 0.6 1.0"/>
+      </material>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/wrist_palm_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/wrist_palm_link_convex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/wrist_palm_stump_joint" type="fixed">
+    <parent link="wam/wrist_palm_link"/>
+    <child link="wam/wrist_palm_stump_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.06"/>
+  </joint>
+  <link name="wam/wrist_palm_stump_link">
+    <inertial>
+      <mass value="0.000001"/>
+      <inertia ixx="0.0000001" ixy="0.0" ixz="0.0" iyy="0.0000001" iyz="0.0" izz="0.0000001"/>
+    </inertial>
+  </link>
+  <joint name="wam/bhand/bhand_base_joint" type="fixed">
+    <parent link="wam/wrist_palm_link"/>
+    <child link="wam/bhand/bhand_palm_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.06"/>
+  </joint>
+  <gazebo reference="wam/bhand/bhand_palm_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+  </gazebo>
+  <link name="wam/bhand/bhand_palm_link">
+    <inertial>
+      <origin xyz="-5.1098e-005 0.0050433 0.036671"/>
+      <mass value="0.50573"/>
+      <inertia ixx="3.8374e-005" ixy="-5.604e-008" ixz="-4.2034e-005" iyy="0.00022405" iyz="1.3283e-007" izz="0.00020045"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_palm_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_palm_link_convex_decomposition.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/bhand_palm_surface_joint" type="fixed">
+    <parent link="wam/bhand/bhand_palm_link"/>
+    <child link="wam/bhand/bhand_palm_surface_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.08"/>
+    <limit effort="5" lower="0" upper="3.14159265359" velocity="5.0"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <link name="wam/bhand/bhand_palm_surface_link">
+    <inertial>
+      <mass value="0.000001"/>
+      <inertia ixx="0.000001" ixy="0.0" ixz="0.0" iyy="0.000001" iyz="0.0" izz="0.000001"/>
+    </inertial>
+  </link>
+  <joint name="wam/bhand/bhand_grasp_joint" type="fixed">
+    <parent link="wam/bhand/bhand_palm_link"/>
+    <child link="wam/bhand/bhand_grasp_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.12"/>
+    <limit effort="5" lower="0" upper="3.14159265359" velocity="5.0"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <link name="wam/bhand/bhand_grasp_link">
+    <inertial>
+      <mass value="0.000001"/>
+      <inertia ixx="0.000001" ixy="0.0" ixz="0.0" iyy="0.000001" iyz="0.0" izz="0.000001"/>
+    </inertial>
+  </link>
+  <joint name="wam/bhand/finger_1/prox_joint" type="revolute">
+    <parent link="wam/bhand/bhand_palm_link"/>
+    <child link="wam/bhand/finger_1/prox_link"/>
+    <origin rpy="0 0 -1.57079632679" xyz="-0.025 0.0 0.0415"/>
+    <axis xyz="0 0 -1"/>
+    <limit effort="5" lower="0" upper="3.14159265359" velocity="5.0"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_1/prox_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_1/prox_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+  </gazebo>
+  <link name="wam/bhand/finger_1/prox_link">
+    <inertial>
+      <origin xyz="0.023133 0.00078642 0.00052792"/>
+      <mass value="0.14109"/>
+      <inertia ixx="4.872e-006" ixy="1.7103e-006" ixz="3.4041e-008" iyy="7.6588e-005" iyz="2.3133e-008" izz="7.7733e-005"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_prox_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_prox_link_convex_decomposition.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_1/med_joint" type="revolute">
+    <parent link="wam/bhand/finger_1/prox_link"/>
+    <child link="wam/bhand/finger_1/med_link"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.05 0.0 0.03390"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="0" upper="2.44346095279" velocity="5.00"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_1/med_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_1/med_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+  </gazebo>
+  <link name="wam/bhand/finger_1/med_link">
+    <inertial>
+      <origin xyz="0.023133 0.00078642 0.00052792"/>
+      <mass value="0.062139"/>
+      <inertia ixx="4.872e-006" ixy="1.7103e-006" ixz="3.4041e-008" iyy="7.6588e-005" iyz="2.3133e-008" izz="7.7733e-005"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_med_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_med_link_convex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_1/dist_joint" type="revolute">
+    <parent link="wam/bhand/finger_1/med_link"/>
+    <child link="wam/bhand/finger_1/dist_link"/>
+    <origin rpy="0 0 0.785398163397" xyz="0.06994 0.003 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="0" upper="0.837758040957" velocity="5.00"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_1/dist_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_1/dist_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <link name="wam/bhand/finger_1/dist_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.022825 0.0010491 0.0004203"/>
+      <mass value="0.041377"/>
+      <inertia ixx="3.1582e-006" ixy="1.4308e-006" ixz="1.0106e-007" iyy="3.8376e-005" iyz="0" izz="3.7275e-005"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 -0.785398163397"/>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_dist_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.785398163397"/>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_dist_link_convex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_1/tip_joint" type="fixed">
+    <parent link="wam/bhand/finger_1/dist_link"/>
+    <child link="wam/bhand/finger_1/tip_link"/>
+    <origin rpy="0 0 0" xyz="0.05 0.0 0.0"/>
+  </joint>
+  <link name="wam/bhand/finger_1/tip_link">
+    <inertial>
+      <mass value="1E-6"/>
+      <inertia ixx="1E-6" ixy="0" ixz="0" iyy="1E-6" iyz="0" izz="1E-6"/>
+    </inertial>
+  </link>
+  <joint name="wam/bhand/finger_2/prox_joint" type="revolute">
+    <parent link="wam/bhand/bhand_palm_link"/>
+    <child link="wam/bhand/finger_2/prox_link"/>
+    <origin rpy="0 0 -1.57079632679" xyz="0.025 0.0 0.0415"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="0" upper="3.14159265359" velocity="5.0"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_2/prox_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_2/prox_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+  </gazebo>
+  <link name="wam/bhand/finger_2/prox_link">
+    <inertial>
+      <origin xyz="0.023133 0.00078642 0.00052792"/>
+      <mass value="0.14109"/>
+      <inertia ixx="4.872e-006" ixy="1.7103e-006" ixz="3.4041e-008" iyy="7.6588e-005" iyz="2.3133e-008" izz="7.7733e-005"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_prox_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_prox_link_convex_decomposition.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_2/med_joint" type="revolute">
+    <parent link="wam/bhand/finger_2/prox_link"/>
+    <child link="wam/bhand/finger_2/med_link"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.05 0.0 0.03390"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="0" upper="2.44346095279" velocity="5.00"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_2/med_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_2/med_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+  </gazebo>
+  <link name="wam/bhand/finger_2/med_link">
+    <inertial>
+      <origin xyz="0.023133 0.00078642 0.00052792"/>
+      <mass value="0.062139"/>
+      <inertia ixx="4.872e-006" ixy="1.7103e-006" ixz="3.4041e-008" iyy="7.6588e-005" iyz="2.3133e-008" izz="7.7733e-005"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_med_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_med_link_convex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_2/dist_joint" type="revolute">
+    <parent link="wam/bhand/finger_2/med_link"/>
+    <child link="wam/bhand/finger_2/dist_link"/>
+    <origin rpy="0 0 0.785398163397" xyz="0.06994 0.003 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="0" upper="0.837758040957" velocity="5.00"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_2/dist_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_2/dist_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <link name="wam/bhand/finger_2/dist_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.022825 0.0010491 0.0004203"/>
+      <mass value="0.041377"/>
+      <inertia ixx="3.1582e-006" ixy="1.4308e-006" ixz="1.0106e-007" iyy="3.8376e-005" iyz="0" izz="3.7275e-005"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 -0.785398163397"/>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_dist_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.785398163397"/>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_dist_link_convex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_2/tip_joint" type="fixed">
+    <parent link="wam/bhand/finger_2/dist_link"/>
+    <child link="wam/bhand/finger_2/tip_link"/>
+    <origin rpy="0 0 0" xyz="0.05 0.0 0.0"/>
+  </joint>
+  <link name="wam/bhand/finger_2/tip_link">
+    <inertial>
+      <mass value="1E-6"/>
+      <inertia ixx="1E-6" ixy="0" ixz="0" iyy="1E-6" iyz="0" izz="1E-6"/>
+    </inertial>
+  </link>
+  <joint name="wam/bhand/finger_3/med_joint" type="revolute">
+    <parent link="wam/bhand/bhand_palm_link"/>
+    <child link="wam/bhand/finger_3/med_link"/>
+    <origin rpy="1.57079632679 0 1.57079632679" xyz="0 0.05 0.0754"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="0" upper="2.44346095279" velocity="5.00"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_3/med_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_3/med_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+  </gazebo>
+  <link name="wam/bhand/finger_3/med_link">
+    <inertial>
+      <origin xyz="0.023133 0.00078642 0.00052792"/>
+      <mass value="0.062139"/>
+      <inertia ixx="4.872e-006" ixy="1.7103e-006" ixz="3.4041e-008" iyy="7.6588e-005" iyz="2.3133e-008" izz="7.7733e-005"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_med_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_med_link_convex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_3/dist_joint" type="revolute">
+    <parent link="wam/bhand/finger_3/med_link"/>
+    <child link="wam/bhand/finger_3/dist_link"/>
+    <origin rpy="0 0 0.785398163397" xyz="0.06994 0.003 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="0" upper="0.837758040957" velocity="5.00"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_3/dist_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_3/dist_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <link name="wam/bhand/finger_3/dist_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.022825 0.0010491 0.0004203"/>
+      <mass value="0.041377"/>
+      <inertia ixx="3.1582e-006" ixy="1.4308e-006" ixz="1.0106e-007" iyy="3.8376e-005" iyz="0" izz="3.7275e-005"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 -0.785398163397"/>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_dist_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.785398163397"/>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_dist_link_convex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_3/tip_joint" type="fixed">
+    <parent link="wam/bhand/finger_3/dist_link"/>
+    <child link="wam/bhand/finger_3/tip_link"/>
+    <origin rpy="0 0 0" xyz="0.05 0.0 0.0"/>
+  </joint>
+  <link name="wam/bhand/finger_3/tip_link">
+    <inertial>
+      <mass value="1E-6"/>
+      <inertia ixx="1E-6" ixy="0" ixz="0" iyy="1E-6" iyz="0" izz="1E-6"/>
+    </inertial>
+  </link>
+</robot>
+

pyrobolearn/robots/urdfs/wam/wam_gripper_with_wrist.urdf

@@ -0,0 +1,580 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from wam7_bhand.urdf.xacro          | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="wam" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!-- Materials -->
+  <material name="WAMGrey">
+    <color rgba="0.6 0.6 0.6 1.0"/>
+  </material>
+  <material name="black">
+    <color rgba="0.2 0.2 0.2 1"/>
+  </material>
+  <material name="white">
+    <color rgba="0.9 0.9 0.9 1"/>
+  </material>
+  <material name="dark_gray">
+    <color rgba="0.3 0.3 0.3 1"/>
+  </material>
+  <material name="gray">
+    <color rgba="0.5 0.5 0.5 1"/>
+  </material>
+  <material name="light_gray">
+    <color rgba="0.6 0.6 0.6 1"/>
+  </material>
+  <material name="pure_red">
+    <color rgba="1.0 0.0 0.0 1"/>
+  </material>
+  <material name="red">
+    <color rgba="0.5 0.1 0.2 1"/>
+  </material>
+  <material name="pure_green">
+    <color rgba="0.0 1.0 0.0 1"/>
+  </material>
+  <material name="pure_blue">
+    <color rgba="0.0 0.0 1.0 1"/>
+  </material>
+  <material name="blue">
+    <color rgba="0.0 0.2 0.8 1"/>
+  </material>
+  <!-- Root link is in the world frame-->
+  <!--link name="world">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+  </link> <!-- if you put mass/inertia to 0 for the base_link in pybullet then the robot will not be affected by gravity (i.e. it will float). Just removing the base_link fixes the problem. -->
+  <!--joint name="wam/wam_fixed_joint" type="fixed">
+    <parent link="world"/>
+    <child link="wam/base_link"/>
+    <origin rpy="0 0 0" xyz="0.0 0.0 1.0"/>
+  </joint-->
+  <link name="wam/wrist_yaw_link">
+    <inertial>
+      <origin xyz="0.00008921 0.00435824 -0.00511217 "/>
+      <mass value="1.05376019"/>
+      <inertia ixx="0.00005029" ixy="0.00000020" ixz="0.00007582" iyy="0.00007582" iyz="-0.00000359" izz="0.00006270"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.6 0.6 0.6 1.0"/>
+      </material>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/wrist_yaw_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/wrist_yaw_link_convex_decomposition.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/wrist_pitch_joint" type="revolute">
+    <parent link="wam/wrist_yaw_link"/>
+    <child link="wam/wrist_pitch_link"/>
+    <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" lower="-1.5707" upper="1.5707" velocity="2.00"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/wrist_pitch_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <link name="wam/wrist_pitch_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.00012262 -0.02468336 -0.01703194 "/>
+      <mass value="0.51797364"/>
+      <inertia ixx="0.00055516" ixy="0.00000061" ixz="-0.00000074" iyy="0.00024367" iyz="-0.00004590" izz="0.00045358"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.6 0.6 0.6 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/wrist_pitch_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/wrist_pitch_link_convex_decomposition.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/palm_yaw_joint" type="revolute">
+    <parent link="wam/wrist_pitch_link"/>
+    <child link="wam/wrist_palm_link"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" lower="-3.0" upper="3.0" velocity="2.00"/>
+    <!-- 2.95 is a limit taken in from 3 due to saftey and differences between arms. -->
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/palm_yaw_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <link name="wam/wrist_palm_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.055"/>
+      <mass value="0.08286134"/>
+      <inertia ixx="0.00010859" ixy="0.00000000" ixz="-0.00000000" iyy="0.00020683" iyz="-0.0000000" izz="0.00010851"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.6 0.6 0.6 1.0"/>
+      </material>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/wrist_palm_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/wrist_palm_link_convex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/wrist_palm_stump_joint" type="fixed">
+    <parent link="wam/wrist_palm_link"/>
+    <child link="wam/wrist_palm_stump_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.06"/>
+  </joint>
+  <link name="wam/wrist_palm_stump_link">
+    <inertial>
+      <mass value="0.000001"/>
+      <inertia ixx="0.0000001" ixy="0.0" ixz="0.0" iyy="0.0000001" iyz="0.0" izz="0.0000001"/>
+    </inertial>
+  </link>
+  <joint name="wam/bhand/bhand_base_joint" type="fixed">
+    <parent link="wam/wrist_palm_link"/>
+    <child link="wam/bhand/bhand_palm_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.06"/>
+  </joint>
+  <gazebo reference="wam/bhand/bhand_palm_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+  </gazebo>
+  <link name="wam/bhand/bhand_palm_link">
+    <inertial>
+      <origin xyz="-5.1098e-005 0.0050433 0.036671"/>
+      <mass value="0.50573"/>
+      <inertia ixx="3.8374e-005" ixy="-5.604e-008" ixz="-4.2034e-005" iyy="0.00022405" iyz="1.3283e-007" izz="0.00020045"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_palm_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_palm_link_convex_decomposition.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/bhand_palm_surface_joint" type="fixed">
+    <parent link="wam/bhand/bhand_palm_link"/>
+    <child link="wam/bhand/bhand_palm_surface_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.08"/>
+    <limit effort="5" lower="0" upper="3.14159265359" velocity="5.0"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <link name="wam/bhand/bhand_palm_surface_link">
+    <inertial>
+      <mass value="0.000001"/>
+      <inertia ixx="0.000001" ixy="0.0" ixz="0.0" iyy="0.000001" iyz="0.0" izz="0.000001"/>
+    </inertial>
+  </link>
+  <joint name="wam/bhand/bhand_grasp_joint" type="fixed">
+    <parent link="wam/bhand/bhand_palm_link"/>
+    <child link="wam/bhand/bhand_grasp_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.12"/>
+    <limit effort="5" lower="0" upper="3.14159265359" velocity="5.0"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <link name="wam/bhand/bhand_grasp_link">
+    <inertial>
+      <mass value="0.000001"/>
+      <inertia ixx="0.000001" ixy="0.0" ixz="0.0" iyy="0.000001" iyz="0.0" izz="0.000001"/>
+    </inertial>
+  </link>
+  <joint name="wam/bhand/finger_1/prox_joint" type="revolute">
+    <parent link="wam/bhand/bhand_palm_link"/>
+    <child link="wam/bhand/finger_1/prox_link"/>
+    <origin rpy="0 0 -1.57079632679" xyz="-0.025 0.0 0.0415"/>
+    <axis xyz="0 0 -1"/>
+    <limit effort="5" lower="0" upper="3.14159265359" velocity="5.0"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_1/prox_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_1/prox_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+  </gazebo>
+  <link name="wam/bhand/finger_1/prox_link">
+    <inertial>
+      <origin xyz="0.023133 0.00078642 0.00052792"/>
+      <mass value="0.14109"/>
+      <inertia ixx="4.872e-006" ixy="1.7103e-006" ixz="3.4041e-008" iyy="7.6588e-005" iyz="2.3133e-008" izz="7.7733e-005"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_prox_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_prox_link_convex_decomposition.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_1/med_joint" type="revolute">
+    <parent link="wam/bhand/finger_1/prox_link"/>
+    <child link="wam/bhand/finger_1/med_link"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.05 0.0 0.03390"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="0" upper="2.44346095279" velocity="5.00"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_1/med_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_1/med_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+  </gazebo>
+  <link name="wam/bhand/finger_1/med_link">
+    <inertial>
+      <origin xyz="0.023133 0.00078642 0.00052792"/>
+      <mass value="0.062139"/>
+      <inertia ixx="4.872e-006" ixy="1.7103e-006" ixz="3.4041e-008" iyy="7.6588e-005" iyz="2.3133e-008" izz="7.7733e-005"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_med_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_med_link_convex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_1/dist_joint" type="revolute">
+    <parent link="wam/bhand/finger_1/med_link"/>
+    <child link="wam/bhand/finger_1/dist_link"/>
+    <origin rpy="0 0 0.785398163397" xyz="0.06994 0.003 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="0" upper="0.837758040957" velocity="5.00"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_1/dist_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_1/dist_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <link name="wam/bhand/finger_1/dist_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.022825 0.0010491 0.0004203"/>
+      <mass value="0.041377"/>
+      <inertia ixx="3.1582e-006" ixy="1.4308e-006" ixz="1.0106e-007" iyy="3.8376e-005" iyz="0" izz="3.7275e-005"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 -0.785398163397"/>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_dist_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.785398163397"/>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_dist_link_convex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_1/tip_joint" type="fixed">
+    <parent link="wam/bhand/finger_1/dist_link"/>
+    <child link="wam/bhand/finger_1/tip_link"/>
+    <origin rpy="0 0 0" xyz="0.05 0.0 0.0"/>
+  </joint>
+  <link name="wam/bhand/finger_1/tip_link">
+    <inertial>
+      <mass value="1E-6"/>
+      <inertia ixx="1E-6" ixy="0" ixz="0" iyy="1E-6" iyz="0" izz="1E-6"/>
+    </inertial>
+  </link>
+  <joint name="wam/bhand/finger_2/prox_joint" type="revolute">
+    <parent link="wam/bhand/bhand_palm_link"/>
+    <child link="wam/bhand/finger_2/prox_link"/>
+    <origin rpy="0 0 -1.57079632679" xyz="0.025 0.0 0.0415"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="0" upper="3.14159265359" velocity="5.0"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_2/prox_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_2/prox_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+  </gazebo>
+  <link name="wam/bhand/finger_2/prox_link">
+    <inertial>
+      <origin xyz="0.023133 0.00078642 0.00052792"/>
+      <mass value="0.14109"/>
+      <inertia ixx="4.872e-006" ixy="1.7103e-006" ixz="3.4041e-008" iyy="7.6588e-005" iyz="2.3133e-008" izz="7.7733e-005"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_prox_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_prox_link_convex_decomposition.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_2/med_joint" type="revolute">
+    <parent link="wam/bhand/finger_2/prox_link"/>
+    <child link="wam/bhand/finger_2/med_link"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.05 0.0 0.03390"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="0" upper="2.44346095279" velocity="5.00"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_2/med_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_2/med_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+  </gazebo>
+  <link name="wam/bhand/finger_2/med_link">
+    <inertial>
+      <origin xyz="0.023133 0.00078642 0.00052792"/>
+      <mass value="0.062139"/>
+      <inertia ixx="4.872e-006" ixy="1.7103e-006" ixz="3.4041e-008" iyy="7.6588e-005" iyz="2.3133e-008" izz="7.7733e-005"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_med_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_med_link_convex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_2/dist_joint" type="revolute">
+    <parent link="wam/bhand/finger_2/med_link"/>
+    <child link="wam/bhand/finger_2/dist_link"/>
+    <origin rpy="0 0 0.785398163397" xyz="0.06994 0.003 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="0" upper="0.837758040957" velocity="5.00"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_2/dist_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_2/dist_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <link name="wam/bhand/finger_2/dist_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.022825 0.0010491 0.0004203"/>
+      <mass value="0.041377"/>
+      <inertia ixx="3.1582e-006" ixy="1.4308e-006" ixz="1.0106e-007" iyy="3.8376e-005" iyz="0" izz="3.7275e-005"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 -0.785398163397"/>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_dist_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.785398163397"/>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_dist_link_convex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_2/tip_joint" type="fixed">
+    <parent link="wam/bhand/finger_2/dist_link"/>
+    <child link="wam/bhand/finger_2/tip_link"/>
+    <origin rpy="0 0 0" xyz="0.05 0.0 0.0"/>
+  </joint>
+  <link name="wam/bhand/finger_2/tip_link">
+    <inertial>
+      <mass value="1E-6"/>
+      <inertia ixx="1E-6" ixy="0" ixz="0" iyy="1E-6" iyz="0" izz="1E-6"/>
+    </inertial>
+  </link>
+  <joint name="wam/bhand/finger_3/med_joint" type="revolute">
+    <parent link="wam/bhand/bhand_palm_link"/>
+    <child link="wam/bhand/finger_3/med_link"/>
+    <origin rpy="1.57079632679 0 1.57079632679" xyz="0 0.05 0.0754"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="0" upper="2.44346095279" velocity="5.00"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_3/med_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_3/med_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+  </gazebo>
+  <link name="wam/bhand/finger_3/med_link">
+    <inertial>
+      <origin xyz="0.023133 0.00078642 0.00052792"/>
+      <mass value="0.062139"/>
+      <inertia ixx="4.872e-006" ixy="1.7103e-006" ixz="3.4041e-008" iyy="7.6588e-005" iyz="2.3133e-008" izz="7.7733e-005"/>
+    </inertial>
+    <visual>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_med_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_med_link_convex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_3/dist_joint" type="revolute">
+    <parent link="wam/bhand/finger_3/med_link"/>
+    <child link="wam/bhand/finger_3/dist_link"/>
+    <origin rpy="0 0 0.785398163397" xyz="0.06994 0.003 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5" lower="0" upper="0.837758040957" velocity="5.00"/>
+    <dynamics damping="0.11" friction="0"/>
+  </joint>
+  <gazebo reference="wam/bhand/finger_3/dist_joint">
+    <provideFeedback>true</provideFeedback>
+    <implicitSpringDamper>true</implicitSpringDamper>
+    <!--<kp>0.0</kp>[> CFM <]-->
+    <!--<kd>0.0</kd>[> ERP <]-->
+    <!--<cfmDamping>true</cfmDamping>-->
+    <!--<dynamics damping="5.5" friction="1"/>-->
+  </gazebo>
+  <gazebo reference="wam/bhand/finger_3/dist_link">
+    <mu1>0.8</mu1>
+    <mu2>0.8</mu2>
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <link name="wam/bhand/finger_3/dist_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.022825 0.0010491 0.0004203"/>
+      <mass value="0.041377"/>
+      <inertia ixx="3.1582e-006" ixy="1.4308e-006" ixz="1.0106e-007" iyy="3.8376e-005" iyz="0" izz="3.7275e-005"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 -0.785398163397"/>
+      <material name="WAMGrey">
+        <color rgba="0.9 0.9 0.9 1.0"/>
+      </material>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_dist_link_fine.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.785398163397"/>
+      <geometry>
+        <mesh filename="meshes/bhand/bhand_finger_dist_link_convex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wam/bhand/finger_3/tip_joint" type="fixed">
+    <parent link="wam/bhand/finger_3/dist_link"/>
+    <child link="wam/bhand/finger_3/tip_link"/>
+    <origin rpy="0 0 0" xyz="0.05 0.0 0.0"/>
+  </joint>
+  <link name="wam/bhand/finger_3/tip_link">
+    <inertial>
+      <mass value="1E-6"/>
+      <inertia ixx="1E-6" ixy="0" ixz="0" iyy="1E-6" iyz="0" izz="1E-6"/>
+    </inertial>
+  </link>
+</robot>
+

pyrobolearn/robots/urdfs/youbot/youbot_gripper.urdf

@@ -0,0 +1,208 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from robots/youbot_arm_only.urdf.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="youbot" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
+  <material name="Orange">
+    <color rgba="1.0 0.4 0.0 1.0"/>
+  </material>
+  <material name="Grey">
+    <color rgba="0.7 0.7 0.7 1.0"/>
+  </material>
+  <material name="DarkGrey">
+    <color rgba="0.3 0.3 0.3 1.0"/>
+  </material>
+  <material name="White">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  <material name="Black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="1.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.0 1.0 0.0 1.0"/>
+  </material>
+  <material name="Blue">
+    <color rgba="0.0 0.0 0.8 1.0"/>
+  </material>
+  <material name="LightGrey">
+    <color rgba="0.82 0.82 0.82 1.0"/>
+  </material>
+  &gt;
+
+	
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_controller"/>
+  </gazebo>
+  <!-- Now we can start using the macros included above to define the actual youbot -->
+  <!-- The first use of a macro.  This one was defined in base.urdf.xacro above.
+       A macro like this will expand to a set of link and joint definitions, and to additional
+       Gazebo-related extensions (sensor plugins, etc).  The macro takes an argument, name, 
+       that equals "base", and uses it to generate names for its component links and joints 
+       (e.g., base_link).  The included origin block is also an argument to the macro.  By convention, 
+       the origin block defines where the component is w.r.t its parent (in this case the parent 
+       is the world frame). For more, see http://www.ros.org/wiki/xacro -->
+  <link name="arm_link_5">
+    <inertial>
+      <mass value="0.251"/>
+      <origin rpy="0 0 0" xyz="0.0 0.00115 -0.01683"/>
+      <inertia ixx="0.000280" ixy="0.000000" ixz="0.000000" iyy="0.000339" iyz="0.000000" izz="0.000119"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.003 0 -0.034"/>
+      <geometry>
+        <mesh filename="meshes/youbot_arm/arm5.dae"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.003 0 -0.034"/>
+      <geometry>
+        <mesh filename="meshes/youbot_arm/arm5_convex.dae"/>
+        <!--<box size="0.054 0.096 0.030"/>-->
+      </geometry>
+    </collision>
+  </link>
+  
+  <gazebo reference="arm_link_5">
+    <!--<material value="youbot/DarkGrey" />-->
+    <turnGravityOff>false</turnGravityOff>
+  </gazebo>
+ 
+  <transmission name="arm_trans_5">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="arm_motor_5">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="arm_joint_5">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <!-- joint between arm_7_link and sdh_palm_link -->
+  <joint name="gripper_palm_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="arm_link_5"/>
+    <child link="gripper_palm_link"/>
+  </joint>
+  <link name="gripper_palm_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/youbot_gripper/palm.dae"/>
+      </geometry>
+      <material name="Black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <!--<box size="0.050 0.094 0.054"/>-->
+        <mesh filename="meshes/youbot_gripper/palm_convex.dae"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="gripper_finger_joint_l" type="prismatic">
+    <origin rpy="0 0 3.14159265359" xyz="0.004 0.008 0.046"/>
+    <parent link="gripper_palm_link"/>
+    <child link="gripper_finger_link_l"/>
+    <axis xyz="0 -1 0"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <limit effort="1" lower="0" upper="0.0115" velocity="0.1"/>
+    <safety_controller k_position="20" k_velocity="25" soft_lower_limit="0.001" soft_upper_limit="0.011"/>
+  </joint>
+  <link name="gripper_finger_link_l">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 -0.001 0"/>
+      <mass value="0.01"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/youbot_gripper/finger.dae"/>
+      </geometry>
+      <material name="Black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <!--<box size="0.014 0.020 0.046"/>-->
+        <mesh filename="meshes/youbot_gripper/finger_convex.dae"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="gripper_finger_joint_r" type="prismatic">
+    <origin rpy="0 0 0" xyz="0.004 -0.008 0.046"/>
+    <parent link="gripper_palm_link"/>
+    <child link="gripper_finger_link_r"/>
+    <axis xyz="0 -1 0"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <limit effort="1" lower="0" upper="0.0115" velocity="0.1"/>
+    <safety_controller k_position="20" k_velocity="25" soft_lower_limit="0.001" soft_upper_limit="0.011"/>
+  </joint>
+  <link name="gripper_finger_link_r">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.000 -0.001 0"/>
+      <mass value="0.01"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/youbot_gripper/finger.dae"/>
+      </geometry>
+      <material name="Black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.000 0 0"/>
+      <geometry>
+        <!--<box size="0.014 0.020 0.046"/>-->
+        <mesh filename="meshes/youbot_gripper/finger_convex.dae"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="gripper_palm_link">
+    <material>Gazebo/Black</material>
+    <turnGravityOff>false</turnGravityOff>
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="gripper_finger_l_link">
+    <material>Gazebo/Black</material>
+    <turnGravityOff>false</turnGravityOff>
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="gripper_finger_r_link">
+    <material>Gazebo/Black</material>
+    <turnGravityOff>false</turnGravityOff>
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <transmission name="gripper_finger_l_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="gripper_finger_l_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="gripper_finger_joint_l">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="gripper_finger_r_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="gripper_finger_r_motor">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="gripper_finger_joint_r">
+      <hardwareInterface>EffortJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+</robot>
+

pyrobolearn/robots/usv.py

@@ -21,4 +21,15 @@ class USVRobot(Robot):
     """
 
     def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1.):
+        """
+        Initialize the USV.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(USVRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)

pyrobolearn/robots/uuv.py

@@ -21,4 +21,15 @@ class UUVRobot(Robot):
     """
 
     def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1.):
+        """
+        Initialize the UUV.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(UUVRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)

pyrobolearn/robots/walker2d.py

@@ -19,15 +19,10 @@ class Walker2D(Robot):
     r"""Walker 2D Mujoco Model
 
     References:
-        [1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
+        - [1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(-0.5, 0, 0.1),
-                 orientation=(0, 0.707, 0, 0.707),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(-0.5, 0, 0.1), orientation=(0, 0.707, 0, 0.707), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/mjcfs/walker2d.xml'):
         # check parameters
         if position is None:

pyrobolearn/robots/walkman.py

@@ -25,18 +25,25 @@ class Walkman(BipedRobot, BiManipulator):
     each ankle).
 
     References:
-        [1] "WALK-MAN: A High-Performance Humanoid Platform for Realistic Environments", Tsagarakis et al., 2017
-        [2] https://github.com/ADVRHumanoids/iit-walkman-ros-pkg
+        - [1] "WALK-MAN: A High-Performance Humanoid Platform for Realistic Environments", Tsagarakis et al., 2017
+        - [2] https://github.com/ADVRHumanoids/iit-walkman-ros-pkg
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 1.14),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
-                 urdf=os.path.dirname(__file__) + '/urdfs/walkman/walkman.urdf',
-                 lower_body=False):  # 'walkman_lower_body.urdf'
+    def __init__(self, simulator, position=(0, 0, 1.14), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 lower_body=False, urdf=os.path.dirname(__file__) + '/urdfs/walkman/walkman.urdf'):
+        # 'walkman_lower_body.urdf'
+        """
+        Initialize the Walkman robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            lower_body (bool): if True, it will instantiate only the lower part of the robot (i.e. the legs).
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 1.14)

pyrobolearn/robots/wam.py

@@ -1,10 +1,11 @@
 #!/usr/bin/env python
-"""Provide the WAM robotic platform.
+"""Provide the WAM robotic platform and the Barrett hand gripper.
 """
 
 import os
 
 from pyrobolearn.robots.manipulator import Manipulator
+from pyrobolearn.robots.gripper import AngularGripper
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -19,17 +20,23 @@ class WAM(Manipulator):
     r"""Wam robot
 
     References:
-        [1] https://advanced.barrett.com/wam-arm-1
-        [2] https://github.com/jhu-lcsr/barrett_model
+        - [1] https://advanced.barrett.com/wam-arm-1
+        - [2] https://github.com/jhu-lcsr/barrett_model
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=True,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=True, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/wam/wam.urdf'):
+        """
+        Initialize the WAM robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the gripper.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.)
@@ -46,6 +53,45 @@ class WAM(Manipulator):
         # self.disable_motor()
 
 
+class BarrettHand(AngularGripper):
+    r"""BarretHand (Gripper)
+
+    References:
+        - [1] https://advanced.barrett.com/wam-arm-1
+        - [2] https://github.com/jhu-lcsr/barrett_model
+    """
+
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/wam/wam_gripper.urdf'):
+        """
+        Initialize the Barrett hand/gripper.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the gripper will be fixed in the world.
+            scale (float): scaling factor that is used to scale the gripper.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
+        # 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 = True
+
+        super(BarrettHand, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'barrett_hand'
+
+
+# alias
+WAMGripper = BarrettHand
+
+
 # Test
 if __name__ == "__main__":
     import numpy as np

pyrobolearn/robots/wheeled_robot.py

@@ -24,6 +24,17 @@ class WheeledRobot(Robot):
     """
 
     def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1.):
+        """
+        Initialize the wheeled robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(WheeledRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
 
         self.wheels = []
@@ -122,15 +133,26 @@ class DifferentialWheeledRobot(WheeledRobot):
         \omega_L &= v - \frac{d}{2r} \omega
 
     References:
-        [1] Wikipedia: https://en.wikipedia.org/wiki/Differential_wheeled_robot
-        [2] "Pros and cons for different types of drive selection":
+        - [1] Wikipedia: https://en.wikipedia.org/wiki/Differential_wheeled_robot
+        - [2] "Pros and cons for different types of drive selection":
             https://robohub.org/pros-and-cons-for-different-types-of-drive-selection/
-        [3] Wheel Control Theory:
+        - [3] Wheel Control Theory:
             http://www.robotplatform.com/knowledge/Classification_of_Robots/wheel_control_theory.html
-        [4] "Robotics: Modelling, Planning and Control" (section 11.2), Siciliano et al., 2010
+        - [4] "Robotics: Modelling, Planning and Control" (section 11.2), Siciliano et al., 2010
     """
 
     def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1.):
+        """
+        Initialize the differential wheeled robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(DifferentialWheeledRobot, self).__init__(simulator, urdf, position, orientation, fixed_base,
                                                        scale)
 
@@ -170,14 +192,25 @@ class AckermannWheeledRobot(WheeledRobot):
     Check also [2] for the different types of drive.
 
     References:
-        [1] Wikipedia: https://en.wikipedia.org/wiki/Ackermann_steering_geometry
-        [2] "Pros and cons for different types of drive selection":
+        - [1] Wikipedia: https://en.wikipedia.org/wiki/Ackermann_steering_geometry
+        - [2] "Pros and cons for different types of drive selection":
             https://robohub.org/pros-and-cons-for-different-types-of-drive-selection/
-        [3] Wheel Control Theory:
+        - [3] Wheel Control Theory:
             http://www.robotplatform.com/knowledge/Classification_of_Robots/wheel_control_theory.html
     """
 
     def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1.):
+        """
+        Initialize the Ackermann wheeled robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+        """
         super(AckermannWheeledRobot, self).__init__(simulator, urdf, position, orientation, fixed_base,
                                                     scale)
 

pyrobolearn/robots/youbot.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 """Provide the Youbot robotic platforms.
 
-These include: YoubotBase, KukaYoubotArm, Youbot, YoubotDualArm
+These include: YoubotBase, KukaYoubotArm, Youbot, YoubotDualArm, YoubotGripper
 """
 
 import os
@@ -9,6 +9,8 @@ import numpy as np
 
 from pyrobolearn.robots.manipulator import Manipulator, BiManipulator
 from pyrobolearn.robots.wheeled_robot import DifferentialWheeledRobot
+from pyrobolearn.robots.gripper import ParallelGripper
+
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -23,16 +25,22 @@ class YoubotBase(DifferentialWheeledRobot):
     r"""Youbot Base robot
 
     References:
-        [1] https://github.com/youbot
+        - [1] https://github.com/youbot
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.085),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.085), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/youbot/youbot_base_only.urdf'):
+        """
+        Initialize the Youbot base robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.085)
@@ -55,16 +63,22 @@ class KukaYoubotArm(Manipulator):
     r"""Kuka Youbot arm robot
 
     References:
-        [1] https://github.com/youbot
+        - [1] https://github.com/youbot
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.03),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=True,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.03), orientation=(0, 0, 0, 1), fixed_base=True, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/youbot/youbot_arm_only.urdf'):
+        """
+        Initialize the Kuka Youbot manipulator.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.03)
@@ -83,16 +97,22 @@ class Youbot(Manipulator, DifferentialWheeledRobot):
     r"""Youbot robot
 
     References:
-        [1] https://github.com/youbot
+        - [1] https://github.com/youbot
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.085),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.085), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/youbot/youbot.urdf'):
+        """
+        Initialize the Youbot robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.085)
@@ -115,16 +135,22 @@ class YoubotDualArm(BiManipulator, DifferentialWheeledRobot):
     r"""Youbot dual arm robot
 
     References:
-        [1] https://github.com/youbot
+        - [1] https://github.com/youbot
     """
 
-    def __init__(self,
-                 simulator,
-                 position=(0, 0, 0.085),
-                 orientation=(0, 0, 0, 1),
-                 fixed_base=False,
-                 scale=1.,
+    def __init__(self, simulator, position=(0, 0, 0.085), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
                  urdf=os.path.dirname(__file__) + '/urdfs/youbot/youbot_dual_arm.urdf'):
+        """
+        Initialize the Youbot dual arm robot.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the robot base will be fixed in the world.
+            scale (float): scaling factor that is used to scale the robot.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
         # check parameters
         if position is None:
             position = (0., 0., 0.085)
@@ -143,6 +169,40 @@ class YoubotDualArm(BiManipulator, DifferentialWheeledRobot):
         # self.wheel_directions = np.ones(len(self.wheels))
 
 
+class YoubotGripper(ParallelGripper):
+    r"""Youbot Gripper
+
+    References:
+        - [1] https://github.com/youbot
+    """
+
+    def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/youbot/youbot_gripper.urdf'):
+        """
+        Initialize the Youbot gripper.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            position (np.array[3]): Cartesian world position.
+            orientation (np.array[4]): Cartesian world orientation expressed as a quaternion [x,y,z,w].
+            fixed_base (bool): if True, the gripper will be fixed in the world.
+            scale (float): scaling factor that is used to scale the gripper.
+            urdf (str): path to the urdf. Do not change it unless you know what you are doing.
+        """
+        # 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 = True
+
+        super(YoubotGripper, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'youbot_gripper'
+
+
 # Test
 if __name__ == "__main__":
     from itertools import count