Problem with path_line trajectory, C code, bad quaternions in trajectory

[ketchup007]

trafficstars

I have problem with easy path line trajectory. Only move i z axis, both orientations are the same. But in trajectory I see changes in quaternions.

I rewrite Yours example to show issue:

#include <kdl/frames.hpp>
#include <kdl/frames_io.hpp>
#include <kdl/trajectory.hpp>
#include <kdl/trajectory_segment.hpp>
#include <kdl/trajectory_stationary.hpp>
#include <kdl/trajectory_composite.hpp>
#include <kdl/trajectory_composite.hpp>
#include <kdl/velocityprofile_trap.hpp>
#include <kdl/path_line.hpp>
#include <kdl/rotational_interpolation_sa.hpp>
#include <kdl/utilities/error.h>
#include <kdl/trajectory_composite.hpp>

int main(int argc,char* argv[]) {
	using namespace KDL;
	try {
        KDL::Frame start_frame(KDL::Rotation::Quaternion(0.70711, 0.70711, 0, 0), KDL::Vector(0.8, 0.5, 0.3));
        KDL::Frame end_frame  (KDL::Rotation::Quaternion(0.70711, 0.70711, 0, 0), KDL::Vector(0.8, 0.5, 0.3));
        KDL::Path_Line* path = new KDL::Path_Line(start_frame, end_frame, new KDL::RotationalInterpolation_SingleAxis(), 0.0, true);

		VelocityProfile* velpref = new VelocityProfile_Trap(0.5,0.1);
		velpref->SetProfile(0, path->PathLength());  

		Trajectory* traject = new Trajectory_Segment(path, velpref);

		// use the trajectory
		double dt=0.1;
		std::ofstream of("./trajectory.dat");
		for (double t=0.0; t <= traject->Duration(); t+= dt) {
			Frame current_pose;
			current_pose = traject->Pos(t);

                        // Pobranie pozycji x, y, z
                        double x = current_pose.p.x();
                        double y = current_pose.p.y();
                        double z = current_pose.p.z();

                        // Pobranie orientacji jako kwaternion
                        double qx, qy, qz, qw;
                        current_pose.M.GetQuaternion(qx, qy, qz, qw);

                        // Zapisanie pozycji i orientacji do pliku
                        of << x << "\t" << y << "\t" << z << "\t" << qx << "\t" << qy << "\t" << qz << "\t" << qw << "\n";

		}
		of.close();


        std::cout << " trajectory written to the ./trajectory.dat file " << std::endl;

        delete traject;
	} catch(Error& error) {
		std::cout <<"I encountered this error : " << error.Description() << std::endl;
		std::cout << "with the following type " << error.GetType() << std::endl;
	}

}

and the output is:

0.8	0.5	0	0.707112	0.707108	0	0
0.8	0.5	0.0005	0.708957	0.705258	0	0
0.8	0.5	0.002	0.714474	0.699668	0	0
0.8	0.5	0.0045	0.723572	0.690256	0	0
0.8	0.5	0.008	0.736099	0.67688	0	0
0.8	0.5	0.0125	0.751842	0.65935	0	0
0.8	0.5	0.018	0.770516	0.637428	0	0
0.8	0.5	0.0245	0.791759	0.61084	0	0
0.8	0.5	0.032	0.815131	0.579284	0	0
0.8	0.5	0.0405	0.840097	0.542444	0	0
0.8	0.5	0.05	0.866029	0.500003	0	0
0.8	0.5	0.0605	0.892196	0.451658	0	0
0.8	0.5	0.072	0.917759	0.39715	0	0
0.8	0.5	0.0845	0.941769	0.336275	0	0
0.8	0.5	0.098	0.963167	0.268921	0	0
0.8	0.5	0.1125	0.98079	0.195091	0	0
0.8	0.5	0.128	0.993378	0.114938	0	0
0.8	0.5	0.1445	0.999591	0.0287941	0	0
0.8	0.5	0.161538	0.998181	-0.0603778	0	-0
0.8	0.5	0.177679	0.989522	-0.144422	0	-0
0.8	0.5	0.19282	0.974977	-0.222334	0	-0
0.8	0.5	0.206961	0.955859	-0.293848	0	-0
0.8	0.5	0.220102	0.933394	-0.358869	0	-0
0.8	0.5	0.232243	0.908711	-0.41744	0	-0
0.8	0.5	0.243384	0.882828	-0.469709	0	-0
0.8	0.5	0.253525	0.856655	-0.515901	0	-0
0.8	0.5	0.262666	0.830991	-0.556296	0	-0
0.8	0.5	0.270807	0.80653	-0.591202	0	-0
0.8	0.5	0.277948	0.783866	-0.620938	0	-0
0.8	0.5	0.284089	0.763499	-0.645817	0	-0
0.8	0.5	0.28923	0.74584	-0.666132	0	-0
0.8	0.5	0.293372	0.731222	-0.682146	0	-0
0.8	0.5	0.296513	0.719905	-0.694079	0	-0
0.8	0.5	0.298654	0.712079	-0.702106	0	-0
0.8	0.5	0.299795	0.707872	-0.706347	0	-0