In this episode, Audrow Nash speaks to Steve Macenski, who is the Open Source Robotics Engineering Lead at Samsung Research America. Steve leads the Nav2 project, which is an open source robot navigation framework. In this conversation, they talk about the problem and challenges of robot navigation, how Nav2 works at a high level, hybrid planners, and about Steve’s experience working with the Nav2 community.

Start

Introducing Steve and Nav2

History of the navigation stack and Nav2

What problems does Nav2 solve?

How Nav2, MoveIt, and ROS Control relate

Challenges in navigation

Working in a grid world

Motion primitives

Potential fields

Unknown maps

Relating paths to wheel velocities

When is Pathplanning used?

Hybrid planners

Changing environments

Behavior trees

Behavior tree example

Behavior trees versus alternatives

Nav2 and SLAM

Nav2 for different types of surface environments

Experience with the Nav2 community

Open Source as a business model

Getting involved

Career advice

This is a conversation with Steve Macenski, who is the Open Source Robotics Engineering lead at Samsung Research America. Steve leads the Nav2 two project, which is an open source robot navigation framework. In this interview, Steve and I talk about the problem and challenges in robot navigation, how Nav2 works at a high level, hybrid planners, and about Steve's experience working with the Nav2 community. This is the Sense Think Act Podcast. I'm Audrow. Nash. Thank you to our founding sponsor Open Robotics. And now here's my conversation with Steve. Steve, would you introduce yourself?

Hi, yeah, I'm Steven Macenski. And the open source engineering lead for a box at Samsung research. So I'm the lead developer on the ROS to navigation stack. And I sit on the TSC. For us, too, as well. And the TSC is the Technical Steering Committee. Correct? Yeah. So it's like the the board for ROS, let's say. So it's a bunch of different companies that kind of come together with Idris and ROS 2 to make sure that we can see the project the right direction.

So yeah, tell me a bit about the navigation stack or not. And then how that relates to NAV two.

So I'm going to navigate stack was the the, you know, the software built at Willow Garage to do you know, mobile based application, you know, primarily for the PR to robot. But now even in those early days, they understood that a lot of the same technology could be applied to other mobile platforms. So I made at the time they made it, it was awesome. The fact that there was a software out there that was free and open source and with a strong community around it with documentation and examples and all that kind of good stuff. But you know, over time, you know, that was, you know, in the very early days of robotics, where basically the light robot you could buy if your home was like, you know, an iRobot Roomba. And there really were few of any kind of service robots, you know, in application. What year do you think this was? Was this like, 22,000? Or when do you think it was like 12 ish is when started. And then there's a big push to update some algorithms. And I think the summer of 2011 2012, when a bunch of insurance guy came in, and absolute new new variations of the algorithms.

Okay, so you had this open source navigation stack? And that was with Willow Garage. And then how did that evolve? How did that how did we go from there? Yeah,

so it's after we'll closed in the 2013 timeframe, kind of all major developments on the navigation, kind of kind of halted. So you had folks like David Liu and Mike Ferguson that continued to maintain things over time, to date. So David was, you know, working at Lucas robotics and working as a consultant doing doing various navigation work. And then Mike Ferguson was starting, what was the precursor to Fetch robotics, then later, Fetch robotics. And so he was using in the early days, some of the navigation stuff, I think, these days, I have to assume everything's been stripped out their systems at this point, but in the early days to try and build that MVP to get the venture capital money, that that's the he was working on on the napkin stuff, as well. But then, you know, nothing really major change from from 2013, basically, until about, you know, 2019 or so when, you know, ROS 2 started hitting kind of more mature milestones. So I think with without see is when Matt Hanson, in the open source robotics team at Intel, had a big post a discourse saying that, hey, there, they want to work on this roster navigation stack. They didn't want it to just be a straight port for boss, one that wants to redesign from the ground up based on like, the current requirements that are basically is asking people to air their grievances about the boss one stack, or what kinds of things they were swapping out the stack because they didn't meet the requirements. And so that that actually, that discourse posts, I still today occasionally open up and go down a list of refers to it to make sure Yeah, to make sure like, okay, oh, like I forgot about this, we still haven't addressed this point. And I'll add that to my queue. But at this point, a lot of those lob abroad, things that people said are have not been handled for the most part. So really, Matt, and the team at Intel, really put like the legwork in to get this get the ball rolling. And when they did that's kind of when I when I jumped on on the bandwagon to start working on things. But that's essentially how that app to project kind of got going. And how we got a lot of the architectural elements we see today.

Gotcha. So if I understand correctly, so it was started at Willow Garage in 2010. Ish. And the as a general navigation library, and then ROS 1 came out. And it was used with ROS 1. So the general navigation code was using ROS 1. It continued and people were using that. And then ROS 2 came out in an early version, he said bouncy, and then you then ask the community for what was wrong or what could be improved. And then this is how we have come to NAV to

Yeah, I mean, that's it It wasn't me that that was Matt and the team at Intel, they were really the ones in the early days when you're porting stuff over and starting to do some of the core architecture elements that were taking the lead. So I think they had like six people working on it. So they there was a lot of professional software engineering time put into it. And in those early

days, nice, and how did you get involved?

Yeah, so I've been kind of doing open source stuff for a bit before that point. So when I was doing the robotics team at semi robotics, I built the STL layer, as well as the SAM toolbox library. And so I've already done a couple of ROS con talks on those topics. So Samsung was looking to hire somebody to work with the open source group on various open source box technologies. And they've reached out to Matt Hanson asking kind of who they might recommend that would be good for this. And they said that, you know, I might be good, good, good candidate. At the time, I was leaving Sydney robotics, and so is really good transition to go over to Samsung research, to work on this open source technology full time, versus just, you know, working on these problems that are specific to Cindy, for the end products, and then open sourcing the little bits here and there that makes sense open source. You know, now I can start working on open source things and make big infrastructure changes that are that are fully in the open.

Yeah, it's awesome. It's an awesome position to be in. It's fun. Oh, yeah. With. So tell me kind of at a high level, what problems does not have to solve?

For work to solve? Yeah, so now, nav two is basically sitting between you have a bunch of motors and a motor controller that can take take philosophy commands to go forward and your autonomy application where you want robots, you say I want robot to go there. So it kind of deals with everything in the middle from the the mapping and localization systems, which I consider to be slightly separated from navigation, I consider the navigation, one problem set and localization to be another problem set. But we do have reference implementations that work with AMC LSF toolbox for that localization element. And then on the navigation side, and we say, well, now that we know where we are within some global representation of the world, and we have this global representation of the world, how do we, you know, navigate the space in order to, you know, go to our to our final goal. So this could be in a warehouse, like the position on a shelf, where you want to have go pick a box, or with your Roomba say, you know, what's this little pattern I want to nap on the floor in order to clean my space? So we solved that problem of saying, how do we take some sensor data from from just hardware and populate a representation of our environment, given the current state of the world? And then once we have the state of the world, how do we make a route that goes from where I am to where I want to be within that environment. And then finally, dealing with the the lower level system of okay, how I now follow this path that goes from where I am to where I want to be accurately efficiently. And there's obviously more little bells and whistles on widgets, here and there to help make that you know, either more efficient or more configurable or deal with fault tolerances and things like that. But that'd be kind of the core problem navigation and actually tries to solve.

So it's, it's sounds like several things. How do you? How do you find a good delimiter? Between math two and other ROS libraries? Like ROS to control for example, or move it or any of these other ones?

Yeah, so I guess these all become parallel pillars. I mean, these are all by themselves really significant applications that have their own specific developer code bases and don't own individual ecosystems, and are they themselves you know, equivalently complex challenges to solve. So move that I say in the most simplistic terms is our navigation, which was actually the the the library name from Willow Garage that became move it was application. So it's often the same kinds of problems as as the mobile robot navigation that nav two solves, but for arms, so it's taking potentially sensor data to build an environmental representation. In the case of movement, they're using Okta map and with 3d Octrees, then they're using sampling based motion planning in order to get a a rough plan from point A to point B. And using various smoothing techniques usually optimization base to smooth those out to be a little bit less clunky on the on the motors earlier while you're going through the sampling base planner would give you naturally and then finally, then having those the same kind of local trajectory controllers, we have Neptune now with the hyperplane schema. So there's somebody a very, very analog problem, pretty much everything they exist in that one nephew, rather, and move and have some sort of direct analog to each other for the most part. And then your ROS control is is kind of sitting in that lower level on the actual electronics solving the motor control issue of saying, you know, I have this I have this motor and I want to control this motor to do something interesting for me. So if you have a differential drive robot, how do I take this this block The command I get for navigation and and follow that with, with my my motor efforts, or for manipulation, which is frankly, more of what ROS control is used for has, you know more advanced impedance controllers and other other type of modeling to deal with, you know, the kind of manipulation problem. For the most part, the navigation control systems at the lower level are, you know, very simple, right, this is the kind of stuff that undergraduates might do in their, their, their intro to controls lab, it's not not overly complex,

like a, like a proportional integral derivative or PID controller kind of thing or, yeah, to try to get it to a position.

Yeah, pictures are pretty, pretty common, as well as it potentially if you're working on warehousing robot where you have, you know, different different weights of opposite objects on top of it. You there's other slightly more advanced control scheme you might use, that's based on on efforts rather than just just photocurrents

Petrit. Now, why is can you tell me some of the challenges and making something perform navigation? Like, what are some of the problems that nav two is solving, and there's some flexibility around the ways it salts but so

yeah, so, you know, one of the biggest challenges is just taking both the representation of your environment you have from from mapping or localization or you know, other previous data that you've collected and somehow assembled. And then the current data you have about the environment. So if you have a 2d laser scanner on it, or you have a RGB camera on it, which provides you depth information, as well as color camera information, in, you know, radars, sonars tall sensors, I mean, there's, there's a large variety of different sensors that you can utilize that robot. And most robot systems actually have multiple these types of sensors on them. So the problem is dealing with how do we take all that sort of information and populate something that is now useful to solve the navigation problem. And so we do that with a navigation stack with the the cost of 2d package, which is essentially a direct port. For last one, we have some additional plugin layers, we've added for more advanced capabilities, but you know, high level, exact same package, long term, we'd like to actually replace that with something a bit a bit more advanced. But at the moment, that's where we're at. So that's one of the big problems. Another problem is saying, Okay, well, now that we have this populated representation of my environment with current sensor data is being fused into it. How do I actually plan within that space so that my robot can can, you know, achieve its goal? And so there's a variety of different methods of doing that, obviously. So there's, there's, you know, holonomic planners that work well with like 2d, or grid, basically techniques that work very well with

holonomic. It means it's like a car or something, right? And non holonomic. robot can drive like sideways and things where a car has to go forward or backward. That's what all means, right? Or?

Yeah, it's a hot holonomic means it can move in any direction. So think about a robot with Swiss wheels, or whatever they're called, or yeah, those little roller wheels. Yeah. Or, or potentially, where you can pick the wheels themselves to be able move sideways. Yep, those are considered to be holonomic robots. On a first order approximation, we also say that differential drive robots, if they're circular are also holonomic. Because if you can pit it perfectly in place as a different drive robot, and you're circular, so your your footprint isn't moving, you can you can achieve the same kind of behavior as with a truly holonomic robot,

because the angle is facing and then you can drive anyway. I see. Yeah.

Yeah, precisely. So for those kind of robots, it's really easy to leverage just like, you know, your usual a storage actress algorithm that you might learn in, like an intro to algorithms class, but those are perfectly serviceable, you probably want to do some smoothing on it, if you are doing a direct research so that you don't have like little jaggedy lines on thing. But you know, it's there's there's different ways we do that through navigation functions, which are basically generating these potential fields of cost that you're you're tracing on instead of just like directly just doing a grid search. But I mean, at a at a conceptual level, they're, they're, you know, the same kind of class of algorithm. As a star. Yeah, it is. They solve the same kind of problems. Yeah, they're, they're giving you a path through space.

Yep. And if I remember correctly, so for a star, it's a heuristic, a search that you say the goal is that way and I'm going to try to search in that direction. And then you find the optimal in a grid world you'll find the optimal solution of what's the shortest path to get there. And what was extras? Because it is it more general thing or forget how it relates?

Just breath first, it just goes in every direction at the same time. You don't have there's not a heuristic that that saying, like go in that direction, it's just gonna go out and expand its hole, find the optimal solution.

Okay, and then we feel for these if you have a start and an end spot, and so it's going to search the space, and it's going to find it. And we're representing space in a grid world, or some sort of representation. And so that's why you would have these kind of jagged edges, rather than a smooth way to get there, per se, if you go up into diagonal to the grid.

Yeah, so So you had like, basically, the motion model motion model, so to speak, that you're using is, is the the search neighborhood. So you're saying, I can search, you know, left, right up down for like the four connects neighborhood or say uptown, you know, up, down, left, right, and then the diagonal? So that's the eight connected space, and so that they'll let you then work at, you know, how do you plan to go 45 degrees, but you're still restricted only to these, these, you know, exactly the angle increments of either going, you know, zero 90 or 45 degrees for your planner. So this works, you know, fairly well, when you're working with just a simple, you know, simple base that like you might see on your on your on your Roomba over this kind of breaks down, it becomes more of a significant issue is when you're working with your larger noncircular robots. So think about like a forklift, or you're thinking about like, I think that's robotic cells, like a big platform robot, I don't, I don't know they call it. It's like something 1500 bucks. The thing is, and those are obviously like, very noncircular. And so even though they are a differential drive robot and get pivot in place, if you need to do a bit bit more work to make sure that it's a viable the plant or a space when you're in a narrow aisle way where maybe the robot can just trivially rotate in place,

because it could swing and hit something because it Yeah, exactly. It's rectangular or something. Okay,

yeah. And the other big kind of class of problem that that you have working with those techniques is, when you have drive trains aren't, you know, trivial that you can treat essentially as holonomic. So you mentioned before, like cars like the Ackermann model, which is the car model we talked about. So that that's, that's one example of where if you just plan a path as is go here, go straight to the right, you know, your car can't do that, right. That's not, that's not a feasible way of getting from point A to point B for the actual drive train at the robot. So you have to then take into account, not just like the the emotion model of saying this for a cap space, but instead, using in your search space, user space planner, actual motion primitives that better describe the capabilities of your drive train. And that's where we have some of the new algorithms that Neptune provides in the snap planner, which is the hybrid, a star planner, and the state lattice planner, which would enable us to set different search patterns, so that you can create physically feasible paths that robots with with constrained drive trains can actually navigate exactly and not not Not, not not not treat as holonomic.

So by search patterns, human kind of simulating the position in space, to try to see if it's valid from one position to another. So like, I have a car, I'm going to try to move it from one spot to another. And I'm going to kind of project it out into the future and try to see if that path gets us there, or what do you mean by search?

Yeah, so So imagining in this this 2d example, we said, If I'm at the cell, I can, the cells I can recently traverse are either up down and left or right in this forecasted space. So that that will be the motion model that we're using to, to to expand search. Because then once you go up, we apply the same for then you just keep doing that recursively. And you get, you know, eventually like a chessboard span basically.

So with for instance, let's, let's talk about the simpler one first. So for hybrid, a star, which the name kind of breaking that down you a hybrid. So you know, there's something different about this than a star. So you're still using this, like heuristic search algorithm at its core. And what that's doing is, is what we're first who is hybrid sampling, which is that instead of just naively saying we're going to, we're going to visit each of these grid cells just as the grid structure. Instead, we're going to treat our search neighborhood as primitives of actual velocity commands the robot can achieve. So for a car like model, you can either turn left and drive a bit, you can go straight and drive bit or you can turn right and those are the premises.

The turn left a little bit turn right a little bit go straight. Yeah, I suppose backwards as well. And backwards, or whatever. Okay, precisely.

Yeah. So those are the vacancies. Yeah. And then, you know, for the sake of implementation, the length of these primitives you want to set is something interesting and not, you know, arbitrarily just like driving the future a bunch because then you'd otherwise you'd end up going in circles on the left circles on the right and so straight forever. You have to decide, like, what are the lengths of these things I want to work with.

Okay, and you want to swap it basically, so that you can find things okay. So you don't go all the way in that circle, if you turn your wheel a little bit to the left kind of thing. Yeah.

But because we're also still working. So to go back, we're still working in this grid space, though. So we need to make sure that we go small amount, but needs to be enough. So that guarantees that we leave our current cells. So as we're expanding the search over time, we don't have situations where we're revisiting the same cell we currently had, was certainly something that can be dealt with. Yep. Yeah. Or actually, they, it's all geometry, you have read the code. And this is like, you know, six lines, where I go through step by step how to mentally drive this, this this idea. But while the paper itself of the hybrid star from from Thron, and folks from the DARPA Urban Challenge, or grand challenge, one of the challenges, they they, I think, allow you to do that to have some sort of solution space for how they select which one to use. I instead say I don't want to deal with that problem. I'm assuming I printed sufficiently long, so we always guaranteed to leave a cell. So there are a minimum square root of two cell lengths away from each other. So that way, we're guaranteed for any time that the proof hits a cell, it's always guaranteed to leave that cell in the next iteration.

Yeah, um, well, I guess they don't don't have to be. But I mean, the first I have to they are, yeah, gotcha. They could be circles. They could be hexagons, I've actually I did some, some research and a couple papers I've, I've just not written for publication, but written terms of like, you know, to take in regarding actually having an environmental Model B, you have a hex hexagonal cells, rather than grid cells. But that ends up having a whole lot of other issues. But it's interesting to think

about. Okay, so you have this grid, and you have a way of describing how your robot moves in this grid. If you have a robot that's like a car or something you have some idea for, if you tell it to drive forward at left for this angle, or something, how it goes into another cell? How do you just it sounds like a hard representation, because I'm imagining, if I turn the wheel 10 degrees to the left, like, and I drive, I'm not in the middle of a cell, but I guess I still am in a cell. And you can use that for planning your trajectory.

So rather than just so with these grid based methods, or essentially visiting, let's say, like, we go up, down, left and right, yeah, we could say that, that, you know, you're you're not busy and arbitrary place in that cell, right. Because if you blow that any, any any space, we blow it up infinitely, to go to look at it. So we look a little cell and make it really big. It's like, well, we're visiting any random place we're visiting, let's say the center or an edge, depending on how you want to find it, of that cell and each cell are visiting the center of, but when you're working with these motion primitives, you're right, we don't actually have exact angle, these aren't computers that were ending exactly in the center of every single cell every single time. So we do actually have the cash in the planning sequence, where within that, that, that that that cell, we've actually landed, and then when we go back, and we visit that cell to expand it, again, we're actually not using the center of that cell, we're again using that that position within the cell that we actually visited, which then, you know, if you compound that over and over again, to build a path, that means that it's exactly drivable in this grid

Yeah, um, so if you were, if you were to have, for instance, like a naive implementation of this, you might think about saying, let's approximate that I had this this this primitive that I'm projecting forward at a curve, and it ends at within a cell, but not in the center of the cell, it might be natural for you to think if you come from an from like a 2d grid lands space to say, Oh, well, you know, these, these, these cells, these B cells are sufficiently small that I can just store that as the as I've entered the cell. And then the next iteration, when you when you visit that cell to expand it to get its neighbors, you could then say, Okay, well, let's just pretend like we started at the center and expand it again. But if you keep doing that, then the path you get at the end of it actually isn't then respecting the constraints on that motion model. Because you're you're not accurate, you're that the small errors compound, each of each individual primitive. So instead of, yeah, so So doing that when we when we, we visit, we visit a cell and we're trying to find its neighbors, we're not using an approximation of the position that we're using. As like the center, we're using the real position of the actual cell coordinates used, so that as that compounds together that's going to be guaranteed to be drivable within the kinematics of the of the the robot system.

Interesting. So how does it work if I want to go from one spot to another spot, and I have these motion primitives, which say I can turn and for these motion primitives, if I have a car? Do you have like basically infinite motion primitives because you have the whole degree of angles that the car can go or how Is it? How do you go from one spot to another? Do you know what I'm asking? With? Motion permitted? Yeah, yeah. So you have the continuous range between, say, minus 45 and positive 45, that the car can travel in, that the wheels could face.

Yeah, so so we subsample that space. So essentially, we have for the hybrid star, and in the current implementation that that exists today, you have a left turn, you have a right turn either straight turn, and then you have the reverse equivalents of those things. So you have either three or six motion primitives you're working with, that are the most extreme terms, right? The the your your robot model can obtain, and it randomly samples in

that space, or just uses one of one of the six will expand.

So expand all of them, right. So every time we visit a cell from an A star, we expand every single cell, and we set a heuristic cost at the end value of that cell that goes into a priority queue to sort who I the the most optimal trajectory possible. And then when we go back through and visit the next cell in that priority queue, you're getting back out that that next cell that you want to go to, so you're not, that's not a goal. It's not like a the, the, it was not a heuristic search, and you're using a set A dexterous algorithm on this kind of thing. And right, every single aisle six, aisle six, aisle six, and you just be spreading and spreading and spreading out, which is problematic. That's where your priority,

so which is very clever. We're Yeah, it seems like you're going to do it.

Yeah. That's pretty standard for a star's that that's kind of, at least to my understanding, that's how most people do it.

Gotcha. So you, but it's not. So you're only using these primitives of like extreme angles. So it's all the way this all the way left all the way right or straight? If we're just doing the forward case. In this case, if I How do you arrive at a solution where the robot doesn't drive like incredibly manically doing all the way left all the way? Right, on its way towards its goal?

Yeah, so that's where we have a smoothing step at the end of this. So this we do this for

the house space, basically, this is this is for you arriving at a solution. And then from that solution, you smooth it.

Yeah. So so we have. So we thought through this, we have now a path that is rival maybe, yeah, little suurvey. But drivable. And so what we do is we take that path, and we put it into a smoothing algorithm, which will, you know, take out some of those extreme turns and make them a little bit more, or I guess, less aggressive. And then also goes through the boundary conditions to make sure that those remain tight during the entire entire process. So that part

of boundary conditions and make sure, yeah, what do you mean by that?

So sorry, to type that word. But so essentially,

we mean, optimal at night, but what do you mean, like a good solo error? Or I don't know what you mean, actually.

Yeah, so we have. So in the hyperstar, paper that is published that, you know, an autonomous driving company might use, for instance, has a pretty advanced move there. And in fact, this is the some of the information we used to use within the spec planner, that takes into account, you know, keeping spacing, the sames, which helps us smoothness, using our Fermoy diagram to help you drive away from obstacles and then limiting the curvature so that you you keep those constraints that you had during the planning process. And I think a couple other terms as well. And those then spin through an optimization problem to give you an optimal solution. Given those those cost functions, this ends up being a very computationally expensive thing to do. And it's ultimately why I removed that, and instead, Swift changed up a bit of the internal systems of how the search the search, or restrict operates, so that we don't have to use that kind of smoothing technique. But what we use instead is essentially like a super, super naive, smoother, like you might find in like, I don't know, like, one of those Udacity nano degree kind of things, we just go through very simply and say, like, let's make these things as close together as possible. And then iterate until we have very little change left in our in our in our outside path between

these things, which actually things have close to each other. What I mean by that

each of the the points on the path. Okay, so

in opposition to land, it's maybe not as intuitive to somebody who isn't who's worked with it. But it's something that that's generally a very successful way of smoothing is just say, all my points my path, I want to make sure that they're each as close they're evenly distributed from each other. And when you iterate over that and try to minimize that cost function, you end up actually smoothing out the path from from turn to such which is a convenient and easy Oh, that's a waste smoothie. I

see. It's basically like the shortest path from something is straight to it. And so if you're making all your points closer to each Other, you probably are working towards that. Because they will be further. That is a cool heuristic to use to smooth. Yeah. Interesting. So if

you had no no other, other things doubting that solution, and essentially what you're right, and just being exact, perfect. Just ignore everything. Yeah, yeah. But that's why I have to add in things like, collision looks otherwise Yeah, definitely all into it, for sure. Yeah. And then, because we derive it kinematics, these will path we also want to add some weights on the original path itself. So that, like, if you have a little squiggle that could be made into a straight line, we want that to smooth out. But we don't necessarily want something to turn from a nice smooth turn, just like you know,

Yeah, where it's no longer actually also valid by the current motion model. And so I have, I tried this more in the actual the readme file. And I don't think it's worth getting to the specific details of this at the moment. But essentially, you can, you can, through logical proofs, find that the only time when you break feasibility is during sharp turns at the beginning and the end of the path. And so those are the boundary conditions, okay, so the boundary conditions totally break during this process. So if you were to try and drive the robot using just that pseudo described, both the start and the end would not be guaranteed to be drivable. So we do is we have an additional post processing step on the beginning and the end of it, where we use the Kinect motion model of the robot that we use previously during that planning technique to take the Star Pose and then sample across the new smooth path until we find a the shortest path that goes from the start the actual starting point to a point on the smooth path, or to give us a drivable connection point between those two areas. And so by doing that, then we end up with a smooth path that also then then meets the boundary conditions of drivability, and therefore the end path is

drivable. So I don't know that I fully understand that. But can you think of it as just if the start and end will be wrong because of some weird boundary conditions in the optimization, you just extend them a little bit. So you consider like a little before the start and a little after the end? Assuming the motion primitive, just continue with something like this or?

So I actually I tried that it didn't work. But yeah, I did. I did try that I tried a bunch of stuff before I found the solution that we currently use. So essentially, so this is where it gets it gets a little out into the weeds. So within these planners, we also have a concept called analytic expansion, where occasionally during our search process, we take our search position and our goal position. And we use a library called Oh MPL, the open motion planning library, which is commonly used for sampling bass planning implementations. They also have a very good implementation of the Ackermann models that you can use both for sampling but also in our

model shows, our models are for tire with two steering ones, the front ones here.

Yeah, yeah, like a double bicycle, or whatever. Is that? Yeah, yeah. So we can use that to then say, here's our start point, here's our end point and points opposes rights, their orientation associated with them, and then give me a path to where I am to where I want to go. And then we then check for collisions to make sure it's actually valid. And most the time during your your planning, right, that isn't valid, right, it's going through a wall or something. So you throw it out and continue searching, and then maybe another 20 iterations to try again, until you find a solution. And that helps a lot with the speed of the planner. So most of your planning time and a kingdom activities will planner is spent actually on just getting to that exact goal, position and orientation. So if you can speed it up by having something they'll find exact curve fits, that helps you quite a bit. So we apply that same concept, then after the smoothing process to say, here's our starting point, which is fixed as our as our starting point or our end goal point, which are both fixed. And then we basically say every, I think it's like five or 10, or some some number of points on the on the smooth output, we find the feasible solution between those two things, and we measure the length of it, and we check the feasibility for it. And we do that for you know, up until basically, you know, for I think it's bounded, but you know, far, far into the into the path until we find the shortest collision free path possible. The shortest point here is actually very important is if you were to try to find the feasible solution between your starting point and a point that's, you know, just just directly in front of it, that's how shifted just a little bit away, you don't get gained this loop to loop maneuver to make that feasible. You don't really want your robot doing a loop de loop. So we check so we continue to march through the path a few times until essentially that loop de loop goes away. And that now be a shorter path, then that full circular path

I'm thinking of if you have like a piece of thread or something and you try to move it, you grab it in the middle somewhere with two hands and then you move it close to each other it'll do a loop and this is like your hands are the points and it has to do a loop to go through both points. Yes kind of thing.

So you Yeah, exactly. So we we, we don't just just randomly marks through a bike five points or whatever, there's, there's a more principled approach we use based on on, you know, certainly the fact that we know that we build is exactly a circle. And therefore we can we can select interesting parts of the path based on that on that circle, the quarter circle. And yeah, that that's how we constrain the boundary conditions to be to be reasonable. And while that sounds very, it sounds like a very naive approach to it, I will say is very effective. And other things I did, which were maybe a bit more principled, did not work nearly as well. So this is the the solution, ultimately, we select strikes

me that that's often the way in robotics, where it's like, this simple thing works really well, and is fast, and solves most of the problems. Like a lot of our heuristics seem to be this way.

So yeah, I mean, there's obviously exceptions. But I'm just amazed how far you can get with simple representations for a lot of things. The so is this. Actually, once you have your, so you have your grid world, in your grid world, you use your motion primitives to kind of explore it for to get from one cell to another, and then you get this rough path that goes from start to end. And then once you have that rough path with like crazy, if you're driving like crazy, all the way left all the way, right turns, so it's all jagged. Once you have that, then you run this smoothing function on it, and the smoothing function is going to smoother on it. And that's going to make it so that you have a smoother path. And the clever thing about that is you look for what makes the points the closest distance because that's more likely the or that that is the more optimal solution. Right? Is that correct?

Yeah. More or less? More or less? Yeah, don't wait, don't wait. yet. Don't only crush important, Vic, there's so when you get that path out from my brain star, it's it's not nearly as this crazy thing as possible. Because right, we're talking about this really small motion primitives. So if one thing is it's a little tweak, tweak, you know, you barely even notice it without a path when you have something happening.

It's true. But if you had a robot, it would be crazy. Like imagine being on a car that was following that one, it would turn all the way left, then all the way right, to try to do something crazy. So but

you also have like a local trajectory plan that's processing, that information has to be smoothed out yet. But before we get into that too much, the point I want to make there was that, because they're small, they really don't make that much of an impact it like super small, like like to to grid cells wide, you know, kind of a

big grid. So we have an autonomous car. How big is a grid cell? In this

application? Yeah, in my example, it's five centimeters, so it'd be five and 10 centimeters in size. Yeah, so it's very small. And because we're we're working on with optimal heuristics, it's not like we're like, here's the free space, and we're Whoo, you're wobbling all over the place. So it's very much like being driven towards that goal in a very reasonable way. And so that output path is actually already pretty, pretty smooth and pretty good looking. It's mostly just like small discrete discrete planning errors that you're dealing with, because we are working in this grid space, not continuous coordinates notes. So that kind of feeds a little bit to the heuristic functions that we're using here. So like I said, we're able to simplify the smoother quite a bit, because we made some changes internally to the planner for how the search is performed. So that the two ways that we approach that is in our traversal function, rather than just just looking at the cost value, and the length of primitive, we also enter penalty functions so that we penalize the path from making those extreme turns that if it starts, if it starts to turn, we penalize it stopping turning or penalizing, if it's going straight, to start turning just a little bit, you know, a percent 2%, you have something very small, which ends up making us the things are quite a bit smoother on the output. And the second thing we do is that rather than just using like a distance heuristic to say, like, here's how far I here's my goal, here I am, just just just compute the Euclidean distance between those two points and use as a heuristic function. Instead, we use to to heuristics, though take the maximum on one is the actual length of the path that would represent in Ackerman space. So we take using that same idea we talked about with the analog expansions and the boundary conditions, we take our current pose, our actual pose, we find what that minimum curve would be. So we set the maximum value and the value of a a new to this to this implementation that I'm writing paper about actually early, where it takes to count the obstacles within the space the same way the the Highbury star has a similar heuristic. But it takes that into a cost aware environmental representation. So rather than the smoother having a cost function in it, that takes into account the for noise diagram, which is similar to a potential field, to smooth out the path away from obstacles, we have our actual heuristic function that's driving the search take into account the search or the cost. And during that process,

and the potential field, that's just a way of keeping things away from something else, right, it basically says Don't come close to this obstacle.

Is that true? Or? Yeah, yes, essentially. Yeah. So so the NOI diagram is computing a potential field where costs are high or low neuron schools and lower high in the centers of Ioway spaces. So essentially, it finds these like, is a graph of the centers. And the notes were, like spaces like, you know, collide with each other. Which if you're dealing with a stack environment, like if you're dealing with, if you're planning on a map, that does not change over time, like you might do an autonomous driving application, that makes a lot of sense, your EQ compute that once super efficient, knows exactly where things around the center works over time. But when you're working with an environment, mental representation that's changing constantly, there's a bunch of data

around or something and you're driving a robot. Yeah, okay.

Yeah, you have to, you have to compute that every single iteration, which is super expensive, but we already actually actually have potential fields that we're working with, it's already pre computed as part of the inflation layer of the navigation stack. So we actually use the potential field computed from the inflation layer inflation to Yeah, so so we inflate from from political obstacles, like like my couch behind me, we're going to add a potential function off of that, so that we have more cost, the closer you come in proximity to that, that obstacle. And then, given the radius of your robot, if it's a circular robot, we apply then a lethal ban of cost in front of it so that, you know, if the robot would be centered at that cell, the part of the edge might be heading heading the couch. So because we have this this potential field already computed, we can use that to drive the robot away. The robot, the heuristic search away from the muscles just to be guessing.

So you have because you have these, sorry, what did you call them? What was the cost? Or was it the potential fields? You have the potential fields, and you basically say, this area, because it's static, we know we don't want to go near and that one area will be defined? That's what you're saying?

Yeah, yeah, we also apply that to, to new obstacles to right people and stuff, you still want to run out?

Yeah, for sure. And it's but it's similar, you can use this kind of expensive computational, or this computationally expensive representation for static objects, but then a different one, that's not as expensive for moving objects.

So the potential field that we use for an inflation layer is very quick to update. So it's actually not very computationally expensive at all. And that's why we use that within within the, the planning for discipline, rotation, and not not necessary what you do for autonomous driving. Another reason why we do this, which is like heart, it's a little subtle to explain, but so autonomous driving, in general, you want to stay in the center of the lane, or you want to stay in center of a space, which you are allowed to drive to maintain maximum distance away from from everything else around you, or Cynefin of your lane since. But in the case of autonomous robotics, that's not always optimal. Like sometimes you do want to do that, that that's really true. But a lot of times, maybe you want the robot to hug, hug a shelf, so that things could go in the opposite direction. If you're working in the scene of a lot of robot systems, or there's just different behaviors, you may want to have that curl in the NASDAQ, we allow you to to, essentially by tuning that inflation distribution on that inflation layer potential field. And so basically, because we're using that information in the planning itself, that means that your plants are going to actually respect the tuning to have this the tune system behavior you want to have based on the current the plantings that you that uses that potential oil, so you're actually getting more flexibility where you could still define system to operate that way if you want to, but you don't have to you can.

That's cool. How does what do you call it inflation?

So essentially, it's inflating the cause static obstacles or dynamic offices in the city. Yeah, inflates the cost. And in the the grid map representation.

I was thinking inflation like in the economy or something and it's like, I don't I don't get the map. Okay, okay. It's inflating the cost, it's making it larger.

Another way of thinking about it is risk which I think is really the the proper term I should be using here. It's it's the risk that look Let's say that the race is cool.

Okay. And then so when you are driving the right when you are planning, what you'll do is at each location, you evaluate it for its potential and or for it for like, how good if the how good is the position? And one of these things that you take into consideration after being like, is this valid or not, is the risk. And so if you want to minimize the risk, then this is used in your priority queue that you mentioned to to sample more feasible and lower risk locations.

Yeah, and that's another fancy function that has that can be tuned. So right, you can you can sense that. So I'm super sensitive to risk. And so I always want to stay in the center of aisles, that that's really important to me. So you can say, you know, I mean, obviously, nothing would ever give you a diversity inclusion path, because that that's invalid, or they're always going to be valid. They can say that maybe like, oh, yeah, I'm okay if it if it's a little closer obstacles, because I want to be able to, like, you know, hug a wall or hug a shelf or something, for instance. So these are all all tuple behaviors you have access to, because we're using the cost information in the search itself. And using the inflation layer to kind of see that based on the behavior of the robot system, you wanna you want to how does

how does this work? Is this only true of known maps? So if I have a known cost map, then I can use these? Or is it is the ability to tune inflation or risk of things? Is it something that I can do as I am building a map of the environment?

maybe you have, like heuristics for features of the environment? Yeah.

Yeah, I mean, both, I think I mean, if you have some, I mean, if you if you blindfold me, and then kidnapped me and throw me somewhere and say, Build me a map, and like, choose some parameters, right? Like there are there is a reasonable band of parameters space that you can, you can select, and you know, if you're in narrow spaces might be better parameters than if you're a super wide open spaces. So if I do nothing, it's like, right, yeah, I couldn't tell you. But usually, when you deploy a robot, you have some general understanding that kind of

working so you can parameterize it based on heuristics about what you know about the space, if it's very open, or if it's very tight, you might set parameters differently that would change how these how the inflation of the costs map is generated? Yeah. Yeah. Can you tell me a bit about the priority queue? I think that's really interesting.

I mean, that's just a that's just part of a star. Right. So I think that that's, I haven't any, any resource talking about here. So search would would explain it more articulately than I wouldn't the moment.

Gotcha. Actually. Yeah, I guess you're right, that it is just a normal thing of a star because you're only searching a subset on your way to the solution, a subset of possible options. until hopefully, your or your solution.

Actually, there's other ways of implementing it as well. That's just at least to me, that was just my knee jerk reaction thing, I have to do this thing. All right, I need to sort by lowest cost, then oh, ergo, prior to me. So easy. But

so then, for you to once you have this path that started as kind, of course, but I guess the grid cells are so small, that they it's not that course, I guess, or it's not that jerky. But you have this you perform smoothing on it. And then you map it to motion from or you match it, map it to velocities? Do you get this kind of for free with the motion primitives? Or how do you relate your path to motion to like the velocities that each of the wheels should go?

1 post with the navigation tag