examples/partial_inverse.cpp

Go to the documentation of this file.

 
/*****************************************************************************
** Platform Check
*****************************************************************************/
 
#include <ecl/config.hpp>
 
/*****************************************************************************
** Includes
*****************************************************************************/
 
#include <iostream>
#include <ecl/linear_algebra.hpp>
#include <ecl/formatters/floats.hpp>
#include <ecl/geometry/legacy_pose2d.hpp>
#include <ecl/math.hpp>
#include "../../include/ecl/mobile_robot/differential_drive.hpp"
 
/*****************************************************************************
** Using
*****************************************************************************/
 
using std::cout;
using std::endl;
using ecl::RightAlign;
using ecl::Format;
using ecl::LegacyPose2D;
using ecl::linear_algebra::Vector2d;
using ecl::linear_algebra::Vector3d;
 
/*****************************************************************************
** Main program
*****************************************************************************/
 
int main(int argc, char **argv) {
 
    Format<double> format; format.width(8); format.precision(2); format.align(RightAlign);
 
    cout << endl;
    cout << "***********************************************************" << endl;
    cout << "                 Partial Inverse" << endl;
    cout << "***********************************************************" << endl;
    cout << endl;
 
//    Vector3d pose_a; pose_a << 1.0, 2.0, ecl::pi/2.0;
//    Vector3d pose_b; pose_b << 2.0, 3.0, ecl::pi;
    LegacyPose2D<double> a(1.0, 2.0, ecl::pi/2.0);
    LegacyPose2D<double> b(2.0, 3.0, ecl::pi);
 
    Vector2d partial_inverse;
//    partial_inverse = ecl::DifferentialDrive::Kinematics::Inverse(pose_a, pose_b);
//    std::cout << partial_inverse << std::endl;
    partial_inverse = ecl::DifferentialDrive::Kinematics::PartialInverse(a, b);
    std::cout << partial_inverse << std::endl;
 
    cout << endl;
    cout << "***********************************************************" << endl;
    cout << "                      Passed" << endl;
    cout << "***********************************************************" << endl;
    cout << endl;
 
    return 0;
}