fix errors in examples when using Python 3

examples/rewards/manipulator.py

@@ -15,16 +15,19 @@ world = prl.worlds.BasicWorld(sim)
 
 # create robot
 robot = world.load_robot('kuka_iiwa')
-print(robot)
+end_effector_id = robot.end_effectors[0]
+robot.print_info()
 
 # desired position
-sphere = world.load_visual_sphere([0.5, 0., 0.5], radius=0.05, color=(1, 0, 0, 0.5), return_body=True)
+sphere = world.load_visual_sphere([0.5, 0., 0.], radius=0.05, color=(1, 0, 0, 0.5), return_body=True)
 
 # create state
-state = prl.states.LinkPositionState(robot, link_ids=robot.end_effectors)
+state = prl.states.LinkWorldPositionState(robot, link_ids=end_effector_id)
 
 # create reward
-# reward = prl.rewards.DistanceCost()
+# note that the given 'sphere' to the cost is not a state, and thus a PositionState will automatically be created
+# for that 'sphere', and called at each time the reward is computed.
+reward = prl.rewards.DistanceCost(state, sphere)
 
 # run simulation
 for t in count():
@@ -32,7 +35,7 @@ for t in count():
     state()
 
     # compute reward
-    # print("Reward value = {}".format(reward()))
+    print("Reward value = {}".format(reward()))
 
     # perform a step in the simulator
     world.step(sim.dt)

pyrobolearn/rewards/cost.py

@@ -19,6 +19,7 @@ import copy
 import numpy as np
 import torch
 
+import pyrobolearn as prl
 from pyrobolearn.robots.robot import Robot
 # from objective import Objective
 import pyrobolearn.states as states
@@ -406,31 +407,68 @@ class ContactInvariantCost(Cost):
         super(ContactInvariantCost, self).__init__()
 
 
-class PowerConsumptionCost(Cost):
-    """Power Consumption Cost.
-
-    It penalizes power consumption using u^TWu where W is a weight matrix, and u is the control vector.
-    """
-
-    def __init__(self):
-        super(PowerConsumptionCost, self).__init__()
-
-    def loss(self, robot):
-        return np.dot(robot.get_joint_torques(), robot.get_joint_velocities())
-
-
 class DistanceCost(Cost):
     """Distance Cost.
 
     It penalizes the distance between 2 objects. One of the 2 objects must be movable in order for this
     cost to change.
+
+    Mathematically, the cost is given by:
+
+    .. math:: c(l1, l2) = d(l1, l2) = - || l1 - l2 ||_2
+
+    where :math:`l1` represents a link attached to the first body, and :math:`l2` represents a link attached on the
+    second body. The distance function used is the Euclidean distance (=L2 norm).
     """
 
-    def __init__(self, body1, body2):
+    def __init__(self, body1, body2, link_id1=-1, link_id2=-1):
+        r"""
+        Initialize the distance cost.
+
+        Args:
+            body1 (BasePositionState, PositionState, LinkWorldPositionState, Body, Robot): first position state. If
+                Body, it will wrap it with a `PositionState`. If Robot, it will wrap it with a `PositionState` or
+                `LinkPositionState` depending on the value of :attr:`link_id1`.
+            body2 (BasePositionState, PositionState, LinkWorldPositionState, Body, Robot): second position state. If
+                Body, it will wrap it with a `PositionState`. If Robot, it will wrap it with a `PositionState` or
+                `LinkPositionState` depending on the value of :attr:`link_id1`.
+            link_id1 (int): link id associated with the first body that we are interested in. This is only used if
+                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.
+        """
         super(DistanceCost, self).__init__()
 
-    def loss(self, object1, object2):
-        pass
+        def check_body_type(body, id_, link_id):
+            update_state = False
+            if isinstance(body, prl.robots.Body):
+                body = states.PositionState(body)
+                update_state = True
+            elif isinstance(body, Robot):
+                if link_id == -1:
+                    body = states.PositionState(body)
+                else:
+                    body = states.LinkWorldPositionState(body, link_ids=link_id)
+                update_state = True
+            elif not isinstance(body, (states.BasePositionState, states.PositionState, states.LinkWorldPositionState)):
+                raise TypeError("Expecting the given 'body"+str(id_)+"' to be an instance of `Body`, `Robot`, "
+                                "`BasePositionState`, `PositionState` or `LinkWorldPositionState`, instead got: "
+                                "{}".format(type(body), id_))
+            return body, update_state
+
+        self.body1, self.update_state1 = check_body_type(body1, id_=1, link_id=link_id1)
+        self.body2, self.update_state2 = check_body_type(body2, id_=2, link_id=link_id2)
+
+    def compute(self):
+        if self.update_state1:
+            self.body1()
+        if self.update_state2:
+            self.body2()
+        p1 = self.body1.data[0]
+        p2 = self.body2.data[0]
+        print("P1: {}".format(p1))
+        print("P2: {}".format(p2))
+        return - np.linalg.norm(p1 - p2)
 
 
 class ImpactCost(Cost):
@@ -442,7 +480,7 @@ class ImpactCost(Cost):
     def __init__(self):
         super(ImpactCost, self).__init__()
 
-    def loss(self, object1, object2):
+    def compute(self, object1, object2):
         pass
 
 
@@ -455,7 +493,7 @@ class DriftCost(Cost):
     def __init__(self):
         super(DriftCost, self).__init__()
 
-    def loss(self, object, direction):
+    def compute(self, object, direction):
         pass
 
 
@@ -467,7 +505,7 @@ class ShakeCost(Cost):
     def __init__(self):
         super(ShakeCost, self).__init__()
 
-    def loss(self, object, direction):
+    def compute(self, object, direction):
         pass
 
 

pyrobolearn/rewards/reward.py

@@ -15,6 +15,7 @@ Dependencies:
 - `pyrobolearn.actions`
 """
 
+import sys
 import numpy as np
 import collections
 import operator
@@ -366,7 +367,13 @@ class Reward(object):
         b = b.range if isinstance(b, Reward) else (b, b)
 
         # check that you do not have a possible division or modulo by zero
-        if op in {operator.__div__, operator.__floordiv__, operator.__truediv__, operator.__mod__}:
+
+        if sys.version_info[0] == 2:  # Python 2
+            dangerous_operators = {operator.__div__, operator.__floordiv__, operator.__truediv__, operator.__mod__}
+        else:  # In Python 3, there is no __div__
+            dangerous_operators = {operator.__floordiv__, operator.__truediv__, operator.__mod__}
+
+        if op in dangerous_operators:
             if b[0] <= 0 <= b[1]:
                 raise ValueError("Zero is between the lower and upper bound of the range of `other`. This is not "
                                  "accepted as it can lead to a division or modulo by zero.")

pyrobolearn/robots/sensors/camera.py

@@ -99,8 +99,8 @@ class CameraSensor(LinkSensor):
         """
         super(CameraSensor, self).__init__(simulator, body_id, link_id, position, orientation, rate)
 
-        self.width = width
-        self.height = height
+        self.width = int(width)
+        self.height = int(height)
         self.distance = distance
 
         # compute projection matrix (orthographic or perspective matrix)
@@ -123,15 +123,14 @@ class CameraSensor(LinkSensor):
         """
         Get the associated projection matrix.
         """
-        return np.array(self._P).reshape(4, 4).T
+        return self._P
 
     @property
     def V(self):
         """
         Get the associated view matrix.
         """
-        self.getV()
-        return np.array(self._V).reshape(4, 4).T
+        return self.getV()
 
     def getV(self):
         """
@@ -169,10 +168,10 @@ class CameraSensor(LinkSensor):
         """
         Return the captured RGBA image. 'A' stands for alpha channel (for opacity/transparency)
         """
-        img = np.array(self.sim.get_camera_image(self.width, self.height, self.getV(), self._P,
-                                                 shadow=1,  # lightDirection=[1,1,1],
-                                                 # renderer=self.sim.ER_TINY_RENDERER)[2])
-                                                 renderer=self.sim.ER_BULLET_HARDWARE_OPENGL)[2])
+        img = self.sim.get_camera_image(self.width, self.height, self.getV(), self._P,
+                                        shadow=1,  # lightDirection=[1,1,1],
+                                        # renderer=self.sim.ER_TINY_RENDERER)[2])
+                                        renderer=self.sim.ER_BULLET_HARDWARE_OPENGL)[2]
         img = img.reshape(self.width, self.height, 4)  # RGBA
         return img
 
@@ -180,8 +179,8 @@ class CameraSensor(LinkSensor):
         """
         Return the depth image.
         """
-        img = np.array(self.sim.get_camera_image(self.width, self.height, self.getV(), self._P,
-                                                 renderer=self.sim.ER_BULLET_HARDWARE_OPENGL)[3])
+        img = self.sim.get_camera_image(self.width, self.height, self.getV(), self._P,
+                                        renderer=self.sim.ER_BULLET_HARDWARE_OPENGL)[3]
         img = img.reshape(self.width, self.height)
         return img
 
@@ -190,8 +189,8 @@ class CameraSensor(LinkSensor):
         Return the RGBA and depth images.
         """
         rgba, depth = self.sim.get_camera_image(self.width, self.height, self.getV(), self._P)[2:4]
-        rgba = np.array(rgba).reshape(self.width, self.height, 4)
-        depth = np.array(depth).reshape(self.width, self.height)
+        rgba = rgba.reshape(self.width, self.height, 4)
+        depth = depth.reshape(self.width, self.height)
         if concatenate:
             return np.dstack((rgba, depth))
         return rgba, depth

pyrobolearn/simulators/bullet.py

@@ -2686,6 +2686,7 @@ class Bullet(Simulator):
         if flags is not None:
             kwargs['flags'] = flags
 
+        width, height = int(width), int(height)
         width, height, rgba, depth, segmentation = self.sim.getCameraImage(width, height, **kwargs)
         rgba = np.asarray(rgba).reshape(width, height, 4)
         depth = np.asarray(depth).reshape(width, height)

pyrobolearn/simulators/bullet_ros.py

@@ -213,7 +213,6 @@ class BulletROS(Bullet):  # , ROS):
                 module = importlib.import_module('pyrobolearn.robots.ros.' + name)
                 classes = dict(inspect.getmembers(module, inspect.isclass))
                 cls = classes['Robot' + name.capitalize()]
-                print("class: ".format(cls))
                 dictionary[id_] = cls(name=robot_directory_name, id_=id_)
 
             # load subscriber in simulator