Stomp::solve() return a invalid trajectory

[akjay]

I use industrial_moveit_test_moveit_config pkg in industrial_moveit to learn about stomp planner.
The problem definition is below:

1.Use group manipulator_tool;
2.Initial start from all joint 0 point;
3.First target is setted by name 'start_tool_pose';
4.After step 3, then set target to 'end_tool_pose';

While the problem is about to happen in step 4.At this step, the problem can be describe as:

[ INFO] [1596591035.929044645]: Start joint value: 1.3483 1.1972 -0.3137 0.676 -1.0295 -1.2011 -1.51
[ INFO] [1596591035.929063378]: Goal joint value: -0.8539 1.5237 -0.3137 0.676 -3.2945 -1.2011 -1.51

When I call move_group.plan() to plan this trajectory, always get no solution.So I review the source code try to figure out what's wrong with this stage.Finally I find things seemed happen in function bool Stomp::solve(const Eigen::MatrixXd& initial_parameters, Eigen::MatrixXd& parameters_optimized), it seems that this function is been used to optimize the initial solution got by computeInitialTrajectory.Here is some logs,based on the plan problem, the function computeInitialTrajectory will return a initialTrajectory but maybe in collision,but somehow, this trajectory has a good result like this:

1.3483    1.32851    1.30837    1.28185    1.24931    1.21111    1.16762    1.11921    1.06623    1.00906   0.948059   0.883589   0.816018    0.74571    0.67303   0.598343   0.522014   0.444408    0.36589   0.286824   0.207576    0.12851  0.0499921 -0.0276139  -0.103943   -0.17863   -0.25131  -0.321618  -0.389189  -0.453659  -0.514662  -0.571834  -0.624809  -0.673224  -0.716712  -0.754909  -0.787451  -0.813971  -0.834107    -0.8539
1.1972    1.20013    1.20312    1.20705    1.21188    1.21754    1.22399    1.23117    1.23902     1.2475    1.25654     1.2661    1.27612    1.28654    1.29732    1.30839    1.31971    1.33121    1.34285    1.35458    1.36632    1.37805    1.38969    1.40119    1.41251    1.42358    1.43436    1.44478     1.4548    1.46436     1.4734    1.48188    1.48973    1.49691    1.50336    1.50902    1.51385    1.51778    1.52077     1.5237
-0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137    -0.3137
0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676      0.676
-1.0295   -1.04986   -1.07057   -1.09784   -1.13131    -1.1706   -1.21533   -1.26512   -1.31961   -1.37841   -1.44115   -1.50746   -1.57696   -1.64927   -1.72403   -1.80084   -1.87935   -1.95917   -2.03993   -2.12125   -2.20275   -2.28407   -2.36483   -2.44465   -2.52316   -2.59997   -2.67473   -2.74704   -2.81654   -2.88285   -2.94559   -3.00439   -3.05888   -3.10867    -3.1534   -3.19269   -3.22616   -3.25343   -3.27414    -3.2945
-1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011    -1.2011
-1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51      -1.51

We can see, the first point of this trajectory is the start point and same as the last one.But this trajectory is a contact between robot and obstacle,so the planner will call bool Stomp::solve(const Eigen::MatrixXd& initial_parameters,Eigen::MatrixXd& parameters_optimized) to optimize the trajectory, here the initial_parameters is the trajecory shown above, parameters_optimized is the optimized trajectory, after this function called, I print the final trajectory:

1.98952e-14    0.494402    0.857336     1.10962        1.27     1.35528      1.3804     1.35854     1.30119     1.21831     1.11835      1.0084    0.894285    0.780634    0.670998    0.567942    0.473143    0.387489     0.31117    0.243785    0.184429    0.131797   0.0842818   0.0400653 -0.00277837  -0.0461884  -0.0920192   -0.141943   -0.197354   -0.259268   -0.328228   -0.404207   -0.486507   -0.573667   -0.663362   -0.752305   -0.836155   -0.909413    -0.96533   -0.995807
3.22886      2.4082     1.78484     1.32386    0.994289     0.76894    0.624154    0.539603     0.49807     0.48524     0.48948    0.501628     0.51478     0.52407    0.526462    0.520533    0.506256     0.48479    0.458265    0.429564    0.402112    0.379662    0.366078    0.365122    0.380243    0.414354    0.469627    0.547275    0.647334    0.768456    0.907687     1.06026     1.21937     1.37598     1.51859    1.633     1.70216     1.70589     1.62071      1.4196
1.48353    0.779714    0.260294   -0.105699   -0.346145   -0.485974   -0.547312   -0.549625   -0.509857   -0.442576   -0.360116   -0.272717   -0.188673    -0.11447  -0.0549299  -0.0133526  0.00833966  0.00946134 -0.00957954   -0.047424   -0.101905   -0.170188   -0.248918   -0.334357   -0.422531   -0.509369   -0.590847   -0.663131   -0.722722   -0.766592   -0.785398   -0.785398   -0.783734    -0.74895   -0.695428   -0.625988    -0.54492   -0.458132   -0.373289   -0.299956
-1.27275   -0.487904   0.0782014    0.465438    0.709251    0.840887    0.887635    0.873047    0.817177    0.736808    0.645684    0.554742    0.472342    0.404497    0.355106    0.326186    0.318099    0.329788    0.359004    0.402538    0.456455    0.516321    0.577437    0.635067    0.684674    0.722145    0.744027    0.747755    0.731886    0.696326    0.642564    0.573903    0.495692    0.415551    0.343612     0.29274    0.278771    0.320741    0.441116    0.666025
0.0091686   -0.238875   -0.394872   -0.476564   -0.500016   -0.479681   -0.428464    -0.35779   -0.277669   -0.196759   -0.122436  -0.0608528  -0.0170108  0.00517856  0.00282674  -0.0259967  -0.0823297   -0.166383   -0.277606   -0.414751   -0.575939   -0.758728   -0.960172    -1.17689    -1.40514    -1.64087    -1.87978    -2.11741    -2.34919    -2.57051    -2.77677    -2.96347    -3.12627    -3.26102    -3.36389    -3.43138     -3.4604    -3.44837    -3.39322    -3.29352
1.80942    0.678001   -0.160429   -0.755884    -1.15316     -1.3921    -1.50781    -1.53098    -1.48808    -1.40168    -1.29063     -1.1704    -1.05328   -0.948658   -0.863286   -0.801522   -0.765596   -0.755868   -0.771083   -0.808632   -0.864805   -0.935054    -1.01425    -1.09694    -1.17759    -1.25089    -1.31193    -1.35657    -1.38157    -1.38495    -1.36621    -1.32657    -1.26927    -1.19977    -1.12608    -1.05894    -1.01216     -1.0028    -1.05147    -1.18259
0.16035   -0.495766   -0.982872    -1.33054    -1.56512    -1.70992    -1.78534    -1.80907    -1.79625    -1.75963    -1.70975    -1.65508    -1.60221    -1.55602    -1.51984    -1.49559    -1.48399    -1.48472    -1.49655    -1.51754    -1.54521    -1.57669    -1.60889    -1.63865    -1.66297     -1.6791    -1.68475    -1.67824    -1.65869    -1.62614    -1.58178    -1.52808    -1.46893    -1.40987    -1.35821    -1.32322    -1.31628    -1.35106    -1.44367    -1.61286

Neither the first point nor the last one is the same with my problem define, so this trajectory can't be execute because the first point is different with the robot current state, so execute it will cause an error.
But I'm confused that this could only happen in step 4, which means if my robot initial pose is all joint set to be 0, like step 2, then you will get a right trajectory(step 3), otherwise, the STOMP failed to find a soluiton (step 4).
All the job is doing with the default config parameter.

[akjay]

Here is the test code.

  ros::init(argc, argv, "stomp_named_target");
  ros::NodeHandle nh;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  moveit::planning_interface::MoveGroupInterface move_group("manipulator_tool");
  moveit::planning_interface::MoveGroupInterface::Plan plan;
  moveit::planning_interface::MoveItErrorCode ret;

  move_group.setNamedTarget("start_tool_pose");
  ret = move_group.plan(plan);
  if(ret)
    move_group.execute(plan);
  else
    ROS_WARN("plan failed");
  move_group.setNamedTarget("end_tool_pose");
  ret = move_group.plan(plan);
  if(ret)
    move_group.execute(plan);
  else
    ROS_WARN("plan failed");
  ros::shutdown();

  return 0;

[akjay]

Maybe something wrong in this function, I still working on it.bool Stomp::runSingleIteration()

[akjay]

I have post a PR to fix this.Here