mpc + dstar improvements #55
Conversation
| d1 += np.random.uniform(-0.01, 0.01) | ||
| d2 += np.random.uniform(-0.01, 0.01) | ||
| d3 += np.random.uniform(-0.01, 0.01) | ||
| d1 += np.random.uniform(-0.05, 0.05) |
There was a problem hiding this comment.
Why is trilaterate.py edited in this PR?
There was a problem hiding this comment.
an edit that sneaked its way in there, I can remove it. that code unit tests the trilateration algorithm, so should move it to test code sometime that runs on CI changes.
| odom_topic = rospy.get_param("/odom_topic") | ||
| goal_topic = rospy.get_param("/nav_goal_topic") | ||
|
|
||
| radius = 8 # robot rad (grid units) | ||
| # /nav params |
There was a problem hiding this comment.
Should we move smoothing to MPC to avoid publishing extra points? (And just have dstar call path.smooth when publishing?)
There was a problem hiding this comment.
@ashviniyer21 why? publishing 8pts vs 12pts vs 30pts isn't that much of a difference computationally. ros should handle that just fine. also more points may result in mpc taking a longer time to converge (or would need to update the cost function of mpc to handle more waypoints
| std::vector<Eigen::MatrixXd> random_vels_raw(count); | ||
| std::vector<Eigen::MatrixXd> random_vels_clamped(count); | ||
|
|
||
| #pragma omp parallel for |
There was a problem hiding this comment.
Is this tested to provide any improvement?
There was a problem hiding this comment.
need to officially benchmark. i couldn't tell. it seems to still run at 5-7hz. also our benchmarks should be done on the jetson because that's a better reflection of the multiprocessing that can be done. i also personally think we should do gpu acceleration rather than cpu acceleration.
| for (int j = 0; j < rollouts[i].first.rows(); ++j) { | ||
| mean(j, 0) += rollouts[i].first(j, 3); | ||
| mean(j, 1) += rollouts[i].first(j, 4); | ||
| mean(j, 0) += rollouts[i].first(j, 5); |
There was a problem hiding this comment.
This is also on me but maybe have comments / better way to understand this than just idx 5 / 6
There was a problem hiding this comment.
Yeah, lets make some #define statements for all these indexes?
| ((position(0) - path_[path_cost_i][0]) * (position(0) - path_[path_cost_i][0]) + | ||
| (position(1) - path_[path_cost_i][1]) * (position(1) - path_[path_cost_i][1])); | ||
|
|
||
| cost += this->w_waypoint_rot_ * (position(2) - path_[path_cost_i][2]) * |
There was a problem hiding this comment.
We may want to redo how we calculate cost / reward for path following if we used a smooth path. Idk how many more points you generate but we may be constraining ourselves too strictly to the path with the current method. Not sure how results look tho.
- mpc accel cost was computed incorrectly - keeping two config files with all params for mpc node, dstar. plan is to keep all params in the real_robot.yml and sim.yml
raghavauppuluri13
force-pushed
the
hotfix/mpc-with-clamps
branch
from
d04ffa9 to
21924b6
Compare
I am still getting weird behavior right now though. I remember it worked well directly after i added the clamps (nothing else). so some bugs to find in here.
I turned the smoothing off in sim and made the w_waypoint_rot = 0, so the only changes that should be applied is the smooth clamps. something is tripping though