def test_MPPI(mp, N, goal=np.array([0.,0.,0.])):
init_input = np.array([0.,0.,0.,0.,1.,0.,0.,0.])
pose = np.array([3.2,3.5,0.6])
mp.goal_tensor = torch.from_numpy(goal.astype(np.float32)).type(mp.dtype)
# print("Start:", pose)
mp.ctrl.zero_()
# Initialize the mppi first pose
mp.mppi_cb(Utils.particle_to_posestamped(pose, 'map'))
for i in range(0,N):
# ROLLOUT your MPPI function to go from a known location to a specified
# goal pose. Convince yourself that it works.
input_msg = Utils.particle_to_posestamped(pose, 'map')
pose = mp.mppi_cb(input_msg)
def test_MPPI(mp, N, goal=torch.Tensor([0., 0., 0.])):
pose = torch.Tensor([0., 0., 0.]).type(mp.dtype)
# Calculate costs for each of K trajectories
mp.goal_tensor = goal.type(mp.dtype) # (3,)
print("Start:", pose)
mp.ctrl.zero_()
# Initialize the mppi first pose
mp.mppi_cb(Utils.particle_to_posestamped(pose, 'map'))
for i in range(0, N):
# ROLLOUT your MPPI function to go from a known location to a specified
# goal pose. Convince yourself that it works.
input_msg = Utils.particle_to_posestamped(pose, 'map')
pose = mp.mppi_cb(input_msg)
print("Now:", pose.cpu().numpy())
print("End:", pose.cpu().numpy())
def visualize(self, poses):
if self.path_pub.get_num_connections() > 0:
frame_id = 'map'
for i in range(0, self.num_viz_paths):
pa = Path()
pa.header = Utils.make_header(frame_id)
pa.poses = []
for pose in poses.cpu().numpy()[i, :, :]:
pa.poses.append(
Utils.particle_to_posestamped(pose, str(frame_id)))
self.path_pub.publish(pa)
def mppi_cb(self, msg):
#print("callback")
if self.last_pose is None:
self.last_pose = np.array([msg.pose.position.x,
msg.pose.position.y,
Utils.quaternion_to_angle(msg.pose.orientation)])
# Default: initial goal to be where the car is when MPPI node is
# initialized
# if we are testing, keep the goal that we passed in!!
if not self.testing:
self.goal_tensor = torch.from_numpy(self.last_pose.astype(np.float32)).type(self.dtype)
self.lasttime = msg.header.stamp.to_sec()
return
theta = Utils.quaternion_to_angle(msg.pose.orientation)
curr_pose = np.array([msg.pose.position.x,
msg.pose.position.y,
theta])
# TODO: if close to the goal there's nothing to do
# XY_THRESHOLD = 0.05
# THETA_THRESHOLD = 0.17 # about 10 degrees
# difference_from_goal = curr_pose - self.goal_tensor
# xy_distance_to_goal = np.linalg.norm(difference_from_goal[:2])
# theta_distance_to_goal = np.abs(difference_from_goal[2])
# if xy_distance_to_goal < XY_THRESHOLD and theta_distance_to_goal < THETA_THRESHOLD:
# print 'Close to goal'
# return
self.last_pose = curr_pose
self.pose_pub.publish(Utils.particle_to_posestamped(curr_pose, 'map'))
run_ctrl, poses = self.mppi(curr_pose)
self.send_controls( run_ctrl[0], run_ctrl[1] )
self.visualize(poses)
## For testing: send control into model and pretend this is the real location
if self.testing:
self.model.particles = curr_pose.reshape(1,3)
self.model.controls = run_ctrl.cpu().numpy().reshape(1,2)
self.model.ackerman_equations()
return self.model.particles.reshape(3)
def visualizePlan(self):
if self.plan_pub.get_num_connections() > 0:
frame_id = 'map'
pa = Path()
pa.header = Utils.make_header(frame_id)
pa.poses = []
for i in range(self.currentPlanWaypointIndex,len(plan)):
next_segment = plan[i]
next_goal_point = next_segment[0]
gx = next_goal_point[0]
gy = next_goal_point[1]
gt = next_segment[1]
goalPixelSpace = self.dtype([[gx, gy, gt]])
Utils.map_to_world(goalPixelSpace, self.map_info)
thegoal = [goalPixelSpace[0][0], goalPixelSpace[0][1], goalPixelSpace[0][2]]
pa.poses.append(Utils.particle_to_posestamped(thegoal, frame_id))
self.plan_pub.publish(pa)
def mppi_cb(self, msg):
#print("callback")
if self.last_pose is None:
self.last_pose = torch.Tensor([
msg.pose.position.x, msg.pose.position.y,
Utils.quaternion_to_angle(msg.pose.orientation)
]).type(self.dtype)
# Default: initial goal to be where the car is when MPPI node is initialized
# If we are testing, don't update the goal (we modify it in the test instead)
if not self.testing:
self.goal_tensor = self.last_pose.clone()
self.lasttime = msg.header.stamp.to_sec()
return
theta = Utils.quaternion_to_angle(msg.pose.orientation)
curr_pose = torch.Tensor(
[msg.pose.position.x, msg.pose.position.y, theta]).type(self.dtype)
difference_from_goal = np.sqrt((curr_pose[0] -
self.goal_tensor[0])**2 +
(curr_pose[1] - self.goal_tensor[1])**2)
if difference_from_goal < 0.5:
self.MIN_VEL = -0.45 #TODO make sure these are right
self.MAX_VEL = 0.45
# else:
# self.MIN_VEL = -0.75 #TODO make sure these are right
# self.MAX_VEL = 0.75
# don't do any goal checking for testing purposes
pose_dot = curr_pose - self.last_pose # get state
self.last_pose = curr_pose
timenow = msg.header.stamp.to_sec()
dt = timenow - self.lasttime
self.lasttime = timenow
nn_input = torch.Tensor([
pose_dot[0], pose_dot[1], pose_dot[2],
np.sin(theta),
np.cos(theta), 0.0, 0.0, dt
]).type(self.dtype)
self.pose_pub.publish(Utils.particle_to_posestamped(curr_pose, 'map'))
poses = self.mppi(curr_pose, nn_input)
run_ctrl = None
if not self.cont_ctrl:
run_ctrl = self.get_control().cpu().numpy()
self.recent_controls[self.control_i] = run_ctrl
self.control_i = (self.control_i +
1) % self.recent_controls.shape[0]
pub_control = self.recent_controls.mean(0)
self.speed = pub_control[0]
self.steering_angle = pub_control[1]
if self.viz:
self.visualize(poses)
# For testing: send control into model and pretend this is the real location
if self.testing:
test_nn_input = nn_input.clone()
test_nn_input[5:7] = run_ctrl
test_pose_dot = self.model(
Variable(test_nn_input, requires_grad=False))
test_pose_dot = test_pose_dot.data
test_pose = curr_pose + test_pose_dot
test_pose[2] = Utils.clamp_angle(test_pose[2])
return test_pose
def mppi_cb(self, msg):
#print("callback")
if self.last_pose is None:
self.last_pose = torch.Tensor([
msg.pose.position.x, msg.pose.position.y,
Utils.quaternion_to_angle(msg.pose.orientation)
]).type(self.dtype)
self.lasttime = msg.header.stamp.to_sec()
if self.goal_tensor is None:
# Default: initial goal to be where the car is when MPPI node is initialized
self.goal_tensor = torch.Tensor([
msg.pose.position.x, msg.pose.position.y,
Utils.quaternion_to_angle(msg.pose.orientation)
]).type(self.dtype)
return
theta = Utils.quaternion_to_angle(msg.pose.orientation)
curr_pose = torch.Tensor(
[msg.pose.position.x, msg.pose.position.y, theta]).type(self.dtype)
# don't do any goal checking for testing purposes
if not self.testing:
if self.at_goal:
print 'Already at goal'
return
# TODO: if close to the goal, there's nothing to do
XY_THRESHOLD = 0.35
THETA_THRESHOLD = 0.5 # about 10 degrees
difference_from_goal = (curr_pose - self.goal_tensor).abs()
xy_distance_to_goal = difference_from_goal[:2].norm()
theta_distance_to_goal = difference_from_goal[2] % (2 * np.pi)
if xy_distance_to_goal < XY_THRESHOLD and theta_distance_to_goal < THETA_THRESHOLD:
print 'Goal achieved'
self.at_goal = True
self.speed = 0
self.steering_angle = 0
return
pose_dot = curr_pose - self.last_pose # get state
self.last_pose = curr_pose
timenow = msg.header.stamp.to_sec()
dt = timenow - self.lasttime
self.lasttime = timenow
nn_input = torch.Tensor([
pose_dot[0], pose_dot[1], pose_dot[2],
np.sin(theta),
np.cos(theta), 0.0, 0.0, dt
]).type(self.dtype)
self.pose_pub.publish(Utils.particle_to_posestamped(curr_pose, 'map'))
poses = self.mppi(curr_pose, nn_input)
run_ctrl = None
if not self.cont_ctrl:
run_ctrl = self.get_control()
self.speed = run_ctrl[0]
self.steering_angle = run_ctrl[1]
if self.viz:
self.visualize(poses)
# For testing: send control into model and pretend this is the real location
if self.testing:
# TODO kinematic model can't handle just (3,)?
self.model.particles = curr_pose.view(1, 3)
self.model.controls = torch.Tensor(
[self.speed, self.steering_angle]).type(self.dtype).view(1, 2)
self.model.ackerman_equations()
return self.model.particles.view(3)