removed final instances of tf use

Author: GustavoSilvera

costs/quad_cost_with_wrapping.py

@@ -1,6 +1,6 @@
-import tensorflow as tf
 from costs.cost import DiscreteCost
 from utils.angle_utils import angle_normalize
+import numpy as np
 
 
 class QuadraticRegulatorRef(DiscreteCost):
@@ -16,7 +16,7 @@ def __init__(self, trajectory_ref, system, params):
                 system: A system dynamics object (used for parsing trajectory objects)
                 state that corresponds to angles and should be wrapped.
         """
-        
+
         # Used for parsing trajectory objects into tensors of states and actions
         self.params = params
         self.system = system
@@ -26,7 +26,8 @@ def __init__(self, trajectory_ref, system, params):
 
         self.trajectory_ref = trajectory_ref
         self.update_shape()
-        super(QuadraticRegulatorRef, self).__init__(x_dim=self._x_dim, u_dim=self._u_dim)
+        super(QuadraticRegulatorRef, self).__init__(
+            x_dim=self._x_dim, u_dim=self._u_dim)
 
         self.isTimevarying = True
         self.isNonquadratic = False
@@ -38,52 +39,58 @@ def update_shape(self):
 
         x_dim, u_dim = self._x_dim, self._u_dim
 
-        C_gg = tf.diag(p.quad_coeffs, name='lqr_coeffs_quad')
-        c_g = tf.constant(p.linear_coeffs, name='lqr_coeffs_linear', dtype=tf.float32)
+        C_gg = np.linalg.diag(p.quad_coeffs, name='lqr_coeffs_quad')
+        c_g = np.constant(
+            p.linear_coeffs, name='lqr_coeffs_linear', dtype=np.float32)
         # Check dimensions
-        assert ((tf.reduce_all(tf.equal(C_gg[:x_dim, x_dim:], tf.transpose(C_gg[x_dim:, :x_dim]))).numpy()))
-        assert ((x_dim + u_dim) == C_gg.shape[0].value == C_gg.shape[1].value == c_g.shape[0].value)
+        assert ((np.reduce_all(
+            np.equal(C_gg[:x_dim, x_dim:], np.transpose(C_gg[x_dim:, :x_dim])))))
+        assert ((x_dim + u_dim) ==
+                C_gg.shape[0].value == C_gg.shape[1].value == c_g.shape[0].value)
 
         trajectory_ref = self.trajectory_ref
         n, k, g = trajectory_ref.n, trajectory_ref.k, C_gg.shape[0]
-        self._C_nkgg = tf.broadcast_to(C_gg, (n, k, g, g))
-        self._c_nkg = tf.broadcast_to(c_g, (n, k, g))
+        self._C_nkgg = np.broadcast_to(C_gg, (n, k, g, g))
+        self._c_nkg = np.broadcast_to(c_g, (n, k, g))
 
     def compute_trajectory_cost(self, trajectory, trials=1):
-        with tf.name_scope('compute_traj_cost'):
-            z_nkg = self.construct_z(trajectory)
-            C_nkgg, c_nkg = self._C_nkgg, self._c_nkg
-            Cz_nkg = tf.squeeze(tf.matmul(C_nkgg, z_nkg[:, :, :, None]))
-            zCz_nk = tf.reduce_sum(z_nkg*Cz_nkg, axis=2)
-            cz_nk = tf.reduce_sum(c_nkg*z_nkg, axis=2)
-            cost_nk = .5*zCz_nk + cz_nk
-            return cost_nk, tf.reduce_sum(cost_nk, axis=1)
+        # with tf.name_scope('compute_traj_cost'):
+        z_nkg = self.construct_z(trajectory)
+        C_nkgg, c_nkg = self._C_nkgg, self._c_nkg
+        Cz_nkg = np.squeeze(np.matmul(C_nkgg, z_nkg[:, :, :, None]))
+        zCz_nk = np.sum(z_nkg * Cz_nkg, axis=2)
+        cz_nk = np.sum(c_nkg * z_nkg, axis=2)
+        cost_nk = .5 * zCz_nk + cz_nk
+        return cost_nk, np.sum(cost_nk, axis=1)
 
     def quad_coeffs(self, trajectory, t=None):
         # Return terms H_xx, H_xu, H_uu, J_x, J_u
-        with tf.name_scope('quad_coeffs'):
-            H_nkgg = self._C_nkgg
-            J_nkg = self._c_nkg
-            z_nkg = self.construct_z(trajectory)
-            Hz_nkg = tf.squeeze(tf.matmul(H_nkgg, z_nkg[:, :, :, None]), axis=-1)
-            return H_nkgg[:, :, :self._x_dim, :self._x_dim], \
-                   H_nkgg[:, :, :self._x_dim, self._x_dim:], \
-                   H_nkgg[:, :, self._x_dim:, self._x_dim:], \
-                   J_nkg[:, :, :self._x_dim] + Hz_nkg[:, :, :self._x_dim], \
-                   J_nkg[:, :, self._x_dim:] + Hz_nkg[:, :, self._x_dim:]
+        # with tf.name_scope('quad_coeffs'):
+        H_nkgg = self._C_nkgg
+        J_nkg = self._c_nkg
+        z_nkg = self.construct_z(trajectory)
+        Hz_nkg = np.squeeze(
+            np.matmul(H_nkgg, z_nkg[:, :, :, None]), axis=-1)
+        return H_nkgg[:, :, :self._x_dim, :self._x_dim], \
+            H_nkgg[:, :, :self._x_dim, self._x_dim:], \
+            H_nkgg[:, :, self._x_dim:, self._x_dim:], \
+            J_nkg[:, :, :self._x_dim] + Hz_nkg[:, :, :self._x_dim], \
+            J_nkg[:, :, self._x_dim:] + Hz_nkg[:, :, self._x_dim:]
 
     # TODO: Currently calling numpy() here as tfe.DEVICE_PLACEMENT_SILENT is not working in the eager mode to place
     # non-gpu ops (i.e. mod) on the cpu turning tensors into numpy arrays is a hack around this.
     def construct_z(self, trajectory):
         """ Input: A trajectory with x_dim =d and u_dim=f
             Output: z_nkg - a tensor of size n,k,g where g=d+f
         """
-        with tf.name_scope('construct_z'):
-            x_nkd, u_nkf = self.system.parse_trajectory(trajectory)
-            x_ref_nkd, u_ref_nkf = self.system.parse_trajectory(self.trajectory_ref)
-            delx_nkd = x_nkd - x_ref_nkd
-            delu_nkf = u_nkf - u_ref_nkf
-            z_nkg = tf.concat([delx_nkd[:, :, :self.angle_dims],
-                               angle_normalize(delx_nkd[:, :, self.angle_dims:self.angle_dims+1].numpy()),
-                               delx_nkd[:, :, self.angle_dims+1:], delu_nkf], axis=2)
-            return z_nkg
+        # with tf.name_scope('construct_z'):
+        x_nkd, u_nkf = self.system.parse_trajectory(trajectory)
+        x_ref_nkd, u_ref_nkf = self.system.parse_trajectory(
+            self.trajectory_ref)
+        delx_nkd = x_nkd - x_ref_nkd
+        delu_nkf = u_nkf - u_ref_nkf
+        z_nkg = np.concatenate([delx_nkd[:, :, :self.angle_dims],
+                                angle_normalize(
+            delx_nkd[:, :, self.angle_dims:self.angle_dims + 1]),
+            delx_nkd[:, :, self.angle_dims + 1:], delu_nkf], axis=2)
+        return z_nkg

waypoint_grids/projected_image_space_grid.py

@@ -1,5 +1,4 @@
 import numpy as np
-import tensorflow as tf
 from waypoint_grids.uniform_sampling_grid import UniformSamplingGrid
 
 
@@ -11,36 +10,39 @@ def __init__(self, params):
         # Compute the image size bounds based on the focal length and the field of view
         params = self.compute_image_bounds(params)
         super(ProjectedImageSpaceGrid, self).__init__(params)
-        
+
         # Compute the rotation and translation vectors from the world frame to the optical frame and vice-versa
         self.compute_rotation_and_translation_transformations()
-        
+
     @staticmethod
     def compute_image_bounds(params):
         """
         Compute the image size bounds based on the focal length and the field of view.
         """
         eps = 1e-2
         # The projection point of the outermost point in the filed of view
-        dx = params.projected_grid_params.f * np.tan(params.projected_grid_params.fov)
-        
+        dx = params.projected_grid_params.f * \
+            np.tan(params.projected_grid_params.fov)
+
         # Compute x_min and x_max
         x_min = -1. * dx
         x_max = 1. * dx
-    
+
         # Compute y_min
         if params.projected_grid_params.tilt > params.projected_grid_params.fov:
             y_min = -1. * dx
         else:
             # An eps is subtracted from the tilt to clip the far end of the camera
-            y_min = -1. * params.projected_grid_params.f * np.tan(params.projected_grid_params.tilt - eps)
-        
+            y_min = -1. * params.projected_grid_params.f * \
+                np.tan(params.projected_grid_params.tilt - eps)
+
         # Compute y_max
-        if params.projected_grid_params.tilt + params.projected_grid_params.fov < np.pi/2:
+        if params.projected_grid_params.tilt + params.projected_grid_params.fov < np.pi / 2:
             y_max = 1. * dx
         else:
             # An eps is subtracted from the tilt to clip the far end of the camera
-            y_max = params.projected_grid_params.f * np.tan(np.pi/2 - params.projected_grid_params.tilt - eps)
+            y_max = params.projected_grid_params.f * \
+                np.tan(np.pi / 2 - params.projected_grid_params.tilt - eps)
 
         params.bound_min = [x_min, y_min, params.bound_min[2]]
         params.bound_max = [x_max, y_max, params.bound_max[2]]
@@ -55,23 +57,24 @@ def sample_egocentric_waypoints(self, vf=0.):
         wf_n11 = np.zeros_like(wx_n11)
         # Project the (x, y, theta) points back in the world coordinates.
         return self.generate_worldframe_waypoints_from_imageframe_waypoints(wx_n11, wy_n11, wtheta_n11, vf_n11, wf_n11)
-    
+
     def generate_worldframe_waypoints_from_imageframe_waypoints(self, wx_n11, wy_n11, wtheta_n11,
                                                                 vf_n11=None, wf_n11=None):
         """
         Project the (x, y, theta) waypoints in the image space back in the world coordinates. In the world frame x
         correspond to Z (the depth) and y corresponds to the x direction in the image plane. The theta in the image
         plane is measured positively anti-clockwise from the x-axis.
         """
-        X_n1, _, Z_n1 = self.project_image_space_points_to_ground(np.hstack([wx_n11[:, :, 0], wy_n11[:, :, 0]]))
+        X_n1, _, Z_n1 = self.project_image_space_points_to_ground(
+            np.hstack([wx_n11[:, :, 0], wy_n11[:, :, 0]]))
         # Project coordinates for determining angle (take a vector of magnitude 1e-5 in the direction of angle)
         X_plus_delta_n1, _, Z_plus_delta_n1 = self.project_image_space_points_to_ground(
-            np.hstack([wx_n11[:, :, 0] + 1e-5*np.cos(wtheta_n11[:, :, 0]),
-                       wy_n11[:, :, 0] + 1e-5*np.sin(wtheta_n11[:, :, 0])]))
+            np.hstack([wx_n11[:, :, 0] + 1e-5 * np.cos(wtheta_n11[:, :, 0]),
+                       wy_n11[:, :, 0] + 1e-5 * np.sin(wtheta_n11[:, :, 0])]))
         return Z_n1[:, :, np.newaxis], X_n1[:, :, np.newaxis], \
-               np.arctan2(X_plus_delta_n1 - X_n1, Z_plus_delta_n1 - Z_n1)[:, :, np.newaxis], \
-               vf_n11, wf_n11
-    
+            np.arctan2(X_plus_delta_n1 - X_n1, Z_plus_delta_n1 - Z_n1)[:, :, np.newaxis], \
+            vf_n11, wf_n11
+
     def generate_imageframe_waypoints_from_worldframe_waypoints(self, wx_n11, wy_n11, wtheta_n11,
                                                                 vf_n11=None, wf_n11=None):
         """
@@ -81,24 +84,27 @@ def generate_imageframe_waypoints_from_worldframe_waypoints(self, wx_n11, wy_n11
         """
         # Project points in the optical coordinates
         n = wx_n11.shape[0]
-        XYZ_world_coordinates_n3 = np.hstack([wy_n11[:, :, 0], np.zeros((n, 1)), wx_n11[:, :, 0]])
-        XYZ_optical_coordinates_n3 = self.convert_world_coordinates_to_optical_coordinates(XYZ_world_coordinates_n3)
-        
+        XYZ_world_coordinates_n3 = np.hstack(
+            [wy_n11[:, :, 0], np.zeros((n, 1)), wx_n11[:, :, 0]])
+        XYZ_optical_coordinates_n3 = self.convert_world_coordinates_to_optical_coordinates(
+            XYZ_world_coordinates_n3)
+
         # Project points in the optical coordinates for theta transformation
         XYZ_plus_delta_world_coordinates_n3 = np.hstack([wy_n11[:, :, 0] + 1e-5 * np.sin(wtheta_n11[:, :, 0]),
                                                          np.zeros((n, 1)),
                                                          wx_n11[:, :, 0] + 1e-5 * np.cos(wtheta_n11[:, :, 0])])
         XYZ_plus_delta_optical_coordinates_n3 = self.convert_world_coordinates_to_optical_coordinates(
             XYZ_plus_delta_world_coordinates_n3)
-        
+
         # Project optical coordinates into image space
-        x_n1, y_n1 = self.project_optical_coordinates_to_image_space(XYZ_optical_coordinates_n3)
+        x_n1, y_n1 = self.project_optical_coordinates_to_image_space(
+            XYZ_optical_coordinates_n3)
         x_plus_delta_n1, y_plus_delta_n1 = self.project_optical_coordinates_to_image_space(
             XYZ_plus_delta_optical_coordinates_n3)
-        
+
         return x_n1[:, :, np.newaxis], y_n1[:, :, np.newaxis], \
-               np.arctan2(y_plus_delta_n1 - y_n1, x_plus_delta_n1 - x_n1)[:, :, np.newaxis], \
-               vf_n11, wf_n11
+            np.arctan2(y_plus_delta_n1 - y_n1, x_plus_delta_n1 - x_n1)[:, :, np.newaxis], \
+            vf_n11, wf_n11
 
     def worldframe_waypoint_direction_indicator(self, wx_n11, wy_n11, wtheta_n11, vf_n11=None,
                                                 wf_n11=None):
@@ -108,18 +114,22 @@ def worldframe_waypoint_direction_indicator(self, wx_n11, wy_n11, wtheta_n11, vf
         -1 if "behind the camera" (Z coordinate negative).
         """
         n = wx_n11.shape[0]
-        XYZ_world_coordinates_n3 = np.hstack([wy_n11[:, :, 0], np.zeros((n, 1)), wx_n11[:, :, 0]])
-        XYZ_optical_coordinates_n3 = self.convert_world_coordinates_to_optical_coordinates(XYZ_world_coordinates_n3)
-        return tf.sign(XYZ_optical_coordinates_n3[:, 2])[:, None, None]
+        XYZ_world_coordinates_n3 = np.hstack(
+            [wy_n11[:, :, 0], np.zeros((n, 1)), wx_n11[:, :, 0]])
+        XYZ_optical_coordinates_n3 = self.convert_world_coordinates_to_optical_coordinates(
+            XYZ_world_coordinates_n3)
+        return np.sign(XYZ_optical_coordinates_n3[:, 2])[:, None, None]
 
     def project_optical_coordinates_to_image_space(self, XYZ_n3):
         """
         Project a series of coordinates from the optical frame to the image space.
         """
-        x_n = -self.params.projected_grid_params.f * XYZ_n3[:, 0] / XYZ_n3[:, 2]
-        y_n = -self.params.projected_grid_params.f * XYZ_n3[:, 1] / XYZ_n3[:, 2]
+        x_n = -self.params.projected_grid_params.f * \
+            XYZ_n3[:, 0] / XYZ_n3[:, 2]
+        y_n = -self.params.projected_grid_params.f * \
+            XYZ_n3[:, 1] / XYZ_n3[:, 2]
         return x_n[:, np.newaxis], y_n[:, np.newaxis]
-    
+
     def project_image_space_points_to_ground(self, xy_n2):
         """
         Project a series of points in the image space to the ground plane.
@@ -132,12 +142,14 @@ def project_image_space_points_to_ground(self, xy_n2):
         tilt = self.params.projected_grid_params.tilt
         h = self.params.projected_grid_params.h
         n = xy_n2.shape[0]
-        deno_n1 = self.params.projected_grid_params.f * np.sin(tilt) + xy_n2[:, 1:2] * np.cos(tilt)
-        X_n1 = -1. * xy_n2[:, 0:1] * h/deno_n1
+        deno_n1 = self.params.projected_grid_params.f * \
+            np.sin(tilt) + xy_n2[:, 1:2] * np.cos(tilt)
+        X_n1 = -1. * xy_n2[:, 0:1] * h / deno_n1
         Y_n1 = np.zeros((n, 1))
-        Z_n1 = h * (self.params.projected_grid_params.f * np.cos(tilt) - xy_n2[:, 1:2] * np.sin(tilt)) / deno_n1
+        Z_n1 = h * (self.params.projected_grid_params.f *
+                    np.cos(tilt) - xy_n2[:, 1:2] * np.sin(tilt)) / deno_n1
         return X_n1, Y_n1, Z_n1
-    
+
     def convert_world_coordinates_to_optical_coordinates(self, xyz_n3):
         """
         Convert a series of coordinates from the world frame to the optical frame (the one that is aligned with the
@@ -146,15 +158,15 @@ def convert_world_coordinates_to_optical_coordinates(self, xyz_n3):
         # First translate the points and then rotate them.
         xyz_n3 = xyz_n3 - self.T_world_optical
         return xyz_n3.dot(self.R_world_optical.transpose())
-        
+
     def convert_optical_coordinates_to_world_coordinates(self, xyz_n3):
         """
         Convert a series of coordinates from the optical frame to the world frame.
         """
         # First rotate and then translate the points.
         xyz_n3 = xyz_n3.dot(self.R_optical_world.transpose())
         return xyz_n3 - self.T_optical_world
-        
+
     def compute_rotation_and_translation_transformations(self):
         """
         Compute the rotation and translation matrices from the world frame to the optical frame and vice-versa.
@@ -170,9 +182,11 @@ def compute_rotation_and_translation_transformations(self):
         self.R_optical_world = np.array([[1., 0., 0.],
                                          [0., np.cos(tilt), -np.sin(tilt)],
                                          [0., np.sin(tilt), np.cos(tilt)]])
-        self.T_world_optical = np.array([0., self.params.projected_grid_params.h, 0.])
-        self.T_optical_world = np.array([0., -self.params.projected_grid_params.h, 0.])
-    
+        self.T_world_optical = np.array(
+            [0., self.params.projected_grid_params.h, 0.])
+        self.T_optical_world = np.array(
+            [0., -self.params.projected_grid_params.h, 0.])
+
     @property
     def descriptor_string(self):
         """Returns a unique string identifying