From cc3a2e632364fb70a99b9bdf4eeb58d1b1962cc2 Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Wed, 31 May 2023 18:40:49 +0200
Subject: [PATCH] np.flip and np.pi/2 removed (following)
---
social_mpc/mpc.py | 6 +++---
social_mpc/path.py | 16 +++++++++-------
social_mpc/utils.py | 18 +++++++++---------
3 files changed, 21 insertions(+), 19 deletions(-)
diff --git a/social_mpc/mpc.py b/social_mpc/mpc.py
index e9f39cc..c0130d4 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 e506f6d..0621fbd 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 381d824..a6a6a5a 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):
--
GitLab