Smooth Linear Spline causing massive deviations in position about ecl_core

[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