trajopt icon indicating copy to clipboard operation
trajopt copied to clipboard

Published 20 hours ago • verified publisher icon tesseract-robotics

TrajOpt Development TODO List

Open mpowelson opened this issue 6 years ago • 11 comments

  • [ ] Implement Conical Constraint
    • [ ] #6 Implementation
    • [ ] Look into swing-twist decomposition. See stack overflow question 3684269
  • [ ] Dynamic Variable Creation
  • [ ] Time parameterization
    • [x] #51
    • [ ] Add joint accel, jerk
    • [ ] Cartesian terms
  • [ ] Remove optimizer and replace with plugin interface (or other framework as discussed in #67 )
  • [ ] Issue tracker and bug fixes
    • [ ] #33 - Add const functions in OptProb
    • [ ] #46 - Differentiation on small trajectories
    • [ ] #12 - Add easily accessible documentation
    • [ ] Clean up stale/resolved issues and address others
    • [x] Update doxygen on costs - specifically default values
    • [x] #55 Fix examples with init_info
    • [x] Add TT_CNT to collision
    • [ ] Check that safety margin vector is the right size
    • [x] Add unit test that runs examples and makes sure they don't crash
    • [ ] Make sure that all C++ functionality is exposed in json interface
  • [ ] Review examples
  • [ ] Clean up code - break problem description up
  • [x] Update Clang in CI from 5.0 to 7.0
  • [ ] Add generic interface to eliminate dependency on Tesseract (comment)

mpowelson avatar Jan 28 '19 16:01 mpowelson

@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)?

gavanderhoorn avatar Feb 07 '19 15:02 gavanderhoorn

@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

  • Fix #55
  • Make sure they are all up to date (for instance we now have JOINT_INTERPOLATED initialization, etc)
  • Review them and see if we need to add any more

mpowelson avatar Feb 07 '19 15:02 mpowelson

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

gavanderhoorn avatar Feb 07 '19 16:02 gavanderhoorn

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)

mpowelson avatar Feb 07 '19 17:02 mpowelson

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 avatar Feb 08 '19 12:02 gavanderhoorn

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 avatar Feb 08 '19 13:02 gavanderhoorn

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

gavanderhoorn avatar Feb 08 '19 13:02 gavanderhoorn

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

mpowelson avatar Feb 08 '19 14:02 mpowelson

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.

gavanderhoorn avatar Feb 08 '19 14:02 gavanderhoorn

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).

dmillard avatar Jun 13 '19 21:06 dmillard

@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

Levi-Armstrong avatar Oct 17 '19 13:10 Levi-Armstrong