diff --git a/examples/rewards/manipulator.py b/examples/rewards/manipulator.py index 76cc7ba..c234ba9 100644 --- a/examples/rewards/manipulator.py +++ b/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) diff --git a/pyrobolearn/rewards/cost.py b/pyrobolearn/rewards/cost.py index 7535926..3bcb8bf 100644 --- a/pyrobolearn/rewards/cost.py +++ b/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 diff --git a/pyrobolearn/rewards/reward.py b/pyrobolearn/rewards/reward.py index f92518b..ca9b375 100644 --- a/pyrobolearn/rewards/reward.py +++ b/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.") diff --git a/pyrobolearn/robots/sensors/camera.py b/pyrobolearn/robots/sensors/camera.py index afc71af..355315a 100644 --- a/pyrobolearn/robots/sensors/camera.py +++ b/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 diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py index 3b958fd..c2986bb 100644 --- a/pyrobolearn/simulators/bullet.py +++ b/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) diff --git a/pyrobolearn/simulators/bullet_ros.py b/pyrobolearn/simulators/bullet_ros.py index 0c14dd1..df002eb 100644 --- a/pyrobolearn/simulators/bullet_ros.py +++ b/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