Open ketchup007 opened 9 months ago
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
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:
and the output is: