moveit
moveit copied to clipboard
ros-planning
Trajopt with no dependency to tesseract
The necessary files are copied and edited so they only depend on MoveIt. This is a continuation of #1593 .
Is this including the changes in #1593? I don't think it does, and would be much easier to review if it did.
Is this including the changes in #1593? I don't think it does, and would be much easier to review if it did.
It does, I think the reason that you do not see the changes compared to #1593 is that files are in different directories. But they definitely have the changes from the branch that is merged in, that was the whole point of it.
the reason that you do not see the changes compared to #1593 is that files are in different directories
I see, that makes sense, and is fine. And sorry, what I meant to ask is if this PR is rebased on the master post-#1593 merge. If it was, all of the changes would show up as renamed or moved, not as added. Please rebase on master.
the reason that you do not see the changes compared to #1593 is that files are in different directories
I see, that makes sense, and is fine. And sorry, what I meant to ask is if this PR is rebased on the master post-#1593 merge. If it was, all of the changes would show up as renamed or moved, not as added. Please rebase on master.
Rebased it.
@ommmid @davetcoleman I would remove bpmpd before merging this, for licensing reasons. or at least include a license file for it, i.e. http://www.netlib.org/ampl/solvers/bpmpd/README
Yes, we should definitely remove that .so file
Don't recommend. BPMPD is the core optimizer for TrajOpt, it won't work without it. I think SWRI has been trying to use other optimizers, but I don't know how efficient they are.
The original TrajOpt impl is BSD-2 and included this .so, so I think including the README should be fine.
@BryceStevenWilley a couple of other points to the discussion:bpmpd is indeed the free solver that at the moment works best with the nonlinear optimization routine implemented as in the paper. Still, it's an old, unmaintained solver, with known bugs and a flaky interface implementation. I personally implemented the osqp and qpOASES backends for trajopt, admittedly with mixed results in certain cases. At SWRI they will move towards a more robust implementation of the overall optimization scheme, but for now I would, if anything, put a little bit of extra effort and try ipopt instead of bpmpd (both interior point methods) as the standard free solver (gurobi works nicely, by the way).
I agree with @arocchi that BPMPD is problematic as the default solver, mostly because of license issues (or rather, the lack of any license). ipopt seems like a reasonable default choice:
- It's actively maintained.
- It has a permissive license (Eclipse Public License).
- It's easy to install on currently supported versions of Ubuntu: https://packages.ubuntu.com/source/bionic/coinor-ipopt
I don't know how similar the BPMPD and Ipopt API's are, though.
After digging for a while in the last Travis errors I think they are related to panda_moveit_config -- trajopt_planning.yaml added after the last panda_moveit_config sync. and Travis don't install it from source please see Travis logs L516 -- releasing a new version of the package should fix the errors
This's how I reproduced it by running the test manually
cd path-to-trajopt/trajopt_interface
mkdir build && cd build
cmake .. && make && make run_tests
gives
roslaunch.core.RLException: error loading <rosparam> tag:
file does not exist [/opt/ros/melodic/share/panda_moveit_config/config/trajopt_planning.yaml]
XML is <rosparam command="load" file="$(find panda_moveit_config)/config/trajopt_planning.yaml"/>
I added the yaml file manually and the test passes
@JafarAbdi , Right, We do have trajopt_planning.yaml file in panda_moveit_config merged in (melodic-devel branch) : https://github.com/ros-planning/panda_moveit_config/tree/melodic-devel/config but the debian should have a new release.
With the last commit, OSQP is the default solver that is downloaded and installed in build directory. The version of OSQP whose API works with the osqp interface in MoveIt is: https://github.com/oxfordcontrol/osqp/tree/release-0.5.0
@ommmid can you make sure the CI checks pass?
we need a new debian release for panda_moveit_config. that would resolve the issue
Are you sure that this is the issue? I looked at the Travis logs (reran the Travis CI build today, just to make sure) and didn't see any mention of the panda_moveit_config. Instead, I see a cryptic message like this:
Test log of build/moveit_planners_trajopt/test_results/moveit_planners_trajopt/MISSING-rostest-test_trajectory_test.xml
2177<?xml version="1.0" encoding="UTF-8"?>
2178<testsuite tests="1" failures="1" time="1" errors="0" name="rostest-test_trajectory_test.xml">
2179 <testcase name="test_ran" status="run" time="1" classname="Results">
2180 <failure message="Unable to find test results for rostest-test_trajectory_test.xml, test did not run.
2181Expected results in /root/ros_ws/build/moveit_planners_trajopt/test_results/moveit_planners_trajopt/rostest-test_trajectory_test.xml" type=""/>
2182 </testcase>
(this is from the melodic build). If you are sure it's the Debian release of panda_moveit_config, can you ping the person responsible for creating a new release of this package?
In the meantime, there are still a few compilation warnings you could fix:
Warnings << trajopt_sco:make /root/ros_ws/logs/trajopt_sco/build.make.001.log
1920/root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_sco/test/solver-interface-unit.cpp:15:13: warning: redundant redeclaration of ‘void sco::simplify2(sco::IntVec&, sco::DblVec&)’ in same scope [-Wredundant-decls]
1921 extern void simplify2(IntVec& inds, DblVec& vals);
1922 ^~~~~~~~~
1923In file included from /root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/expr_ops.hpp:3:0,
1924 from /root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_sco/test/solver-interface-unit.cpp:8:
1925/root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/solver_interface.hpp:246:6: note: previous declaration of ‘void sco::simplify2(sco::IntVec&, sco::DblVec&)’
1926 void simplify2(IntVec& inds, DblVec& vals);
1927 ^~~~~~~~~
1928In file included from /root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_sco/test/solver-interface-unit.cpp:4:0:
1929/usr/src/googletest/googletest/include/gtest/gtest.h: In instantiation of ‘testing::AssertionResult testing::internal::CmpHelperEQ(const char*, const char*, const T1&, const T2&) [with T1 = long unsigned int; T2 = int]’:
1930/usr/src/googletest/googletest/include/gtest/gtest.h:1421:23: required from ‘static testing::AssertionResult testing::internal::EqHelper<lhs_is_null_literal>::Compare(const char*, const char*, const T1&, const T2&) [with T1 = long unsigned int; T2 = int; bool lhs_is_null_literal = false]’
1931/root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_sco/test/solver-interface-unit.cpp:34:3: required from here
1932/usr/src/googletest/googletest/include/gtest/gtest.h:1392:11: warning: comparison between signed and unsigned integer expressions [-Wsign-compare]
1933 if (lhs == rhs) {
1934 ~~~~^~~~~~
and:
Warnings << moveit_planners_trajopt:make /root/ros_ws/logs/moveit_planners_trajopt/build.make.001.log
1984In file included from /root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_interface/test/trajectory_test.cpp:22:0:
1985/usr/src/googletest/googletest/include/gtest/gtest.h: In instantiation of ‘testing::AssertionResult testing::internal::CmpHelperEQ(const char*, const char*, const T1&, const T2&) [with T1 = long unsigned int; T2 = int]’:
1986/usr/src/googletest/googletest/include/gtest/gtest.h:1421:23: required from ‘static testing::AssertionResult testing::internal::EqHelper<lhs_is_null_literal>::Compare(const char*, const char*, const T1&, const T2&) [with T1 = long unsigned int; T2 = int; bool lhs_is_null_literal = false]’
1987/root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_interface/test/trajectory_test.cpp:100:3: required from here
1988/usr/src/googletest/googletest/include/gtest/gtest.h:1392:11: warning: comparison between signed and unsigned integer expressions [-Wsign-compare]
1989 if (lhs == rhs) {
1990 ~~~~^~~~~~
Are you that this is the issue? I looked at the Travis logs (reran the Travis CI build today, just to make sure) and didn't see any mention of the panda_moveit_config. Instead, I see a cryptic message like this:
Test log of build/moveit_planners_trajopt/test_results/moveit_planners_trajopt/MISSING-rostest-test_trajectory_test.xml 2177<?xml version="1.0" encoding="UTF-8"?> 2178<testsuite tests="1" failures="1" time="1" errors="0" name="rostest-test_trajectory_test.xml"> 2179 <testcase name="test_ran" status="run" time="1" classname="Results"> 2180 <failure message="Unable to find test results for rostest-test_trajectory_test.xml, test did not run. 2181Expected results in /root/ros_ws/build/moveit_planners_trajopt/test_results/moveit_planners_trajopt/rostest-test_trajectory_test.xml" type=""/> 2182 </testcase>(this is from the melodic build). If you are sure it's the Debian release of panda_moveit_config, can you ping the person responsible for creating a new release of this package?
In the meantime, there are still a few compilation warnings you could fix:
Warnings << trajopt_sco:make /root/ros_ws/logs/trajopt_sco/build.make.001.log 1920/root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_sco/test/solver-interface-unit.cpp:15:13: warning: redundant redeclaration of ‘void sco::simplify2(sco::IntVec&, sco::DblVec&)’ in same scope [-Wredundant-decls] 1921 extern void simplify2(IntVec& inds, DblVec& vals); 1922 ^~~~~~~~~ 1923In file included from /root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/expr_ops.hpp:3:0, 1924 from /root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_sco/test/solver-interface-unit.cpp:8: 1925/root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_sco/include/trajopt_sco/solver_interface.hpp:246:6: note: previous declaration of ‘void sco::simplify2(sco::IntVec&, sco::DblVec&)’ 1926 void simplify2(IntVec& inds, DblVec& vals); 1927 ^~~~~~~~~ 1928In file included from /root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_sco/test/solver-interface-unit.cpp:4:0: 1929/usr/src/googletest/googletest/include/gtest/gtest.h: In instantiation of ‘testing::AssertionResult testing::internal::CmpHelperEQ(const char*, const char*, const T1&, const T2&) [with T1 = long unsigned int; T2 = int]’: 1930/usr/src/googletest/googletest/include/gtest/gtest.h:1421:23: required from ‘static testing::AssertionResult testing::internal::EqHelper<lhs_is_null_literal>::Compare(const char*, const char*, const T1&, const T2&) [with T1 = long unsigned int; T2 = int; bool lhs_is_null_literal = false]’ 1931/root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_sco/test/solver-interface-unit.cpp:34:3: required from here 1932/usr/src/googletest/googletest/include/gtest/gtest.h:1392:11: warning: comparison between signed and unsigned integer expressions [-Wsign-compare] 1933 if (lhs == rhs) { 1934 ~~~~^~~~~~and:
Warnings << moveit_planners_trajopt:make /root/ros_ws/logs/moveit_planners_trajopt/build.make.001.log 1984In file included from /root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_interface/test/trajectory_test.cpp:22:0: 1985/usr/src/googletest/googletest/include/gtest/gtest.h: In instantiation of ‘testing::AssertionResult testing::internal::CmpHelperEQ(const char*, const char*, const T1&, const T2&) [with T1 = long unsigned int; T2 = int]’: 1986/usr/src/googletest/googletest/include/gtest/gtest.h:1421:23: required from ‘static testing::AssertionResult testing::internal::EqHelper<lhs_is_null_literal>::Compare(const char*, const char*, const T1&, const T2&) [with T1 = long unsigned int; T2 = int; bool lhs_is_null_literal = false]’ 1987/root/ros_ws/src/moveit/moveit_planners/trajopt/trajopt_interface/test/trajectory_test.cpp:100:3: required from here 1988/usr/src/googletest/googletest/include/gtest/gtest.h:1392:11: warning: comparison between signed and unsigned integer expressions [-Wsign-compare] 1989 if (lhs == rhs) { 1990 ~~~~^~~~~~
@JafarAbdi also confirmed that we need a new release. trajectory_test needs the yaml file to load, and if it is not there it gives error
I'm not sure if we have clarity on if @JafarAbdi can start doing releases, need to run it by @rhaschke. I'll move this to a new thread.
With the last commit, OSQP is the default solver that is downloaded and installed in build directory. The version of OSQP whose API works with the osqp interface in MoveIt is: https://github.com/oxfordcontrol/osqp/tree/release-0.5.0
There has been a recent change in trajopt so that osqp 0.6 is supported. Might be good idea to upgrade/fetch osqp_interface from upstream.
If we install osqp from the trajopt_sco and check osqp_FOUND, we will get a false. It can not find it in the same CMakeLists that built it. I put the installing external project in trajopt_utils and used the installed osqp directory in trajopt_sco for checking osqp_FOUND. It works fine now but any other suggestion, @mamoll ?
Is there a chance that you can pick this back up anytime soon @ommmid ? This demo looks so sweet - I would love to see that in our tutorials, too :)
I now receive the following error when trying to build #1626 from source on ROS noetic:
/home/ricks/Development/work/test_panda_moveit_config_ws/src/moveit/moveit_planners/trajopt/trajopt/src/collision_terms.cpp:498:73: error: array must be initialized with a brace-enclosed initializer
498 | Eigen::Vector3d moveit_cc_nearest_points[2] = contact_vector[k].nearest_points;
I tried using this solution but didn't yet find the right syntax. As I currently do now have the time to debug this issue, I will report it here.
System info
OS: Ubuntu 20.04 CATKIN TOOLS: 0.7.1 gcc: 9.3.0
