update robots, states, actions, costs, and envs

pyrobolearn/actions/__init__.py

@@ -3,10 +3,10 @@
 from .action import Action
 
 # import basic actions
-from .basic_actions import *
+from .basic_actions import FixedAction, FunctionalAction
 
 # import robot actions
 from .robot_actions import *
 
 # import gym actions
-from .gym_actions import *
+from .gym_actions import GymAction

pyrobolearn/actions/robot_actions/__init__.py

@@ -1,9 +1,13 @@
 
 # import the basic robot actions
-from .robot_actions import *
+from .robot_actions import RobotAction
 
 # import the joint actions
-from .joint_actions import *
+from .joint_actions import JointAction, JointPositionAction, JointPositionChangeAction, JointVelocityAction, \
+    JointVelocityChangeAction, JointPositionAndVelocityAction, JointPositionAndVelocityChangeAction, \
+    JointTorqueAction, JointForceAction, JointTorqueGravityCompensationAction, JointTorqueChangeAction, \
+    JointAccelerationAction
 
 # import the link / end-effector actions
-from .link_actions import *
+from .link_actions import LinkAction, LinkPositionAction, LinkPositionChangeAction, LinkVelocityAction, \
+    LinkVelocityChangeAction, LinkForceAction

pyrobolearn/actions/robot_actions/joint_actions.py

@@ -77,6 +77,17 @@ class JointPositionAction(JointAction):
     """
 
     def __init__(self, robot, joint_ids=None, kp=None, kd=None, max_force=None):
+        """
+        Initialize the joint position action.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id(s). If None, it will take all the actuated joints.
+            kp (float, np.array[N], None): position gain(s)
+            kd (float, np.array[N], None): velocity gain(s)
+            max_force (float, np.array[N], None, bool): maximum motor torques / forces. If True, it will apply the
+                default maximum force values.
+        """
         self.kp, self.kd, self.max_force = kp, kd, max_force
         super(JointPositionAction, self).__init__(robot, joint_ids)
         self.data = robot.get_joint_positions(self.joints)
@@ -107,6 +118,36 @@ class JointPositionAction(JointAction):
         return action
 
 
+class JointPositionChangeAction(JointPositionAction):
+    r"""Joint Position Change Action
+
+    Set the joint positions using position control; this class expect to receive a change in the joint positions
+    (i.e. instantaneous joint velocities). That is, the current joint positions are added to the given joint position
+    changes. If none are provided, it will stay at the current configuration.
+    """
+
+    def __init__(self, robot, joint_ids=None, kp=None, kd=None, max_force=None):
+        """
+        Initialize the joint position change action.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id(s). If None, it will take all the actuated joints.
+            kp (float, np.array[N], None): position gain(s)
+            kd (float, np.array[N], None): velocity gain(s)
+            max_force (float, np.array[N], None, bool): maximum motor torques / forces. If True, it will apply the
+                default maximum force values.
+        """
+        super(JointPositionChangeAction, self).__init__(robot, joint_ids, kp=kp, kd=kd, max_force=max_force)
+        self.data = np.zeros(len(self.joints))
+
+    def _write(self, data):
+        """apply the action data on the robot."""
+        # add the original joint positions
+        data += self.robot.get_joint_positions(self.joints)
+        super(JointPositionChangeAction, self)._write(data)
+
+
 class JointVelocityAction(JointAction):
     r"""Joint Velocity Action
 
@@ -114,6 +155,13 @@ class JointVelocityAction(JointAction):
     """
 
     def __init__(self, robot, joint_ids=None):
+        """
+        Initialize the joint velocity action.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints.
+        """
         super(JointVelocityAction, self).__init__(robot, joint_ids)
         self.data = robot.get_joint_velocities(self.joints)
 
@@ -122,14 +170,50 @@ class JointVelocityAction(JointAction):
         self.robot.set_joint_velocities(data, self.joints)
 
 
+class JointVelocityChangeAction(JointAction):
+    r"""Joint Velocity Change Action
+
+    Set the joint velocities using velocity control;  this class expect to receive a change in the joint velocities
+    (i.e. instantaneous joint accelerations). That is, the current joint velocities are added to the given joint
+    velocity changes. If none are provided, it will keep the current joint velocities.
+    """
+
+    def __init__(self, robot, joint_ids=None):
+        """
+        Initialize the joint velocity change action.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints.
+        """
+        super(JointVelocityChangeAction, self).__init__(robot, joint_ids)
+        self.data = np.zeros(len(self.joints))
+
+    def _write(self, data):
+        """apply the action data on the robot."""
+        data += self.robot.get_joint_velocities(self.joints)
+        super(JointVelocityChangeAction, self)._write(data)
+
+
 class JointPositionAndVelocityAction(JointAction):
     r"""Joint position and velocity action
 
-    Set the joint position using position control using PD control, where the contraint error to be minimized is
+    Set the joint position using position control using PD control, where the constraint error to be minimized is
     given by: :math:`error = kp * (q^* - q) - kd * (\dot{q}^* - \dot{q})`.
     """
 
     def __init__(self, robot, joint_ids=None, kp=None, kd=None, max_force=None):
+        """
+        Initialize the joint position and velocity action.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id(s). If None, it will take all the actuated joints.
+            kp (float, np.array[N], None): position gain(s)
+            kd (float, np.array[N], None): velocity gain(s)
+            max_force (float, np.array[N], None, bool): maximum motor torques / forces. If True, it will apply the
+                default maximum force values.
+        """
         super(JointPositionAndVelocityAction, self).__init__(robot, joint_ids)
         self.kp, self.kd, self.max_force = kp, kd, max_force
         pos, vel = robot.get_joint_positions(self.joints), robot.get_joint_velocities(self.joints)
@@ -163,6 +247,35 @@ class JointPositionAndVelocityAction(JointAction):
         return action
 
 
+class JointPositionAndVelocityChangeAction(JointPositionAndVelocityAction):
+    r"""Joint position and velocity action
+
+    Set the joint position using position control using PD control, where the constraint error to be minimized is
+    given by: :math:`error = kp * (q^* - q) - kd * (\dot{q}^* - \dot{q})`.
+    """
+
+    def __init__(self, robot, joint_ids=None, kp=None, kd=None, max_force=None):
+        """
+        Initialize the joint position and velocity change action.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id(s). If None, it will take all the actuated joints.
+            kp (float, np.array[N], None): position gain(s)
+            kd (float, np.array[N], None): velocity gain(s)
+            max_force (float, np.array[N], None, bool): maximum motor torques / forces. If True, it will apply the
+                default maximum force values.
+        """
+        super(JointPositionAndVelocityChangeAction, self).__init__(robot, joint_ids, kp=kp, kd=kd, max_force=max_force)
+        self.data = np.zeros(2*len(self.joints))
+
+    def _write(self, data):
+        """apply the action data on the robot."""
+        pos, vel = self.robot.get_joint_positions(self.joints), self.robot.get_joint_velocities(self.joints)
+        data += np.concatenate((pos, vel))
+        super(JointPositionAndVelocityChangeAction, self)._write(data)
+
+
 # class JointPositionVelocityAccelerationAction(JointAction):
 #     r"""Set the joint positions, velocities, and accelerations.
 #
@@ -172,14 +285,23 @@ class JointPositionAndVelocityAction(JointAction):
 #     pass
 
 
-class JointForceAction(JointAction):
-    r"""Joint Force Action
+class JointTorqueAction(JointAction):
+    r"""Joint Torque/Force Action
 
     Set the joint force/torque using force/torque control.
     """
 
     def __init__(self, robot, joint_ids=None, f_min=-np.infty, f_max=np.infty):
-        super(JointForceAction, self).__init__(robot, joint_ids)
+        """
+        Initialize the joint torque/force action.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints.
+            f_min (float, np.array[N], None): minimum torques/forces.
+            f_max (float, np.array[N], None): maximum torques/forces.
+        """
+        super(JointTorqueAction, self).__init__(robot, joint_ids)
         self.data = robot.get_joint_torques(self.joints)
         self.f_min = f_min
         self.f_max = f_max
@@ -210,6 +332,42 @@ class JointForceAction(JointAction):
         return action
 
 
+# alias
+JointForceAction = JointTorqueAction
+
+
+class JointTorqueGravityCompensationAction(JointTorqueAction):
+    r"""Joint torque action with gravity compensation enabled.
+
+    This adds the given torques to the gravity compensation torques. That is, if a torque of 0 is provided, the robot
+    will be in a gravity compensation mode.
+    """
+    def __init__(self, robot, joint_ids=None, f_min=-np.infty, f_max=np.infty):
+        """
+        Initialize the joint torque/force action with gravity compensation.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints.
+            f_min (float, np.array[N], None): minimum torques/forces.
+            f_max (float, np.array[N], None): maximum torques/forces.
+        """
+        super(JointTorqueGravityCompensationAction, self).__init__(robot, joint_ids, f_min=f_min, f_max=f_max)
+        self.q_indices = self.robot.get_q_indices(joint_ids=self.joints)
+        self.data = np.zeros(len(self.joints))
+
+    def _write(self, data):
+        """apply the action data on the robot."""
+        # add gravity compensation torques
+        data += self.robot.get_gravity_compensation_torques(q_idx=self.q_indices)
+        super(JointTorqueGravityCompensationAction, self)._write(data)
+
+
+# alias
+# JointForceGravityCompensationAction = JointTorqueGravityCompensationAction
+JointTorqueChangeAction = JointTorqueGravityCompensationAction
+
+
 class JointAccelerationAction(JointAction):
     r"""Joint Acceleration Action
 
@@ -219,6 +377,15 @@ class JointAccelerationAction(JointAction):
     """
 
     def __init__(self, robot, joint_ids=None, a_min=-np.infty, a_max=np.infty):
+        """
+        Initialize the joint acceleration action.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints.
+            a_min (float, np.array[N], None): minimum accelerations.
+            a_max (float, np.array[N], None): maximum accelerations.
+        """
         super(JointAccelerationAction, self).__init__(robot, joint_ids)
         self.data = robot.get_joint_accelerations(self.joints)
         self.a_min = a_min

pyrobolearn/actions/robot_actions/link_actions.py

@@ -6,9 +6,10 @@ This includes notably the link positions, velocities, and force/torque actions.
 
 import copy
 from abc import ABCMeta
+import numpy as np
 
 from pyrobolearn.actions.robot_actions.robot_actions import RobotAction
-
+from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -60,46 +61,216 @@ class LinkAction(RobotAction):
 
 
 class LinkPositionAction(LinkAction):
-    r"""Link position action
+    r"""Link world position action
 
-    Set the position using IK for the specified robot link(s).
+    Set the world position using IK for the specified robot link(s).
     """
 
     def __init__(self, robot, link_ids=None):
         """
-        Initialize the link position action.
+        Initialize the link world position action.
 
         Args:
             robot (Robot): robot instance
             link_ids (int, int[N]): link id or list of link ids
         """
         super(LinkPositionAction, self).__init__(robot, link_ids)
+        self.data = self.robot.get_link_world_positions(link_ids=self.links, flatten=True)  # (N*3,)
 
     def _write(self, data):
         """apply the action data on the robot."""
-        self.robot.set_link_positions(self.links, data)
+        self.robot.set_link_positions(link_ids=self.links, positions=data.reshape(-1, 3))
 
 
-class LinkVelocityAction(LinkAction):
-    r"""Link velocity action
+class LinkPositionChangeAction(LinkPositionAction):
+    r"""Link world position change action
 
-    Set the cartesian velocity(ies) for the specified robot link(s).
+    Set the world position using IK for the specified robot link(s). Instead of specifying directly the desired
+    cartesian position(s), the amount of change in the current positions is provided.
     """
 
     def __init__(self, robot, link_ids=None):
         """
-        Initialize the link position action.
+        Initialize the link world position change action.
+
+        Args:
+            robot (Robot): robot instance
+            link_ids (int, int[N]): link id or list of link ids
+        """
+        super(LinkPositionChangeAction, self).__init__(robot, link_ids)
+        self.data = np.zeros(len(self.links) * 3)
+
+    def _write(self, data):
+        """apply the action data on the robot."""
+        data += self.robot.get_link_world_positions(link_ids=self.links)
+        super(LinkPositionChangeAction, self)._write(data)
+
+
+class LinkOrientationAction(LinkAction):
+    r"""Link world orientation action
+
+    Set the world orientation using IK for the specified robot link(s).
+    """
+
+    def __init__(self, robot, link_ids=None):
+        """
+        Initialize the link world orientation action.
+
+        Args:
+            robot (Robot): robot instance
+            link_ids (int, int[N]): link id or list of link ids
+        """
+        super(LinkOrientationAction, self).__init__(robot, link_ids)
+        self.data = self.robot.get_link_world_orientations(link_ids=self.links, flatten=True)  # (N*4,)
+
+    def _write(self, data):
+        """apply the action data on the robot."""
+        self.robot.set_link_positions(link_ids=self.links, orientations=data.reshape(-1, 4))
+
+
+class LinkOrientationChangeAction(LinkOrientationAction):
+    r"""Link world orientation change action
+
+    Set the world orientation using IK for the specified robot link(s). Instead of specifying directly the desired
+    cartesian orientation(s), the amount of change in the current orientations is provided.
+
+    Warnings: the difference in orientations should be provided as a change in roll-pitch-yaw angles (in radians), and
+    not as quaternions.
+    """
+
+    def __init__(self, robot, link_ids=None):
+        """
+        Initialize the link world orientation change action.
+
+        Args:
+            robot (Robot): robot instance
+            link_ids (int, int[N]): link id or list of link ids
+        """
+        super(LinkOrientationChangeAction, self).__init__(robot, link_ids)
+        self.data = np.zeros(len(self.links) * 4)
+
+    def _write(self, data):
+        """apply the action data on the robot."""
+        # get current orientations and convert them to RPY angles
+        orientations = self.robot.get_link_world_orientations(link_ids=self.links).reshape(-1, 4)  # (N,4)
+        orientations = get_rpy_from_quaternion(orientations)  # (N,3)
+
+        # add change in orientations
+        data = data.reshape(-1, 3)  # (N,3)
+        data += orientations
+
+        # convert them back to quaternions
+        data = get_quaternion_from_rpy(data)  # (N,4)
+        super(LinkOrientationChangeAction, self)._write(data)
+
+
+class LinkPoseAction(LinkAction):
+    r"""Link world pose action
+
+    Set the world pose using IK for the specified robot link(s).
+    """
+
+    def __init__(self, robot, link_ids=None):
+        """
+        Initialize the link world pose action.
+
+        Args:
+            robot (Robot): robot instance
+            link_ids (int, int[N]): link id or list of link ids
+        """
+        super(LinkPoseAction, self).__init__(robot, link_ids)
+        self.data = self.robot.get_link_world_poses(link_ids=self.links, flatten=True)
+
+    def _write(self, data):
+        """apply the action data on the robot."""
+        data = data.reshape(-1, 7)
+        self.robot.set_link_positions(link_ids=self.links, positions=data[:, :3], orientations=data[:, 3:])
+
+
+class LinkPoseChangeAction(LinkPoseAction):
+    r"""Link world change pose action
+
+    Set the world pose using IK for the specified robot link(s). Instead of specifying directly the desired
+    cartesian pose(s), the amount of change in the current poses is provided.
+
+    Warnings: the difference in orientations should be provided as a change in roll-pitch-yaw angles (in radians), and
+    not as quaternions.
+    """
+
+    def __init__(self, robot, link_ids=None):
+        """
+        Initialize the link world pose change action.
+
+        Args:
+            robot (Robot): robot instance
+            link_ids (int, int[N]): link id or list of link ids
+        """
+        super(LinkPoseChangeAction, self).__init__(robot, link_ids)
+        self.data = np.zeros(len(self.links) * 6)
+
+    def _write(self, data):
+        """apply the action data on the robot."""
+        # get current poses
+        positions = self.robot.get_link_world_poses(link_ids=self.links, flatten=False).reshape(-1, 3)  # (N,3)
+        orientations = self.robot.get_link_world_orientations(link_ids=self.links, flatten=False).reshape(-1, 4)  # (N,4)
+        orientations = get_rpy_from_quaternion(orientations)  # (N,3)
+
+        # add changes
+        data = data.reshape(-1, 6)  # (N,6)
+        data[:, :3] += positions
+        data[:, 3:] += orientations
+
+        # convert back orientations to quaternions
+        data = np.hstack((data[:, :3], get_quaternion_from_rpy(data[:, 3:])))
+
+        # write poses
+        super(LinkPoseChangeAction, self)._write(data)
+
+
+class LinkVelocityAction(LinkAction):
+    r"""Link world velocity action
+
+    Set the cartesian world velocity(ies) for the specified robot link(s).
+    """
+
+    def __init__(self, robot, link_ids=None):
+        """
+        Initialize the link world velocity action.
 
         Args:
             robot (Robot): robot instance
             link_ids (int, int[N]): link id or list of link ids
         """
         super(LinkVelocityAction, self).__init__(robot, link_ids)
+        self.data = self.robot.get_link_world_velocities(link_ids=self.links)
 
     def _write(self, data):
         """apply the action data on the robot."""
-        # self.robot
-        pass
+        self.robot.set_link_velocities(link_ids=self.links, positions=data.reshape(-1, 6))
+
+
+class LinkVelocityChangeAction(LinkVelocityAction):
+    r"""Link world velocity change action
+
+    Set the cartesian world velocity(ies) for the specified robot link(s). Instead of specifying directly the desired
+    cartesian velocity(ies), the amount of change in the current velocities is provided.
+    """
+
+    def __init__(self, robot, link_ids=None):
+        """
+        Initialize the link world velocity change action.
+
+        Args:
+            robot (Robot): robot instance
+            link_ids (int, int[N]): link id or list of link ids
+        """
+        super(LinkVelocityAction, self).__init__(robot, link_ids)
+        self.data = np.zeros(len(self.links) * 6)
+
+    def _write(self, data):
+        """apply the action data on the robot."""
+        data += self.robot.get_link_world_velocities(link_ids=self.links)
+        super(LinkVelocityChangeAction, self)._write(data)
 
 
 class LinkForceAction(LinkAction):
@@ -109,7 +280,7 @@ class LinkForceAction(LinkAction):
     """
     def __init__(self, robot, link_ids=None):
         """
-        Initialize the link position action.
+        Initialize the link force action.
 
         Args:
             robot (Robot): robot instance

pyrobolearn/envs/control/control.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+"""Provide the abstract control environment from which all the other control environments inherit from.
+"""
+
+from pyrobolearn.envs.env import Env
+
+
+__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 ControlEnv(Env):
+    r"""Control Environment (abstract)
+
+    This is the abstract control environment from which all control environments inherit from.
+    """
+
+    def __init__(self, world, states, rewards=None, terminal_conditions=None, initial_state_generators=None,
+                 physics_randomizers=None, extra_info=None, actions=None):
+        """
+        Initialize the control environment.
+
+        Args:
+            world (World): world of the environment. The world contains all the objects (including robots), and has
+                access to the simulator.
+            states ((list of) State): states that are returned by the environment at each time step.
+            rewards (None, Reward): The rewards can be None when for instance we are in an imitation learning setting,
+                instead of a reinforcement learning one. If None, only the state is returned by the environment.
+            terminal_conditions (None, callable, TerminalCondition, list of TerminalCondition): A callable function or
+                object that check if the policy has failed or succeeded the task.
+            initial_state_generators (None, StateGenerator, list of StateGenerator): state generators which are used
+                when resetting the environment to generate the initial states.
+            physics_randomizers (None, PhysicsRandomizer, list of PhysicsRandomizer): physics randomizers. This will be
+                called each time you reset the environment.
+            extra_info (None, callable): Extra info returned by the environment at each time step.
+            actions ((list of) Action): actions that are given to the environment. Note that this is not used here in
+                the current environment as it should be the policy that performs the action. This is useful when
+                creating policies after the environment (that is, the policy can uses the environment's states and
+                actions).
+        """
+        super(ControlEnv, self).__init__(world=world, states=states, rewards=rewards,
+                                         terminal_conditions=terminal_conditions,
+                                         initial_state_generators=initial_state_generators,
+                                         physics_randomizers=physics_randomizers, extra_info=extra_info,
+                                         actions=actions)

pyrobolearn/envs/control/pendulum.py

@@ -0,0 +1,97 @@
+#!/usr/bin/env python
+"""Provide the inverted pendulum swing-up environment.
+
+This is based on the control problem proposed in OpenAI Gym:
+"The inverted pendulum swingup problem is a classic problem in the control literature. In this version of the problem,
+the pendulum starts in a random position, and the goal is to swing it up so it stays upright." [1]
+
+References:
+    - [1] Pendulum environment in OpenAI Gym: https://gym.openai.com/envs/Pendulum-v0/
+"""
+
+import os
+import numpy as np
+
+import pyrobolearn as prl
+from pyrobolearn.envs.control.control import ControlEnv
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["OpenAI", "Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class InvertedPendulumSwingUpEnv(ControlEnv):
+    r"""Inverted Pendulum Swing-up Environment
+
+    This is based on the control problem proposed in OpenAI Gym:
+    "The inverted pendulum swingup problem is a classic problem in the control literature. In this version of the
+    problem, the pendulum starts in a random position, and the goal is to swing it up so it stays upright." [1]
+
+    Here are the various environment features:
+
+    - world: basic world with gravity, a basic floor, and the pendulum.
+    - state: the state is given by :math:`[np.cos(q_1), np.sin(q_1), dq_1]`
+    - action: the action is the joint torque :math:`\tau_1`
+    - reward:
+
+    References:
+        - [1] Pendulum environment in OpenAI Gym: https://gym.openai.com/envs/Pendulum-v0/
+    """
+
+    def __init__(self, simulator=None, verbose=False):
+        """
+        Initialize the inverted pendulum swing-up environment.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            verbose (bool): if True, it will print information when creating the environment
+        """
+        # create basic world
+        world = prl.worlds.BasicWorld(simulator)
+        robot = world.load_robot('pendulum')
+        robot.disable_motor()
+        # robot.print_info()
+
+        # create state
+        position_state = prl.states.JointTrigonometricPositionState(robot=robot)
+        velocity_state = prl.states.JointVelocityState(robot=robot)
+        state = position_state + velocity_state
+
+        # create action
+        action = prl.actions.JointTorqueAction(robot, f_min=-2., f_max=2.)
+
+        # create reward
+        position_cost = prl.rewards.JointPositionCost(prl.states.JointPositionState(robot),
+                                                      np.zeros(len(robot.joints)))
+        velocity_cost = prl.rewards.JointVelocityCost(velocity_state)
+        torque_cost = prl.rewards.JointTorqueCost(prl.states.JointForceTorqueState(robot=robot))
+        reward = position_cost + 0.1 * velocity_cost + 0.001 * torque_cost
+
+        # create initial state generator
+        initial_state_generator = None
+
+        # create environment using composition
+        super(InvertedPendulumSwingUpEnv, self).__init__(world=world, states=state, rewards=reward, actions=action,
+                                                         initial_state_generators=initial_state_generator)
+
+
+# Test
+if __name__ == "__main__":
+    from itertools import count
+    import pyrobolearn as prl
+
+    # create simulator
+    sim = prl.simulators.Bullet()
+
+    # create environment
+    env = InvertedPendulumSwingUpEnv(sim)
+
+    # run simulation
+    for _ in count():
+        env.step(sleep_dt=1./240)

pyrobolearn/envs/env.py

@@ -27,7 +27,7 @@ from pyrobolearn.states.generators import StateGenerator
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "GNU GPLv3"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"
@@ -299,7 +299,7 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
         states = [state.reset() for state in self.states]
         return self._convert_state_to_data(states)
 
-    def step(self, actions=None):
+    def step(self, actions=None, sleep_dt=None):
         """
         Run one timestep of the environment's dynamics. When end of episode is reached, you are responsible for
         calling `reset()` to reset this environment's state. Accepts an action and returns a tuple (observation,
@@ -312,6 +312,7 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
                 can appear by providing the actions in the environment instead of letting the policy executes them.
                 For instance, think about when there are multiple policies, when using multiprocessing, or when the
                 environment runs in real-time.
+            sleep_dt (float):
 
         Returns:
             observation (object): agent's observation of the current environment
@@ -335,7 +336,7 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
         #     actions()
 
         # perform a step forward in the simulation which computes all the dynamics
-        self.world.step()
+        self.world.step(sleep_dt=sleep_dt)
 
         # compute reward
         # rewards = [reward.compute() for reward in self.rewards]

pyrobolearn/envs/manipulation/manipulation.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+"""Provide the abstract manipulation environment from which all the other manipulation environments inherit from.
+"""
+
+from pyrobolearn.envs.env import Env
+
+
+__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 ManipulationEnv(Env):
+    r"""Manipulation Environment (abstract)
+
+    This is the abstract manipulation environment from which all manipulation environments inherit from.
+    """
+
+    def __init__(self, world, states, rewards=None, terminal_conditions=None, initial_state_generators=None,
+                 physics_randomizers=None, extra_info=None, actions=None):
+        """
+        Initialize the manipulation environment.
+
+        Args:
+            world (World): world of the environment. The world contains all the objects (including robots), and has
+                access to the simulator.
+            states ((list of) State): states that are returned by the environment at each time step.
+            rewards (None, Reward): The rewards can be None when for instance we are in an imitation learning setting,
+                instead of a reinforcement learning one. If None, only the state is returned by the environment.
+            terminal_conditions (None, callable, TerminalCondition, list of TerminalCondition): A callable function or
+                object that check if the policy has failed or succeeded the task.
+            initial_state_generators (None, StateGenerator, list of StateGenerator): state generators which are used
+                when resetting the environment to generate the initial states.
+            physics_randomizers (None, PhysicsRandomizer, list of PhysicsRandomizer): physics randomizers. This will be
+                called each time you reset the environment.
+            extra_info (None, callable): Extra info returned by the environment at each time step.
+            actions ((list of) Action): actions that are given to the environment. Note that this is not used here in
+                the current environment as it should be the policy that performs the action. This is useful when
+                creating policies after the environment (that is, the policy can uses the environment's states and
+                actions).
+        """
+        super(ManipulationEnv, self).__init__(world=world, states=states, rewards=rewards,
+                                              terminal_conditions=terminal_conditions,
+                                              initial_state_generators=initial_state_generators,
+                                              physics_randomizers=physics_randomizers, extra_info=extra_info,
+                                              actions=actions)

pyrobolearn/envs/manipulation/reaching.py

@@ -0,0 +1,130 @@
+#!/usr/bin/env python
+"""Provide the reaching manipulation environment.
+
+The goal is to reach a certain 3D (fixed or movable) target with the end-effector of a robot.
+"""
+
+import re
+import numpy as np
+
+import pyrobolearn as prl
+from pyrobolearn.envs.manipulation.manipulation import ManipulationEnv
+
+
+__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 ReachingManipulationEnv(ManipulationEnv):
+    r"""Reaching Manipulation Env
+
+    The goal is to reach a certain 3D (fixed or movable) target with the end-effector of a robot.
+    """
+
+    def __init__(self, world, target=(0.5, 0., 0.5), robot='kuka_iiwa', end_effector_id=None, control_mode='position',
+                 *args, **kwargs):
+        """
+        Initialize the reaching manipulation environment.
+
+        Args:
+            world (World, Simulator): world/simulator instance.
+            target (list/tuple of 3 floats, np.array[3], Body): target to reach. If a list, tuple, or array is
+                provided, it will load a visual sphere at the specified target location.
+            robot (str, Robot): robot instance, or robot name to load in the world.
+            end_effector_id (int, None): end effector link id that has to reach the target. If None, it will check in
+                the end_
+            control_mode (str): joint action control mode, select between {'position', 'position change', 'velocity',
+                'velocity change', 'torque', 'torque with gravity compensation'/'torque change'}. Note that 'torque
+                change' and 'torque with gravity compensation' are the same (they are synonyms).
+            args (list): list of arguments that are given to the `world.load_robot` method.
+            kwargs (dict): dict of arguments that are given to the `world.load_robot` method.
+        """
+        # create basic world if not already created
+        if isinstance(world, prl.simulators.Simulator):
+            world = prl.worlds.BasicWorld(world)
+        elif not isinstance(world, prl.worlds.World):
+            raise TypeError("Expecting the world to be an instance of `World` or `Simulator`, instead got: "
+                            "{}".format(type(world)))
+
+        # load robot
+        if isinstance(robot, str):
+            robot = world.load_robot(robot, *args, **kwargs)
+        elif isinstance(robot, prl.robots.Robot):
+            # check if robot already loaded
+            if robot.id not in world.bodies:
+                world.load_robot(robot)
+        else:
+            raise TypeError("Expecting the given robot to be a string or an instance of `Robot`, instead got: "
+                            "{}".format(type(robot)))
+        self.robot = robot
+
+        # load target
+        if not isinstance(target, prl.robots.Body):
+            if not isinstance(target, (list, tuple, np.ndarray)):
+                raise TypeError("Expecting the target to be list/tuple/array of 3 floats representing the target "
+                                "position, or an instance of `Body`, but got instead: {}".format(type(target)))
+            target = world.load_visual_sphere(position=target, radius=0.05, color=(1, 0, 0, 0.5), return_body=True)
+        # save target body such that the user can use it (to change its position for instance)
+        self.target = target
+
+        # check end effector id
+        if end_effector_id is None:
+            if not hasattr(robot, 'end_effectors'):
+                raise ValueError("We could not find any end effectors for the given robot... Please specify one by "
+                                 "setting the 'end_effector_id' parameter.")
+            end_effector_id = robot.end_effectors[0]
+        if not isinstance(end_effector_id, int):
+            raise TypeError("Expecting the 'end_effector_id' to be None or an integer, but got instead: "
+                            "{}".format(type(end_effector_id)))
+
+        # create state
+        state = prl.states.LinkWorldPositionState(robot, link_ids=end_effector_id)
+
+        # create action based on the specified joint action control mode
+        control_mode = control_mode.lower()
+        control_mode = ' '.join(re.findall(r"[a-z]*[^\-\_]", control_mode))
+        if control_mode == 'position':
+            action = prl.actions.JointPositionAction(robot)
+        elif control_mode == 'position change':
+            action = prl.actions.JointPositionChangeAction(robot)
+        elif control_mode == 'velocity':
+            action = prl.actions.JointVelocityAction(robot)
+        elif control_mode == 'velocity change':
+            action = prl.actions.JointVelocityChangeAction(robot)
+        elif control_mode == 'torque':
+            action = prl.actions.JointTorqueAction(robot)
+        elif control_mode == 'torque with gravity compensation' or control_mode == 'torque change':
+            action = prl.actions.JointTorqueGravityCompensationAction(robot)
+        else:
+            raise ValueError("Please select the `control_mode` to be between ['position', 'position change', "
+                             "'velocity', 'velocity change', 'torque', "
+                             "'torque with gravity compensation'/'torque change'], and not: "
+                             "{}".format(control_mode))
+
+        # create distance cost
+        reward = prl.rewards.DistanceCost(state, target)
+
+        # create environment using composition
+        super(ReachingManipulationEnv, self).__init__(world=world, states=state, rewards=reward, actions=action)
+
+
+# Test
+if __name__ == "__main__":
+    from itertools import count
+    import pyrobolearn as prl
+
+    # create simulator
+    sim = prl.simulators.Bullet()
+
+    # create environment
+    env = ReachingManipulationEnv(sim)
+
+    # run simulation
+    for _ in count():
+        env.step(sleep_dt=1./240)

pyrobolearn/models/promp/promp.py

@@ -259,7 +259,7 @@ class ProMP(object):  # Model
     @property
     def total_num_basis(self):
         """Return the total number of basis functions"""
-        return self.Phi.shape[0] / self.num_dofs
+        return int(self.Phi.shape[0] / self.num_dofs)
 
     @property
     def basis_matrix(self):

pyrobolearn/rewards/cost.py

@@ -62,13 +62,16 @@ def min_angle_difference(q1, q2):
     Return the minimum angle difference between two angles.
 
     Args:
-        q1 (Cost, float): first angle
-        q2 (Cost, float): second angle
+        q1 (float, np.array[N]): first angle(s)
+        q2 (float, np.array[N]): second angle(s)
 
     Returns:
-        callable, float: minimum angle difference
+        float, np.array[N]: minimum angle difference(s)
     """
-    return
+    diff = np.maximum(q1, q2) - np.minimum(q1, q2)
+    if diff > np.pi:
+        diff = 2 * np.pi - diff
+    return diff
 
 
 class AngularVelocityErrorCost(Cost):
@@ -80,7 +83,7 @@ class AngularVelocityErrorCost(Cost):
     the sensitivity factor, and :math:`\hat{\omega}` and :math:`\omega` being the target and current angular velocity.
 
     References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
     """
 
     def __init__(self, angular_velocity_state, target_angular_velocity_state, sensitivity):
@@ -89,7 +92,7 @@ class AngularVelocityErrorCost(Cost):
         self.target_state = target_angular_velocity_state
         self.sensitivity = sensitivity
 
-    def compute(self):
+    def _compute(self):
         error = np.linalg.norm(self.target_state.data - self.state.data)
         return - logistic_kernel_function(error, self.sensitivity)
 
@@ -103,7 +106,7 @@ class LinearVelocityErrorCost(Cost):
     the sensitivity factor, and :math:`\hat{v}` and :math:`v` being the target and current linear velocity.
 
     References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
     """
 
     def __init__(self, velocity_state, velocity_target_state, sensitivity):
@@ -112,7 +115,7 @@ class LinearVelocityErrorCost(Cost):
         self.target_state = velocity_target_state
         self.sensitivity = sensitivity
 
-    def compute(self):
+    def _compute(self):
         error = np.linalg.norm(self.target_state.data - self.state.data)
         return - logistic_kernel_function(error, self.sensitivity)
 
@@ -123,7 +126,7 @@ class HeightCost(Cost):
     Height cost defined in [1] as :math:`cost = 1.0` if height < threshold, otherwise 0.
 
     References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
     """
 
     def __init__(self, height_state, threshold):
@@ -131,7 +134,7 @@ class HeightCost(Cost):
         self.height = height_state
         self.threshold = threshold
 
-    def compute(self):
+    def _compute(self):
         if self.height.data < self.threshold:
             return -1.
         return 0
@@ -145,7 +148,7 @@ class JointPositionErrorCost(Cost):
     current angles.
 
     References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
     """
 
     def __init__(self, joint_state, target_joint_state):
@@ -153,15 +156,15 @@ class JointPositionErrorCost(Cost):
         self.state = joint_state
         self.target_state = target_joint_state
 
-    def compute(self):
-        return - min_angle_difference(self.state.data, self.target_state.data)
+    def _compute(self):
+        return - min_angle_difference(self.state.data[0], self.target_state.data[0])
 
 
 class OrientationGravityCost(Cost):
     r"""Orientation Gravity Cost
 
     References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
     """
 
     def __init__(self, gravity_state, gravity_vector=[0., 0., -1.]):
@@ -169,23 +172,82 @@ class OrientationGravityCost(Cost):
         self.gravity_state = gravity_state
         self.gravity = np.array(gravity_vector)
 
-    def compute(self):
+    def _compute(self):
         return np.linalg.norm(self.gravity_state.data - self.gravity)
 
 
-class TorqueCost(Cost):
+class JointPositionCost(Cost):
+    r"""Joint Position Cost
+
+    Return the cost such that measures the L2 norm between the current joint positions and the target joint positions:
+    :math:`||d(q_{target},q)||^2` where :math:`d(\cdot, \cdot) \in [-\pi, \pi]` is the minimum distance between two
+    angles.
+    """
+
+    def __init__(self, joint_position_state, target_joint_position):
+        r"""
+        Initialize the joint position cost.
+
+        Args:
+            joint_position_state (JointPositionState): joint position state.
+            target_joint_position (np.array[N]): target joint positions.
+        """
+        super(JointPositionCost, self).__init__()
+        if not isinstance(joint_position_state, prl.states.JointPositionState):
+            raise TypeError("Expecting the given 'joint_position_state' to be an instance of `JointPositionState`, "
+                            "but instead got: {}".format(type(joint_position_state)))
+        self.q = joint_position_state
+        self.target_q = target_joint_position
+
+    def _compute(self):
+        """Compute and return the cost value."""
+        return - np.sum(min_angle_difference(self.q.data[0], self.target_q)**2)
+
+
+class JointVelocityCost(Cost):
+    r"""Joint Velocity Cost
+
+    Return the cost due to the joint velocities: :math:`|| \dot{q} ||^2`
+    """
+    def __init__(self, joint_velocity_state):
+        """
+        Initialize the joint velocity cost.
+
+        Args:
+            joint_velocity_state (JointVelocityState): joint velocity state.
+        """
+        super(JointVelocityCost, self).__init__()
+        if not isinstance(joint_velocity_state, prl.states.JointVelocityState):
+            raise TypeError("Expecting the given 'joint_velocity_state' to be an instance of `JointVelocityState`, "
+                            "but instead got: {}".format(type(joint_velocity_state)))
+        self.dq = joint_velocity_state
+
+    def _compute(self):
+        """Compute and return the cost value."""
+        return - np.sum(self.dq.data[0] ** 2)
+
+
+class JointTorqueCost(Cost):
     r"""Torque Cost
 
-    Return the cost due to the torques; :math:`cost = ||\tau||^2`.
-
-    References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+    Return the cost due to the joint torques; :math:`|| \tau ||^2`.
     """
-    def __init__(self, torque_state):
-        super(TorqueCost, self).__init__()
-        self.tau = torque_state
 
-    def compute(self):
+    def __init__(self, joint_torque_state):
+        """
+        Initialize the joint torque cost.
+
+        Args:
+            joint_torque_state (JointForceTorqueState): joint torque state.
+        """
+        super(JointTorqueCost, self).__init__()
+        if not isinstance(joint_torque_state, prl.states.JointForceTorqueState):
+            raise TypeError("Expecting the given 'joint_torque_state' to be an instance of `JointForceTorqueState`, "
+                            "but instead got: {}".format(type(joint_torque_state)))
+        self.tau = joint_torque_state
+
+    def _compute(self):
+        """Compute and return the cost value."""
         return - np.sum(self.tau.data[0]**2)
 
 
@@ -195,7 +257,7 @@ class PowerCost(Cost):
     Return the power consumption cost, where the power is computed as the torque times the velocity.
 
     References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
     """
     def __init__(self, torque_state, velocity_state):
         super(PowerCost, self).__init__()
@@ -256,7 +318,7 @@ class JointAccelerationCost(Cost):
     Return the joint acceleration cost defined notably in [1] as :math:`cost = ||\ddot{q}||^2`
 
     References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
     """
     def __init__(self, joint_acceleration_state):
         super(JointAccelerationCost, self).__init__()
@@ -272,7 +334,7 @@ class JointSpeedCost(Cost):
     Return the joint speed cost as computed in [1].
 
     References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
     """
     def __init__(self, joint_velocity_state, max_joint_speed=None):
         super(JointSpeedCost, self).__init__()
@@ -291,21 +353,21 @@ class BodyImpulseCost(Cost):
     Return the body impulse cost as computed in [1].
 
     References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
     """
     def __init__(self, robot):
         super(BodyImpulseCost, self).__init__()
         self.robot = robot
 
     def compute(self):
-        return
+        pass
 
 
 class BodySlippageCost(Cost):
     r"""Body Slippage Cost
 
     References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
     """
     def __init__(self):
         super(BodySlippageCost, self).__init__()
@@ -322,8 +384,8 @@ class FootSlippageCost(Cost):
     measures the distance between the two bodies and a contact force' [2]
 
     References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
-        [2] Unilateral Contact (Wikipedia): https://en.wikipedia.org/wiki/Unilateral_contact
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [2] Unilateral Contact (Wikipedia): https://en.wikipedia.org/wiki/Unilateral_contact
     """
     def __init__(self):
         super(FootSlippageCost, self).__init__()
@@ -340,8 +402,8 @@ class FootClearanceCost(Cost):
     measures the distance between the two bodies and a contact force' [2]
 
     References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
-        [2] Unilateral Contact (Wikipedia): https://en.wikipedia.org/wiki/Unilateral_contact
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [2] Unilateral Contact (Wikipedia): https://en.wikipedia.org/wiki/Unilateral_contact
     """
     def __init__(self):
         super(FootClearanceCost, self).__init__()
@@ -354,7 +416,7 @@ class SelfCollisionCost(Cost):
     r"""Self Collision Cost
 
     References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
     """
     def __init__(self):
         super(SelfCollisionCost, self).__init__()
@@ -367,7 +429,7 @@ class ActionDifferenceCost(Cost):
     r"""Action Difference Cost
 
     References:
-        [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
     """
     def __init__(self, action):
         super(ActionDifferenceCost, self).__init__()
@@ -383,7 +445,8 @@ class PhysicsViolationCost(Cost):
     This cost defines ...
     It was formally defined in [1]. It accepts two arguments.
 
-    [1] 'Automated Discovery and Learning of Complex Movement Behaviors' (PhD thesis), Mordatch, 2015
+    References:
+    - [1] 'Automated Discovery and Learning of Complex Movement Behaviors' (PhD thesis), Mordatch, 2015
 
     .. seealso: `cio.py` in 'pyrobolearn/optim' which uses this cost.
     """
@@ -421,7 +484,7 @@ class DistanceCost(Cost):
     second body. The distance function used is the Euclidean distance (=L2 norm).
     """
 
-    def __init__(self, body1, body2, link_id1=-1, link_id2=-1):
+    def __init__(self, body1, body2, link_id1=-1, link_id2=-1, offset=None):
         r"""
         Initialize the distance cost.
 
@@ -436,6 +499,7 @@ class DistanceCost(Cost):
                 the given :attr:`body1` is not a state.
             link_id2 (int): link id associated with the second body that we are interested in. This is only used if
                 the given :attr:`body2` is not a state.
+            offset (None, np.array[3]): 3d offset between body1 and body2.
         """
         super(DistanceCost, self).__init__()
 
@@ -466,8 +530,8 @@ class DistanceCost(Cost):
             self.body2()
         p1 = self.body1.data[0]
         p2 = self.body2.data[0]
-        print("P1: {}".format(p1))
-        print("P2: {}".format(p2))
+        # print("P1: {}".format(p1))
+        # print("P2: {}".format(p2))
         return - np.linalg.norm(p1 - p2)
 
 

pyrobolearn/robots/README.md

@@ -74,6 +74,7 @@ The folder contains different kind of robots including manipulators, legged robo
 - [Shadow hand](https://github.com/shadow-robot/sr_common)
 - [Soft hand](https://github.com/CentroEPiaggio/pisa-iit-soft-hand)
 - [Swimmer](https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf)
+- [UR](https://github.com/ros-industrial/universal_robot)
 - [Valkyrie](https://github.com/openhumanoids/val_description)
 - [Walker 2D](https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf)
 - [Walk-man](https://github.com/ADVRHumanoids/iit-walkman-ros-pkg)

pyrobolearn/robots/__init__.py

@@ -62,6 +62,10 @@ from .phantomx import PhantomX
 from .morphex import Morphex
 from .rhex import Rhex
 
+# Control
+from .acrobot import Acrobot
+from .pendulum import Pendulum
+
 # Manipulators
 from .rrbot import RRBot
 from .wam import WAM, BarrettHand
@@ -73,6 +77,7 @@ from .sawyer import Sawyer
 from .edo import Edo
 from .kr5 import KR5
 from .manipulator2d import Manipulator2D
+from .ur import UR3, UR5, UR10
 
 # Bi-Manipulators
 from .baxter import Baxter, BaxterGripper

pyrobolearn/robots/acrobot.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python
+"""Provide the acrobot robotic platform.
+"""
+
+import os
+import numpy as np
+
+from pyrobolearn.robots.robot import Robot
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class Acrobot(Robot):  # TODO: create the acrobot dynamically instead of loading from the URDF
+    r"""Acrobot
+
+    Note that in the URDF, the continuous joints were replace by revolute joints. Be careful, that the limit values
+    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,
+                 urdf=os.path.dirname(__file__) + '/urdfs/rrbot/acrobot.urdf'):
+        """
+        Initialize the acrobot 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.)
+        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(Acrobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'acrobot'
+
+        # set initial joint positions
+        self.reset_joint_states(q=[np.pi, 0.], joint_ids=self.joints)
+
+    def get_force_torque_sensor(self, idx=0):
+        return np.array(self.sim.getJointState(self.id, 2)[2])
+
+
+# Test
+if __name__ == "__main__":
+    from itertools import count
+    import pyrobolearn as prl
+
+    # create simulator
+    sim = prl.simulators.Bullet()
+
+    # create world
+    world = prl.worlds.BasicWorld(sim)
+
+    # load robot
+    robot = Acrobot(sim)
+    robot.disable_motor()
+    robot.print_info()
+    # robot.add_joint_slider()
+
+    # run simulation
+    for _ in count():
+        world.step(sleep_dt=1./240)

pyrobolearn/robots/gripper.py

@@ -2,8 +2,10 @@
 """Provide the Gripper abstract classes.
 """
 
+from pyrobolearn.simulators.simulator import Simulator
 from pyrobolearn.robots.robot import Robot
 
+
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2019, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
@@ -59,7 +61,26 @@ class Gripper(Robot):
         Close the gripper. This has to be implemented in the child class.
 
         Args:
-            factor (float): float representing how much the gripper is closed (1=completely close, 0=clompletely open).
+            factor (float): float representing how much the gripper is closed (1=completely close, 0=completely open).
+        """
+        pass
+
+    def grasp(self, strength, point=None, frame=Simulator.LINK_FRAME):
+        """
+        Grasp an object. Compared to :func:`~open` or :func:`~close` which uses position control, this uses force
+        (impedance) control using attractor points. The strength factor increases the stiffness of the grasping; i.e.
+        it increases the applied torques in the gripper joints.
+
+        Args:
+            strength (float): scalar describing how much to increases the stiffness (the torques that are applied on
+                the joint fingers). If positive, it closes the gripper fingers. If negative, it opens the gripper
+                fingers.
+            point (np.array[3], list of np.array[3], None): attractor point(s) described in the specified frame.
+                If multiple points are specified, they have to match the number of fingers and will be used in the
+                same order. If None, it will grasp in a "natural" way (which is let to the user that has implemented
+                this method).
+            frame (int): integer describing if the above given point is described in the world frame
+                (``Simulator.WORLD_FRAME``), or in the link frame of the gripper base (``Simulator.LINK_FRAME``).
         """
         pass
 

pyrobolearn/robots/hand.py

@@ -33,6 +33,118 @@ class Hand(AngularGripper):
         """
         super(Hand, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
 
+    def get_fist_configuration(self):
+        """Return the joint configuration for the hand (and fingers) to form a fist."""
+        pass
+
+    def get_open_configuration(self):
+        """Return the joint configuration for the hand (and fingers) to form an open hand (where the fingers are
+        attached/close to each other)."""
+        pass
+
+    def get_fully_open_configuration(self):
+        """Return the joint configuration for the hand (and fingers) to form a fully open hand (where the distance
+        between the fingers is maximal)."""
+        pass
+
+    def get_thumb_up_configuration(self, factor=1.):
+        """
+        Return the joint configuration for the fingers to have the thumb up.
+
+        Args:
+            factor (float): 1 = completely up, 0 = completely down
+        """
+        pass
+
+    def get_thumb_down_configuration(self, factor=1.):
+        """
+        Return the joint configuration for the fingers to have the thumb up.
+
+        Args:
+            factor (float): 1 = completely down, 0 = completely up
+        """
+        return self.get_thumb_up_configuration(factor=1.-factor)
+
+    def get_index_up_configuration(self, factor=1.):
+        """
+        Return the joint configuration for the fingers to have the index finger up.
+
+        Args:
+            factor (float): 1 = completely up, 0 = completely down
+        """
+        pass
+
+    def get_index_down_configuration(self, factor=1.):
+        """
+        Return the joint configuration for the fingers to have the index finger up.
+
+        Args:
+            factor (float): 1 = completely down, 0 = completely up
+        """
+        return self.get_index_up_configuration(factor=1.-factor)
+
+    def get_middle_up_configuration(self, factor=1.):
+        """
+        Return the joint configuration for the fingers to have the middle finger up.
+
+        Args:
+            factor (float): 1 = completely up, 0 = completely down
+        """
+        pass
+
+    def get_middle_down_configuration(self, factor=1.):
+        """
+        Return the joint configuration for the fingers to have the middle finger up.
+
+        Args:
+            factor (float): 1 = completely down, 0 = completely up
+        """
+        return self.get_middle_up_configuration(factor=1.-factor)
+
+    def get_ring_up_configuration(self, factor=1.):
+        """
+        Return the joint configuration for the fingers to have the ring finger up.
+
+        Args:
+            factor (float): 1 = completely up, 0 = completely down
+        """
+        pass
+
+    def get_ring_down_configuration(self, factor=1.):
+        """
+        Return the joint configuration for the fingers to have the ring finger up.
+
+        Args:
+            factor (float): 1 = completely down, 0 = completely up
+        """
+        return self.get_ring_up_configuration(factor=1.-factor)
+
+    def get_pinky_up_configuration(self,  factor=1.):
+        """
+        Return the joint configuration for the fingers to have the pinky finger up.
+
+        Args:
+            factor (float): 1 = completely up, 0 = completely down
+        """
+        pass
+
+    def get_pinky_down_configuration(self,  factor=1.):
+        """
+        Return the joint configuration for the fingers to have the pinky finger up.
+
+        Args:
+            factor (float): 1 = completely down, 0 = completely up
+        """
+        return self.get_pinky_up_configuration(factor=1.-factor)
+
+    def get_ok_configuration(self):
+        """Return the joint configuration for the fingers to perform the OK or ring gesture.
+
+        References:
+            - https://en.wikipedia.org/wiki/OK_gesture
+        """
+        pass
+
 
 class TwoHand(Hand):
     r"""Two hand end-effectors

pyrobolearn/robots/pendulum.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python
+"""Provide the pendulum robotic platform.
+"""
+
+import os
+import numpy as np
+
+from pyrobolearn.robots.robot import Robot
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class Pendulum(Robot):  # TODO: create the pendulum dynamically instead of loading from the URDF
+    r"""Pendulum
+
+    Note that in the URDF, the continuous joints were replace by revolute joints. Be careful, that the limit values
+    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,
+                 urdf=os.path.dirname(__file__) + '/urdfs/rrbot/pendulum.urdf'):
+        """
+        Initialize the pendulum 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.)
+        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(Pendulum, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'pendulum'
+
+        # set initial joint positions
+        self.reset_joint_states(q=[np.pi / 4], joint_ids=self.joints)
+
+    def get_force_torque_sensor(self, idx=0):
+        return np.array(self.sim.getJointState(self.id, 2)[2])
+
+
+# Test
+if __name__ == "__main__":
+    from itertools import count
+    import pyrobolearn as prl
+
+    # create simulator
+    sim = prl.simulators.Bullet()
+
+    # create world
+    world = prl.worlds.BasicWorld(sim)
+
+    # load robot
+    robot = Pendulum(sim)
+    robot.disable_motor()
+    robot.print_info()
+    # robot.add_joint_slider()
+
+    # run simulation
+    for _ in count():
+        world.step(sleep_dt=1./240)

pyrobolearn/robots/robot.py

@@ -1089,8 +1089,8 @@ class Robot(ControllableBody):
         Set the velocity of the given joint(s) (using velocity control).
 
         Args:
-            joint_ids (int, int[N], None): joint id, or list of joint ids. If None, get all the actuated joints.
             velocities (float, np.array[N]): desired velocity, or list of desired velocities [rad/s]
+            joint_ids (int, int[N], None): joint id, or list of joint ids. If None, get all the actuated joints.
             forces (float, np.array[N], None, bool): maximum motor torques / forces. If True, it will apply the
                 default maximum force values.
             max_velocity (float, bool, None): if True, it will make sure that the given velocity(ies) are below their
@@ -1684,6 +1684,34 @@ class Robot(ControllableBody):
             q.reshape(-1)
         return q
 
+    def get_link_world_poses(self, link_ids=None, flatten=True):
+        r"""
+        Return the CoM pose (position and orientation (expressed as a quaternion [x,y,z,w] in the Cartesian world
+        space) of the given link(s).
+
+        Args:
+            link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the pose of all links
+                associated to actuated joints.
+            flatten (bool): if True, it will return a 1D array of float numbers instead of a 2D array of shape [N,7].
+
+        Returns:
+            if 1 link:
+                np.array[7]: Cartesian pose of the link CoM
+            if multiple links:
+                np.array[N*7], np.array[N,7]: CoM pose of each link
+        """
+        # get positions and orientations
+        positions = self.get_link_world_positions(link_ids=link_ids, flatten=False)  # (N,3)
+        orientations = self.get_link_world_orientations(link_ids=link_ids, flatten=False)  # (N,4)
+
+        # concatenate to form the pose
+        poses = np.hstack((positions, orientations))  # (N,7)
+
+        # check if we need to flatten the 2D array
+        if flatten:
+            return poses.reshape(-1)  # (N*7,)
+        return poses  # (N,7)
+
     def get_link_world_linear_velocities(self, link_ids=None, flatten=True):
         r"""
         Return the linear velocity of the link(s) expressed in the Cartesian world space coordinates.

pyrobolearn/robots/rrbot.py

@@ -5,7 +5,8 @@
 import os
 import numpy as np
 
-from pyrobolearn.robots.manipulator import Manipulator
+from pyrobolearn.robots.robot import Robot
+
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -16,7 +17,7 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class RRBot(Manipulator):
+class RRBot(Robot):
     r"""RRBot
 
     Note that in the URDF, the continuous joints were replace by revolute joints. Be careful, that the limit values

pyrobolearn/robots/ur.py

@@ -0,0 +1,153 @@
+#!/usr/bin/env python
+"""Provide the universal robot platforms.
+
+Specifically, it provides the classes for UR3, UR5, and UR10.
+
+References:
+    - [1] Universal robots: https://www.universal-robots.com/
+    - [2] UR description: https://github.com/ros-industrial/universal_robot
+"""
+
+import os
+
+from pyrobolearn.robots.manipulator import Manipulator
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class UR3(Manipulator):
+    r"""UR3 manipulator
+
+    References:
+        - [1] Universal robots: https://www.universal-robots.com/
+        - [2] UR description: https://github.com/ros-industrial/universal_robot
+    """
+
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=True, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/ur/ur3.urdf'):
+        """
+        Initialize the UR3 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.)
+        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(UR3, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'ur3'
+
+
+class UR5(Manipulator):
+    r"""UR5 manipulator
+
+    References:
+        - [1] Universal robots: https://www.universal-robots.com/
+        - [2] UR description: https://github.com/ros-industrial/universal_robot
+    """
+
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=True, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/ur/ur5.urdf'):
+        """
+        Initialize the UR3 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.)
+        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(UR5, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'ur5'
+
+
+class UR10(Manipulator):
+    r"""UR10 manipulator
+
+    References:
+        - [1] Universal robots: https://www.universal-robots.com/
+        - [2] UR description: https://github.com/ros-industrial/universal_robot
+    """
+
+    def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=True, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/ur/ur10.urdf'):
+        """
+        Initialize the UR3 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.)
+        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(UR10, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'ur10'
+
+
+# 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
+    ur3 = UR3(sim, position=[0., -1., 0.])
+    ur5 = UR5(sim, position=[0., 0., 0.])
+    ur10 = UR10(sim, position=[0., 1., 0.])
+
+    # print information about the robot
+    ur3.print_info()
+    # H = ur3.get_mass_matrix()
+    # print("Inertia matrix: H(q) = {}".format(H))
+
+    for i in count():
+        # step in simulation
+        world.step(sleep_dt=1./240)

pyrobolearn/robots/urdfs/references

@@ -38,6 +38,7 @@ Here is the list of repos where you can find the original URDF/meshes of each ro
     * Baxter: https://github.com/RethinkRobotics/baxter_common
     * Jaco: https://github.com/JenniferBuehler/jaco-arm-pkgs
     * e.DO: https://github.com/Comau/eDO_description
+    * UR: https://github.com/ros-industrial/universal_robot
 
 * Hexapod
     * phantomx: https://github.com/HumaRobotics/phantomx_description

pyrobolearn/robots/urdfs/rrbot/acrobot.urdf

@@ -0,0 +1,169 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from rrbot.xacro                    | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<!-- Revolute-Revolute Manipulator -->
+<robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!-- Space btw top of beam and the each joint -->
+  <!-- ros_control plugin -->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/rrbot</robotNamespace>
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+    </plugin>
+  </gazebo>
+  <!-- Link1 -->
+  <gazebo reference="link1">
+    <material>Gazebo/Orange</material>
+  </gazebo>
+  <!-- Link2 -->
+  <gazebo reference="link2">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <!-- Link3 -->
+  <gazebo reference="link3">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/Orange</material>
+  </gazebo>
+
+  <material name="black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <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.2 0.2 0.2 1.0"/>
+  </material>
+  <material name="orange">
+    <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+  </material>
+  <material name="brown">
+    <color rgba="0.870588235294 0.811764705882 0.764705882353 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>
+
+  <!-- Used for fixing robot to Gazebo 'base_link' -->
+  <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="fixed" type="fixed">
+    <parent link="world"/>
+    <child link="link1"/>
+  </joint>
+  <!-- Base Link -->
+  <link name="link1">
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 1.0"/>
+      <geometry>
+        <box size="0.1 0.1 2"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 1.0"/>
+      <geometry>
+        <box size="0.1 0.1 2"/>
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 1.0"/>
+      <mass value="1"/>
+      <inertia ixx="0.334166666667" ixy="0.0" ixz="0.0" iyy="0.334166666667" iyz="0.0" izz="0.00166666666667"/>
+    </inertial>
+  </link>
+  <joint name="joint1" type="continuous">
+    <parent link="link1"/>
+    <child link="link2"/>
+    <origin rpy="0 0 0" xyz="0 0.1 1.95"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="0.7"/>
+  </joint>
+  <!-- Middle Link -->
+  <link name="link2">
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.45"/>
+      <geometry>
+        <box size="0.1 0.1 1"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.45"/>
+      <geometry>
+        <box size="0.1 0.1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.45"/>
+      <mass value="1"/>
+      <inertia ixx="0.0841666666667" ixy="0.0" ixz="0.0" iyy="0.0841666666667" iyz="0.0" izz="0.00166666666667"/>
+    </inertial>
+  </link>
+  <joint name="joint2" type="continuous">
+    <parent link="link2"/>
+    <child link="link3"/>
+    <origin rpy="0 0 0" xyz="0 0.1 0.9"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="0.7"/>
+  </joint>
+  <!-- Top Link -->
+  <link name="link3">
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.45"/>
+      <geometry>
+        <box size="0.1 0.1 1"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.45"/>
+      <geometry>
+        <box size="0.1 0.1 1"/>
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.45"/>
+      <mass value="1"/>
+      <inertia ixx="0.0841666666667" ixy="0.0" ixz="0.0" iyy="0.0841666666667" iyz="0.0" izz="0.00166666666667"/>
+    </inertial>
+  </link>
+
+  <transmission name="tran1">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint1">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor1">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="tran2">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint2">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor2">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+</robot>
+

pyrobolearn/robots/urdfs/rrbot/pendulum.urdf

@@ -0,0 +1,125 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from rrbot.xacro                    | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<!-- Revolute-Revolute Manipulator -->
+<robot name="pendulum" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!-- Space btw top of beam and the each joint -->
+  <!-- ros_control plugin -->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/rrbot</robotNamespace>
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+    </plugin>
+  </gazebo>
+  <!-- Link1 -->
+  <gazebo reference="link1">
+    <material>Gazebo/Orange</material>
+  </gazebo>
+  <!-- Link2 -->
+  <gazebo reference="link2">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/Black</material>
+  </gazebo>
+
+  <material name="black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <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.2 0.2 0.2 1.0"/>
+  </material>
+  <material name="orange">
+    <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+  </material>
+  <material name="brown">
+    <color rgba="0.870588235294 0.811764705882 0.764705882353 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>
+
+  <!-- Used for fixing robot to Gazebo 'base_link' -->
+  <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="fixed" type="fixed">
+    <parent link="world"/>
+    <child link="link1"/>
+  </joint>
+  <!-- Base Link -->
+  <link name="link1">
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 1.0"/>
+      <geometry>
+        <box size="0.1 0.1 2"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 1.0"/>
+      <geometry>
+        <box size="0.1 0.1 2"/>
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 1.0"/>
+      <mass value="1"/>
+      <inertia ixx="0.334166666667" ixy="0.0" ixz="0.0" iyy="0.334166666667" iyz="0.0" izz="0.00166666666667"/>
+    </inertial>
+  </link>
+  <joint name="joint1" type="continuous">
+    <parent link="link1"/>
+    <child link="link2"/>
+    <origin rpy="0 0 0" xyz="0 0.1 1.95"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="0.7"/>
+  </joint>
+  <!-- Middle Link -->
+  <link name="link2">
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.45"/>
+      <geometry>
+        <box size="0.1 0.1 1"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.45"/>
+      <geometry>
+        <box size="0.1 0.1 1"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.45"/>
+      <mass value="1"/>
+      <inertia ixx="0.0841666666667" ixy="0.0" ixz="0.0" iyy="0.0841666666667" iyz="0.0" izz="0.00166666666667"/>
+    </inertial>
+  </link>
+
+  <transmission name="tran1">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint1">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor1">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+</robot>
+

pyrobolearn/robots/urdfs/ur/LICENSE

@@ -0,0 +1,27 @@
+Copyright (c) 2012, Wim Meeussen, Kelsey Hawkins, Mathias Ludtke, Felix Messmer
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+1. Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright
+   notice, this list of conditions and the following disclaimer in the
+   documentation and/or other materials provided with the distribution.
+3. All advertising materials mentioning features or use of this software
+   must display the following acknowledgement:
+   This product includes software developed by the <organization>.
+4. Neither the name of the <organization> nor the
+   names of its contributors may be used to endorse or promote products
+   derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY <COPYRIGHT HOLDER> ''AS IS'' AND ANY
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

pyrobolearn/robots/urdfs/ur/ur10.urdf

@@ -0,0 +1,338 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from ur10_robot.urdf.xacro          | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="ur10" xmlns:xacro="http://wiki.ros.org/xacro">
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="ros_control">
+      <!--robotNamespace>/</robotNamespace-->
+      <!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
+    </plugin>
+    <!--
+    <plugin name="gazebo_ros_power_monitor_controller" filename="libgazebo_ros_power_monitor.so">
+      <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>
+  <!--
+  Author: Kelsey Hawkins
+  Contributers: Jimmy Da Silva, Ajit Krisshna N L, Muhammad Asif Rana
+-->
+  <!-- measured from model -->
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur10/visual/base.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur10/collision/base.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="4.0"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
+    </inertial>
+  </link>
+  <joint name="shoulder_pan_joint" type="revolute">
+    <parent link="base_link"/>
+    <child link="shoulder_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.1273"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="330.0" lower="-6.28318530718" upper="6.28318530718" velocity="2.16"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="shoulder_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur10/visual/shoulder.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur10/collision/shoulder.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="7.778"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.0314743125769" ixy="0.0" ixz="0.0" iyy="0.0314743125769" iyz="0.0" izz="0.021875625"/>
+    </inertial>
+  </link>
+  <joint name="shoulder_lift_joint" type="revolute">
+    <parent link="shoulder_link"/>
+    <child link="upper_arm_link"/>
+    <origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.220941 0.0"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="330.0" lower="-6.28318530718" upper="6.28318530718" velocity="2.16"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="upper_arm_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur10/visual/upperarm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur10/collision/upperarm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="12.93"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.306"/>
+      <inertia ixx="0.421753803798" ixy="0.0" ixz="0.0" iyy="0.421753803798" iyz="0.0" izz="0.036365625"/>
+    </inertial>
+  </link>
+  <joint name="elbow_joint" type="revolute">
+    <parent link="upper_arm_link"/>
+    <child link="forearm_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1719 0.612"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="forearm_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur10/visual/forearm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur10/collision/forearm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="3.87"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.28615"/>
+      <inertia ixx="0.111069694097" ixy="0.0" ixz="0.0" iyy="0.111069694097" iyz="0.0" izz="0.010884375"/>
+    </inertial>
+  </link>
+  <joint name="wrist_1_joint" type="revolute">
+    <parent link="forearm_link"/>
+    <child link="wrist_1_link"/>
+    <origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.0 0.5723"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="54.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="wrist_1_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur10/visual/wrist1.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur10/collision/wrist1.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.0051082479567" ixy="0.0" ixz="0.0" iyy="0.0051082479567" iyz="0.0" izz="0.0055125"/>
+    </inertial>
+  </link>
+  <joint name="wrist_2_joint" type="revolute">
+    <parent link="wrist_1_link"/>
+    <child link="wrist_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.1149 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="54.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="wrist_2_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur10/visual/wrist2.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur10/collision/wrist2.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.96"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.0051082479567" ixy="0.0" ixz="0.0" iyy="0.0051082479567" iyz="0.0" izz="0.0055125"/>
+    </inertial>
+  </link>
+  <joint name="wrist_3_joint" type="revolute">
+    <parent link="wrist_2_link"/>
+    <child link="wrist_3_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.1157"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="54.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="wrist_3_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur10/visual/wrist3.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur10/collision/wrist3.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.202"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.000526462289415" ixy="0.0" ixz="0.0" iyy="0.000526462289415" iyz="0.0" izz="0.000568125"/>
+    </inertial>
+  </link>
+  <joint name="ee_fixed_joint" type="fixed">
+    <parent link="wrist_3_link"/>
+    <child link="ee_link"/>
+    <origin rpy="0.0 0.0 1.57079632679" xyz="0.0 0.0922 0.0"/>
+  </joint>
+  <link name="ee_link">
+    <collision>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="-0.01 0 0"/>
+    </collision>
+    <inertial> <!-- added by Brian for pybullet -->
+      <mass value="0.001"/>
+      <origin rpy="0 0 0" xyz="-0.01 0.0 0.0"/>
+      <inertia ixx="1.6667e-8" ixy="0.0" ixz="0.0" iyy="1.6667e-8" iyz="0.0" izz="1.6667e-8"/>
+    </inertial>
+  </link>
+  <transmission name="shoulder_pan_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_pan_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="shoulder_pan_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="shoulder_lift_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_lift_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="shoulder_lift_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="elbow_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="elbow_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="elbow_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_1_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_1_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_1_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_2_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_2_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_2_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_3_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_3_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_3_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <gazebo reference="shoulder_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="upper_arm_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="forearm_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="wrist_1_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="wrist_3_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="wrist_2_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="ee_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+
+  <!-- ROS base_link to UR 'Base' Coordinates transform -->
+  <!--link name="base"/>
+  <joint name="base_link-base_fixed_joint" type="fixed">
+    <!-- NOTE: this rotation is only needed as long as base_link itself is
+                 not corrected wrt the real robot (ie: rotated over 180
+                 degrees)-->
+    <!--origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <!-- Frame coincident with all-zeros TCP on UR controller -->
+  <!--link name="tool0"/>
+  <joint name="wrist_3_link-tool0_fixed_joint" type="fixed">
+    <origin rpy="-1.57079632679 0 0" xyz="0 0.0922 0"/>
+    <parent link="wrist_3_link"/>
+    <child link="tool0"/>
+  </joint>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+  </joint-->
+</robot>
+

pyrobolearn/robots/urdfs/ur/ur3.urdf

@@ -0,0 +1,337 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from ur3_robot.urdf.xacro           | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="ur3" xmlns:xacro="http://wiki.ros.org/xacro">
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="ros_control">
+      <!--robotNamespace>/</robotNamespace-->
+      <!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
+    </plugin>
+    <!--
+    <plugin name="gazebo_ros_power_monitor_controller" filename="libgazebo_ros_power_monitor.so">
+      <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>
+  <!--
+  Author: Felix Messmer
+-->
+  <!-- measured from model -->
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur3/visual/base.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur3/collision/base.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="2.0"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.0030531654454" ixy="0.0" ixz="0.0" iyy="0.0030531654454" iyz="0.0" izz="0.005625"/>
+    </inertial>
+  </link>
+  <joint name="shoulder_pan_joint" type="revolute">
+    <parent link="base_link"/>
+    <child link="shoulder_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.1519"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="330.0" lower="-6.28318530718" upper="6.28318530718" velocity="2.16"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="shoulder_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur3/visual/shoulder.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur3/collision/shoulder.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="2.0"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.0080931634294" ixy="0.0" ixz="0.0" iyy="0.0080931634294" iyz="0.0" izz="0.005625"/>
+    </inertial>
+  </link>
+  <joint name="shoulder_lift_joint" type="revolute">
+    <parent link="shoulder_link"/>
+    <child link="upper_arm_link"/>
+    <origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.1198 0.0"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="330.0" lower="-6.28318530718" upper="6.28318530718" velocity="2.16"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="upper_arm_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur3/visual/upperarm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur3/collision/upperarm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="3.42"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.121825"/>
+      <inertia ixx="0.0217284832211" ixy="0.0" ixz="0.0" iyy="0.0217284832211" iyz="0.0" izz="0.00961875"/>
+    </inertial>
+  </link>
+  <joint name="elbow_joint" type="revolute">
+    <parent link="upper_arm_link"/>
+    <child link="forearm_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.0925 0.24365"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="forearm_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur3/visual/forearm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur3/collision/forearm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.26"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.106625"/>
+      <inertia ixx="0.00654680644378" ixy="0.0" ixz="0.0" iyy="0.00654680644378" iyz="0.0" izz="0.00354375"/>
+    </inertial>
+  </link>
+  <joint name="wrist_1_joint" type="revolute">
+    <parent link="forearm_link"/>
+    <child link="wrist_1_link"/>
+    <origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.0 0.21325"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="54.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="wrist_1_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur3/visual/wrist1.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur3/collision/wrist1.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.8"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.002084999166" ixy="0.0" ixz="0.0" iyy="0.002084999166" iyz="0.0" izz="0.00225"/>
+    </inertial>
+  </link>
+  <joint name="wrist_2_joint" type="revolute">
+    <parent link="wrist_1_link"/>
+    <child link="wrist_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.08505 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="54.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="wrist_2_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur3/visual/wrist2.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur3/collision/wrist2.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.8"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.002084999166" ixy="0.0" ixz="0.0" iyy="0.002084999166" iyz="0.0" izz="0.00225"/>
+    </inertial>
+  </link>
+  <joint name="wrist_3_joint" type="revolute">
+    <parent link="wrist_2_link"/>
+    <child link="wrist_3_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.08535"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="54.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="wrist_3_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur3/visual/wrist3.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur3/collision/wrist3.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.35"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.000912187135125" ixy="0.0" ixz="0.0" iyy="0.000912187135125" iyz="0.0" izz="0.000984375"/>
+    </inertial>
+  </link>
+  <joint name="ee_fixed_joint" type="fixed">
+    <parent link="wrist_3_link"/>
+    <child link="ee_link"/>
+    <origin rpy="0.0 0.0 1.57079632679" xyz="0.0 0.0819 0.0"/>
+  </joint>
+  <link name="ee_link">
+    <collision>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="-0.01 0 0"/>
+    </collision>
+    <inertial> <!-- added by Brian for pybullet -->
+      <mass value="0.001"/>
+      <origin rpy="0 0 0" xyz="-0.01 0.0 0.0"/>
+      <inertia ixx="1.6667e-8" ixy="0.0" ixz="0.0" iyy="1.6667e-8" iyz="0.0" izz="1.6667e-8"/>
+    </inertial>
+  </link>
+  <transmission name="shoulder_pan_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_pan_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="shoulder_pan_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="shoulder_lift_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_lift_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="shoulder_lift_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="elbow_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="elbow_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="elbow_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_1_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_1_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_1_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_2_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_2_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_2_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_3_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_3_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_3_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <gazebo reference="shoulder_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="upper_arm_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="forearm_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="wrist_1_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="wrist_3_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="wrist_2_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="ee_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+
+  <!-- ROS base_link to UR 'Base' Coordinates transform -->
+  <!--link name="base"/>
+  <joint name="base_link-base_fixed_joint" type="fixed">
+    <!-- NOTE: this rotation is only needed as long as base_link itself is
+                 not corrected wrt the real robot (ie: rotated over 180
+                 degrees) -->
+    <!--origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <!-- Frame coincident with all-zeros TCP on UR controller -->
+  <!--link name="tool0"/>
+  <joint name="wrist_3_link-tool0_fixed_joint" type="fixed">
+    <origin rpy="-1.57079632679 0 0" xyz="0 0.0819 0"/>
+    <parent link="wrist_3_link"/>
+    <child link="tool0"/>
+  </joint-->
+  <!--link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+  </joint-->
+</robot>
+

pyrobolearn/robots/urdfs/ur/ur5.urdf

@@ -0,0 +1,347 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from ur5_robot.urdf.xacro           | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="ur5" xmlns:xacro="http://wiki.ros.org/xacro">
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="ros_control">
+      <!--robotNamespace>/</robotNamespace-->
+      <!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
+    </plugin>
+    <!--
+    <plugin name="gazebo_ros_power_monitor_controller" filename="libgazebo_ros_power_monitor.so">
+      <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>
+  <!-- measured from model -->
+  <!--property name="shoulder_height" value="0.089159" /-->
+  <!--property name="shoulder_offset" value="0.13585" /-->
+  <!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 -->
+  <!--property name="upper_arm_length" value="0.42500" /-->
+  <!--property name="elbow_offset" value="0.1197" /-->
+  <!-- CAD measured -->
+  <!--property name="forearm_length" value="0.39225" /-->
+  <!--property name="wrist_1_length" value="0.093" /-->
+  <!-- CAD measured -->
+  <!--property name="wrist_2_length" value="0.09465" /-->
+  <!-- In CAD this distance is 0.930, but in the spec it is 0.09465 -->
+  <!--property name="wrist_3_length" value="0.0823" /-->
+  <!-- manually measured -->
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur5/visual/base.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur5/collision/base.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="4.0"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
+    </inertial>
+  </link>
+  <joint name="shoulder_pan_joint" type="revolute">
+    <parent link="base_link"/>
+    <child link="shoulder_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.089159"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="shoulder_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur5/visual/shoulder.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur5/collision/shoulder.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="3.7"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.010267495893" ixy="0.0" ixz="0.0" iyy="0.010267495893" iyz="0.0" izz="0.00666"/>
+    </inertial>
+  </link>
+  <joint name="shoulder_lift_joint" type="revolute">
+    <parent link="shoulder_link"/>
+    <child link="upper_arm_link"/>
+    <origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.13585 0.0"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="upper_arm_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur5/visual/upperarm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur5/collision/upperarm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="8.393"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.28"/>
+      <inertia ixx="0.22689067591" ixy="0.0" ixz="0.0" iyy="0.22689067591" iyz="0.0" izz="0.0151074"/>
+    </inertial>
+  </link>
+  <joint name="elbow_joint" type="revolute">
+    <parent link="upper_arm_link"/>
+    <child link="forearm_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1197 0.425"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="forearm_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur5/visual/forearm.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur5/collision/forearm.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="2.275"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.25"/>
+      <inertia ixx="0.049443313556" ixy="0.0" ixz="0.0" iyy="0.049443313556" iyz="0.0" izz="0.004095"/>
+    </inertial>
+  </link>
+  <joint name="wrist_1_joint" type="revolute">
+    <parent link="forearm_link"/>
+    <child link="wrist_1_link"/>
+    <origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.0 0.39225"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="28.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="wrist_1_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur5/visual/wrist1.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur5/collision/wrist1.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.219"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
+    </inertial>
+  </link>
+  <joint name="wrist_2_joint" type="revolute">
+    <parent link="wrist_1_link"/>
+    <child link="wrist_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.093 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="28.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="wrist_2_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur5/visual/wrist2.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur5/collision/wrist2.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.219"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
+    </inertial>
+  </link>
+  <joint name="wrist_3_joint" type="revolute">
+    <parent link="wrist_2_link"/>
+    <child link="wrist_3_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="28.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="wrist_3_link">
+    <visual>
+      <geometry>
+        <mesh filename="meshes/ur5/visual/wrist3.dae"/>
+      </geometry>
+      <material name="LightGrey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="meshes/ur5/collision/wrist3.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1879"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.0171364731454" ixy="0.0" ixz="0.0" iyy="0.0171364731454" iyz="0.0" izz="0.033822"/>
+    </inertial>
+  </link>
+  <joint name="ee_fixed_joint" type="fixed">
+    <parent link="wrist_3_link"/>
+    <child link="ee_link"/>
+    <origin rpy="0.0 0.0 1.57079632679" xyz="0.0 0.0823 0.0"/>
+  </joint>
+  <link name="ee_link">
+    <collision>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="-0.01 0 0"/>
+    </collision>
+    <inertial> <!-- added by Brian for pybullet -->
+      <mass value="0.001"/>
+      <origin rpy="0 0 0" xyz="-0.01 0.0 0.0"/>
+      <inertia ixx="1.6667e-8" ixy="0.0" ixz="0.0" iyy="1.6667e-8" iyz="0.0" izz="1.6667e-8"/>
+    </inertial>
+  </link>
+  <transmission name="shoulder_pan_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_pan_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="shoulder_pan_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="shoulder_lift_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_lift_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="shoulder_lift_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="elbow_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="elbow_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="elbow_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_1_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_1_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_1_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_2_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_2_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_2_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="wrist_3_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_3_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wrist_3_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <gazebo reference="shoulder_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="upper_arm_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="forearm_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="wrist_1_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="wrist_3_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="wrist_2_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="ee_link">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+
+  <!-- ROS base_link to UR 'Base' Coordinates transform -->
+  <!--link name="base"/>
+  <joint name="base_link-base_fixed_joint" type="fixed">
+    <!-- NOTE: this rotation is only needed as long as base_link itself is
+                 not corrected wrt the real robot (ie: rotated over 180
+                 degrees)-->
+    <!--origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <!-- Frame coincident with all-zeros TCP on UR controller -->
+  <!--link name="tool0"/>
+  <joint name="wrist_3_link-tool0_fixed_joint" type="fixed">
+    <origin rpy="-1.57079632679 0 0" xyz="0 0.0823 0"/>
+    <parent link="wrist_3_link"/>
+    <child link="tool0"/>
+  </joint>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+  </joint-->
+</robot>
+

pyrobolearn/states/robot_states/__init__.py

@@ -1,12 +1,15 @@
 
 # import the basic robot states
-from .robot_states import *
+from .robot_states import RobotState, BasePositionState, BaseHeightState, BaseOrientationState, BasePoseState, \
+    BaseLinearVelocityState, BaseAngularVelocityState, BaseVelocityState
 
 # import the joint states
-from .joint_states import *
+from .joint_states import JointState, JointPositionState, JointTrigonometricPositionState, JointVelocityState, \
+    JointForceTorqueState, JointAccelerationState
 
 # import the link states
-from .link_states import *
+from .link_states import LinkState, LinkPositionState, LinkWorldPositionState, LinkOrientationState, \
+    LinkVelocityState, LinkLinearVelocityState, LinkAngularVelocityState
 
 # import the sensor states
-from .sensor_states import *
+from .sensor_states import SensorState, CameraState, ContactState, FeetContactState

pyrobolearn/states/robot_states/joint_states.py

@@ -6,6 +6,8 @@ This includes notably the joint positions, velocities, and force/torque states.
 
 import copy
 from abc import ABCMeta
+import numpy as np
+from gym import spaces
 
 from pyrobolearn.states.robot_states.robot_states import RobotState, Robot
 
@@ -116,6 +118,46 @@ class JointPositionState(JointState):
         self.data = self.robot.get_joint_positions(self.joints)
 
 
+class JointTrigonometricPositionState(JointState):
+    r"""Joint Trigonometric Position State
+
+    Return the trigonometric joint positions as the state. That is, it returns
+    :math:`[\cos(q_1), \sin(q_1), ..., \cos(q_n), \sin(q_n)]`. All the values are between -1 and 1.
+    """
+
+    def __init__(self, robot, joint_ids=None, window_size=1, axis=None, ticks=1):
+        """
+        Initialize the trigonometric joint position state.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, int[N]): joint id or list of joint ids.
+            window_size (int): window size of the state. This is the total number of states we should remember. That
+                is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`,
+                the window size is 2. By default, the :attr:`window_size` is one which means we only remember the
+                current state. The window size has to be bigger than 1. If it is below, it will be set automatically
+                to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states,
+                but is given some :attr:`data`.
+            axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with
+                shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting
+                state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack
+                the states in the specified axis. With the example, for axis=0, the resulting state has a shape of
+                (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the
+                state is not a combination of states, but is given some :attr:`data`.
+            ticks (int): number of ticks to sleep before getting the next state data.
+        """
+        super(JointTrigonometricPositionState, self).__init__(robot, joint_ids, window_size=window_size, axis=axis,
+                                                              ticks=ticks)
+
+        high = np.ones(len(self.joints))
+        self._space = spaces.Box(low=-high, high=high, dtype=np.float32)
+
+    def _read(self):
+        """Read the next joint position state."""
+        q = self.robot.get_joint_positions(self.joints)
+        self.data = np.vstack((np.cos(q), np.sin(q))).T.reshape(-1)
+
+
 class JointVelocityState(JointState):
     r"""Joint Velocity State