Active questions tagged rrt - Robotics Stack Exchange most recent 30 from robotics.stackexchange.com 2019-11-14T22:28:34Z https://robotics.stackexchange.com/feeds/tag/rrt https://creativecommons.org/licenses/by-sa/4.0/rdf https://robotics.stackexchange.com/q/18755 1 Statistical distance between degenerate distribution and any probability distribution Kandarp Gandhi https://robotics.stackexchange.com/users/22951 2019-05-08T11:34:47Z 2019-05-08T14:53:06Z <p>In sample-based motion planning, sampling methods would change the cost of path and computation time for the same planning algorithm. I would like to compare different sampling methods.</p> <p>So, the different stochastic process generates sample points. The task is to identify which stochastic process/distribution is the closest to the target value. </p> <p>In literature, I am not able to find a comparison method which is standard. If any standard method is available, then help me.</p> <p>Wasserstein distance between the target value(smooth path cost, time_min = 0.0) and the vector of sample results would be used or just cost should be considered. </p> <p>Kindly give your suggestions.</p> https://robotics.stackexchange.com/q/18064 0 Arm path planning with obstacle avoidance in Unity rahul1210 https://robotics.stackexchange.com/users/22114 2019-01-21T05:04:58Z 2019-01-21T15:47:13Z <p>My application is not specifically in Robotics, but the problem that I'm trying to solve seems to have been studied primarily from the viewpoint of Robotics, so I am hoping to get an answer here. Apologies if it's not the correct place.</p> <h1>Problem description</h1> <p>I am creating a virtual human agent in Unity. The agent will primarily interact with blocks on a table by sliding or carrying them from one point to another in 3D. Apart from the constraints that accompany human arms, the planned path should not result in arm colliding with other blocks, nor should the blocks collide with one another. The former case is a possibility since blocks could possibly be stacked on top of one another to form towers.</p> <h1>My solution</h1> <p>I don't have any experience in Robotics, so my solution may not be the best: First, use an IK solver to find initial and goal arm orientations, so I have initial and goal state in configuration space. This could be done by an IK solver. I'm thinking to use Cyclic Coordinate Descent, since it feels very easy to implement. Next, I'd have to implement RRT-Connect to find the path between those configurations. My knowledge on this is limited to what I've read in the RRT-Connect paper.</p> <h1>Questions</h1> <p>My questions are three-fold:</p> <ul> <li>Does the stated approach seem to be reasonable? Lacking background in Robotics, I'm afraid I might be missing something obvious.</li> <li>Is there a way to find initial and goal configurations for arm that are also collision free?</li> <li>Finally, in the tradition of being lazy as a programmer, I've been looking at OMPL to use an existing implementation for a path planner, especially since there are so many, and they'll also be well optimized. Problem is I'm not sure if there's a way to build it for .NET (C#) since that's what is used in Unity for scripting.</li> </ul> <p>Any help would be greatly appreciated!</p> https://robotics.stackexchange.com/q/16318 1 RRT star Convergence Ajin2305 https://robotics.stackexchange.com/users/20789 2018-09-02T18:10:05Z 2018-11-03T01:00:22Z <p><strong>My Code gives the following convergence characteristics, I wanted to know if it is correct</strong> <a href="https://i.stack.imgur.com/NCbHT.jpg" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/NCbHT.jpg" alt="Convergence for non-holonomic body 2000 iterations"></a></p> <p>Updated code</p> <p>{</p> <pre><code> %Basic RRT star algorithm for non-holonomic body with obstacles close all clc clear all %Map and Initialization Data x_max = 100; y_max = 100; obs1 = [30,0,20,20]; obs2= [30,60,20,20]; EPS = 5; % Step Size Iter = 2000; q_start.pos = [10 10]; q_start.cost = 0; q_start.parent = 0; q_start.child=[]; q_goal.pos = [90,90]; q_goal.cost = 1e9; q_goal.child=[]; q_new.pos =[0,0]; q_new.cost=0; q_new.parent=0; q_new.child=[]; goal_reached=0; tree(1) = q_start; figure(1) axis([0 x_max 0 y_max]) rectangle('Position',obs1,'FaceColor','b') rectangle('Position',obs2,'FaceColor','b') hold on plot(q_start.pos(1),q_start.pos(2),'.','MarkerSize',10,'Color','m') plot(q_goal.pos(1),q_goal.pos(2),'.','MarkerSize',10,'Color','b') goal_nodes=[]; best_goal=[]; finalnodes=[]; goals=0; fc=[]; t=cputime; for i=1:1:Iter q_rand=random_state(x_max,y_max);%Sampling a random state from the configuration space [q_near,val,idx] = nearest_neighbour(q_rand,tree); % Obtaining the nearest neighbour from the tree and its distance from q_rand q_new.pos=move(q_rand,q_near.pos,val,EPS); %Checking if goal has been reached stat= distance(q_rand,q_goal.pos); if (stat&lt;=4 &amp;&amp;~isCollision(q_rand,q_near.pos,obs1)&amp;&amp;~isCollision(q_rand,q_rand,obs2)) goal_reached = 1; goals=goals+1; goal_nodes(goals,:)=q_rand; q_new.pos=q_rand; end if(~isCollision(q_new.pos,q_near.pos,obs1)&amp;&amp;~isCollision(q_new.pos,q_near.pos,obs2)) %line([q_near.pos(1), q_new.pos(1)], [q_near.pos(2), q_new.pos(2)], 'Color', 'k', 'LineWidth', 1); q_new.cost = distance(q_new.pos, q_near.pos) + q_near.cost; % Within a radius of r, find all existing nodes q_nearest = []; r = 10; neighbor_count = 0; ni=[]; for j = 1:1:length(tree) if (noCollision(tree(j).pos,q_new.pos,obs1)&amp;&amp;noCollision(tree(j).pos,q_new.pos,obs2)&amp;&amp;distance(tree(j).pos, q_new.pos) &lt;= r) neighbor_count = neighbor_count+1; q_nearest(neighbor_count).pos = tree(j).pos; q_nearest(neighbor_count).cost = tree(j).cost; ni=[ni,j]; end end % Initialize cost to currently known value q_min = q_near; C_min = q_new.cost; % Iterate through all nearest neighbors to find alternate lower % cost paths for k = 1:1:length(q_nearest) if (q_nearest(k).cost + distance(q_nearest(k).pos, q_new.pos) &lt; C_min) q_min = q_nearest(k); C_min = q_nearest(k).cost + distance(q_nearest(k).pos, q_new.pos); end end % Update parent to least cost-from node for j = 1:1:length(tree) if tree(j).pos == q_min.pos q_new.parent = j; q_new.cost = C_min; break end end % Add to tree tree = [tree,q_new]; tree(q_new.parent).child=[tree(q_new.parent).child,length(tree)]; %Rewire %Iterate through all nearest neighbors to rewire them with lower cost %path for k = 1:1:length(q_nearest) if ( q_new.cost + distance(q_nearest(k).pos, q_new.pos) &lt; q_nearest(k).cost) tree(ni(k)).parent=length(tree); tree(ni(k)).cost= q_new.cost + distance(q_nearest(k).pos, q_new.pos); updatecost(tree,ni(k)); %Update cost of children end end end best_cost=1e9; best_goalindex=0; if(goal_reached) for k=1:1:length(goal_nodes(:,2)) for j = 1:1:length(tree) if(tree(j).pos(1)==goal_nodes(k,1)&amp;&amp;tree(j).pos(2)==goal_nodes(k,2)&amp;&amp;tree(j).cost&lt;best_cost) best_goal=tree(j); best_cost=tree(j).cost; best_goalindex=j; end end end % Search backwards from goal to start to find the optimal least cost path q_goal.parent = best_goalindex; q_end = q_goal; finalcost=0; tree = [tree q_goal]; while q_end.parent ~= 0 start = q_end.parent; finalcost=finalcost+distance(q_end.pos,tree(start).pos); plot(q_end.pos(1),q_end.pos(2),'*','Color','r') line([q_end.pos(1), tree(start).pos(1)], [q_end.pos(2), tree(start).pos(2)], 'Color', 'r', 'LineWidth', 2); hold on q_end = tree(start); end finalnodes=[finalnodes,i]; fc=[fc,finalcost]; drawnow end end e=cputime-t figure(2) plot(finalnodes,fc); title('RRT* Convergence') xlabel('Iteration') ylabel('Path Cost') } </code></pre> https://robotics.stackexchange.com/q/16253 0 Converging to better solutions by running RRTstar for multiple Iterations Ajin2305 https://robotics.stackexchange.com/users/20789 2018-08-20T07:14:34Z 2018-08-20T07:59:17Z <p>In the Literature I have read so far, I saw RRT star was running in multiple loops to converge to a better solution ( near optimal). I was wondering how I could implement the same, as most pseudocodes don't explain that part.</p> https://robotics.stackexchange.com/q/16043 1 How can I get the right sample in RRT star Dubins? Ajin2305 https://robotics.stackexchange.com/users/20789 2018-07-16T13:13:09Z 2018-08-09T13:21:59Z <p>I am trying to find a solution in S(1)*R^2 (x,y, orientation) with obstacles (refer to image) using RRT star and Dubins Model. </p> <p><a href="https://i.stack.imgur.com/NHB0R.jpg" rel="nofollow noreferrer" title="Obstacles"><img src="https://i.stack.imgur.com/NHB0R.jpg" alt="Obstacles" title="Obstacles"></a></p> <p>The code takes a lot of time to find a suitable random sample with x,y, theta such that a successful Dubins path can be connected between the two points without the vehicle (a rectangle colliding any of the obstacles). The fact that the random sample needs to be at the correct angle so that the vehicle's path is collision free is 1 out of 100,000 random samples. This makes the code very slow even when my computer is at its full processing power. None of my internal codes take much time. I timed all of them, only the fact of achieving that 1 out of 100,000 sample causes the code to take so much time. I tried decreasing my discretization space by half but the problem still exists.</p> https://robotics.stackexchange.com/q/15566 0 How to plan path for robotic arm with RRT? Long Smith https://robotics.stackexchange.com/users/15392 2018-04-18T10:21:55Z 2018-04-19T23:38:46Z <p>I have written simple RRT planner however I am not sure how to apply it to a robotic arm path planning.</p> <p>The issue is that of analytical solution absence to inverse kinematics problem. Let me explain. There are two possible spaces which can be used to plan path:</p> <p><strong>Joint space.</strong></p> <p>Since we know exact joints' angles at each planning step it allows us to easily account for end effector orientation and collisions. However it requires goal position to be defined in joint space which requires inverse kinematics solution but I am not sure how to solve IK for the exact angles without analytical solution which not always exists. I use inverse jacobian method which is iterative and requires small timestamps(i.e. trajectory and not a geometric path) to have precise movement therefore it is not clear how to use it to calculate goal position in joint space. </p> <p><strong>Operational space</strong></p> <p>Does not require goal to be defined in joint space however on each planning step it is also necessary to solve IK to be able to account for orientation and collisions.</p> <p>The only way I can see is to make RRT step small enough to be able iteratively compute IK on each planning step however it casts doubts on performance. </p> <p><strong>Question</strong></p> <p>So the question is how can I account for orientation and collisions(including self collisions) when planning motion without analytical solution to inverse kinematics problem?</p> https://robotics.stackexchange.com/q/9320 2 why is quadrotor motion planning hard? boon https://robotics.stackexchange.com/users/8916 2016-03-04T04:47:19Z 2017-10-02T16:01:26Z <p>With introduction of incremental sampling algorithms, like PRM and RRT planning in higher dimensional spaces in reasonable computation time has become possible though it is PSPACE-hard. But why is a quadrotor motion planning problem still difficult even with simplified quadrotor model? </p> <p>I was solving a dynamic car problem with OMPL, which produced solution within 10s but I set a planning time of 100s for quadrotor, but it still does not find a solution.</p> https://robotics.stackexchange.com/q/14201 1 How to include a space and robot geometry with OMPL fabrice https://robotics.stackexchange.com/users/16030 2017-09-12T15:20:10Z 2017-09-12T15:20:10Z <p>Let us assume that I am able to create a 3D map (octree) based on information gathered by a Lidar and/or a camera on a quadrotor. Now based on the current 3D representation of the world, I want to go from the current pose to another one (which is in the known 3D environment).</p> <p>To do so I want:</p> <ul> <li>First to create a set of waypoints by using rrt*;</li> <li>Then based on the waypoints, build polynomial curves.</li> </ul> <p>So I took a look at OMPL, and although I understood the basics (well I think), I am still do not understand how to define the geometry of the robot (a box in my case) and how to define the working space (here a binary octree for instance).</p> <p>From the examples I found, I see that goal and start spaces are defined, then a planner along with the problem definition is chosen, and finally solve. But I cannot see where the geometry and/or kinematics is defined no the world space...</p> <p>Sorry for the naive question, but if somebody can provide a hint it will be very helpfull</p> <p>Thank you</p> https://robotics.stackexchange.com/q/5 14 Nearest-neighbor data structure for non-Euclidean configuration space giogadi https://robotics.stackexchange.com/users/13 2012-10-23T19:43:48Z 2017-07-08T19:44:29Z <p>I'm trying to implement a nearest-neighbor structure for use in an RRT motion planner. In order to do better than a linear brute-force nearest-neighbor search, I'd like to implement something like a kd-tree. However, it seems like the classical implementation of the kd-tree assumes that each dimension of the space can be split into "left" and "right". This notion doesn't seem to apply to non-Euclidean spaces like SO(2), for instance.</p> <p>I'm working with a serial manipulator arm with fully rotational links, meaning that each dimension of the robot's configuration space is SO(2), and therefore non-Euclidean. Can the kd-tree algorithm be modified to handle these kinds of subspaces? If not, is there another nearest-neighbor structure that can handle these non-Euclidean subspaces while still being easy to update and query? I also took a look at <a href="http://www.cs.ubc.ca/~mariusm/index.php/FLANN/FLANN">FLANN</a>, but it wasn't clear to me from their documentation whether they can handle non-Euclidean subspaces.</p> https://robotics.stackexchange.com/q/12719 0 Do the distance function and steering function in an RRT have to be related? Anand https://robotics.stackexchange.com/users/16611 2017-06-28T21:18:09Z 2017-06-29T17:31:51Z <p>I am developing an RRT (rapidly exploring random tree) for car-like robots in SE2 space using Dubins steering function and have a question that has implications on the performance of RRTs.</p> <p>In order for an RRT to be performant, an efficient nearest neighbor data structure needs to be used. There are efficient nearest neighbor data structures for metric spaces (like Euclidean space), however, none that I know of for a non-metric space (like the Dubins space).</p> <p>This leads me to wonder if I can use a different distance function than the Dubins curve length in my RRT despite using the Dubins steering function to connect states.</p> https://robotics.stackexchange.com/q/11204 0 How to make RRT to work for dynamic systems? freezer https://robotics.stackexchange.com/users/15133 2016-12-12T11:46:14Z 2017-01-11T16:33:07Z <p>I want to make path planning algorithm for a quadrotor with RRT in my thesis. I have searched lots of articles and come up with the concept of "dynamic RTT" and one of the articles has a title "kinodynamic RRT*". I have emailed the author of the article with no response. </p> <p>The main point that I couldn't understand is, we need to sample random state for dynamic RRT like 2 position and 2 velocity values for planar vehicle or an angle and its rate in case of 2D-quadrotor.</p> <p>How should the samples be so that speeds and positions does not confused and when should I consider the saturation limits of the actuators or vehicle acceleration limits.</p> <p>I can't understand how to handle what if two consecutive samples for positions are A(0,0) and B(10,10) this needs positive velocity at the point B but sampling can cause negative velocity. Am I wrong?</p> <p>Other issue is, how should the control signal be determined so that it can be applied for duration of delta t to move as close as possible to the sampled point. I am not sure how to determine the input or move the vehicle. </p> <p>Do I need optimizations so that it can reach to the sampled point in shortest time possible?</p> <p>Please let me know if there is a missing part to be understood.</p> <p>Thanks in advance.</p> <p>Wish a hopeful new year.</p> https://robotics.stackexchange.com/q/9143 4 How is homotopy used in planning algorithms? boon https://robotics.stackexchange.com/users/8916 2016-02-10T07:04:36Z 2016-02-11T09:10:27Z <p>What is an intuitive understanding for homotopy? At what stage is homotopy (I understand it as stretching or bending of path) in a planning algorithm? Is homotopy involved, for example, while implementing an algorithm like RRT?</p> https://robotics.stackexchange.com/q/8709 1 BiRRT: Getting path from an array of 7 DOF angle configurations Iche https://robotics.stackexchange.com/users/11004 2015-12-20T20:47:06Z 2015-12-23T22:07:51Z <p>I've kind of finished implementing a BiRRT for a 7 DOF arm, using a KD-tree from numpy.spatial in order to get nearest queries. A picture is below:</p> <p><a href="https://i.stack.imgur.com/QA2Qj.png" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/QA2Qj.png" alt="BIRRT pseudocode"></a></p> <p>I'm currently having trouble with the fact that it is impossible to retrieve a path from the start node to a particular node using a KD-tree, and while I do have an array of of all the nodes, and there are edges that can be calculated between subsets of the array, but the edges are not in any useful order. Can anyone give me some tips on how I'd retrieve a path from the starting node in the first array, and the ending node in the second array? Are there any useful data structures that would let me do this? Below is my code</p> <pre><code>def makeLine(distance, q_near, xrand, nodes): num = int((distance)/0.01) for i in range(1, num+1): qnext = (xrand - q_near)/distance * 0.01 * i + q_near #check for collision at qnext, if no collision detected: nodes = numpy.append(nodes, qnext) #else if there is collision, return 0, nodes, ((xrand - q_near)/distance*0.01*(i-1)+q_near return 1, nodes, qnext def BIRRT(start, goal): startNode = numpy.array([start]) goalNode = numpy.array([goal]) limits = numpy.array([[-2.461, .890],[-2.147,1.047],[-3.028,3.028],[-.052,2.618],[-3.059,3.059],[-1.571,2.094],[-3.059,3.059]]) for i in range(1, 10000): xrand = numpy.array([]) for k in range(0, len(limits)): xrand = numpy.append(xrand, random.uniform(limits[k,:], limits[k,:])) kdTree = scipy.spatial.KDTree(startNode[:, 0:7]) distance, index = kdTree.query(xrand) q_near = kdTree.data[index] success, startNode, qFinal = makeLine(distance, q_near, xrand, startNode) kdTree2 = scipy.spatial.KDTree(goalNode[:, 0:7]) distance2, index2 = kdTree2.query(qFinal) q_near2 = kdTree2.data[index2] success, startNode, qFinal2 = makeLine(distance2, qFinal, q_near2, startNode) if success: return 1, startNode, goalNode, 1, qFinal, qFinal2 xrand = numpy.array([]) for k in range(0, len(limits)): xrand = numpy.append(xrand, random.uniform(limits[k,:], limits[k,:])) kdTree = scipy.spatial.KDTree(goalNode[:, 0:7]) distance, index = kdTree.query(xrand) q_near = kdTree.data[index] success, goalNode, qFinal = makeLine(distance, q_near, xrand, goalNode) kdTree2 = scipy.spatial.KDTree(startNode[:, 0:7]) distance2, index2 = kdTree2.query(qFinal) q_near2 = kdTree2.data[index2] success, goalNode, qFinal2 = makeLine(distance2, qFinal, q_near2, goalNode) if success: return 1, startNode, goalNode, 2, qFinal, qFinal2 return 0 </code></pre> https://robotics.stackexchange.com/q/649 13 Does RRT* guarantee asymptotic optimality for a minimum clearance cost metric? giogadi https://robotics.stackexchange.com/users/13 2012-12-10T17:07:02Z 2013-01-09T17:50:47Z <p>The optimal sampling-based motion planning algorithm $\text{RRT}^*$ (described <a href="http://sertac.scripts.mit.edu/web/wp-content/papercite-data/pdf/karaman.frazzoli-ijrr11.pdf" rel="nofollow">in this paper</a>) has been shown to yield collision-free paths which converge to the optimal path as planning time increases. However, as far as I can see, the optimality proofs and experiments have assumed that the path cost metric is Euclidean distance in configuration space. Can $\text{RRT}^*$ also yield optimality properties for other path quality metrics, such as maximizing minimum clearance from obstacles throughout the path?</p> <p>To define minimum clearance: for simplicity, we can consider a point robot moving about in Euclidean space. For any configuration $q$ that is in the collision-free configuration space, define a function $d(q)$ which returns the distance between the robot and the nearest C-obstacle. For a path $\sigma$, the minimum clearance $\text{min_clear}(\sigma)$ is the minimum value of $d(q)$ for all $q \in \sigma$. In optimal motion planning, one might wish to <strong>maximize</strong> minimum clearance from obstacles along a path. This would mean defining some cost metric $c(\sigma)$ such that $c$ increases as the minimum clearance decreases. One simple function would be $c(\sigma) = \exp(-\text{min_clear}(\sigma))$.</p> <p>In the <a href="http://sertac.scripts.mit.edu/web/wp-content/papercite-data/pdf/karaman.frazzoli-rss10.pdf" rel="nofollow">first paper</a> introducing $\text{RRT}^*$, several assumptions are made about the path cost metric so that the proofs hold; one of the assumptions concerned additivity of the cost metric, which doesn't hold for the above minimum clearance metric. However, in the more recent <a href="http://sertac.scripts.mit.edu/web/wp-content/papercite-data/pdf/karaman.frazzoli-ijrr11.pdf" rel="nofollow">journal article</a> describing the algorithm, several of the prior assumptions weren't listed, and it seemed that the minimum clearance cost metric might also be optimized by the algorithm.</p> <p>Does anyone know if the proofs for the optimality of $\text{RRT}^*$ can hold for a minimum clearance cost metric (perhaps not the one I gave above, but another which has the same minimum), or if experiments have been performed to support the algorithm's usefulness for such a metric?</p>