User intuition update

[jontromanab]

@akgoins . This is the update for incorporating robot model in user intuition method. Previously, in the UI method, when the user selects the method only an arrow marker user to appear for placing base (different than pose marker). Also if the method is changed, there was some unexpected behaviour.

With this PR, when the user selects the UI method, a robot model (green) will come which the user can rotate and drag to select desired position. Clicking the marker will place a robot model (grey) in the desired positions. Now after clicking the "place base" button, final results will be shown with the placed robot model markers.

Please have a look and merge this.

[akgoins]

@jontromanab, the code looks fine to me, but I am unable to get it to plan using the User Intuition method. Every time I hit Find Base, I get a return of position (0,0,0) orientation (-nan, -nan, -nan). I am running roslaunch base_placement_plugin base_placement.launch, I then load the inverse reachability map, insert two markers, change the method to User Intuition, and output type to RobotModel then click Find Base. Am I missing something here?

Also, I am getting an odd error on startup which may or not be related: [ERROR] Is your selected group is a manipulator??

[jontromanab]

@akgoins I guess now its the time I should make a video of that, as the instructions are getting complicated. Have you already selected some base poses too? (It needs two types of poses from the user->1)The task poses[pink] and 2)Robot base poses [A transparent robot model should appear when you change your method to user Intuition] IF you click on the robot model it would also select the current position of the robot as one user-defined base pose. You can also drag around the marker to select other base poses. I guess that is the problem as you have not selected any base pose, it is giving the error.

[ERROR] Is your selected group is a manipulator?? =>This error is just related to selecting the right move_group of the robot. If user selects the end-effector, it shows odd behavior. In the next PR, this selection is done automatically from the srdf, so it will be updated in the next PR.

Have you looked at the 7DOF reachability map codes and self-collision detection, in my new repo?

Please let me know if userIntuition is working.

[akgoins]

@jontromanab, User Intuition works only if the task poses are reachable by the user provided position. I had expected that if the robot base pose was too far away, that Reuleaux would find a nearby valid solution and show that, but I am still getting NaN's for the solution. Also, when I select VerticalRobotModel for the solution method, several robot models show up during solve time, but when it is done (FindBase Task Finished is printed to console) the robot models disappear, leaving me to guess what the final solution(s) was. Any thoughts on this?

I have not looked at the new repo yet, but I plan to tomorrow.

[jontromanab]

@akgoins Thanks for noticing that. It is showing nan because the solutions provided by userintuition method by the user are not valid. I thought that was the purpose of userintuition method, that the user provided base poses are valid or not. We can surely add that section that if the user has not provided any valid base pose, reuleaux will find a suitable base pose and show the solution to user.

There is still slight bug with showing robot model, the solution would be shown for some predefined time, after that the markers will go away. I am still struggling with how to show the solution. One idea is that, I can add a button to the interface, that will clear the robot models (until pressing, the robot models will be there) Please provide suggestions.

[akgoins]

@jontromanab, Ah, I did not know that is how the user intuition method was supposed to work. I was thinking that it would work the second way you described. The user provides an input, and Reuleaux would use that as a seed for the base placement task and show the optimized results. I didn't understand it probably because the button said "Find Base", but really for user intuition, all it does is check the proposed base place.

As for visualization of the robot solutions, I was thinking that you could show an array of interactive markers, like before where you showed green arrows for the final solutions. But if you make them interactive markers rather than individual markers, you could add a right click menu with the option "show robot visualization" that would switch the robot visualization for that marker on, and maybe turn the visualization for the other markers off. This way, the user can select which marker they want to show. Do you think that will work?

[jontromanab]

Yes. We can surely work that out. But selecting the option of arrow/robot model is still there. Even with the arrows, the time constraint still exists. The arrows are also supposed to vanish after a certain time. As the interface is created in Qt, it does not allow the user to do anything, until the process is completed. You can see that, when it calculates the base placement, everything is disabled (I can look at that). I have two suggestions for the robot visualization:

1) The robot model will still be there, until the user starts a new task (seems better) Similar to moveit "showing trajectory loop" 2) Easier one: there will be a button, which will clear the robot model. 3) Right clicking on the arrow and changing to "show robot visualization" will show the robot model on that specific base pose. But we have to show ik solutions on the robot model too. (Thats why it is an interactive marker) Otherwise it will just robot model with default joint solns. (the unreachable red robot models)

Please let me know which one you like better. I will change it to that accordingly.

[jontromanab]

Thanks, @akgoins for merging that. It was long due. Maybe it is time to clean up the whole thing and rewrite the structure of the package. Are you still into it?