search

hungpham2511 / toppra

robotic motion planning library
https://hungpham2511.github.io/toppra/index.html
MIT License
603 stars 167 forks source link

Error during building of the project, which uses toppra #194

Closed Gryogor closed 2 years ago

Gryogor commented 2 years ago

During the build of catkin ros project the following error occurs.

Errors     << toppra_trajectory:make /home/gryogor/catkin_robobar_ws/logs/toppra_trajectory/build.make.013.log                                                                                                    
In file included from /usr/include/x86_64-linux-gnu/c++/9/bits/c++allocator.h:33,
                 from /usr/include/c++/9/bits/allocator.h:46,
                 from /usr/include/c++/9/string:41,
                 from /usr/include/c++/9/bits/locale_classes.h:40,
                 from /usr/include/c++/9/bits/ios_base.h:41,
                 from /usr/include/c++/9/ios:42,
                 from /usr/include/c++/9/ostream:38,
                 from /usr/include/c++/9/iostream:39,
                 from /home/gryogor/catkin_robobar_ws/src/robobar/toppra_trajectory/src/toppra_trajectory.cpp:1:
/usr/include/c++/9/ext/new_allocator.h: In instantiation of ‘void __gnu_cxx::new_allocator<_Tp>::construct(_Up*, _Args&& ...) [with _Up = toppra::PiecewisePolyPath; _Args = {std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >&, std::vector<int, std::allocator<int> >&}; _Tp = toppra::PiecewisePolyPath]’:
/usr/include/c++/9/bits/alloc_traits.h:482:2:   required from ‘static void std::allocator_traits<std::allocator<_CharT> >::construct(std::allocator_traits<std::allocator<_CharT> >::allocator_type&, _Up*, _Args&& ...) [with _Up = toppra::PiecewisePolyPath; _Args = {std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >&, std::vector<int, std::allocator<int> >&}; _Tp = toppra::PiecewisePolyPath; std::allocator_traits<std::allocator<_CharT> >::allocator_type = std::allocator<toppra::PiecewisePolyPath>]’
/usr/include/c++/9/bits/shared_ptr_base.h:548:39:   required from ‘std::_Sp_counted_ptr_inplace<_Tp, _Alloc, _Lp>::_Sp_counted_ptr_inplace(_Alloc, _Args&& ...) [with _Args = {std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >&, std::vector<int, std::allocator<int> >&}; _Tp = toppra::PiecewisePolyPath; _Alloc = std::allocator<toppra::PiecewisePolyPath>; __gnu_cxx::_Lock_policy _Lp = __gnu_cxx::_S_atomic]’
/usr/include/c++/9/bits/shared_ptr_base.h:679:16:   required from ‘std::__shared_count<_Lp>::__shared_count(_Tp*&, std::_Sp_alloc_shared_tag<_Alloc>, _Args&& ...) [with _Tp = toppra::PiecewisePolyPath; _Alloc = std::allocator<toppra::PiecewisePolyPath>; _Args = {std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >&, std::vector<int, std::allocator<int> >&}; __gnu_cxx::_Lock_policy _Lp = __gnu_cxx::_S_atomic]’
/usr/include/c++/9/bits/shared_ptr_base.h:1344:71:   required from ‘std::__shared_ptr<_Tp, _Lp>::__shared_ptr(std::_Sp_alloc_shared_tag<_Tp>, _Args&& ...) [with _Alloc = std::allocator<toppra::PiecewisePolyPath>; _Args = {std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >&, std::vector<int, std::allocator<int> >&}; _Tp = toppra::PiecewisePolyPath; __gnu_cxx::_Lock_policy _Lp = __gnu_cxx::_S_atomic]’
/usr/include/c++/9/bits/shared_ptr.h:359:59:   required from ‘std::shared_ptr<_Tp>::shared_ptr(std::_Sp_alloc_shared_tag<_Tp>, _Args&& ...) [with _Alloc = std::allocator<toppra::PiecewisePolyPath>; _Args = {std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >&, std::vector<int, std::allocator<int> >&}; _Tp = toppra::PiecewisePolyPath]’
/usr/include/c++/9/bits/shared_ptr.h:701:14:   required from ‘std::shared_ptr<_Tp> std::allocate_shared(const _Alloc&, _Args&& ...) [with _Tp = toppra::PiecewisePolyPath; _Alloc = std::allocator<toppra::PiecewisePolyPath>; _Args = {std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >&, std::vector<int, std::allocator<int> >&}]’
/usr/include/c++/9/bits/shared_ptr.h:717:39:   required from ‘std::shared_ptr<_Tp> std::make_shared(_Args&& ...) [with _Tp = toppra::PiecewisePolyPath; _Args = {std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >&, std::vector<int, std::allocator<int> >&}]’
/home/gryogor/catkin_robobar_ws/src/robobar/toppra_trajectory/src/toppra_trajectory.cpp:134:91:   required from here
/usr/include/c++/9/ext/new_allocator.h:145:20: error: no matching function for call to ‘toppra::PiecewisePolyPath::PiecewisePolyPath(std::vector<Eigen::Matrix<double, -1, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, -1, 1> > >&, std::vector<int>&)’
  145 |  noexcept(noexcept(::new((void *)__p)
      |                    ^~~~~~~~~~~~~~~~~~
  146 |        _Up(std::forward<_Args>(__args)...)))
      |        ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Here is the CMakeLists.txt file:

cmake_minimum_required(VERSION 3.0.2)
project(toppra_trajectory)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  moveit_core
  moveit_ros_planning_interface
  geometry_msgs
)

find_package(Boost REQUIRED COMPONENTS system)
find_package(Eigen3)
find_package(toppra)

include_directories(${EIGEN_INCLUDE_DIRS})

catkin_package(

)

include_directories(

  ${catkin_INCLUDE_DIRS}
)

add_executable(toppra_trajectory_optimizer src/toppra_trajectory.cpp)
target_link_libraries(toppra_trajectory_optimizer 
  PUBLIC toppra::toppra
  ${catkin_LIBRARIES})

and the code:

#include <iostream>
#include <numeric>

#include "ros/ros.h"

#include <toppra/algorithm/toppra.hpp>
#include <toppra/constraint/linear_joint_acceleration.hpp>
#include <toppra/constraint/linear_joint_velocity.hpp>
#include <toppra/geometric_path/piecewise_poly_path.hpp>
#include <toppra/parametrizer/const_accel.hpp>
#include <toppra/toppra.hpp>

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

static const std::string PLANNING_GROUP = "panda_arm";
bool _recieved = false;
geometry_msgs::Pose target_pose;
void targetCb(geometry_msgs::Pose);

double length(const std::vector<double> &q0, const std::vector<double> &q1)
{
    auto dof = q0.size();
    double retval = 0;

    for (size_t i = 0; i < dof; i++) {
        retval += (q0[i] - q1[i]) * (q0[i] - q1[i]);
    }
    return sqrt(retval) / dof;
}
inline toppra::Vectors makeVectors(
    const std::vector<std::vector<toppra::value_type>> &v)
{
    toppra::Vectors ret;
    for (auto vi : v) {
        toppra::Vector vi_eigen(vi.size());
        for (std::size_t i = 0; i < vi.size(); i++) {
            vi_eigen(i) = vi[i];
        }
        ret.push_back(vi_eigen);
    }
    return ret;
}

inline toppra::BoundaryCond makeBoundaryCond(
    int order, const std::vector<toppra::value_type> &values)
{
    toppra::BoundaryCond cond;
    cond.order = order;
    cond.values.resize(values.size());
    for (std::size_t i = 0; i < values.size(); i++) {
        cond.values(i) = values[i];
    }
    return cond;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "moveit_toppra_trajectory_optimizer");
  ros::NodeHandle _nh;

  ros::Subscriber sub = _nh.subscribe("target_pose", 1, targetCb);

  ros::AsyncSpinner spinner(2);  
  spinner.start();

  moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  const moveit::core::JointModelGroup* joint_model_group =
    move_group_interface.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

while (ros::ok())
{
  if (_recieved)
  {
    move_group_interface.setPoseTarget(target_pose);
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    std:: cout << "Result: " << success << std::endl;
    std::cout << my_plan.trajectory_ << std::endl;

    /*
     *
     *
     * Feed trajectory to toppra and get it back
     * 
     * 
     */

    int plan_size = my_plan.trajectory_.joint_trajectory.points.size();

    std::vector<std::vector<double>> joint_positions;
    for (int i = 0; i < plan_size; i++)
    {
      joint_positions.push_back(my_plan.trajectory_.joint_trajectory.points[i].positions);
    }

    for (std::vector<double> x : joint_positions)
    {
      for (double j : x)
      {
        std::cout << j << '\t';
      }
      std::cout << std::endl;
    }
    _recieved = false;

    toppra::Vector times;
    int dof = 7;
    int numPoints = joint_positions.size();
    times.resize(numPoints);
    times(0) = 0.;
    for (int i = 1; i < numPoints; i++)
    {
      times(i) = times(i - 1) + length(joint_positions[i - 1], joint_positions[i]);
    }

    toppra::BoundaryCond bc = makeBoundaryCond(3, { 0, 0, 0, 0, 0, 0, 0 });
    std::array<toppra::BoundaryCond, 2> bc_type{ bc, bc };

    auto toppra_joint_positions = makeVectors(joint_positions);

    //auto path = std::make_shared<toppra::PiecewisePolyPath>(toppra_joint_positions, times, bc_type);

    std::vector<int> points(joint_positions.size());
    std::iota (std::begin(points), std::end(points), 0); 
    auto path = std::make_shared<toppra::PiecewisePolyPath>(toppra_joint_positions, points);

    /* ^^^^^^^^^^^^^^^^^^^^^^
     * both of them give an error
     *
     * 
     * 
     */

    auto velLimitLower = -2.1750 * toppra::Vector::Ones(dof);
    auto velLimitUpper = 2.1750 * toppra::Vector::Ones(dof);
    auto accLimitLower = -1.8750 * toppra::Vector::Ones(dof);
    auto accLimitUpper = 1.8750 * toppra::Vector::Ones(dof);

//    std::vector<double> velLimit = {2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100};
//    std::vector<double> accLimit = {3.75, 1.875, 2.5, 3.125, 3.75, 5.0, 5.0};

//    ^^^
//    later find how to work with toppra's vectors

    toppra::LinearConstraintPtr ljv, lja;
    ljv = std::make_shared<toppra::constraint::LinearJointVelocity>
      (velLimitLower, velLimitUpper);
    lja = std::make_shared<toppra::constraint::LinearJointAcceleration>
      (accLimitLower, accLimitUpper);

    toppra::LinearConstraintPtrs constrains =
      toppra::LinearConstraintPtrs{ljv, lja};

    toppra::PathParametrizationAlgorithmPtr algo =
      std::make_shared<toppra::algorithm::TOPPRA>(constrains, path);

    toppra::ReturnCode rc1 = algo->computePathParametrization(0, 0);

    std::cout << "rc1 = " << int(rc1) << std::endl;
    toppra::ParametrizationData pd = algo->getParameterizationData();

    //all info
    // \/\/\/\/
    std::cout << "pd.gridpoints \n " << pd.gridpoints.transpose() << std::endl;
    std::cout << "pd.parametrization \n " << pd.parametrization.transpose() << std::endl;
    std::cout << "pd.controllable_sets \n " << pd.controllable_sets.transpose() << std::endl;
    std::cout << "pd.feasible_sets \n " << pd.feasible_sets.transpose() << std::endl;
    std::cout << "pd.ret_code = " << int(pd.ret_code) << std::endl;

    // move_group_interface.execute(my_plan)
    // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    //toppra plan instead
  }
}

  return 0;
}

void targetCb(geometry_msgs::Pose msg)
{
  target_pose = msg;
  _recieved = true;
}

Version Develop branch

jmirabel commented 2 years ago

The error message is rather clear:

/usr/include/c++/9/ext/new_allocator.h:145:20: error: no matching function for call to ‘toppra::PiecewisePolyPath::PiecewisePolyPath(std::vector<Eigen::Matrix<double, -1, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, -1, 1> > >&, std::vector<int>&)’

There is no constructor that accepts the argument you provide. The bug is on your side.