Add cables conflicts

[Khaledwahba1994]

This is all the instructions needed to run dbcbs, generate an initial guess and run the optimizer. Visualization for each step is included.

Model

This is a simplified model for the n quadrotors with payload. We are in x-y plane and using 2 double integrators connected with a cable and a point mass in between (i.e., no gravity and 2 2d double integrators instead of UAVs). The model is built with sympy and already added to dynobench along with the optimization problem in dynoplan.
The state of this system is

x = p0x p0y th1 th2 dp0x dp0y dth1 dth2

where p0 is the position of the payload, th1 and th2 are the angles of the cables and the rest is the derivatives of the states. The model is generated here dynomodel

Idea

The idea is we want to use dbcbs to generate the initial guess for the two-robot-cable system. This is done in two steps.

1. we use dbcbs to plan for two double integrators jointly (so normal dbcbs, with no payload). However, we add high level constraints inside dbcbs.
   1.1 First, given the two positions of the robots and we know beforehand the lengths of the cables, we can solve a non linear optimization problem to find an estimation of where the payload could be (using the initial start state of the payload that is assumed to be known).
   1.2 we check if the distances between the estimated payload position and the current position of each robots. If one of these distances is greater than the length of the cable + some slack, then this is set as a high level conflict.
   1.3 Finally, after dbcbs finds a solution, we use the output of dbcbs for the two robots and the positions of the payload solved by the non linear optimization (we used nlopt) to create the initial guess of the two-robot-cable system (using scripts/init_guess_cables.py)
2. Finally we run the optimization using this initial guess.

build

mkdir buildRelease
cd buildRelease/
cmake .. -DCMAKE_PREFIX_PATH="/opt/openrobots/" -DCMAKE_BUILD_TYPE=Release
make -j2

run dbcbs and visualize solution

cd buildRelease/
./db_cbs --input ../example/2integrator2_2d_window.yaml --output ../2integrator2_2d_window_dbcbs.yaml --joint ../2integrator2_2d_window_joint.yaml --optimization ../2integrator2_2d_window_opt.yaml --cfg ../example/algorithms.yaml > ../log_dbcbs.txt
python3 ../scripts/visualize.py --result ../2integrator2_2d_window_dbcbs.yaml --video ../2integrator_2d_window_dbcbs.mp4 ../example/2integrator2_2d_window.yaml 

The last line will generate a .mp4 video for the dbcbs solution.

generate initial guess

cd buildRelease/
python3 ../scripts/init_guess_cables.py 

You will find a generated html file that visualizes the initial guess cables_integrator2_2d_window.html.

run optimization

cd buildRelease/
./dynoplan/test/test_optimization > ../log_opt.txt

visualize solution

cd buildRelease/
python3 ../scripts/visualize_dintCables.py --env ../example/cables_integrator2_2d_window.yaml --result ../cables_integrator2_2d_window_opt.yaml  --output ../cables_integrator2_2d_window_opt.html

problem

1. The solution generated by the optimization doesn't respect the collision constraints (the inter-robot collisions). Something is missing in the optimization itself (maybe the collision constraints are not active?) or we think the initial guess might be not good enough.
2. to check for collision distance for a specific state and visualize it:
   2.1 go to db-CBS/dynoplan/dynobench/example/test_dintegratorCables.py, add the state inside the script you want to investigate to state1
   2.2 build and compile dynobench as described in this PR for the python bindings, then

cd db-CBS/dynoplan/dynobench/buildRelease/
python3 ../../../dynoplan/dynobench/example/test_dintegratorCables.py 

you will find a test_states.html generated with the state you want to investigate and two black and white spheres showing the point of collision.

[quimortiz]

Hi Khaled, at this point the issue and instructions are a little bit overwhelming.
What I did so far:
I run:
./db_cbs --input ../example/2integrator2_2d_window.yaml --output ../2integrator2_2d_window_dbcbs.yaml --joint ../2integrator2_2d_window_joint.yaml --optimization ../2integrator2_2d_window_opt.yaml --cfg ../example/algorithms.yaml > ../log_dbcbs.txt
and it seems to solve the problem.
I attach the log here:
log_dbcbs.txt
Everything seems fine, right?
I visualize the trajectory using

python3 ../scripts/visualize.py --result ../2integrator2_2d_window_dbcbs.yaml --video ../2integrator_2d_window_dbcbs.mp4 ../example/2integrator2_2d_window.yaml

and there are no collisions, but the motion is not smooth.
I can run

python3 ../scripts/init_guess_cables.py 

and see the html, but I don't know what to look at.

If I run optimization,

quim@quim-turin ~/c/k/d/build (add-cables-conflicts)> ./dynoplan/test/test_optimization > ../log_opt.txt                 (/home/quim/code/khaled/db-CBS/env2) 
test_optimization: /home/quim/code/khaled/db-CBS/dynoplan/dynobench/src/DintegratorCables.cpp:240: virtual double dynobench::DintegratorCables::distance(const Eigen::Ref<const Eigen::Matrix<double, -1, 1> >&, const Eigen::Ref<const Eigen::Matrix<double, -1, 1> >&): Assertion `x[2] <= 3.14159265358979323846 && x[2] >= -3.14159265358979323846' failed.

*** 1 failure is detected in the test module "test_optimization"

I receive this error. But anyway, wasn't optimization included in the first command I run? I am lost here.

So to continue I would recommend to test optimization as a standalone process, without the search.
For that, you have to store two (on is also OK, but two is better if we have to tune weights) initial guesses of the planner, and create a test that uses that initial guess. Then you share here the files (add them to the git) with the initial trajectory, and the command we need to run the test and see how optimization fails. Additionally, adapt the visualization commands to see exactly this new input/output.

[Khaledwahba1994]

Hello Quim! I think you were following the right path but got confused in one step. The db_cbs search + the init_guess_cables.py generates the initial guess for the optimization.

In the PR description, there isa section called: run optimization. This is basically the test optimization that takes "init_guess_cables.yaml" as an initial guess and runs the optimization. Please check test_optimization_DintegratorCables.cpp in dynoplan and you will find the test case.

The script init_guess_cables.py generates a yaml file: init_guess_cable.yaml (which is the initial guess), and an html file where you can visualize it.

Now I pushed the init_guess_cables.yaml that can be used as an initial guess for the problem instance: cables_integrator2_2d_window.yaml. Afterwards, you can just run the optimization as mentioned in the run optimization section and you don't need anything from db_cbs.

I will generate another initial guess for you to use for tuning, but for now at least with this you can try to run the optimization itself.

[quimortiz]

If I run:

./dynoplan/test/test_optimization 

I get:

quim@quim-turin ~/c/k/d/build (add-cables-conflicts) [201]> ./dynoplan/test/test_optimization                                                                                          (base) 
Running 1 test case...
Loading yaml file: ../example/cables_integrator2_2d_window.yaml
8
Loading file: ../init_guess_cables.yaml
Robot Factory: loading file: ../dynoplan/dynobench/models/DintegratorCables.yaml
dynamics: DintegratorCables
loading file: ../dynoplan/dynobench/models/DintegratorCables.yaml
init done
Robot name 
Parameters
filename: ../dynoplan/dynobench/models/DintegratorCables.yaml
col_size_robot: 0.1
col_size_payload: 0.01
max_vel: 0.5
max_acc: 2
max_angular_vel: 1
m0: 0.01
m1: 1
m2: 1
l1: 0.5
l2: 0.5
distance_weights: [1,1,0.1,0.1]
size: [0.5,0.25]
u_lb: [4.50588e-321,6.9007e-310,6.95308e-310,4.78982e-310]
u_ub: [1.95224e-312,6.9007e-310,8.39912e-323,-nan]
***
model_robot->ref_dt: 0.1
Report on the init guess 
/home/quim/code/khaled/db-CBS/dynoplan/src/optimization/ocp.cpp:1550"should I copy the first state in the init guess? -- now yes"

test_optimization: /home/quim/code/khaled/db-CBS/env2/include/eigen3/Eigen/src/Core/Dot.h:82: typename Eigen::ScalarBinaryOpTraits<typename Eigen::internal::traits<T>::Scalar, typename Eigen::internal::traits<OtherDerived>::Scalar>::ReturnType Eigen::MatrixBase<Derived>::dot(const Eigen::MatrixBase<OtherDerived>&) const [with OtherDerived = Eigen::Matrix<double, -1, 1>; Derived = Eigen::Matrix<double, -1, 1>; typename Eigen::ScalarBinaryOpTraits<typename Eigen::internal::traits<T>::Scalar, typename Eigen::internal::traits<OtherDerived>::Scalar>::ReturnType = double; typename Eigen::internal::traits<T>::Scalar = double; typename Eigen::internal::traits<OtherDerived>::Scalar = double]: Assertion `size() == other.size()' failed.
unknown location(0): fatal error: in "t_DintegratorCables_optimization": signal: SIGABRT (application abort requested)
/home/quim/code/khaled/db-CBS/dynoplan/test/optimization/test_optimization_DintegratorCables.cpp(10): last checkpoint: "t_DintegratorCables_optimization" test entry

[quimortiz]

and using gdb:

quim@quim-turin ~/c/k/d/build (add-cables-conflicts) [201]> gdb --args ./dynoplan/test/test_optimization                                                                               (base) 
GNU gdb (Ubuntu 12.1-0ubuntu1~22.04.2) 12.1
Copyright (C) 2022 Free Software Foundation, Inc.
License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
This is free software: you are free to change and redistribute it.
There is NO WARRANTY, to the extent permitted by law.
Type "show copying" and "show warranty" for details.
This GDB was configured as "x86_64-linux-gnu".
Type "show configuration" for configuration details.
For bug reporting instructions, please see:
<https://www.gnu.org/software/gdb/bugs/>.
Find the GDB manual and other documentation resources online at:
    <http://www.gnu.org/software/gdb/documentation/>.

For help, type "help".
Type "apropos word" to search for commands related to "word"...
Reading symbols from ./dynoplan/test/test_optimization...
(No debugging symbols found in ./dynoplan/test/test_optimization)
(gdb) c
The program is not being run.
(gdb) r
Starting program: /home/quim/code/khaled/db-CBS/build/dynoplan/test/test_optimization 
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
[New Thread 0x7ffff15ff640 (LWP 7368)]
[New Thread 0x7ffff0dfe640 (LWP 7369)]
[New Thread 0x7fffe85fd640 (LWP 7370)]
[New Thread 0x7fffdfdfc640 (LWP 7371)]
[New Thread 0x7fffd75fb640 (LWP 7372)]
[New Thread 0x7fffc6dfa640 (LWP 7373)]
[New Thread 0x7fffc65f9640 (LWP 7374)]
[New Thread 0x7fffb5df8640 (LWP 7375)]
[New Thread 0x7fffb55f7640 (LWP 7376)]
[New Thread 0x7fffa4df6640 (LWP 7377)]
[New Thread 0x7fffa45f5640 (LWP 7378)]
[New Thread 0x7fff9bdf4640 (LWP 7379)]
[New Thread 0x7fff935f3640 (LWP 7380)]
[New Thread 0x7fff8adf2640 (LWP 7381)]
[New Thread 0x7fff7a5f1640 (LWP 7382)]
Running 1 test case...
Loading yaml file: ../example/cables_integrator2_2d_window.yaml
8
Loading file: ../init_guess_cables.yaml
Robot Factory: loading file: ../dynoplan/dynobench/models/DintegratorCables.yaml
dynamics: DintegratorCables
loading file: ../dynoplan/dynobench/models/DintegratorCables.yaml
init done
Robot name 
Parameters
filename: ../dynoplan/dynobench/models/DintegratorCables.yaml
col_size_robot: 0.1
col_size_payload: 0.01
max_vel: 0.5
max_acc: 2
max_angular_vel: 1
m0: 0.01
m1: 1
m2: 1
l1: 0.5
l2: 0.5
distance_weights: [1,1,0.1,0.1]
size: [0.5,0.25]
u_lb: [4.50588e-321,6.95335e-310,6.95336e-310,4.63557e-310]
u_ub: [1.95224e-312,6.95335e-310,8.39912e-323,-nan]
***
model_robot->ref_dt: 0.1
Report on the init guess 
/home/quim/code/khaled/db-CBS/dynoplan/src/optimization/ocp.cpp:1550"should I copy the first state in the init guess? -- now yes"

test_optimization: /home/quim/code/khaled/db-CBS/env2/include/eigen3/Eigen/src/Core/Dot.h:82: typename Eigen::ScalarBinaryOpTraits<typename Eigen::internal::traits<T>::Scalar, typename Eigen::internal::traits<OtherDerived>::Scalar>::ReturnType Eigen::MatrixBase<Derived>::dot(const Eigen::MatrixBase<OtherDerived>&) const [with OtherDerived = Eigen::Matrix<double, -1, 1>; Derived = Eigen::Matrix<double, -1, 1>; typename Eigen::ScalarBinaryOpTraits<typename Eigen::internal::traits<T>::Scalar, typename Eigen::internal::traits<OtherDerived>::Scalar>::ReturnType = double; typename Eigen::internal::traits<T>::Scalar = double; typename Eigen::internal::traits<OtherDerived>::Scalar = double]: Assertion `size() == other.size()' failed.

Thread 1 "test_optimizati" received signal SIGABRT, Aborted.
__pthread_kill_implementation (no_tid=0, signo=6, threadid=140737333447488) at ./nptl/pthread_kill.c:44
44	./nptl/pthread_kill.c: No such file or directory.
(gdb) bt
#0  __pthread_kill_implementation (no_tid=0, signo=6, threadid=140737333447488) at ./nptl/pthread_kill.c:44
#1  __pthread_kill_internal (signo=6, threadid=140737333447488) at ./nptl/pthread_kill.c:78
#2  __GI___pthread_kill (threadid=140737333447488, signo=signo@entry=6) at ./nptl/pthread_kill.c:89
#3  0x00007ffff6a42476 in __GI_raise (sig=sig@entry=6) at ../sysdeps/posix/raise.c:26
#4  0x00007ffff6a287f3 in __GI_abort () at ./stdlib/abort.c:79
#5  0x00007ffff6a2871b in __assert_fail_base (fmt=0x7ffff6bdd130 "%s%s%s:%u: %s%sAssertion `%s' failed.\n%n", assertion=0x555555a37a6f "size() == other.size()", 
    file=0x555555a37a28 "/home/quim/code/khaled/db-CBS/env2/include/eigen3/Eigen/src/Core/Dot.h", line=82, function=<optimized out>) at ./assert/assert.c:92
#6  0x00007ffff6a39e96 in __GI___assert_fail (assertion=0x555555a37a6f "size() == other.size()", 
    file=0x555555a37a28 "/home/quim/code/khaled/db-CBS/env2/include/eigen3/Eigen/src/Core/Dot.h", line=82, 
    function=0x555555a38aa0 "typename Eigen::ScalarBinaryOpTraits<typename Eigen::internal::traits<T>::Scalar, typename Eigen::internal::traits<OtherDerived>::Scalar>::ReturnType Eigen::MatrixBase<Derived>::dot(const Eigen::Matri"...) at ./assert/assert.c:101
#7  0x00005555556bf18f in Eigen::ScalarBinaryOpTraits<double, Eigen::internal::traits<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::Scalar, Eigen::internal::scalar_product_op<double, Eigen::internal::traits<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::Scalar> >::ReturnType Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::dot<Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&) const ()
#8  0x00005555558ddbbb in dynobench::DintegratorCables::distance(Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > const&, Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > const&) ()
#9  0x00005555557baf3a in dynobench::check_trajectory(std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, std::allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > > const&, std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, std::allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > > const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, std::shared_ptr<dynobench::Model_robot>, bool) ()
#10 0x00005555557b4542 in dynobench::Trajectory::check(std::shared_ptr<dynobench::Model_robot>, bool) ()
#11 0x000055555559e4dc in dynoplan::trajectory_optimization(dynobench::Problem const&, dynobench::Trajectory const&, dynoplan::Options_trajopt const&, dynobench::Trajectory&, dynoplan::Result_opti&) ()
#12 0x000055555557d09e in t_DintegratorCables_optimization::test_method() ()
#13 0x000055555557cd39 in t_DintegratorCables_optimization_invoker() ()
#14 0x00005555555802cf in boost::detail::function::void_function_invoker0<void (*)(), void>::invoke(boost::detail::function::function_buffer&) ()
#15 0x00007ffff7f467be in boost::detail::function::function_obj_invoker0<boost::detail::forward, int>::invoke(boost::detail::function::function_buffer&) ()
   from /home/quim/code/khaled/db-CBS/env2/lib/libboost_unit_test_framework.so.1.82.0
#16 0x00007ffff7f465cd in boost::execution_monitor::catch_signals(boost::function<int ()> const&) () from /home/quim/code/khaled/db-CBS/env2/lib/libboost_unit_test_framework.so.1.82.0
#17 0x00007ffff7f46654 in boost::execution_monitor::execute(boost::function<int ()> const&) () from /home/quim/code/khaled/db-CBS/env2/lib/libboost_unit_test_framework.so.1.82.0
#18 0x00007ffff7f46739 in boost::execution_monitor::vexecute(boost::function<void ()> const&) () from /home/quim/code/khaled/db-CBS/env2/lib/libboost_unit_test_framework.so.1.82.0
#19 0x00007ffff7f6b5a6 in boost::unit_test::unit_test_monitor_t::execute_and_translate(boost::function<void ()> const&, unsigned long) ()
   from /home/quim/code/khaled/db-CBS/env2/lib/libboost_unit_test_framework.so.1.82.0
#20 0x00007ffff7f528f8 in boost::unit_test::framework::state::execute_test_tree(unsigned long, unsigned long, boost::unit_test::framework::state::random_generator_helper const*) [clone .isra.0] () from /home/quim/code/khaled/db-CBS/env2/lib/libboost_unit_test_framework.so.1.82.0
#21 0x00007ffff7f52ba8 in boost::unit_test::framework::state::execute_test_tree(unsigned long, unsigned long, boost::unit_test::framework::state::random_generator_helper const*) [clone .isra.0] () from /home/quim/code/khaled/db-CBS/env2/lib/libboost_unit_test_framework.so.1.82.0
#22 0x00007ffff7f54523 in boost::unit_test::framework::run(unsigned long, bool) () from /home/quim/code/khaled/db-CBS/env2/lib/libboost_unit_test_framework.so.1.82.0
#23 0x00007ffff7f6b1a0 in boost::unit_test::unit_test_main(bool (*)(), int, char**) () from /home/quim/code/khaled/db-CBS/env2/lib/libboost_unit_test_framework.so.1.82.0
#24 0x000055555557c473 in main ()

[quimortiz]

(all of this is using your latest commit: cf2c020 )