industrial_trajectory_filters: unexpected values for some resampled points (moveit trajectory)

[kobesdu]

For example, 1.05 second is the 100th point, 1.1 second should be the 101st point, but the result i get is: the 1.1 second point is 101th point(record the point as A) ,and its positions,velocities,accelerations,effort values are right.but,the 1.1 second point also appear after the 1.95 second point,and its its positions,velocities,accelerations,effort values is not the same with the A points. please look at my links above,you may understand my means .that pictures are the value of positions-times

[kobesdu]

moveit planning position

the Industrial_trajectory_filters planning position

the picture X axis is time_from_start y axis is positions

[kobesdu]

i can post my code here

//industrial_trajectory_filters function
industrial_trajectory_filters::MessageAdapter intest,outtest;
intest.request; 
intest.request.trajectory;
intest.request.trajectory.header.stamp = trajectory_msg.joint_trajectory.header.stamp;
intest.request.trajectory.header.frame_id = trajectory_msg.joint_trajectory.header.frame_id;
intest.request.trajectory.joint_names.resize(6);
intest.request.trajectory.points.resize(6);
intest.request.trajectory.joint_names =trajectory_msg.joint_trajectory.joint_names;
intest.request.trajectory.points = trajectory_msg.joint_trajectory.points;  
//ROS_INFO_STREAM("mytest" << intest.request.trajectory);
//save moveit plan （trajectory_msg）
std::ofstream outFile2;
outFile2.open("/home/yifei/moveitfilter_ws/src/myworkcell_core/src/intest.txt");

outFile2<<intest.request.trajectory;
outFile2.close();
industrial_trajectory_filters::UniformSampleFilter <industrial_trajectory_filters::MessageAdapter> uniformsamplefilter;
uniformsamplefilter.configure();
uniformsamplefilter.update(intest,outtest);

//save industrial_trajectory_filters plan（outtest.request.trajectory）
std::ofstream outFile1;
outFile1.open("/home/yifei/moveitfilter_ws/src/myworkcell_core/src/out.txt");
outFile1<<outtest.request.trajectory;
outFile1.close();

[kobesdu]

the moveit plan Result is saved in this file intest.txt the Industrial_trajectory_filters planning plan Result is saved in this file out.txt my code is this file

myworkcell_nodetest.txt

[kobesdu]

and another thing i want to say is ,when i print the result in the file uniform_sample_filter.cpp the result is right .only when it print the industrial_trajectory_filters::MessageAdapte.request.trajectory ,it is wrong. so i think the process of the plan is right,but the trajectory msg is wrong code is this .

                      ROS_DEBUG_STREAM(
                "p1.pos: " << p1.positions[i] << ", vel: " << p1.velocities[i] << ", acc: " << p1.accelerations[i] << ", tfs: " << p1.time_from_start);

            ROS_DEBUG_STREAM(
                "p2.pos: " << p2.positions[i] << ", vel: " << p2.velocities[i] << ", acc: " << p2.accelerations[i] << ", tfs: " << p2.time_from_start);

            ROS_DEBUG_STREAM(
                "interp_pt.pos: " << interp_pt.positions[i] << ", vel: " << interp_pt.velocities[i] << ", acc: " << interp_pt.accelerations[i] << ", tfs: " << interp_pt.time_from_start);

[gavanderhoorn]

Apologies for the extremely delayed response.

I'm sorry you ran into these issues. The resampling filters are old, and basically haven't been maintained for a few years now.

It's possible that bitrot caused what you've observed (ie: MoveIt has changed, but the filters haven't been updated to keep up).

Without community contributions, this is unlikely to change, as industrial_core is in maintenance mode only at this point (meaning: only real breakages will be investigated).

Taking the age of this issue into account, I'm going to close it.

[ondals]

Hello, I'm using the "update" function in this package, and it works fine. I'm just wondering that this problem is still occurring now? Thanks!

[gavanderhoorn]

I don't believe anyone has looked into this since the last time I commented here.

If you happen to figure something out, please comment here so future users/readers would have an answer.

[ondals]

Thanks for comment!! :) First of all, i didn't observe this problem until now. (When checking through the trail in MoveIt) But just in case, I added the following code to the end of function "interpolatePt" in "uniform_sample_filter.cpp". but I haven't been able to confirm whether this code resolves the problem.

        // Check if interp_pt.positions[i] is not between absolute values of p1.positions[i] and p2.positions[i]
        if (interp_pt.positions[i] < std::min(p1.positions[i], p2.positions[i]) || 
            interp_pt.positions[i] > std::max(p1.positions[i], p2.positions[i]))
        {
            ROS_WARN_STREAM("interp_pt.positions[" << i << "] = " << interp_pt.positions[i] << 
                            " is not between abs(p1.positions[" << i << "]) = " << std::abs(p1.positions[i]) << 
                            " and abs(p2.positions[" << i << "]) = " << std::abs(p2.positions[i]));
            rtn = false;
        }
      }

      ROS_DEBUG_STREAM( "---------------End interpolating joint point---------------");
      rtn = true;