trajectories.cpp
Go to the documentation of this file.
00001 
00008 /*****************************************************************************
00009 ** Includes
00010 *****************************************************************************/
00011 
00012 #include <fstream>
00013 #include <iostream>
00014 #include <string>
00015 #include <gtest/gtest.h>
00016 #include <ecl/containers/array.hpp>
00017 #include <ecl/formatters/floats.hpp>
00018 #include <ecl/formatters/strings.hpp>
00019 #include <ecl/geometry/polynomial.hpp>
00020 #include <ecl/geometry/spline_function.hpp>
00021 #include "../../include/ecl/manipulators/waypoint.hpp"
00022 #include "../../include/ecl/manipulators/trajectory.hpp"
00023 
00024 
00025 /*****************************************************************************
00026 ** Using
00027 *****************************************************************************/
00028 
00029 using std::cout;
00030 using std::endl;
00031 using std::string;
00032 using ecl::Array;
00033 using ecl::RightAlign;
00034 using ecl::Format;
00035 using ecl::CubicPolynomial;
00036 using ecl::GenericSplineFunction;
00037 using ecl::SplineFunction;
00038 using ecl::JointAngles;
00039 using ecl::Trajectory;
00040 using ecl::WayPoint;
00041 
00042 /*****************************************************************************
00043 ** Tests
00044 *****************************************************************************/
00045 
00046 TEST(TrajectoryTests,allEggsInOneBasket) {
00047         // Haven't got around to running this properly through gtests yet.
00048         SUCCEED();
00049 }
00050 
00051 /*****************************************************************************
00052 ** Main program
00053 *****************************************************************************/
00054 
00055 int main(int argc, char **argv) {
00056 
00057     Format<string> string_format; string_format.width(8); string_format.align(RightAlign);
00058     Format<double> format; format.width(8); format.precision(2); format.align(RightAlign);
00059     int n = 200;
00060     
00061     std::ofstream tension_file_output, linear_file_output;
00062         tension_file_output.open("tension_interpolation.dat");
00063         linear_file_output.open("linear_interpolation.dat");
00064         tension_file_output << "# Created by trajectories_output_to_file test." << endl;
00065         tension_file_output << "# name: tension_interpolation" << endl;
00066         tension_file_output << "# type: matrix" << endl;
00067         tension_file_output << "# rows: " << (n + 1) * 3 << endl;
00068         tension_file_output << "# columns: " << 1 + 2 * 3 << endl;
00069         tension_file_output << endl;
00070         linear_file_output << "# Created by trajectories_output_to_file test." << endl;
00071         linear_file_output << "# name: linear_interpolation" << endl;
00072         linear_file_output << "# type: matrix" << endl;
00073         linear_file_output << "# rows: " << (n + 1) * 3 << endl;
00074         linear_file_output << "# columns: " << 1 + 2 * 3 << endl;
00075         linear_file_output << endl;
00076         
00077     std::cout << std::endl;
00078     std::cout << "***********************************************************" << std::endl;
00079     std::cout << "                    Constructor" << std::endl;
00080     std::cout << "***********************************************************" << std::endl;
00081     std::cout << std::endl;
00082 
00083     Trajectory<JointAngles> trajectory(2,"Crash Test Dummy");
00084 
00085     /******************************************
00086     ** Common Waypoint Parameters
00087     *******************************************/
00088     WayPoint<JointAngles> initial_waypoint(2), waypoint(2), final_waypoint(2);
00089 
00090     initial_waypoint.name("Initial Position");
00091     initial_waypoint.angles() << 1.0, 1.0;
00092     initial_waypoint.rates() << 0.0, 2.0;
00093     initial_waypoint.nominalRates(0.5);
00094     trajectory.append(initial_waypoint);
00095 
00096     waypoint.name("Second Waypoint");
00097     waypoint.angles() << 2.0, 0.0;
00098     waypoint.nominalRates(1.0);
00099     trajectory.append(waypoint);
00100 
00101     waypoint.name("Mid Waypoint");
00102     waypoint.angles() << 1.0, 5.0;
00103     waypoint.nominalRates(1.2);
00104     trajectory.append(waypoint);
00105 
00106     waypoint.name("Fourth Waypoint");
00107     waypoint.angles() << 3.0, 3.0;
00108     waypoint.nominalRates(0.5);
00109     trajectory.append(waypoint);
00110 
00111     final_waypoint.name("End Position");
00112     final_waypoint.angles() << 4.0, 2.0;
00113     final_waypoint.nominalRates(0.5);
00114     final_waypoint.rates() << 0.0, 1.0;
00115     trajectory.append(final_waypoint);
00116 
00117     std::cout << trajectory << std::endl;
00118 
00119     trajectory.maxAccelerations(5);
00120 
00121     std::cout << std::endl;
00122     std::cout << "***********************************************************" << std::endl;
00123     std::cout << "              Tension Spline Interpolation" << std::endl;
00124     std::cout << "***********************************************************" << std::endl;
00125     std::cout << std::endl;
00126 
00127     trajectory.tensionSplineInterpolation(4.0); // Tension arg
00128 
00129 //    cout << string_format("t  ");
00130 //    cout << string_format("y  ");
00131 //    cout << string_format("y' ");
00132 //    cout << string_format("y''");
00133 //    cout << string_format("y  ");
00134 //    cout << string_format("y' ");
00135 //    cout << string_format("y''");
00136 //    cout << endl;
00137     for ( int i = 0; i <= n; ++i ) {
00138         double x;
00139         if ( i != n ) {
00140                 x = i*(trajectory.duration())/n;
00141         } else {
00142                 x = trajectory.duration();
00143         }
00144 //        cout << format(x);
00145 //        cout << format(trajectory(0,x));
00146 //        cout << format(trajectory.derivative(0,x));
00147 //        cout << format(trajectory.dderivative(0,x));
00148 //        cout << format(trajectory(1,x));
00149 //        cout << format(trajectory.derivative(1,x));
00150 //        cout << format(trajectory.dderivative(1,x));
00151 //        cout << endl;
00152         
00153         tension_file_output << format(x);
00154         tension_file_output << format(trajectory(0,x));
00155         tension_file_output << format(trajectory.derivative(0,x));
00156         tension_file_output << format(trajectory.dderivative(0,x));
00157         tension_file_output << format(trajectory(1,x));
00158         tension_file_output << format(trajectory.derivative(1,x));
00159         tension_file_output << format(trajectory.dderivative(1,x));
00160         tension_file_output << endl;
00161     }
00162 
00163 
00164     std::cout << std::endl;
00165     std::cout << "***********************************************************" << std::endl;
00166     std::cout << "            Smooth Linear Spline Interpolation" << std::endl;
00167     std::cout << "***********************************************************" << std::endl;
00168     std::cout << std::endl;
00169 
00170     trajectory.maxAccelerations(5);
00171     trajectory.linearSplineInterpolation();
00172 
00173 //    cout << string_format("t  ");
00174 //    cout << string_format("y  ");
00175 //    cout << string_format("y' ");
00176 //    cout << string_format("y''");
00177 //    cout << string_format("y  ");
00178 //    cout << string_format("y' ");
00179 //    cout << string_format("y''");
00180 //    cout << endl;
00181     for ( int i = 0; i <= n; ++i ) {
00182                 double x;
00183                 if ( i != n ) {
00184                         x = i*(trajectory.duration())/n;
00185                 } else {
00186                         x = trajectory.duration();
00187                 }
00188 //        cout << format(x);
00189 //        cout << format(trajectory(0,x));
00190 //        cout << format(trajectory.derivative(0,x));
00191 //        cout << format(trajectory.dderivative(0,x));
00192 //        cout << format(trajectory(1,x));
00193 //        cout << format(trajectory.derivative(1,x));
00194 //        cout << format(trajectory.dderivative(1,x));
00195 //        cout << endl;
00196 
00197         linear_file_output << format(x);
00198         linear_file_output << format(trajectory(0,x));
00199         linear_file_output << format(trajectory.derivative(0,x));
00200         linear_file_output << format(trajectory.dderivative(0,x));
00201         linear_file_output << format(trajectory(1,x));
00202         linear_file_output << format(trajectory.derivative(1,x));
00203         linear_file_output << format(trajectory.dderivative(1,x));
00204         linear_file_output << endl;
00205     }
00206 
00207 
00208     std::cout << std::endl;
00209     std::cout << "***********************************************************" << std::endl;
00210     std::cout << "                      Passed" << std::endl;
00211     std::cout << "***********************************************************" << std::endl;
00212     std::cout << std::endl;
00213 
00214     tension_file_output.close();
00215     linear_file_output.close();
00216         std::cout << "Trajectories written to files 'linear_interpolation.dat', 'tension_interpolation.dat'." << std::endl;
00217         
00218     testing::InitGoogleTest(&argc,argv);
00219     return RUN_ALL_TESTS();
00220 }
00221 


ecl_manipulators
Author(s): Daniel Stonier (d.stonier@gmail.com)
autogenerated on Thu Jan 2 2014 11:11:43