Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

TrajOpt Development TODO List #71

Open
6 of 24 tasks
mpowelson opened this issue Jan 28, 2019 · 11 comments
Open
6 of 24 tasks

TrajOpt Development TODO List #71

mpowelson opened this issue Jan 28, 2019 · 11 comments
Labels
bug Something isn't working enhancement New feature or request

Comments

@mpowelson
Copy link
Contributor

mpowelson commented Jan 28, 2019

@gavanderhoorn
Copy link
Contributor

@mpowelson wrote:

  • Review examples

could you update me on the state of the examples in trajopt_examples? They all compile, but trying to run them leads me to #55 among other things.

Are they supposed to work with current HEAD of this repository (and tesseract_ros)?

@mpowelson
Copy link
Contributor Author

@gavanderhoorn Yes, with the exception of #55 , they should all work with the latest of Trajopt/Tesseract. I get an Eigen error on basic_carteisan (#27 ), but it seems like that is not very widespread and might be something wrong on my end. That said, it has been a while since I have run anything other than glass_upright or basic_cartesian.

This bullet was intended to

@gavanderhoorn
Copy link
Contributor

gavanderhoorn commented Feb 7, 2019

Yes, with the exception of #55 , they should all work with the latest of Trajopt/Tesseract.

Ok, good to hear.

I get an Eigen error on basic_carteisan (#27 ), but it seems like that is not very widespread and might be something wrong on my end. That said, it has been a while since I have run anything other than glass_upright or basic_cartesian.

I've just tried running the puzzle piece examples, and they always result in:

[ WARN] [1549555971.466130507] [/trajopt_examples_puzzle_piece_plan] [ros.trajopt]: Last time step for JointVelTerm comes before first step. Reversing them.
ERROR wrong number of JointVelTermInfo targets. expected 7 got 0
at /home/user/tesseract_ws/src/trajopt_ros/trajopt/src/problem_description.cpp:79
terminate called after throwing an instance of 'std::runtime_error'
  what():  wrong number of JointVelTermInfo targets. expected 7 got 0
Thread 1 "trajopt_example" received signal SIGABRT, Aborted.
0x00007ffff402b428 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:54
54      ../sysdeps/unix/sysv/linux/raise.c: No such file or directory.

for me with backtrace:

#0  0x00007ffff402b428 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:54
#1  0x00007ffff402d02a in __GI_abort () at abort.c:89
#2  0x00007ffff496e84d in __gnu_cxx::__verbose_terminate_handler() () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#3  0x00007ffff496c6b6 in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x00007ffff496c701 in std::terminate() () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x00007ffff496c919 in __cxa_throw () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007ffff7aeb748 in (anonymous namespace)::checkParameterSize (parameter=std::vector of length 0, capacity 0, expected_size=@0x7fffffffc1e8: 7, name="JointVelTermInfo targets", apply_first=@0x7fffffffc350: true)
    at /home/user/tesseract_ws/src/trajopt_ros/trajopt/src/problem_description.cpp:79
#7  0x00007ffff7af806a in trajopt::JointVelTermInfo::hatch (this=0xb9bfe0, prob=...) at /home/user/tesseract_ws/src/trajopt_ros/trajopt/src/problem_description.cpp:889
#8  0x00007ffff7af085d in trajopt::ConstructProblem (pci=...) at /home/user/tesseract_ws/src/trajopt_ros/trajopt/src/problem_description.cpp:475
#9  0x000000000054ecd3 in main (argc=1, argv=0x7fffffffd448) at /home/user/tesseract_ws/src/trajopt_ros/trajopt_examples/src/puzzle_piece_plan.cpp:246

and if I set ->targets = std::vector<double>(7, 0.) for the velocity, acceleration and jerk costs (which I believe would be the thing to do based on your comment in #55) I get:

[ WARN] [1549555865.408898734] [/trajopt_examples_puzzle_piece_plan] [ros.trajopt]: Last time step for JointVelTerm comes before first step. Reversing them.
terminate called after throwing an instance of 'std::out_of_range'
  what():  vector::_M_range_check: __n (which is 18446744073664824746) >= this->size() (which is 3059)
Thread 1 "trajopt_example" received signal SIGABRT, Aborted.
0x00007ffff402b428 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:54
54      ../sysdeps/unix/sysv/linux/raise.c: No such file or directory.

Which looks like somewhere, something/one is trying to use some uninitialised memory/variable.

Backtrace:

#0  0x00007ffff402b428 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:54
#1  0x00007ffff402d02a in __GI_abort () at abort.c:89
#2  0x00007ffff496e84d in __gnu_cxx::__verbose_terminate_handler() () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#3  0x00007ffff496c6b6 in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x00007ffff496c701 in std::terminate() () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x00007ffff496c919 in __cxa_throw () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007ffff49953f7 in std::__throw_out_of_range_fmt(char const*, ...) () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#7  0x00007ffff7a8aee8 in std::vector<sco::Var, std::allocator<sco::Var> >::_M_range_check (this=0x7fffffffc358, __n=18446744073664824746) at /usr/include/c++/5/bits/stl_vector.h:803
#8  0x00007ffff7a87feb in std::vector<sco::Var, std::allocator<sco::Var> >::at (this=0x7fffffffc358, __n=18446744073664824746) at /usr/include/c++/5/bits/stl_vector.h:842
#9  0x00007ffff7a85fc9 in util::BasicArray<sco::Var>::operator() (this=0x7fffffffc350, row=-1233523066, col=0) at /home/user/tesseract_ws/src/trajopt_ros/trajopt_utils/include/trajopt_utils/basic_array.hpp:69
#10 0x00007ffff7a7df54 in trajopt::JointAccEqCost::JointAccEqCost (this=0x11710d0, vars=..., coeffs=..., targets=..., first_step=@0xb9c340: -1233523066, last_step=@0xb9c344: -1076845087)
    at /home/user/tesseract_ws/src/trajopt_ros/trajopt/src/trajectory_costs.cpp:488
#11 0x00007ffff7afa037 in trajopt::JointAccTermInfo::hatch (this=0xb9c2b0, prob=...) at /home/user/tesseract_ws/src/trajopt_ros/trajopt/src/problem_description.cpp:1036
#12 0x00007ffff7af085d in trajopt::ConstructProblem (pci=...) at /home/user/tesseract_ws/src/trajopt_ros/trajopt/src/problem_description.cpp:475
#13 0x000000000054ee98 in main (argc=1, argv=0x7fffffffd448) at /home/user/tesseract_ws/src/trajopt_ros/trajopt_examples/src/puzzle_piece_plan.cpp:246

@mpowelson
Copy link
Contributor Author

Try #73. I ran all of the examples with that, and they all ran except basic_cartesian (which has not worked for me in long time)

@gavanderhoorn
Copy link
Contributor

Has improved things a bit.

I still run into the second crash every now and then though:

terminate called after throwing an instance of 'std::out_of_range'
  what():  vector::_M_range_check: __n (which is 18446744072327639336) >= this->size() (which is 3059)

with backtrace:

#0  0x00007ffff402b428 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:54
#1  0x00007ffff402d02a in __GI_abort () at abort.c:89
#2  0x00007ffff496e84d in __gnu_cxx::__verbose_terminate_handler() () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#3  0x00007ffff496c6b6 in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x00007ffff496c701 in std::terminate() () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x00007ffff496c919 in __cxa_throw () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007ffff49953f7 in std::__throw_out_of_range_fmt(char const*, ...) () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#7  0x00007ffff7a8aee8 in std::vector<sco::Var, std::allocator<sco::Var> >::_M_range_check (this=0x7fffffffc358, __n=18446744072327639336) at /usr/include/c++/5/bits/stl_vector.h:803
#8  0x00007ffff7a87feb in std::vector<sco::Var, std::allocator<sco::Var> >::at (this=0x7fffffffc358, __n=18446744072327639336) at /usr/include/c++/5/bits/stl_vector.h:842
#9  0x00007ffff7a85fc9 in util::BasicArray<sco::Var>::operator() (this=0x7fffffffc350, row=-197416040, col=0) at /home/user/tesseract_ws/src/trajopt_ros/trajopt_utils/include/trajopt_utils/basic_array.hpp:69
#10 0x00007ffff7a7df54 in trajopt::JointAccEqCost::JointAccEqCost (this=0x10cba00, vars=..., coeffs=..., targets=..., first_step=@0xc63c60: -197416040, last_step=@0xc63c64: 436)
    at /home/user/tesseract_ws/src/trajopt_ros/trajopt/src/trajectory_costs.cpp:488
#11 0x00007ffff7afa037 in trajopt::JointAccTermInfo::hatch (this=0xc63bd0, prob=...) at /home/user/tesseract_ws/src/trajopt_ros/trajopt/src/problem_description.cpp:1036
#12 0x00007ffff7af085d in trajopt::ConstructProblem (pci=...) at /home/user/tesseract_ws/src/trajopt_ros/trajopt/src/problem_description.cpp:475
#13 0x000000000054ee98 in main (argc=1, argv=0x7fffffffd448) at /home/user/tesseract_ws/src/trajopt_ros/trajopt_examples/src/puzzle_piece_plan.cpp:246

Haven't been able to follow that yet into trajopt.

Fact that it doesn't always crash seems to point to use of uninitialised memory (sometimes within supported range, sometime outside it).

@gavanderhoorn
Copy link
Contributor

gavanderhoorn commented Feb 8, 2019

Ok. Seems to be crashing in JointAccTermInfo::hatch(TrajOptProb& prob) because first_step appears to be uninitialised if (I assume) JointAccTermInfo::fromJson(..) has not been called before.

It is then passed here to the JointAccEqCost(..) ctor, which uses it as one of the boundaries for a for-loop here, which tries to index into the vars array it has also been passed here, but seeing as it's uninitialised, fails (as it's some huge negative or positive value).

This is the puzzle_piece_plan.cpp demo, which doesn't appear to use the "json method" to initialise the problem (in contrast to fi the cartesian plan demo).

Is this an oversight or are these initialisation approaches different on purpose?


After making sure all integer members of the costs/term types being used by the puzzle piece demo are value initialised, it no longer crashes. Not sure whether it does what it is supposed to, but it I get a "woo-hoo!" from trajopt ..

@gavanderhoorn
Copy link
Contributor

Just saw #68. That was probably what bit me.

@mpowelson
Copy link
Contributor Author

Ah. I knew there was something else I wanted to fix. I'll fix that too.

@gavanderhoorn
Copy link
Contributor

From a cursory look it seems quite a few classes don't properly initialise member variables.

Might be an idea to do a proper review of that -- if TrajOpt is going to be used much more.

@mpowelson mpowelson added bug Something isn't working enhancement New feature or request labels Feb 14, 2019
@dmillard
Copy link

Hi, I'm interested in enforcing joint jerk limits in an optimizing trajectory framework, and this seems like my best shot for now. I see that there is a TermInfo struct for joint jerk limits using finite differencing, but it doesn't seem to support TT_USE_TIME.

  1. Am I correct that TT_USE_TIME is what I need to enforce a maximum/minimum jerk limit across the entire trajectory?
    1a. What is the current struct for if not constrained jerk? I'd assume it's for minimizing jerk across the entire trajectory as a cost term (but not enforcing a hard constraint). Is that correct?
  2. What is missing from TT_USE_TIME being supported in jerk terms? I'm happy to do the work and submit a PR (and for acc too, I suppose).

@Levi-Armstrong
Copy link
Contributor

@mpowelson Add Generic Interface:

Another option would be to have a single type eraser for collision, kinematics and environment.

TrajOpt Requires:

  • Collision
    • Collision Results (Type Eraser)
      • double distance;
      • sting[2] link_names
      • Eigen::Vector3d[2] nearest_points
      • Eigen::Vector3d normal
      • Eigen::Vector3d[2] cc_nearest_points
      • double cc_time
    • Collision (Type Eraser)
      • Set link transforms
      • Contact Test method return a vector of collision results
      • Must have a clone method
  • Kinematics (Type Eraser)
    • calcJacobian(MatrixXd(Returned Jacobian), VectorXd(Joint Values), String(Link Name))
      • This will be changed such that it does not require Adjacency Pair, Change Base or change reference point. These should be self contained in your kinematics type eraser.
      • The function should now return a jacobian with a base at world and the reference point is the link name provided.
    • calcFwdKin(Eigen::Isometry3d(Returned), VectorXd(Joint Values), String(Link Name))
    • Must have a clone method
  • Environment (Type Eraser)
    • Calculate states of the environment given joint values and return a map of <link name, transform>
    • Must have a clone method

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
bug Something isn't working enhancement New feature or request
Projects
None yet
Development

No branches or pull requests

4 participants