diff --git a/social_mpc/mpc.py b/social_mpc/mpc.py
index e9f39cc8820502d6776d66556edd5bde6dd3481a..c0130d41981f4e258d35dcc8a4fde530a98b8573 100644
--- a/social_mpc/mpc.py
+++ b/social_mpc/mpc.py
@@ -240,7 +240,7 @@ class MPC:
 
     def _fw_base_positions(self, actions, start_ang):
         base_angles = self.h * np.cumsum(actions[:, self.n_angles]) + start_ang
-        velocities = np.stack([-actions[:, -1] * np.sin(base_angles), actions[:, -1] * np.cos(base_angles)], axis=1)
+        velocities = np.stack([actions[:, -1] * np.cos(base_angles), actions[:, -1] * np.sin(base_angles)], axis=1)
         base_positions = self.h * np.cumsum(velocities, axis=0)
         return base_angles, base_positions
 
@@ -252,7 +252,7 @@ class MPC:
         cost_map_xres, cost_map_yres = cost_map.shape
         im_x = (positions[:,0] - cost_map_xmin)/(cost_map_xmax - cost_map_xmin) * cost_map_xres
         im_y = (cost_map_ymax - positions[:,1])/(cost_map_ymax - cost_map_ymin) * cost_map_yres
-        int_position = np.stack([im_y, im_x], axis=1)
+        int_position = np.stack([im_x, im_y], axis=1)
         costs = jax.scipy.ndimage.map_coordinates(cost_map, np.transpose(int_position), order=1, mode='constant')
         return np.sum(costs)
 
@@ -303,7 +303,7 @@ class MPC:
         actions_features = np.tile(velocities, (self.horizon, 1))
         _, goal_positions = self.fw_base_positions(actions_features, 0.)
         goal_positions += np.array([x, y])
-        pan_goal_fw = -np.arctan2(goal_positions[:, 0], goal_positions[:, 1])
+        pan_goal_fw = np.arctan2(goal_positions[:, 1], goal_positions[:, 0])
         loss = self.true_fun_maxout_loss(self.fw_angles(state, actions), pan_goal_fw, weight[0], weight[1])
         # loss = self.true_fun_l2_loss(self.fw_angles(state, actions), pan_goal_fw)*weight[1]
         return loss
diff --git a/social_mpc/path.py b/social_mpc/path.py
index e506f6d4588f472e804124c2d83c01b3d6ab25f8..0621fbd6d28863a5aaa99c14583da49a8212e262 100644
--- a/social_mpc/path.py
+++ b/social_mpc/path.py
@@ -32,13 +32,15 @@ class PathPlanner():
                        self.state_config.global_map_width))
 
     def position_to_px_coord(self, pos):
+        """ Global map position (x, y) in meter to position 
+        index in numpy array format """
         scale = self.state_config.global_map_scale
         x_min = self.state_config.global_map_size[0][0]
         y_max = self.state_config.global_map_size[1][1]
         y_min = self.state_config.global_map_size[1][0]
 
-        j = (-y_min + pos[1]) * scale
-        i = (pos[0] - x_min) * scale
+        i = (-y_min + pos[1]) * scale
+        j = (pos[0] - x_min) * scale
         return int(i), int(j)
 
     def px_coord_grid(self):
@@ -72,17 +74,17 @@ class PathPlanner():
 
         if len(xl) == 0 or len(yl) == 0 or len(vxl) == 0 or len(vyl) == 0:
             return []
-        d_angle = utils.constraint_angle(np.arctan2(vyl[0] - pos[1], vxl[0] - pos[0]) - pos[2])
+        d_angle = utils.constraint_angle(np.arctan2(yl[0] - pos[1], xl[0] - pos[0]) - pos[2])
         if dl[0] < self.max_d_orientation:  # if within range, set waypoint = target
             wpt_pos = [self.target[0], self.target[1], self.target[2]]
         elif dl[0] < self.max_d:
             wpt_pos = [self.target[0],
                        self.target[1],
-                       np.arctan2(vyl[0] - pos[1], vxl[0] - pos[0])]
+                       np.arctan2(yl[0] - pos[1], xl[0] - pos[0])]
         elif np.abs(d_angle) > self.max_target_angle:
-            wpt_pos = [xl[0], yl[0], np.arctan2(vyl[0] - pos[1], vxl[0] - pos[0])]
-        else:  # else, point on shortest path with orientation towards target            
-            wpt_pos = [xl[-1], yl[-1], np.arctan2(vyl[-1] - pos[1], vxl[-1] - pos[0])]
+            wpt_pos = [xl[0], yl[0], np.arctan2(yl[0] - pos[1], xl[0] - pos[0])]
+        else:  # else, point on shortest path with orientation towards target
+            wpt_pos = [xl[-1], yl[-1], np.arctan2(yl[-1] - pos[1], xl[-1] - pos[0])]
         return wpt_pos
 
     def run(self,
diff --git a/social_mpc/utils.py b/social_mpc/utils.py
index 381d82419e91054db830b0dd0f949bbb6dd1e9b6..a6a6a5ad498514d293617af3a9d4034a6e32aa8c 100644
--- a/social_mpc/utils.py
+++ b/social_mpc/utils.py
@@ -35,7 +35,7 @@ def local_to_global_np(robot_position, x):
 
 
 def local_to_global_jax(robot_position, x):
-    angle = rotmat_2d_jax(-robot_position[:, -1])
+    angle = rotmat_2d_jax(robot_position[:, -1])
     y = vmap(vmpa_dot_jax, in_axes=(0, None))(jnp.moveaxis(angle, -1, 0), x)
     return jnp.array(y + robot_position[:, :2])
 
@@ -57,28 +57,28 @@ def box2d_frame_to_arrow_frame_ang(angle):
 
 
 def rotmat_2d(angle):
-    return np.matrix([[np.cos(angle), np.sin(angle)],
-                      [-np.sin(angle), np.cos(angle)]])
+    return np.matrix([[np.cos(angle), -np.sin(angle)],
+                      [np.sin(angle), np.cos(angle)]])
 
 def rotmat_2d_jax(angle):
-    return jnp.array([[jnp.cos(angle), jnp.sin(angle)],
-                      [-jnp.sin(angle), jnp.cos(angle)]])
+    return jnp.array([[jnp.cos(angle), -jnp.sin(angle)],
+                      [jnp.sin(angle), jnp.cos(angle)]])
 
 
 def rotmat_2d_np(angle):
-    return np.array([[np.cos(angle), np.sin(angle)],
-                      [-np.sin(angle), np.cos(angle)]])
+    return np.array([[np.cos(angle), -np.sin(angle)],
+                      [np.sin(angle), np.cos(angle)]])
 
 
 def local_to_global(robot_position, x):
     angle = robot_position[-1]
-    y = rotmat_2d(-angle).dot(x) + robot_position[:2]
+    y = rotmat_2d(angle).dot(x) + robot_position[:2]
     return np.array(y).reshape(x.shape)
 
 
 def global_to_local(robot_position, x):
     angle = robot_position[-1]
-    y = rotmat_2d(angle).dot(x - robot_position[:2])
+    y = rotmat_2d(-angle).dot(x - robot_position[:2])
     return np.array(y).reshape(x.shape)
 
 def zero_2pi_angle(angles):