Support on CartTrajectoryPt with flexibility on provided z axis orientation candidates

[yijiangh]

Hi everyone, I'm planning to use descartes in a robotic spatial 3D printing project. We already have a path planning algorithm that can find printing sequence and feasible orientaions for each element's printing and want to use descartes to search for a feasible motion plan. In this scenario, the TrajectoryPt is a crossover between AxialSymmetryPt and CartTrajectoryPt, but with another degree of flexibility on choices from provided feasible orientation.

I'm writing here to ask:

• Is there someone else in this community that is interested (or in need) in this type of TrajectoryPt?
• Is there a way to write this in the framework of descartes_trajectory? If so, any suggestion on how to do that in order to make it useful for future usage?

So to begin with, for each element's printing process, my path planning component will generate a bunch of feasible orientations (illustrated in green in picture below) for motion planner to choose, each orientation guarantees that the end effector won't collide with the work piece.

A picture of my 3D printing end effector is shown below, basically it extrudes the filament at the tip and should maintain the orientation in its printing process (fixed orientation, different from blending application in project godel). But in each printing process (extrusion with fixed orientation with a cartesian path from start point to end point), we still have flexibility (still some kind of semi-constrained cartesian planning) on choices in orientation and axial symmetry about z axis. (check this video for clarification if I make you confused and I apologize for my poor explanation!)

Using the graphical notion that Shaun used in his ROSCon 2015 talk, slide page 10, I can describe the problem in ladder graph:

So basically we have two extra degrees of freedom:

• feasible orientations computed from path planning algorithm
• for each feasible orientation, we have symmetric axis about z axis. But once we choose an orientation and a specific local frame with the orientation as z axis, the motion from start point to end point has to be a strict (traditional moveit type) cartesian path.

Any thoughts and suggestions are welcomed! Should I add more samples for each node when building the ladder graph (which is not an elegant way), or how can I write another TrajectoryPt class under descartes_trajectory framework? Is there anyone else in our community would find this kind of things useful in their application too?

Thanks for your time for reading!

[Jmeyer1292]

@yijiangh Fantastic stuff. I love this idea and the video you showed is super cool. I've been doing thinking about similar problems, and I think the most concise way to handle this is to build a LadderGraph directly that models the constraints you have in your problem. You could then use existing tools to search this for a valid solution.

I've been wanting to move toward a more Graph based approach that gives users more flexibility in how they build their constraints. I'll try to cook up some code for you tomorrow - it'll be a slow day at work.

[BrettHemes]

Very cool work @yijiangh ! Also +1 @Jmeyer1292 regarding building the LadderGraph directly. This is something I have been meaning to dig into and an example snippet would be cool to see.

[ClayFlannigan]

@Jmeyer1292 I think this problem is similar to the one you and I were discussing recently: under-constrained starting pose, but fully constrained trajectory. In my case (grasp planning) I may have degrees of freedom in both position and orientation, and the trajectory might include an interpolation of both position and orientation from start to end pose. It feels like both @yijiangh use case and my use case are handled by implementing a method to sample the starting pose and a method to "plan" the trajectory via interpolation from start to goal. I'm not sure how consistent this approach is with the existing Descartes constructs?

[yijiangh]

@Jmeyer1292 @BrettHemes Thanks for your interest! It's my honor to work with you guys to make descartes better! Looking forward for your small demo! I will definitely test it and perhaps expand it and give you feedback after I try it in my project.

@ClayFlannigan:

under-constrained starting pose, but fully constrained trajectory

I totally agree! Actually I'm thinking about a general framework for this kind of "workpiece processing" application, after combining my spatial 3D printing project experience and what I read and learn from project godel. I hope this discussion is helpful and can fit in the context here. I'll first describe how do I do the motion planning part in the printing project and then try to abstract it into a more general framework.

In the 3D printing project, our path planning algorithm will produce an assembly sequence and associated feasible orientations for each element process. Then we need to find out a motion planning method to actually carry out the task. The work is done before I got to know this brilliant ROS-I community and all the work was done in a graphical programming platform grasshopper with parametric robot control package KUKA|PRC. Although the path planning algorithm give preference to adjacent sequence when searching, we tried to simplify the motion planning process by breaking the element printing process to separated unit process, resetting the robot to a "home pose" after the robot completes each element's printing. We tried to use a genetic algorithm to find a feasible unit process robotic motion - optimizing over a parameterized interpolated connecting path and choose one pose from feasible orientations and associated local frame (illustrated in the picture below, and you're welcomed to check out this video for real robot unit printing process) It's basically a brutal searching for feasible motion, with some acceleration from genetic algorithm. But it's still super slow: finding a solution for a model with 150 elements will take me 7 hours!!! But it at least gave me a solution at that time...

So here I am with all the powerful tools and this incredible community in ROS and ROS-I. Thanks to the brilliant godel project, which is a beacon for me, as it clearly shows a complete scan-n-plan process in super clean code style, how should we code it and in what way we can incorporate descartes into our process (thanks to all the contributors!). After several weeks of reading and understanding, I find out a somewhat similar process layout:

current pose -> "free trajectory plan"(moveit) -> start_retract path (descartes) -> process path (segments) (descartes) -> end_retract path (descartes) -> connecting path to next process -> ... -> "free trajectory plan" -> end process

Although the constraints and flexibility can be different in the "sub-process" here, but the general description framework is essentially similar! So I can describe my unit printing process in the language of ladder gragh:

In the application of 3D printing, for the retract path I can have degrees of freedom in the symmetry of Z axis (just approach the workpiece in low speed cartesian path under constrained) and then print it (fully constrained). @ClayFlannigan I imagine in some "grasping" application, it might require this retract path to be fully constrained cartesian path too, but still have freedom in choosing the pose.

With descartes, I'm also thinking that if it's possible to traverse "continuously" without actually resetting the pose each time we finish one element's printing? The searching problem is aggravated by this requirement. I'm wondering if we can combine the "reset mitigation" and continuous traversing together? So here's my thought:

I think in my case is different with godel's case as the workpiece is not a curved surface with quite big open work space for robot (so we can do interpolation for connecting path and leave it for descartes, code here), but requiring us to carefully design the connecting path to “find its way to walk through the rainforest” of printed structure, e.g. something like this:

I've seen some really cool continuous robotic motion in the ros-i edge processing project (video and video). I've tested some continuous printing motion too. It's time efficient and the printed result is much stronger (due to the "wrapping at the joint") (check out this video).

Any comment, suggestion and discussion are welcomed! I'm wondering:

• do you guys think this kind of framework can be generalized or be reused in some other application?
• For my application, do you have any suggestion or thought on the searching scheme proposed above? Do we have broader application for this method? Any suggestion on how to implement it?

[Jmeyer1292]

I have not forgot you @yijiangh. I worked a bit tonight and wrote something very simple that I hope can address some of your issues. Please allow me a day or two to build some tests for it.

[yijiangh]

Sure no problem! Thanks for your hard work @Jmeyer1292 ! At the meantime I'm working on pose generation, getting ready to be integrated in your demo!

On Mon, Jul 10, 2017 at 12:47 AM Jonathan Meyer notifications@github.com wrote:

I have not forgot you @yijiangh https://github.com/yijiangh. I worked a bit tonight and wrote something very simple that I hope can address some of your issues. Please allow me a day or two to build some tests for it.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/ros-industrial-consortium/descartes/issues/200#issuecomment-314003908, or mute the thread https://github.com/notifications/unsubscribe-auth/ALWrNMvNvYUFY1hO12xyvh0syxBnpO_Zks5sMazqgaJpZM4OLxir .

[Jmeyer1292]

@yijiangh Okay. I made a small-ish demo app that:

• Uses a UR5
• Demonstrates constructing a sequence of linear segments with the constraints you specified
• Planning for them
• Executing in simulation

It is NOT pretty and I don't promise its bugless

I recommend creating a new, test workspace for trying this out and:

• Clone the app directory
• Clone the yijiangh/graph branch of descartes here
• Clone universal_robot package if you don't have it already

To test it out, open two terminals:

• The first roslaunch ur5_demo_support demo_setup.launch
  • You may need to add a RobotModel view and change the reference frame to base_link in Rviz
• The second rosrun descartes_graph_tests descartes_graph_tests_node

To see what's going on, see the main method of the app here

And see the added graph functionality header and src.

Basically, I take a linear segment with some parameters and a set of valid orientations and I build a graph as you drew above. I then append each segments graph in time and search it.

The big questions are:

• Is this approximately what you need?
• Does it scale well to the size of the problem you have?
• Can we figure out how to transition?

The last one is the biggest deal to me. For demonstration purposes, I just connect all joint solutions between the terminal points of each pair of graphs. You might want to define custom logic to compute these costs, or you might apply a heuristic and then go back later and compute the real path based on the search result.

We could try to write something that tests if cartesian interpolation to the next pose is possible and, if so, gives that edge a very low cost. That might not be super simple, but I think it's doable.

At some level, it's all very simple so I hope this is actually useful to you... either way let me know.

[yijiangh]

That's amazing @Jmeyer1292 , thank you so much!!!

Give me some time to digest and test, I'll give you feedback ASAP.

[yijiangh]

@Jmeyer1292 Finally I finished the integration of our descartes "special edition" into my app that I've been working on for the past 2 months. Here's a really simple, small and early-stage demo video[1] demonstrating the entire workflow.

I have to express my gratitude for all the fantastic researchers in our ROS & ROS-I community again, especially "descartes" and "godel" crew. In my project, basically I directly use godel's framework (code architecture, UI and lots of functions that I'd love to reuse) and change it to fit the needs in my project's context. Without godel, it's impossible for me to come up with such a clear workflow and get to where we're right now in 2 months. (Please excuse me for the wrong infos in all package.xml, I will change all the credit and associated license files later.)

In a nutshell, my app (framefab_mpp[2])'s workflow is:

1. take a json file from my path planning algorithm (described in my first post above, I have some issues integrating it into ROS for now, I leave this integration for future) and process it into framefab_mpp.
2. User can choose planning target (don't need to be entire model) and start planning
3. Simulate the planning result. (user can choose to simulate specific unit process)

To answer @Jmeyer1292 's three questions:

Is this approximately what you need?

Yes!!! But I did make some modification to make it customized to my needs. As the unit process in my project contains 4 points instead of 2: retract_start, start, end , retract end, I pack the retract path in ConstrainedSegment[3] and add the discretization of them in sampleConstrainedPath[4]. The graph searching of unit process can be found here[5]. This is just a quick, simple and ugly temporal solution... We can think about cleaner solution to seperate path generation and solving process later.

Does it scale well to the size of the problem you have? Can we figure out how to transition?

My current method of using descartes is depicted below:

There are two observations worth our notice:

1. I deliberately break the graph according to unit printing process . There are no graph edge between two unit process and the solving is carried out independently.
2. Transition and printing process's computation are seperated, without any guidance to each other. We first get the printing process's result, fix it and then hope we can find a solution by interpolation or moveit! (here I use the same strategy in godel's PlanFreeMove[6] for transition).

Because of this naive design, the result is not good in two aspects:

1. The computation is slow: For the "simple frame" example showed in this demo video[7] (you could observe some sudden jump of robot's pose... This should be a bug. I'll take a look at it...), the time returned by swri_profiler is:

7 elements model: Entire computation time: 328 s (thus I cut the computation parts for all the demo video in this post or only have pillars planned....)

• element 1: 1s (pillar, only one feasible orientation offered to descartes)
• element 2: 1s (pillar)
• element 3: 55s
• element 4: 72s
• element 5: 90s
• element 6: 56s
• element 7: 52s
  2. Transition process does not necessarily have solutions. The Solving of printing process and transition must be binded together and carried out in a cost-drived sequential way.

To push our project further, I have two main questions:

1. The biggest issue prevent me from "uniting all process in one ladder graph" is that the collision objects in the scene is added sequentially as the process goes on, which means we need to update the planning scene accordingly when constructing the graph? Do you have any idea how we can address this problem? And as you wrote above, a good heuristic is definitely really important.

2. I don't think descartes's collision checking is working correctly in my project. Check this video for comparison between transition path generated by Moveit! & Descartes Interpolation[8]. The big green plane below is a box collision object, representing the build plate (table). I've been checking very carefully on my descartes setup (see here[9]) and comparing it to godel's code (here[10]). Taking godel's abb_irb2400_descartes[11] as an example, I build ikfast pugin for kuka kr6_r900[12] and wrap framefab_kr6_r900_descartes[13] for descartes interface. I'm wondering if there's any thing I did wrong or forgot to do? I even tried @JeroenDM 's patch as we discussed in descartes_tutorial[14]. But it still didn't work...

I want to say thank you for all of your interest, discussion, advice and help. Any comments and advice are warmly welcomed!!! Thanks for your time and patience! Looking forward to you guys' reply!

[Jmeyer1292]

Oof. I'm on travel for the next couple days but when I get back I'll attempt to build your demo app, framefab_mpp, and see what I can do about computation time, transitions, and collision checking.

[yijiangh]

@Jmeyer1292 Thank you! I just update the README of framefab_mpp [1] and add detailed instructions and guide for installation and testing. It should work for you as I just tested it on a new ubuntu setup.

Please check the kinetic-devel branch.

Looking forward to you feedback.

[1] https://github.com/yijiangh/framefab_mpp

[yijiangh]

@Jmeyer1292 I manage to solve the collision checking problem! It seems like in moveit_state_adapter, unless specifically set the planning_scene using setState function, the planning_scene will not automatically updated. In my case, I constantly need descartes to update its internal planning scene to add the newly printed element as collision objects at the end of each unit printing process (code).

Referring to @JeroenDM 's collision checking patch on descartes [1], used in his descartes_tutorial, I manage to update the planning scene inside descartes using updateInternals function.

The result shows that the collision update does work: video In the example, transition paths are using a mixture of interpolation and moveit free plan, depending on whether interpolation path causes collision - code.

Still WIP:

• graph unification modeling (connecting unit process together)
• The transition plan generated by moveit is somewhat weird though feasible. Planning to give STOMP [2] a try.

[1] https://github.com/JeroenDM/descartes/tree/welding_tutorial [2] https://github.com/ros-industrial/industrial_moveit

[Jmeyer1292]

@yijiangh I've built your application and have been playing around a bit today. Thanks for the instructions on running it.

A few thoughts:

• It seems like both test cases eventually fail to generate a path because of collisions. Even when I changed your tool and gave it a bit more stand off from the collision geometry. I need to narrow this down and figure out if its a problem in how I build a graph or whether there is a segment that has no discretized solution without collisions.
• Not surprisingly, most of the time is spent collision checking. I've been trying to implement a "lazy" collision checker that only checks the final results but I'm running into the above problem.

[Jmeyer1292]

I worked a little more tonight. convertOrientationVector is producing the same orientation for many different vector inputs. This, combined with the tools wide width, is causing a lot of collision checking problems.

[yijiangh]

@Jmeyer1292 Thanks for your work and sorry for my slow response, I was trying to add a small ui interface that allows us to zoom in vertices in the ladder graph (choose path id, orientation id, axial symmetry (local axis) and ik solution, see the picture below), check the collision visually and hoped to write you feedback after I finish it.

I'll look into the issue with convertOrientationVector, try to fix it and give you feedback ASAP.

Thanks for your help again!

[yijiangh]

@Jmeyer1292 , okay, I fixed the orientation issue but encountered a problem that ik fast fails to find solution for "simple pose" which obviously have solution from a human perspective.

Please take a look at the updated code: framefab_mpp: pull hotfix/convertOrientationVector branch [1] descartes: pull hotfix/graph_vert_empty branch [2]

If you built it and test it with the simple frame example, you should have the result showed in this video

Here are the summary of changes in the code update above:

• fixed the problem with convertOrientationVector, I made a mistake about the rotation axis with the Eigen::AngleAxisd in previous commit, thanks for pointing that out @Jmeyer1292 .

• After correct that, I found that the entire planning graph is empty (the rung has no vertices) for unit process No.3. To inspect this, I visualized the generated (interpolated and sampled by descartes_planner/graph_builder) poses for ladder graph, check sampleSingleConfig.

• Thus to narrow down the problem, I turned off the working env objects in urdf, commented out the incrementally-added printed parts collision objects, basically turning off all the environmental collision checking and making it easier for ik solver to find solutions. But still, you can observe the "0 solution" issue in the video.

I'm wondering if there's an issue with my ikfast setup or something else that I'm missing? I'll keep looking into this problem and at the same time, it would be really nice of you if you can take a look at it @Jmeyer1292 .

Thanks for everyone's help again!

[1] https://github.com/yijiangh/framefab_mpp/tree/hotfix/convertOrientationVector [2] https://github.com/yijiangh/descartes/tree/hotfix/graph_vert_empty

[Jmeyer1292]

@yijiangh I'll take a look specifically at what you did soon. I've been playing with your system and I've got the thing "mostly" working for the bunny except I'm currently failing on elements 119 and 139 of the bunny which only have 3 and 2 valid orientations a piece. It's a good place to test at.

[Jmeyer1292]

@yijiangh I've continued messing with things and realized a couple of things:

1. I'm an idiot and only discretize -PI/2 to PI/2 in graph_builder.cpp. That needs to be widened to -PI to PI.
2. The collision model for the tool is a little wider than the original tool. I shrunk it a little to better match the visual model. I hope this is okay.

Now I've managed to solve for every segment. Now to figure out how to connect them intelligently.

[yijiangh]

@Jmeyer1292 :

The collision model for the tool is a little wider than the original tool. I shrunk it a little to better match the visual model. I hope this is okay.

To address this problem, I create a finer-grained collision model for end effector (picture below, compared to previous model in the back) and have merged your commits in branch messing_around in framefab_mpp and also commits in messing_around in descartes.

You can find these changes in branch hotfix/convertOrientationVector [1] in framefab_mpp.

Thanks for your help again.

[1] https://github.com/yijiangh/framefab_mpp/tree/hotfix/convertOrientationVector

[yijiangh]

With the implementation of RRT* on a sparse representation of ladder graph (I call it CLT - Capsulated Ladder Graph), please refer to issue #55 for related discussions.

I have to express my gratitude to everyone here who involved in the discussion, especially @Jmeyer1292 for providing valuable demo code for guiding me out of the mist.

The final product of my project, named Choreo [1], is almost completed, which would not have been possible without ROS-I's support.

With this, I close this issue and keep contributing to this wonderful community in the future.

[1] https://github.com/yijiangh/Choreo