Smooth Linear Spline causing massive deviations in position

[jbeck28]

I've been trying to use the SmoothLinearSpline functionality, and I've found that for the data set I'm using, I get absolutely huge deviations in position for the points I've defined. That is, points that are programmed, not interpolated, still have deviations. I'm not sure if this is an issue with the library or an issue with the data that I'm using. Note that all the positions should be well within plus or minus .5, but I'm seeing 4 orders of magnitude more.

[stonier]

Any chance you could point me to your data set or better yet, a small bit of code that reproduces the problem?

[jbeck28]

Hi, sorry for the massive delay, I missed your email.

Here is the chunk of code which creates the interpolant:

	std::vector<ecl::SmoothLinearSpline> splines;

	const unsigned int numPtsConst = numPts;
	ecl::Array<double> ecl_pos(numPts+1);
	ecl::Array<double> ecl_time(numPts+1);
	for(int i = 0;i<numJts;i++){
		for(int j = 0;j<numPts+1;j++){
			if(j<numPts){
				ecl_pos[j] = t1[i].positions_[j];
				ecl_time[j] = t1[i].time_from_start_[j];
			}
			else if(j == numPts){
				// push back same point, add some time. 
				ecl_pos[j] = t1[i].positions_[j-1];
				ecl_time[j] = t1[i].time_from_start_[j-1]+.5;
			}
		}
		double maxAcc = 6*t1[i].max_velocity_;
		ecl::SmoothLinearSpline spline(ecl_time,ecl_pos,maxAcc);
		splines.push_back(spline);
	}

Note that the above iterates through all six joints, and fits a spline for each of them. The one that I sent you is joint 1.

And then the section of code which evaluates the spline(s):

bool firstpt = 0;
	int m = 0;
	for(int j = 0; j<2*numPts;j++){
		bool firstpt = 0;
		double time = 0;
		double time1 = 0;
		trajectory_msgs::JointTrajectoryPoint testMsg;
		trajectory_msgs::JointTrajectoryPoint testMsg1;
		//std::cout << "pt"<< j << std::endl;
		for(int q = 0;q<numJts;q++){
			if(j == 0 ){
				firstpt = 1;
				// then we want to use testMsg only.
				time = t1[q].time_from_start_[j];
				testMsg.positions.push_back(splines[q](time));
				testMsg.velocities.push_back(splines[q].derivative(time));
				testMsg.accelerations.push_back(splines[q].dderivative(time));
			}else{
				if(j%2==0){
				// in this case we have an actual point which was predefined. 
					time = t1[q].time_from_start_[j/2];
					testMsg.positions.push_back(splines[q](time));
					testMsg.velocities.push_back(splines[q].derivative(time));
					testMsg.accelerations.push_back(splines[q].dderivative(time));
				}else{
					if(j<((2*numPts)-1)){//if not the last point
						double prevtime = t1[q].time_from_start_[(j-1)/2];
						double nexttime = t1[q].time_from_start_[(j+1)/2];
						time = t1[q].time_from_start_[(j-1)/2] +(nexttime-prevtime)/2;
						testMsg.positions.push_back(splines[q](time));
						testMsg.velocities.push_back(splines[q].derivative(time));
						testMsg.accelerations.push_back(splines[q].dderivative(time));
					}else{// the last point, we actually want to make use of the fact that
						// when setting up the ecl arrays, we copied the last point, 
						// and placed this copy a half second away in time. 
						time = t1[q].time_from_start_[(j-1)/2]+.45;
						testMsg.positions.push_back(splines[q](time));
						testMsg.velocities.push_back(splines[q].derivative(time));
						testMsg.accelerations.push_back(splines[q].dderivative(time));
					}
					
				}
			}
			/*std::cout << "pts: "<<j << " jt: "<<q<<std::endl;
			std::cout << "time: "<< time<<std::endl<<std::endl;
			std::cout << "p: " << testMsg.positions[q]<<std::endl;
			std::cout << "v: " << testMsg.velocities[q]<<std::endl;
			std::cout << "a: " << testMsg.accelerations[q]<<std::endl;*/
			}

		testMsg.time_from_start = ros::Duration(time);
		testMsg1.time_from_start = ros::Duration(time1);
		finalTraj.push_back(testMsg);
		if(!ros::ok()){
			return 0;
		}
	
	}

A specific dataset has been attached, the raw data is called before.zip, and contains a .csv file. The second is called after.zip, and also contains a .csv file. Note the columns for before.csv are [time,position], and for after.zip it's [time,position,velocity,acceleration].

Thanks for your reply!
after.zip

before.zip