Error with rest cost

[siddancha]

Currently, when I run python_scripts/ds_mppi/scripts/standalonePlanar2d.py, I get the following error:

warmup run #1 done
warmup run #2 done
warmup run #3 done
warmup run #4 done
warmup run #5 done
warmup run #1 done
warmup run #2 done
warmup run #3 done
warmup run #4 done
warmup run #5 done
Init time: 0.41s
Traceback (most recent call last):
  File "/home/sancha/repos/OptimalModulationDS/python_scripts/ds_mppi/scripts/standalonePlanar2d.py", line 223, in <module>
    main_int()
  File "/home/sancha/repos/OptimalModulationDS/python_scripts/ds_mppi/scripts/standalonePlanar2d.py", line 156, in main_int
    cost = mppi.get_cost()
  File "/home/sancha/repos/OptimalModulationDS/python_scripts/ds_mppi/scripts/../functions/MPPI.py", line 316, in get_cost
    self.cur_cost = self.Cost.evaluate_costs(self.all_traj, self.closest_dist_all)
  File "/home/sancha/repos/OptimalModulationDS/python_scripts/ds_mppi/scripts/../functions/cost.py", line 20, in evaluate_costs
    rest_cost = goal_cost * self.rest_cost(all_traj)
  File "/home/sancha/repos/OptimalModulationDS/python_scripts/ds_mppi/scripts/../functions/cost.py", line 46, in rest_cost
    return (all_traj - self.rest).norm(2, dim=2).sum(dim=1)
RuntimeError: The size of tensor a (2) must match the size of tensor b (7) at non-singleton dimension 2

When I comment out this line in python_scripts/ds_mppi/functions/cost.py:

rest_cost = goal_cost * self.rest_cost(all_traj)

the error goes away.