int main(int argc, char** argv)
{
Eigen::Vector3d vf, vl;
Eigen::Matrix3d rm;
LookAtParams laps;
Pose pose1(1,2,3,45*M_PI/180.,0*M_PI/180.,0*M_PI/180.);
Pose pose2;
//get rotation matrix
pose1.getRotationMatrix(rm);
std::cout << "Rotation matrix: " << std::endl;
std::cout << rm << std::endl;
//get forward and left vectors
pose1.getForwardLeft(vf, vl);
std::cout << "Forward vector: " << std::endl;
std::cout << vf << std::endl;
std::cout << "Left vector: " << std::endl;
std::cout << vl << std::endl;
//get look at params
pose1.getLookAt(laps);
std::cout << "Look at params: " << std::endl;
std::cout << laps.eye_ << std::endl;
std::cout << laps.at_ << std::endl;
std::cout << laps.up_ << std::endl;
//move forward
pose1.moveForward(10.);
pose1.print();
//rotate about X
pose1.rotateAboutX(M_PI/2.);
pose1.print();
pose1.getRotationMatrix(rm);
std::cout << "Rotation matrix: " << std::endl;
std::cout << rm << std::endl;
//rotate about Z
pose1.rotateAboutX(-M_PI/2.); //undo
pose1.rotateAboutZ(-M_PI/4.);
pose1.print();
pose1.getRotationMatrix(rm);
std::cout << "Rotation matrix: " << std::endl;
std::cout << rm << std::endl;
//rotate about Y
pose1.rotateAboutY(-M_PI/2.);
pose1.print();
pose1.getRotationMatrix(rm);
std::cout << "Rotation matrix: " << std::endl;
std::cout << rm << std::endl;
//copy
pose2.setPose(pose1);
return 0;
}
void JointTrajectoryAction::setTargetTrajectory(double angle1,
double angle2,
double duration,
KDL::Trajectory_Composite& trajectoryComposite) {
KDL::Frame pose1(KDL::Rotation::RPY(0, 0, 0), KDL::Vector(angle1, 0, 0));
KDL::Frame pose2(KDL::Rotation::RPY(0, 0, 0), KDL::Vector(angle2, 0, 0));
KDL::Path_Line* path = new KDL::Path_Line(pose1, pose2, new KDL::RotationalInterpolation_SingleAxis(), 0.001);
KDL::VelocityProfile_Spline* velprof = new KDL::VelocityProfile_Spline();
velprof->SetProfileDuration(0, path->PathLength(),
duration);
KDL::Trajectory_Segment* trajectorySegment = new KDL::Trajectory_Segment(path, velprof);
trajectoryComposite.Add(trajectorySegment);
}
TEST(PosesTest, testInitial)
{
pose::HomogeneousTransformationPosition3RotationQuaternionD test;
pose::HomogeneousTransformationPosition3RotationQuaternionD test2(pos::Position3D(1,2,3),rot::RotationQuaternionPD(rot::AngleAxisPD(0.5,1.0,0,0)));
std::cout << "pos: " << test2.getPosition() << std::endl;
std::cout << "rot: " << test2.getRotation() << std::endl;
std::cout << "mat: " << test2.getTransformationMatrix()<< std::endl;
rot::AngleAxisPD aa;
aa = test2.getRotation();
pose::HomogeneousTransformationPosition3RotationQuaternionD pose1(pos::Position3D(4,5,6),rot::RotationQuaternionPD(rot::AngleAxisPD(0.2,Eigen::Vector3d(4,2,5).normalized())));
pos::Position3D posX(22,33,55);
pos::Position3D posRes = pose1.transform(posX);
std::cout << "posX: " << posX << std::endl;
std::cout << "posRes: " << posRes << std::endl;
std::cout << "posX again: " << pose1.inverseTransform(posRes) << std::endl;
posRes = rot::AngleAxisPD(M_PI/2.0,Eigen::Vector3d(0,0,1).normalized()).rotate(pos::Position3D(1,0,0));
std::cout << "posRes: " << posRes << std::endl;
std::cout << "pose1: " << pose1 << std::endl;
}
