refactor and clean sim, body, robot, and world

pyrobolearn/actions/action.py

@@ -916,13 +916,13 @@ class Action(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         actions = [copy.deepcopy(action, memo) for action in self.actions]
         data = copy.deepcopy(self._data)
         space = copy.deepcopy(self._space)
         action = self.__class__(actions=actions, data=data, space=space, name=self.name, ticks=self.ticks)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = action
-
         return action

pyrobolearn/actions/basic_actions.py

@@ -41,6 +41,8 @@ class FixedAction(Action):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         data = copy.deepcopy(self._data)
         action = self.__class__(value=data)
         memo[self] = action
@@ -70,6 +72,8 @@ class FunctionalAction(Action):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         function = copy.deepcopy(self.function)
         data = copy.deepcopy(self._data)
         action = self.__class__(function=function, initial_data=data)

pyrobolearn/actions/gym_actions.py

@@ -59,6 +59,8 @@ class GymAction(Action):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         env = copy.deepcopy(self.env)
         action = self.__class__(gym_env=env)
         memo[self] = action

pyrobolearn/actions/robot_actions/joint_actions.py

@@ -60,6 +60,8 @@ class JointAction(RobotAction):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         robot = copy.deepcopy(self.robot, memo)
         joints = copy.deepcopy(self.joints)
         action = self.__class__(robot=robot, joint_ids=joints)
@@ -94,6 +96,8 @@ class JointPositionAction(JointAction):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         robot = copy.deepcopy(self.robot, memo)
         joints = copy.deepcopy(self.joints)
         kp = copy.deepcopy(self.kp)
@@ -153,6 +157,8 @@ class JointPositionAndVelocityAction(JointAction):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         robot = copy.deepcopy(self.robot, memo)
         joints = copy.deepcopy(self.joints)
         kp = copy.deepcopy(self.kp)
@@ -201,6 +207,8 @@ class JointForceAction(JointAction):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         robot = copy.deepcopy(self.robot, memo)
         joints = copy.deepcopy(self.joints)
         f_min = copy.deepcopy(self.f_min)
@@ -241,6 +249,8 @@ class JointAccelerationAction(JointAction):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         robot = copy.deepcopy(self.robot, memo)
         joints = copy.deepcopy(self.joints)
         a_min = copy.deepcopy(self.a_min)

pyrobolearn/actions/robot_actions/link_actions.py

@@ -50,6 +50,8 @@ class LinkAction(RobotAction):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         robot = copy.deepcopy(self.robot, memo)
         links = copy.deepcopy(self.links)
         action = self.__class__(robot, links)

pyrobolearn/actions/robot_actions/robot_actions.py

@@ -59,6 +59,8 @@ class RobotAction(Action):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         robot = copy.deepcopy(self.robot, memo)
         action = self.__class__(robot=robot)
         memo[self] = action

pyrobolearn/actorcritics/actorcritic.py

@@ -162,15 +162,15 @@ class ActorCritic(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         policy = copy.deepcopy(self.actor, memo) if isinstance(self.actor, Policy) else copy.deepcopy(self.actor)
         value = copy.deepcopy(self.critic, memo) if isinstance(self.critic, ValueApproximator) \
             else copy.deepcopy(self.critic)
         actor_critic = self.__class__(policy=policy, value=value)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = actor_critic
-
         return actor_critic
 
 

pyrobolearn/approximators/approximator.py

@@ -526,6 +526,9 @@ class Approximator(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         def get_inputs_outputs(items):
             if isinstance(items, list):
                 elements = []
@@ -548,10 +551,7 @@ class Approximator(object):
         approximator = self.__class__(inputs=inputs, outputs=outputs, model=model,
                                       preprocessors=preprocessors, postprocessors=postprocessors)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = approximator
-
         return approximator
 
 

pyrobolearn/approximators/nn_approximator.py

@@ -82,11 +82,11 @@ class MLPApproximator(NNApproximator):
         """
 
         # check that the inputs and ouputs are 1D
-        # if not self._check1D(inputs):
+        # if not self._check_1d(inputs):
         #     raise ValueError("Length of input shape should be 1! Instead, got {}".format(inputs.shape))
         # print(outputs)
         # print(outputs.shape)
-        # if not self._check1D(outputs):
+        # if not self._check_1d(outputs):
         #     raise ValueError("Length of output shape should be 1! Instead, got {}".format(outputs.shape))
 
         input_size = self._size(inputs)
@@ -100,7 +100,8 @@ class MLPApproximator(NNApproximator):
         super(MLPApproximator, self).__init__(inputs, outputs, model, preprocessors=preprocessors,
                                               postprocessors=postprocessors)
 
-    def _check1D(self, arg):
+    @staticmethod
+    def _check_1d(arg):
         """Check that the given argument is a 1D vector, or simple array"""
         # if isinstance(arg, np.ndarray):
         shapes = arg.shape

pyrobolearn/dynamics/dynamic.py

@@ -327,6 +327,9 @@ class DynamicModel(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         state = copy.deepcopy(self.state, memo)
         action = copy.deepcopy(self.action, memo)
         next_state = copy.deepcopy(self.next_state, memo)
@@ -334,6 +337,7 @@ class DynamicModel(object):
         postprocessors = [copy.deepcopy(postprocessor, memo) for postprocessor in self.postprocessors]
         dynamic = self.__class__(state=state, action=action, next_state=next_state, preprocessors=preprocessors,
                                  postprocessors=postprocessors)
+
         memo[self] = dynamic
         return dynamic
 
@@ -540,6 +544,9 @@ class ParametrizedDynamicModel(DynamicModel):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         state = copy.deepcopy(self.state, memo)
         action = copy.deepcopy(self.action, memo)
         model = copy.deepcopy(self.model, memo)
@@ -550,6 +557,7 @@ class ParametrizedDynamicModel(DynamicModel):
         dynamic = self.__class__(state=state, action=action, model=model, next_state=next_state,
                                  distributions=distributions, preprocessors=preprocessors,
                                  postprocessors=postprocessors)
+
         memo[self] = dynamic
         return dynamic
 

pyrobolearn/envs/env.py

@@ -407,6 +407,8 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         world = copy.deepcopy(self.world, memo)
         states = [copy.deepcopy(state, memo) for state in self.states]
         rewards = None if self.rewards is None else copy.deepcopy(self.rewards, memo)
@@ -415,9 +417,11 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
         physics_randomizers = [copy.deepcopy(randomizer, memo) for randomizer in self.physics_randomizers]
         extra_info = copy.deepcopy(self.extra_info)
         actions = None if self.actions is None else [copy.deepcopy(action, memo) for action in self.actions]
-        return self.__class__(world=world, states=states, rewards=rewards, terminal_conditions=terminal_conditions,
-                              initial_state_generators=state_generators, physics_randomizers=physics_randomizers,
-                              extra_info=extra_info, actions=actions)
+        env = self.__class__(world=world, states=states, rewards=rewards, terminal_conditions=terminal_conditions,
+                             initial_state_generators=state_generators, physics_randomizers=physics_randomizers,
+                             extra_info=extra_info, actions=actions)
+        memo[self] = env
+        return env
 
 
 class BasicEnv(Env):

pyrobolearn/envs/gym_wrapper.py

@@ -295,9 +295,9 @@ class GymEnvWrapper(gym.Env):
             attribute = functools.partial(attribute)
         return attribute
 
-    def __repr__(self):
-        """Return a representing object."""
-        return self.env.__repr__()
+    # def __repr__(self):
+    #     """Return a representing object."""
+    #     return self.env.__repr__()
 
     def __str__(self):
         """Return a string describing the class."""

pyrobolearn/envs/locomotion_env.py

@@ -4,6 +4,7 @@
 Define the environment to perform a locomotion task; it mainly defines the reward function.
 """
 
+from pyrobolearn.simulators.simulator import Simulator
 from pyrobolearn.envs.env import Env
 from pyrobolearn.worlds import BasicWorld
 from pyrobolearn.states import State
@@ -20,7 +21,7 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class LocomotionEnv(Env):
+class LocomotionEnv(Env):  # TODO
     r"""Locomotion environment
 
     Define a simple environment for a locomotion task.
@@ -63,5 +64,5 @@ class LocomotionEnv(Env):
         terminal_condition = None
 
         super(LocomotionEnv, self).__init__(world, states, rewards=rewards,
-                                            terminal_condition=terminal_condition, extra_info=None)
+                                            terminal_conditions=terminal_condition, extra_info=None)
 

pyrobolearn/policies/policy.py

@@ -718,6 +718,9 @@ class Policy(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         states = copy.deepcopy(self.states, memo)
         actions = copy.deepcopy(self.actions, memo)
         model = copy.deepcopy(self.model, memo)
@@ -727,8 +730,5 @@ class Policy(object):
         policy = self.__class__(states=states, actions=actions, model=model, rate=rate,
                                 preprocessors=preprocessors, postprocessors=postprocessors)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = policy
-
         return policy

pyrobolearn/rewards/reward.py

@@ -301,17 +301,16 @@ class Reward(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         state = copy.deepcopy(self.state, memo)
         action = copy.deepcopy(self.action, memo)
         rewards = [copy.deepcopy(reward, memo) for reward in self.rewards]
         range = copy.deepcopy(self.range)
         reward = self.__class__(state=state, action=action, rewards=rewards, range=range)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = reward
-
-        # return the copy
         return reward
 
     # for unary and binary operators, see `__init__()` method.

pyrobolearn/robots/base.py

@@ -7,8 +7,6 @@ Dependencies:
 """
 
 import copy
-import numpy as np
-# import quaternion
 
 from pyrobolearn.simulators import Simulator
 from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_matrix_from_quaternion
@@ -70,8 +68,11 @@ class Body(object):
     @id.setter
     def id(self, body_id):
         """Set the unique body id."""
-        # if not isinstance(body_id, int):
-        #     raise TypeError("Expecting the given simulator to be an integer, instead got: {}".format(type(body_id)))
+        if not isinstance(body_id, (int, long)):
+            raise TypeError("Expecting the given 'body_id' to be an integer, instead got: {}".format(type(body_id)))
+        if body_id < 0:
+            raise ValueError("The given 'body_id' is not a valid one; it should be positive, unique, and returned by "
+                             "the simulator.")
         self._id = body_id
 
     @property
@@ -90,6 +91,17 @@ class Body(object):
             raise TypeError("Expecting the given name to be a string, instead got: {}".format(type(name)))
         self._name = name
 
+    @property
+    def is_only_visual(self):
+        """Return True if the body doesn't have any collision shapes (i.e. it is only a visual body in the simulator)"""
+        return len(self.sim.get_collision_shape_data(self.id)) == 0
+
+    @property
+    def is_movable(self):
+        """Return True if the body is not fixed in the world. A body is fixed if it has a base mass of 0 and has at
+        least one collision shape."""
+        return not self.is_only_visual and self.base_mass != 0.
+
     @property
     def base_link_id(self):
         """Return the base link id."""
@@ -120,11 +132,21 @@ class Body(object):
         """Return the body position."""
         return self.sim.get_base_position(self.id)
 
+    @position.setter
+    def position(self, position):
+        """Set the body position. This is only valid in the simulator."""
+        self.sim.reset_base_position(self.id, position)
+
     @property
     def orientation(self):
         """Return the body orientation as a quaternion [x,y,z,w]."""
         return self.sim.get_base_orientation(self.id)
 
+    @orientation.setter
+    def orientation(self, quaternion):
+        """Set the body orientation given the quaternion [x,y,z,w]. This is only valid in the simulator."""
+        self.sim.reset_base_orientation(self.id, quaternion)
+
     # alias
     quaternion = orientation
 
@@ -170,8 +192,14 @@ class Body(object):
 
     @property
     def color(self):
+        """Return the color of the object."""
         return self.sim.get_visual_shape_data(self.id)[0][-1]
 
+    @color.setter
+    def color(self, color):
+        """Set the RGBA color of the object. This is only valid in the simulator."""
+        self.sim.change_visual_shape(object_id=self.id, link_id=-1, rgba_color=color)
+
     @property
     def mass(self):
         """Return the total mass of the body."""
@@ -179,10 +207,25 @@ class Body(object):
             self._mass = self.sim.get_mass(self.id)
         return self._mass
 
+    @mass.setter
+    def mass(self, mass):
+        """Set the mass of the body (its base). This is only valid in the simulator."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, mass=mass)
+
+    @property
+    def local_inertia_diagonal(self):
+        """Return the local inertia diagonal."""
+        return self.sim.get_dynamics_info(self.id, link_id=-1)[2]
+
+    @local_inertia_diagonal.setter
+    def local_inertia_diagonal(self, inertia):
+        """Set the local inertia diagonal."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, local_inertia_diagonal=inertia)
+
     @property
     def dimensions(self):
         """Return the dimensions of the body. Warnings: this should not be trusted too much..."""
-        return np.array(self.sim.get_visual_shape_data(self.id)[0][3])
+        return self.sim.get_visual_shape_data(self.id)[0][3]
 
     @property
     def num_joints(self):
@@ -199,6 +242,351 @@ class Body(object):
         """Return the center of mass."""
         return self.sim.get_center_of_mass_position(self.id)
 
+    @property
+    def lateral_friction(self):
+        """Return the floor lateral friction coefficient."""
+        return self.sim.get_dynamics_info(self.id, link_id=-1)[1]
+
+    @lateral_friction.setter
+    def lateral_friction(self, coefficient):
+        """Set the floor lateral friction coefficient."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, lateral_friction=coefficient)
+
+    @property
+    def rolling_friction(self):
+        """Return the floor rolling friction coefficient."""
+        return self.sim.get_dynamics_info(self.id, -1)[6]
+
+    @rolling_friction.setter
+    def rolling_friction(self, coefficient):
+        """Set the floor rolling friction coefficient."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, rolling_friction=coefficient)
+
+    @property
+    def spinning_friction(self):
+        """Return the floor spinning friction coefficient."""
+        return self.sim.get_dynamics_info(self.id, -1)[7]
+
+    @spinning_friction.setter
+    def spinning_friction(self, coefficient):
+        """Set the spinning friction coefficient."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, spinning_friction=coefficient)
+
+    @property
+    def restitution(self):
+        """Return the floor restitution (bounciness) coefficient."""
+        return self.sim.get_dynamics_info(self.id, -1)[5]
+
+    @restitution.setter
+    def restitution(self, coefficient):
+        """Set the floor restitution (bounciness) coefficient."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, restitution=coefficient)
+
+    @property
+    def contact_damping(self):
+        """Return the floor contact damping."""
+        return self.sim.get_dynamics_info(self.id, -1)[8]
+
+    @contact_damping.setter
+    def contact_damping(self, value):
+        """Set the floor contact damping value."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, contact_damping=value)
+
+    @property
+    def contact_stiffness(self):
+        """Return the floor contact stiffness."""
+        return self.sim.get_dynamics_info(self.id, -1)[9]
+
+    @contact_stiffness.setter
+    def contact_stiffness(self, value):
+        """Set the floor contact stiffness value."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, contact_stiffness=value)
+
+    # just create setter
+    def _set_force(self, force):
+        """Set the given force (expressed in the world cartesian frame) on the center of mass of the body."""
+        self.apply_force(link_id=-1, force=force, position=None, frame=Simulator.WORLD_FRAME)
+
+    force = property(fset=_set_force)
+
+    ###########
+    # Methods #
+    ###########
+
+    def set_color(self, color, link_id=-1):
+        """Set the given RGBA color to the specified link. This is only valid in the simulator.
+
+        Args:
+            color (tuple of 4 float): RGBA color where each channel is between 0 and 1.
+            link_id (int): link id. By default, it is the base (-1).
+        """
+        self.sim.change_visual_shape(object_id=self.id, link_id=link_id, rgba_color=color)
+
+    def apply_force(self, link_id=-1, force=(0., 0., 0.), position=None, frame=Simulator.LINK_FRAME):
+        """
+        Apply the given force on the specified link of the current body.
+
+        Warnings:
+            - after each simulation step, the external forces are cleared to 0.
+            - this does not work when using `sim.setRealTimeSimulation(1)`.
+
+        Args:
+            link_id (int): link id to apply the force, if -1 it will apply the force on the base
+            force (np.array[3]): Cartesian forces to be applied on the body
+            position (np.array[3], None): position on the link where the force is applied (expressed in the given
+                cartesian frame, see next attribute :attr:`frame`). If None, it is the center of mass of the body
+                (or the link if specified).
+            frame (int): allows to specify the coordinate system of force/position. sim.LINK_FRAME (=1) for local
+                link frame, and sim.WORLD_FRAME (=2) for world frame. By default, it is the world frame.
+        """
+        self.sim.apply_external_force(self.id, link_id, force, position, frame)
+
+    def apply_external_torque(self, link_id=-1, torque=(0., 0., 0.), frame=Simulator.LINK_FRAME):
+        """
+        Apply an external torque on the body, or a link of the body. Note that after each simulation step, the external
+        torques are cleared to 0.
+
+        Warnings: This does not work when using `sim.setRealTimeSimulation(1)`.
+
+        Args:
+            link_id (int): link id to apply the torque, if -1 it will apply the torque on the base
+            torque (float[3]): Cartesian torques to be applied on the body
+            frame (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for
+                Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates.
+        """
+        self.sim.apply_external_torque(self.id, link_id=link_id, torque=torque, frame=frame)
+
+    def get_dynamics(self, link_id=-1):
+        """
+        Get dynamic information such as the mass, center of mass, friction and other properties of the specified link.
+
+        Args:
+            link_id (int): link/joint index or -1 for the base.
+
+        Returns:
+            float: mass in kg
+            float: lateral friction coefficient
+            np.float[3]: local inertia diagonal. Note that links and base are centered around the center of mass and
+                aligned with the principal axes of inertia.
+            np.float[3]: position of inertial frame in local coordinates of the joint frame
+            np.float[4]: orientation of inertial frame in local coordinates of joint frame
+            float: coefficient of restitution
+            float: rolling friction coefficient orthogonal to contact normal
+            float: spinning friction coefficient around contact normal
+            float: damping of contact constraints. -1 if not available.
+            float: stiffness of contact constraints. -1 if not available.
+        """
+        return self.sim.get_dynamics_info(self.id, link_id=link_id)
+
+    def change_dynamics(self, link_id=-1, mass=None, lateral_friction=None, spinning_friction=None,
+                        rolling_friction=None, restitution=None, linear_damping=None, angular_damping=None,
+                        contact_stiffness=None, contact_damping=None, friction_anchor=None,
+                        local_inertia_diagonal=None, joint_damping=None):
+        """
+        Change dynamic properties of the current body (or link) such as mass, friction and restitution coefficients,
+        etc.
+
+        Args:
+            link_id (int): link index or -1 for the base.
+            mass (float): change the mass of the link (or base for link index -1)
+            lateral_friction (float): lateral (linear) contact friction
+            spinning_friction (float): torsional friction around the contact normal
+            rolling_friction (float): torsional friction orthogonal to contact normal
+            restitution (float): bouncyness of contact. Keep it a bit less than 1.
+            linear_damping (float): linear damping of the link (0.04 by default)
+            angular_damping (float): angular damping of the link (0.04 by default)
+            contact_stiffness (float): stiffness of the contact constraints, used together with `contact_damping`
+            contact_damping (float): damping of the contact constraints for this body/link. Used together with
+                `contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact
+                section.
+            friction_anchor (int): enable or disable a friction anchor: positional friction correction (disabled by
+                default, unless set in the URDF contact section)
+            local_inertia_diagonal (np.float[3]): diagonal elements of the inertia tensor. Note that the base and
+                links are centered around the center of mass and aligned with the principal axes of inertia so there
+                are no off-diagonal elements in the inertia tensor.
+            joint_damping (float): joint damping coefficient applied at each joint. This coefficient is read from URDF
+                joint damping field. Keep the value close to 0.
+                `joint_damping_force = -damping_coefficient * joint_velocity`.
+        """
+        self.sim.change_dynamics(body_id=self.id, link_id=link_id, mass=mass, lateral_friction=lateral_friction,
+                                 spinning_friction=spinning_friction, rolling_friction=rolling_friction,
+                                 restitution=restitution, linear_damping=linear_damping,
+                                 angular_damping=angular_damping, contact_stiffness=contact_stiffness,
+                                 contact_damping=contact_damping, friction_anchor=friction_anchor,
+                                 local_inertia_diagonal=local_inertia_diagonal, joint_damping=joint_damping)
+
+    def get_collision_shape_data(self, link_id=-1):
+        """
+        Get the collision shape data associated with the specified link of the current body.
+
+        Args:
+            link_id (int): link index or -1 for the base.
+
+        Returns:
+            if not has_collision_shape_data:
+                tuple: empty tuple
+            else:
+                int: object unique id.
+                int: link id.
+                int: geometry type; GEOM_BOX (=3), GEOM_SPHERE (=2), GEOM_CAPSULE (=7), GEOM_MESH (=5), GEOM_PLANE (=6)
+                np.float[3]: depends on geometry type:
+                    for GEOM_BOX: extents,
+                    for GEOM_SPHERE: dimensions[0] = radius,
+                    for GEOM_CAPSULE and GEOM_CYLINDER: dimensions[0] = height (length), dimensions[1] = radius.
+                    For GEOM_MESH: dimensions is the scaling factor.
+                str: Only for GEOM_MESH: file name (and path) of the collision mesh asset.
+                np.float[3]: Local position of the collision frame with respect to the center of mass/inertial frame
+                np.float[4]: Local orientation of the collision frame with respect to the inertial frame
+        """
+        return self.sim.get_collision_shape_data(self.id, link_id=link_id)
+
+    def get_visual_shape_data(self, flags=-1):
+        """
+        Get the visual shape data associated with the current body. It will output a list of visual shape data.
+
+        Args:
+            flags (int, None): VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) will also provide `texture_unique_id`.
+
+        Returns:
+            list:
+                int: object unique id.
+                int: link index or -1 for the base
+                int: visual geometry type (TBD)
+                np.float[3]: dimensions (size, local scale) of the geometry
+                str: path to the triangle mesh, if any. Typically relative to the URDF, SDF or MJCF file location, but
+                    could be absolute
+                np.float[3]: position of local visual frame, relative to link/joint frame
+                np.float[4]: orientation of local visual frame relative to link/joint frame
+                list of 4 floats: URDF color (if any specified) in Red / Green / Blue / Alpha
+                int: texture unique id of the shape or -1 if None. This field only exists if using
+                    VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) flag.
+        """
+        return self.sim.get_visual_shape_data(self.id, flags=flags)
+
+    def apply_texture(self, texture, link_id=-1):
+        """
+        Apply the texture on the specified link of the current body.
+
+        Args:
+            texture (str): path to the texture.
+            link_id (int): link id. If -1, it will be the base.
+        """
+        texture = self.sim.load_texture(texture)
+        self.sim.change_visual_shape(self.id, link_id=link_id, texture_id=texture)
+
+    def get_contacts(self):
+        """
+        Return all the contacts made by the robot.
+
+        Warnings: note that in reality, you can't know if your link(s) is/are in contact with an object unless there
+        is a sensor attached to it. However, this can be useful in simulation to optimize, for instance, trajectories.
+
+        Returns:
+            list: list of contact points where each contact point has:
+                int: contact flag
+                int: unique id of body A (this should be the robot id)
+                int: unique id of body B
+                int: link index of body A (-1 for base, this should be the same as the given link)
+                int: link index of body B (-1 for base)
+                float[3]: contact position on A (in Cartesian world coordinates)
+                float[3]: contact position on B (in Cartesian world coordinates)
+                float[3]: contact normal on B pointing towards A
+                float: contact distance (positive for separation and negative for penetration)
+                float: normal force applied during the last simulation step
+        """
+        return self.sim.get_contact_points(body1=self.id)
+
+    def get_link_states(self, link_ids, compute_link_velocity=True, compute_forward_kinematics=True):
+        """
+        Return the state of the given link(s).
+
+        Warning: note that we do not convert the data here.
+
+        Args:
+            link_ids (int, list of int): link id, or list of desired link ids.
+            compute_link_velocity (bool): if True, the Cartesian world velocity will be computed and returned.
+            compute_forward_kinematics (bool): if True, the Cartesian world position/orientation will be recomputed
+                using forward kinematics.
+
+        Returns:
+            if 1 link:
+                [0] np.array[3]: Cartesian position of center of mass
+                [1] np.array[4]: Cartesian orientation of center of mass
+                [2] np.array[3]: local position offset of inertial frame (CoM) expressed in the URDF link frame
+                [3] np.array[4]: local orientation (quat. [x,y,z,w]) offset of the inertial frame expressed in URDF
+                    link frame
+                [4] np.array[3]: world position of the URDF link frame
+                [5] np.array[4]: world orientation of the URDF link frame
+                [6] np.array[3]: Cartesian world linear velocity
+                [7] np.array[3]: Cartesian world angular velocity
+            if multiple links: list of above
+        """
+        if isinstance(link_ids, int):  # one link
+            return self.sim.get_link_state(self.id, link_ids, compute_velocity=compute_link_velocity,
+                                           compute_forward_kinematics=compute_forward_kinematics)
+        # multiple links
+        return self.sim.get_link_states(self.id, link_ids, compute_velocity=compute_link_velocity,
+                                        compute_forward_kinematics=compute_forward_kinematics)
+
+    def get_joint_states(self, joint_ids):
+        """
+        Get the state of the given joint(s).
+
+        Args:
+            joint_ids (int, list of int): id of the joint, or list of joint ids.
+
+        Returns:
+            for 1 joint:
+                float: joint position [rad]
+                float: joint velocity [rad/s]
+                np.array[6]: joint reaction forces [fx,fy,fz,mx,my,mz]
+                float: applied joint motor torque (during the last step)
+            for multiple joints: list of each joint state
+        """
+        if isinstance(joint_ids, int):
+            return self.sim.get_joint_state(self.id, joint_ids)
+        return self.sim.get_joint_states(self.id, joint_ids)
+
+    def get_joint_info(self, joint_ids):
+        """
+        Get information about the given joint(s).
+
+        Note that this method returns a lot of information, so specific methods have been implemented that return
+        only the desired information. Also, note that we do not convert the data here.
+
+        Args:
+            joint_ids (int, list of int): joint id, or list of joint ids.
+
+        Returns:
+            if 1 joint:
+                [0] int:        the same joint id as the input parameter
+                [1] str:        name of the joint (as specified in the URDF/SDF/etc file)
+                [2] int:        type of the joint which implie the number of position and velocity variables.
+                                The types include JOINT_REVOLUTE (=0), JOINT_PRISMATIC (=1), JOINT_SPHERICAL (=2),
+                                JOINT_PLANAR (=3), and JOINT_FIXED (=4).
+                [3] int:        q index - the first position index in the positional state variables for this body
+                [4] int:        dq index - the first velocity index in the velocity state variables for this body
+                [5] int:        flags (reserved)
+                [6] float:      the joint damping value (as specified in the URDF file)
+                [7] float:      the joint friction value (as specified in the URDF file)
+                [8] float:      the positional lower limit for slider and revolute joints
+                [9] float:      the positional upper limit for slider and revolute joints
+                [10] float:     maximum force specified in URDF. Note that this value is not automatically used.
+                                You can use maxForce in 'setJointMotorControl2'.
+                [11] float:     maximum velocity specified in URDF. Note that this value is not used in actual
+                                motor control commands at the moment.
+                [12] str:       name of the link (as specified in the URDF/SDF/etc file)
+                [13] np.array[3]:  joint axis in local frame (ignored for JOINT_FIXED)
+                [14] np.array[3]:  joint position in parent frame
+                [15] np.array[4]:  joint orientation in parent frame (x, y, z, w)
+                [16] int:       parent link index, -1 for base
+
+            if multiple joints: list of joint information (i.e. list of above)
+        """
+        if isinstance(joint_ids, int):
+            return self.sim.get_joint_info(self.id, joint_ids)
+        return [self.sim.get_joint_info(self.id, joint_id) for joint_id in joint_ids]
+
     #############
     # Operators #
     #############
@@ -209,7 +597,7 @@ class Body(object):
 
     def __str__(self):
         """Return a readable string about the class."""
-        return self.__class__.__name__
+        return self.__class__.__name__ + '(' + self.name + ')'
 
     def __copy__(self):
         """Return a shallow copy of the body. This can be overridden in the child class."""
@@ -221,14 +609,13 @@ class Body(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         simulator = copy.deepcopy(self.simulator, memo)
         body = self.__class__(simulator=simulator, body_id=self.id, name=self.name)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = body
-
-        # return the copy
         return body
 
 
@@ -240,6 +627,9 @@ class MovableBody(Body):
 
     def __init__(self, simulator, object_id=0, name=None):
         super(MovableBody, self).__init__(simulator, object_id, name=name)
+        # # check that the body is movable
+        # if not self.is_movable:
+        #     raise ValueError("The given id does not correspond to a movable body.")
 
     # def move(self, position=None, orientation=None):
     #     pass

pyrobolearn/simulators/bullet.py

@@ -117,7 +117,8 @@ class Bullet(Simulator):
         # The items in the history container are in the same order there were called. Each item is a tuple where the
         # first item is the name of the method called, and the second item is the parameters that were passed to that
         # method.
-        self.history = []
+        self.history = []  # keep track of every commands
+        self.ids = []  # keep track of created unique ids
 
         # main camera in the simulator
         self._camera = None
@@ -160,16 +161,6 @@ class Bullet(Simulator):
         """Return the version of the simulator in a year-month-day format."""
         return self.sim.getAPIVersion()
 
-    @property
-    def gravity(self):
-        """Return the gravity in the simulator."""
-        return self.get_physics_properties()['gravity']
-
-    @property
-    def camera(self):
-        """Return the camera (yaw, pitch, distance, target_position) or None."""
-        return self._camera
-
     #############
     # Operators #
     #############
@@ -206,6 +197,10 @@ class Bullet(Simulator):
         Returns:
             Bullet: bullet simulator in DIRECT mode.
         """
+        # if the object has already been copied return the reference to the copied object
+        if self in memo:
+            return memo[self]
+
         # check if the memo has arguments that specify how to deep copy the simulator
         copy_models = memo.get('copy_parameters', False)
         copy_properties = memo.get('copy_properties', False)
@@ -221,10 +216,8 @@ class Bullet(Simulator):
         if copy_properties:
             pass
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
+        # update the memodict
         memo[self] = sim
-
         return sim
 
     ###########
@@ -408,7 +401,7 @@ class Bullet(Simulator):
         Args:
             time_step (float): Each time you call 'step' the time step will proceed with 'time_step'.
         """
-        self.history.append(('set_time_step', {'time_step': time_step}))
+        # self.history.append(('set_time_step', {'time_step': time_step}))
         self.sim.setTimeStep(timeStep=time_step)
 
     def set_real_time(self, enable=True):
@@ -606,6 +599,10 @@ class Bullet(Simulator):
         """
         self.sim.stopStateLogging(logger_id)
 
+    def get_gravity(self):
+        """Return the gravity set in the simulator."""
+        return self.get_physics_properties()['gravity']
+
     def set_gravity(self, gravity=(0, 0, -9.81)):
         """Set the gravity in the simulator with the given acceleration.
 
@@ -754,10 +751,10 @@ class Bullet(Simulator):
             kwargs['globalScaling'] = scale
 
         model_id = self.sim.loadURDF(filename, **kwargs)
-        if model_id > -1:
-            frame = inspect.currentframe()
-            args, _, _, values = inspect.getargvalues(frame)
-            self.history.append(('load_urdf', {arg: values[arg] for arg in args[1:]}))
+        # if model_id > -1:
+        #     frame = inspect.currentframe()
+        #     args, _, _, values = inspect.getargvalues(frame)
+        #     self.history.append(('load_urdf', {arg: values[arg] for arg in args[1:]}))
         return model_id
 
     def load_sdf(self, filename, scaling=1., *args, **kwargs):
@@ -1366,8 +1363,7 @@ class Bullet(Simulator):
         """
         self.sim.resetBaseVelocity(body_id, angularVelocity=angular_velocity)
 
-    def apply_external_force(self, body_id, link_id=-1, force=(0., 0., 0.), position=(0., 0., 0.),
-                             frame=pybullet.LINK_FRAME):
+    def apply_external_force(self, body_id, link_id=-1, force=(0., 0., 0.), position=None, frame=Simulator.LINK_FRAME):
         """
         Apply the specified external force on the specified position on the body / link.
 
@@ -1380,14 +1376,22 @@ class Bullet(Simulator):
             body_id (int): unique body id.
             link_id (int): unique link id. If -1, it will be the base.
             force (np.float[3]): external force to be applied.
-            position (np.float[3]): position on the link where the force is applied. See `flags` for coordinate
-                systems.
+            position (np.float[3], None): position on the link where the force is applied. See `flags` for coordinate
+                systems. If None, it is the center of mass of the body (or the link if specified).
             frame (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for
                 Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates.
         """
+        if position is None:
+            if frame == Simulator.WORLD_FRAME:  # world frame
+                if link_id == -1:
+                    position = self.get_base_pose(body_id)[0]
+                else:
+                    position = self.get_link_state(body_id, link_id)[0]
+            else:  # local frame
+                position = (0., 0., 0.)
         self.sim.applyExternalForce(body_id, link_id, force, position, frame)
 
-    def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.)):
+    def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.), frame=Simulator.LINK_FRAME):
         """
         Apply an external torque on a body, or a link of the body. Note that after each simulation step, the external
         torques are cleared to 0.
@@ -1398,6 +1402,8 @@ class Bullet(Simulator):
             body_id (int): unique body id.
             link_id (int): link id to apply the torque, if -1 it will apply the torque on the base
             torque (float[3]): Cartesian torques to be applied on the body
+            frame (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for
+                Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates.
         """
         self.sim.applyExternalTorque(body_id, link_id, torque)
 
@@ -2912,27 +2918,33 @@ class Bullet(Simulator):
 
     def get_collision_shape_data(self, object_id, link_id=-1):
         """
-        Get the collision shape data associated with the specified object id and link id.
+        Get the collision shape data associated with the specified object id and link id. If the given object_id has
+        no collision shape, it returns an empty tuple.
 
         Args:
             object_id (int): object unique id.
             link_id (int): link index or -1 for the base.
 
         Returns:
-            int: object unique id.
-            int: link id.
-            int: geometry type; GEOM_BOX (=3), GEOM_SPHERE (=2), GEOM_CAPSULE (=7), GEOM_MESH (=5), GEOM_PLANE (=6)
-            np.float[3]: depends on geometry type:
-                for GEOM_BOX: extents,
-                for GEOM_SPHERE: dimensions[0] = radius,
-                for GEOM_CAPSULE and GEOM_CYLINDER: dimensions[0] = height (length), dimensions[1] = radius.
-                For GEOM_MESH: dimensions is the scaling factor.
-            str: Only for GEOM_MESH: file name (and path) of the collision mesh asset.
-            np.float[3]: Local position of the collision frame with respect to the center of mass/inertial frame
-            np.float[4]: Local orientation of the collision frame with respect to the inertial frame
+            if not has_collision_shape_data:
+                tuple: empty tuple
+            else:
+                int: object unique id.
+                int: link id.
+                int: geometry type; GEOM_BOX (=3), GEOM_SPHERE (=2), GEOM_CAPSULE (=7), GEOM_MESH (=5), GEOM_PLANE (=6)
+                np.float[3]: depends on geometry type:
+                    for GEOM_BOX: extents,
+                    for GEOM_SPHERE: dimensions[0] = radius,
+                    for GEOM_CAPSULE and GEOM_CYLINDER: dimensions[0] = height (length), dimensions[1] = radius.
+                    For GEOM_MESH: dimensions is the scaling factor.
+                str: Only for GEOM_MESH: file name (and path) of the collision mesh asset.
+                np.float[3]: Local position of the collision frame with respect to the center of mass/inertial frame
+                np.float[4]: Local orientation of the collision frame with respect to the inertial frame
         """
-        object_id, link_id, geom_type, dimensions, filename, \
-            position, orientation = self.sim.getCollisionShapeData(object_id, link_id)
+        collision = self.sim.getCollisionShapeData(object_id, link_id)
+        if len(collision) == 0:
+            return collision
+        object_id, link_id, geom_type, dimensions, filename, position, orientation = collision
         return object_id, link_id, geom_type, np.array(dimensions), filename, np.array(position), np.array(orientation)
 
     def get_overlapping_objects(self, aabb_min, aabb_max):
@@ -3047,7 +3059,7 @@ class Bullet(Simulator):
         if link2_id is not None:
             kwargs['linkIndexB'] = link2_id
 
-        results = self.sim.getContactPoints(body1, body2, distance, **kwargs)
+        results = self.sim.getClosestPoints(body1, body2, distance, **kwargs)
         if len(results) == 0:
             return results
         return [[r[0], r[1], r[2], r[3], r[4], np.array(r[5]), np.array(r[6]), np.array(r[7]), r[8], r[9], r[10],
@@ -3156,7 +3168,7 @@ class Bullet(Simulator):
 
         Returns:
             float: mass in kg
-            float: friction coefficient
+            float: lateral friction coefficient
             np.float[3]: local inertia diagonal. Note that links and base are centered around the center of mass and
                 aligned with the principal axes of inertia.
             np.float[3]: position of inertial frame in local coordinates of the joint frame
@@ -3177,7 +3189,7 @@ class Bullet(Simulator):
                         contact_stiffness=None, contact_damping=None, friction_anchor=None,
                         local_inertia_diagonal=None, joint_damping=None):
         """
-        Change dynamic properties such as mass, friction and restitution coefficients .
+        Change dynamic properties of the given body (or link) such as mass, friction and restitution coefficients, etc.
 
         Args:
             body_id (int): object unique id, as returned by `load_urdf`, etc.

pyrobolearn/simulators/simulator.py

@@ -45,147 +45,151 @@ class Simulator(object):
     [2] PEP8: https://www.python.org/dev/peps/pep-0008/
     """
 
+    # TODO: this is really bad to have attributes like that... It doesn't generalize well to other simulators...
+
+    B3G_ALT = 65308
+    B3G_BACKSPACE = 65305
+    B3G_CONTROL = 65307
+    B3G_DELETE = 65304
+    B3G_DOWN_ARROW = 65298
+    B3G_END = 65301
+    B3G_F1 = 65280
+    B3G_F10 = 65289
+    B3G_F11 = 65290
+    B3G_F12 = 65291
+    B3G_F13 = 65292
+    B3G_F14 = 65293
+    B3G_F15 = 65294
+    B3G_F2 = 65281
+    B3G_F3 = 65282
+    B3G_F4 = 65283
+    B3G_F5 = 65284
+    B3G_F6 = 65285
+    B3G_F7 = 65286
+    B3G_F8 = 65287
+    B3G_F9 = 65288
+    B3G_HOME = 65302
+    B3G_INSERT = 65303
+    B3G_LEFT_ARROW = 65295
+    B3G_PAGE_DOWN = 65300
+    B3G_PAGE_UP = 65299
+    B3G_RETURN = 65309
+    B3G_RIGHT_ARROW = 65296
+    B3G_SHIFT = 65306
+    B3G_UP_ARROW = 65297
+
+    COV_ENABLE_DEPTH_BUFFER_PREVIEW = 14
+    COV_ENABLE_GUI = 1
+    COV_ENABLE_KEYBOARD_SHORTCUTS = 9
+    COV_ENABLE_MOUSE_PICKING = 10
+    COV_ENABLE_PLANAR_REFLECTION = 16
+    COV_ENABLE_RENDERING = 7
+    COV_ENABLE_RGB_BUFFER_PREVIEW = 13
+    COV_ENABLE_SEGMENTATION_MARK_PREVIEW = 15
+    COV_ENABLE_SHADOWS = 2
+    COV_ENABLE_SINGLE_STEP_RENDERING = 17
+    COV_ENABLE_TINY_RENDERER = 12
+    COV_ENABLE_WIREFRAME = 3
+    COV_ENABLE_Y_AXIS_UP = 11
+
+    DIRECT = 2
+    ER_BULLET_HARDWARE_OPENGL = 131072
+    ER_NO_SEGMENTATION_MASK = 4
+    ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX = 1
+    ER_TINY_RENDERER = 65536
+    ER_USE_PROJECTIVE_TEXTURE = 2
+
+    GEOM_FORCE_CONCAVE_TRIMESH = 1
+    GEOM_SPHERE = 2
+    GEOM_CONCAVE_INTERNAL_EDGE = 2
+    GEOM_BOX = 3
+    GEOM_CYLINDER = 4
+    GEOM_MESH = 5
+    GEOM_PLANE = 6
+    GEOM_CAPSULE = 7
+
+    GUI = 1
+    GUI_MAIN_THREAD = 8
+    GUI_SERVER = 7
+    IK_DLS = 0
+    IK_HAS_JOINT_DAMPING = 128
+    IK_HAS_NULL_SPACE_VELOCITY = 64
+    IK_HAS_TARGET_ORIENTATION = 32
+    IK_HAS_TARGET_POSITION = 16
+    IK_SDLS = 1
+
+    JOINT_FEEDBACK_IN_JOINT_FRAME = 2
+    JOINT_FEEDBACK_IN_WORLD_SPACE = 1
+    JOINT_FIXED = 4
+    JOINT_GEAR = 6
+    JOINT_PLANAR = 3
+    JOINT_POINT2POINT = 5
+    JOINT_PRISMATIC = 1
+    JOINT_REVOLUTE = 0
+    JOINT_SPHERICAL = 2
+
+    KEY_IS_DOWN = 1
+    KEY_WAS_RELEASED = 4
+    KEY_WAS_TRIGGERED = 2
+
+    LINK_FRAME = 1
+    WORLD_FRAME = 2
+
+    MAX_RAY_INTERSECTION_BATCH_SIZE = 16384
+
+    VELOCITY_CONTROL = 0
+    TORQUE_CONTROL = 1
+    POSITION_CONTROL = 2
+    PD_CONTROL = 3
+
+    SENSOR_FORCE_TORQUE = 1
+    SHARED_MEMORY = 3
+    SHARED_MEMORY_KEY = 12347
+    SHARED_MEMORY_KEY2 = 12348
+    SHARED_MEMORY_SERVER = 9
+    STATE_LOGGING_ALL_COMMANDS = 7
+    STATE_LOGGING_CONTACT_POINTS = 5
+    STATE_LOGGING_CUSTOM_TIMER = 9
+    STATE_LOGGING_GENERIC_ROBOT = 1
+    STATE_LOGGING_MINITAUR = 0
+    STATE_LOGGING_PROFILE_TIMINGS = 6
+    STATE_LOGGING_VIDEO_MP4 = 3
+    STATE_LOGGING_VR_CONTROLLERS = 2
+    STATE_LOG_JOINT_MOTOR_TORQUES = 1
+    STATE_LOG_JOINT_TORQUES = 3
+    STATE_LOG_JOINT_USER_TORQUES = 2
+    STATE_REPLAY_ALL_COMMANDS = 8
+
+    TCP = 5
+    UDP = 4
+
+    URDF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024
+    URDF_ENABLE_SLEEPING = 2048
+    URDF_GLOBAL_VELOCITIES_MB = 256
+    URDF_INITIALIZE_SAT_FEATURES = 4096
+    URDF_USE_IMPLICIT_CYLINDER = 128
+    URDF_USE_INERTIA_FROM_FILE = 2
+    URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768
+    URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536
+    URDF_USE_SELF_COLLISION = 8
+    URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32
+    URDF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16
+    URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192
+
     def __init__(self, render=True, **kwargs):
         self._render = render
         self.real_time = False
         self.kwargs = kwargs
 
-        # TODO: this is really bad to have attributes like that... It doesn't generalize well to other simulators...
+        # main camera in the simulator
+        self._camera = None
 
+        # TODO: this is really bad to have attributes like that... It doesn't generalize well to other simulators...
         # import pybullet
         # for attribute in dir(pybullet):
         #     if attribute[0].isupper():
         #         print('self.{} = {}'.format(attribute, getattr(pybullet, attribute)))
 
-        self.B3G_ALT = 65308
-        self.B3G_BACKSPACE = 65305
-        self.B3G_CONTROL = 65307
-        self.B3G_DELETE = 65304
-        self.B3G_DOWN_ARROW = 65298
-        self.B3G_END = 65301
-        self.B3G_F1 = 65280
-        self.B3G_F10 = 65289
-        self.B3G_F11 = 65290
-        self.B3G_F12 = 65291
-        self.B3G_F13 = 65292
-        self.B3G_F14 = 65293
-        self.B3G_F15 = 65294
-        self.B3G_F2 = 65281
-        self.B3G_F3 = 65282
-        self.B3G_F4 = 65283
-        self.B3G_F5 = 65284
-        self.B3G_F6 = 65285
-        self.B3G_F7 = 65286
-        self.B3G_F8 = 65287
-        self.B3G_F9 = 65288
-        self.B3G_HOME = 65302
-        self.B3G_INSERT = 65303
-        self.B3G_LEFT_ARROW = 65295
-        self.B3G_PAGE_DOWN = 65300
-        self.B3G_PAGE_UP = 65299
-        self.B3G_RETURN = 65309
-        self.B3G_RIGHT_ARROW = 65296
-        self.B3G_SHIFT = 65306
-        self.B3G_UP_ARROW = 65297
-
-        self.COV_ENABLE_DEPTH_BUFFER_PREVIEW = 14
-        self.COV_ENABLE_GUI = 1
-        self.COV_ENABLE_KEYBOARD_SHORTCUTS = 9
-        self.COV_ENABLE_MOUSE_PICKING = 10
-        self.COV_ENABLE_PLANAR_REFLECTION = 16
-        self.COV_ENABLE_RENDERING = 7
-        self.COV_ENABLE_RGB_BUFFER_PREVIEW = 13
-        self.COV_ENABLE_SEGMENTATION_MARK_PREVIEW = 15
-        self.COV_ENABLE_SHADOWS = 2
-        self.COV_ENABLE_SINGLE_STEP_RENDERING = 17
-        self.COV_ENABLE_TINY_RENDERER = 12
-        self.COV_ENABLE_WIREFRAME = 3
-        self.COV_ENABLE_Y_AXIS_UP = 11
-
-        self.DIRECT = 2
-        self.ER_BULLET_HARDWARE_OPENGL = 131072
-        self.ER_NO_SEGMENTATION_MASK = 4
-        self.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX = 1
-        self.ER_TINY_RENDERER = 65536
-        self.ER_USE_PROJECTIVE_TEXTURE = 2
-
-        self.GEOM_FORCE_CONCAVE_TRIMESH = 1
-        self.GEOM_SPHERE = 2
-        self.GEOM_CONCAVE_INTERNAL_EDGE = 2
-        self.GEOM_BOX = 3
-        self.GEOM_CYLINDER = 4
-        self.GEOM_MESH = 5
-        self.GEOM_PLANE = 6
-        self.GEOM_CAPSULE = 7
-
-        self.GUI = 1
-        self.GUI_MAIN_THREAD = 8
-        self.GUI_SERVER = 7
-        self.IK_DLS = 0
-        self.IK_HAS_JOINT_DAMPING = 128
-        self.IK_HAS_NULL_SPACE_VELOCITY = 64
-        self.IK_HAS_TARGET_ORIENTATION = 32
-        self.IK_HAS_TARGET_POSITION = 16
-        self.IK_SDLS = 1
-
-        self.JOINT_FEEDBACK_IN_JOINT_FRAME = 2
-        self.JOINT_FEEDBACK_IN_WORLD_SPACE = 1
-        self.JOINT_FIXED = 4
-        self.JOINT_GEAR = 6
-        self.JOINT_PLANAR = 3
-        self.JOINT_POINT2POINT = 5
-        self.JOINT_PRISMATIC = 1
-        self.JOINT_REVOLUTE = 0
-        self.JOINT_SPHERICAL = 2
-
-        self.KEY_IS_DOWN = 1
-        self.KEY_WAS_RELEASED = 4
-        self.KEY_WAS_TRIGGERED = 2
-
-        self.LINK_FRAME = 1
-        self.WORLD_FRAME = 2
-
-        self.MAX_RAY_INTERSECTION_BATCH_SIZE = 16384
-
-        self.VELOCITY_CONTROL = 0
-        self.TORQUE_CONTROL = 1
-        self.POSITION_CONTROL = 2
-        self.PD_CONTROL = 3
-
-        self.SENSOR_FORCE_TORQUE = 1
-        self.SHARED_MEMORY = 3
-        self.SHARED_MEMORY_KEY = 12347
-        self.SHARED_MEMORY_KEY2 = 12348
-        self.SHARED_MEMORY_SERVER = 9
-        self.STATE_LOGGING_ALL_COMMANDS = 7
-        self.STATE_LOGGING_CONTACT_POINTS = 5
-        self.STATE_LOGGING_CUSTOM_TIMER = 9
-        self.STATE_LOGGING_GENERIC_ROBOT = 1
-        self.STATE_LOGGING_MINITAUR = 0
-        self.STATE_LOGGING_PROFILE_TIMINGS = 6
-        self.STATE_LOGGING_VIDEO_MP4 = 3
-        self.STATE_LOGGING_VR_CONTROLLERS = 2
-        self.STATE_LOG_JOINT_MOTOR_TORQUES = 1
-        self.STATE_LOG_JOINT_TORQUES = 3
-        self.STATE_LOG_JOINT_USER_TORQUES = 2
-        self.STATE_REPLAY_ALL_COMMANDS = 8
-
-        self.TCP = 5
-        self.UDP = 4
-
-        self.URDF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024
-        self.URDF_ENABLE_SLEEPING = 2048
-        self.URDF_GLOBAL_VELOCITIES_MB = 256
-        self.URDF_INITIALIZE_SAT_FEATURES = 4096
-        self.URDF_USE_IMPLICIT_CYLINDER = 128
-        self.URDF_USE_INERTIA_FROM_FILE = 2
-        self.URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768
-        self.URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536
-        self.URDF_USE_SELF_COLLISION = 8
-        self.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32
-        self.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16
-        self.URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192
-
     ##############
     # Properties #
     ##############
@@ -195,6 +199,21 @@ class Simulator(object):
         """Return the version of the simulator."""
         return 0
 
+    @property
+    def gravity(self):
+        """Return the gravity in the simulator."""
+        return self.get_gravity()
+
+    @gravity.setter
+    def gravity(self, gravity):
+        """Set the gravity in the simulator."""
+        self.set_gravity(gravity)
+
+    @property
+    def camera(self):
+        """Return the camera (yaw, pitch, distance, target_position) or None."""
+        return self._camera
+
     #############
     # Operators #
     #############
@@ -217,14 +236,14 @@ class Simulator(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass.
         """
+        # if the object has already been copied return the reference to the copied object
+        if self in memo:
+            return memo[self]
+
         # create a new copy of the simulator
         sim = self.__class__(render=self._render, **self.kwargs)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = sim
-
-        # return the copy
         return sim
 
     ###########
@@ -318,8 +337,16 @@ class Simulator(object):
         """Stop the logging."""
         pass
 
+    def get_gravity(self):
+        """Return the gravity set in the simulator."""
+        pass
+
     def set_gravity(self, gravity=(0, 0, -9.81)):
-        """Set the gravity in the simulator."""
+        """Set the gravity in the simulator with the given acceleration.
+
+        Args:
+            gravity (list, tuple of 3 floats): acceleration in the x, y, z directions.
+        """
         pass
 
     def save(self, filename=None, *args, **kwargs):
@@ -835,13 +862,13 @@ class Simulator(object):
             link_id (int): unique link id. If -1, it will be the base.
             force (np.float[3]): external force to be applied.
             position (np.float[3]): position on the link where the force is applied. See `flags` for coordinate
-                systems.
+                systems. If None, it is the center of mass of the body (or the link if specified).
             frame (int): if frame = 1, then the force / position is described in the link frame. If frame = 2, they
                 are described in the world frame.
         """
         pass
 
-    def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.)):
+    def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.), frame=1):
         """
         Apply an external torque on a body, or a link of the body. Note that after each simulation step, the external
         torques are cleared to 0.
@@ -850,6 +877,8 @@ class Simulator(object):
             body_id (int): unique body id.
             link_id (int): link id to apply the torque, if -1 it will apply the torque on the base
             torque (float[3]): Cartesian torques to be applied on the body
+            frame (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for
+                Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates.
         """
         pass
 
@@ -2008,7 +2037,7 @@ class Simulator(object):
 
         Returns:
             float: mass in kg
-            float: friction coefficient
+            float: lateral friction coefficient
             np.float[3]: local inertia diagonal. Note that links and base are centered around the center of mass and
                 aligned with the principal axes of inertia.
             np.float[3]: position of inertial frame in local coordinates of the joint frame
@@ -2026,7 +2055,7 @@ class Simulator(object):
                         contact_stiffness=None, contact_damping=None, friction_anchor=None,
                         local_inertia_diagonal=None, joint_damping=None):
         """
-        Change dynamic properties such as mass, friction and restitution coefficients .
+        Change dynamic properties of the given body (or link) such as mass, friction and restitution coefficients, etc.
 
         Args:
             body_id (int): object unique id, as returned by `load_urdf`, etc.

pyrobolearn/states/state.py

@@ -1076,16 +1076,16 @@ class State(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         states = [copy.deepcopy(state, memo) for state in self.states]
         data = copy.deepcopy(self.window[0]) if self.has_data() else None
         space = copy.deepcopy(self._space)
         state = self.__class__(states=states, data=data, space=space, name=self.name, window_size=self.window_size,
                                axis=self.axis, ticks=self.ticks)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = state
-
         return state
 
     # def __invert__(self):

pyrobolearn/utils/transformation.py

@@ -24,6 +24,42 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
+def get_homogeneous_transform(position, orientation):
+    r"""
+    Return the Homogeneous transform matrix given the position vector and the orientation.
+
+    .. math::
+
+        H = [[R, p],
+             [zeros(3),1]]
+
+    where :math:`R` is the 3x3 rotation matrix, :math:`p` is the 3x1 position vector.
+
+    Args:
+        position (np.array[3]): position vector
+        orientation (np.array[4], np.array[3,3], np.array[3]): orientation (expressed as a quaternion [x,y,z,w],
+            3x3 rotation matrix, or roll-pitch-yaw angles).
+
+    Returns:
+        np.array[4,4]: homogeneous matrix
+    """
+    if isinstance(orientation, quaternion.quaternion):
+        R = quaternion.as_rotation_matrix(orientation)
+    else:
+        orientation = np.array(orientation)
+        if orientation.shape == (3,):  # RPY Euler angles
+            R = get_matrix_from_rpy(orientation)
+        elif orientation.shape == (4,):  # quaternion in the form [x,y,z,w]
+            R = get_matrix_from_quaternion(orientation)
+        elif orientation.shape == (3, 3):  # Rotation matrix
+            R = orientation
+        else:
+            raise ValueError("Expecting a quaternion, RPY Euler angles, or rotation matrix")
+
+    H = np.vstack((np.hstack((R, position.reshape(-1, 1))), np.array([[0, 0, 0, 1]])))
+    return H
+
+
 def get_matrix_from_axis_angle(axis, angle):
     """Return the rotation matrix from the specified axis and angle.