search

ADVRHumanoids / gap_crossing

Master Thesis on "Planning techniques for the Gap Crossing Task" in collaboration with Francesco Roscia e Matteo Sodano, from La Sapienza
2 stars 0 forks source link

Added example SE2 with discrete controls #8

Closed lucarossini-iit closed 4 years ago

lucarossini-iit commented 4 years ago

Hi all, I just pushed a .cpp file containing the commented example of motion planning with discrete controls made on a simple SE2 model. I tried it on my laptop and it works. Please pull the last changes (git pull origin master) and build the package using the CMakeLists.txt. Then, run the executable going in ws_dir/build/gap_crossing and typing ./SE2_discrete_control. Let me know if it works and write here for any question.

francesco-roscia commented 4 years ago

We have downloaded and run the code. It seems clear and the comments are useful to explain it. We don't have any question about it. We'll keep it in mind! Thank you

francesco-roscia commented 4 years ago

We have worked with OMPL in order to move a compass from a starting pose to a goal pose with the presence of a traversable gap. Compass' feet are represented as points. We defined three coordinates to describe the simple rigid body: the position in R^2 of the right foot and orientation of the body. Four primitives have been defined:

Summarizing, only rotations around a foot are admissible and no steps are taken into account . Dynamics and stability issue have been neglected just to have an easy example. We attach the cpp/OMPL code and the python code that we used to make the plot: if run, it shows a very simple animation of the compass' feet, (it's the first time that we use matplotlib). In the plot, you can see the footprints of an admissible solution found by the RRT (footprints are linked by a line that represents the distance between them). The solid line represents the gap, the dashed one represents the clearance that the feet must respect from the gap.

We upload the files in the code section (name Dummy_Compass)

figure_1

francesco-roscia commented 4 years ago

Today, we have tried to expand this dummy example to have a biped capable of stepping. We wanted to represent the system with 5 coordinates (XY coordinates of the two feet and orientation of the body). We have not been able to use this compound state and to initialize it (operations of getting and setting the state values). Can you give us some suggestion?

Moreover, when we print the path as a matrix, it only says which control has been applied. It does not specify how many times it is using it. Do you know how to extract this information? We saw that it prints it when we use the path->print, but we didn't find a function that returns that list of integers (or something like that).

lucarossini-iit commented 4 years ago

Can you please push the code containing the biped example? I will take a look asap

matteosodano commented 4 years ago

Here you find the cpp file we were writing for the biped (we had just started). At lines 55-59 you find the state-space definition. Is it correct? How can we access to the state values (set start and goal, get values, etc.)? Thanks.

biped_ompl.zip

lucarossini-iit commented 4 years ago

One first mistake that I could spot is on the RealVectorBounds. You are defining a variable bounds of dimension 2 and then you write access the 4th index (lines 63-71). This leads to a segmentation fault. Try to increase the dimensione of bounds to 4.

For what concerns the path->print(std::cout) function, with path being a ompl::base::Path, it prints which control is applied, for how many steps and to which state it is applied. Are you sure you are not printing a ompl::geometric::PathGeometric or a ompl::control::PathControl?

matteosodano commented 4 years ago

We have already corrected the dimension of the bounds (we uploaded the wrong zip).

For what concerns the path->print, we do not want to print the number of times a control is applied, but we want to store it in a variable. We were wondering if there is some function returning it directly.

lucarossini-iit commented 4 years ago

A compound state space works as a container of state spaces. Each state space can be accessed with an index exactly as you did with the SO2 state space (which is indeed a particular compound state space). In your example, for instance, in the state validity function you can extract the state values as:

auto comp_state = state->as<ompl::base::CompoundStateSpace::StateType>(); auto feet = comp_state->as<ompl::base::RealVectorStateSpace::StateType>(0)->values; auto rot = comp_state->as<ompl::base::SO2StateSpace::StateType>(1)->value;

feet will be a double* with 4 values inside while rot will be a simple double Same thing if you are planning to use a CompoundControlSpace.

lucarossini-iit commented 4 years ago

Additionally, we use a class called MatLogger2 to extract data from a running cpp code and create a .mat file that can be processed later with MATLAB. @alaurenzi will give you further info about it but in the meanwhile you can start reading the README file at this link:

https://github.com/ADVRHumanoids/MatLogger2

matteosodano commented 4 years ago

Ok. We will try all these things and let you know about it. Thank you.

matteosodano commented 4 years ago

Hi everyone. We have uploaded the final version of our biped in the code section. As we have also written in the README, we have expanded the compass and we took into account also steps. Fourteen primitives have been defined:

We are not showing any image because they are not too clear. We have uploaded the python code that we used for plotting: it is a little animation that shows the solution step-by-step. Moreover, the title of the image describes the applied primitive at each step.