akjay
commented
4 years ago 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;
I use industrial_moveit_test_moveit_config pkg in industrial_moveit to learn about stomp planner. The problem definition is below:
While the problem is about to happen in step 4.At this step, the problem can be describe as:
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 bycomputeInitialTrajectory.Here is some logs,based on the plan problem, the functioncomputeInitialTrajectorywill return a initialTrajectory but maybe in collision,but somehow, this trajectory has a good result like this: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: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.