spline_smoother: spline_smoother: numerical_differentiation_spline

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00037 #ifndef NUMERICAL_DIFFERENTIATION_SPLINE_SMOOTHER_H_
00038 #define NUMERICAL_DIFFERENTIATION_SPLINE_SMOOTHER_H_
00039 
00040 #include <spline_smoother/spline_smoother.h>
00041 #include <spline_smoother/spline_smoother_utils.h>
00042 
00043 namespace spline_smoother
00044 {
00045 
00049 template <typename T>
00050 class NumericalDifferentiationSplineSmoother: public SplineSmoother<T>
00051 {
00052 public:
00053   NumericalDifferentiationSplineSmoother();
00054   virtual ~NumericalDifferentiationSplineSmoother();
00055 
00056   virtual bool smooth(const T& trajectory_in, 
00057                       T& trajectory_out) const;
00058 };
00059 
00060 template <typename T>
00061 NumericalDifferentiationSplineSmoother<T>::NumericalDifferentiationSplineSmoother()
00062 {
00063 }
00064 
00065 template <typename T>
00066 NumericalDifferentiationSplineSmoother<T>::~NumericalDifferentiationSplineSmoother()
00067 {
00068 }
00069 
00070 template <typename T>
00071 bool NumericalDifferentiationSplineSmoother<T>::smooth(const T& trajectory_in, 
00072                                                        T& trajectory_out) const
00073 {
00074   bool success = true;
00075   int size = trajectory_in.trajectory.points.size();
00076   int num_traj = trajectory_in.trajectory.joint_names.size();
00077   trajectory_out = trajectory_in;
00078 
00079   if (!checkTrajectoryConsistency(trajectory_out))
00080     return false;
00081 
00082   // keep the first and last velocities intact
00083 
00084   // for every point in time:
00085   for (int i=1; i<size-1; ++i)
00086   {
00087     double dt1 = (trajectory_in.trajectory.points[i].time_from_start - trajectory_in.trajectory.points[i-1].time_from_start).toSec();
00088     double dt2 = (trajectory_in.trajectory.points[i+1].time_from_start - trajectory_in.trajectory.points[i].time_from_start).toSec();
00089 
00090     // for every (joint) trajectory
00091     for (int j=0; j<num_traj; ++j)
00092     {
00093       double dx1 = trajectory_in.trajectory.points[i].positions[j] - trajectory_in.trajectory.points[i-1].positions[j];
00094       double dx2 = trajectory_in.trajectory.points[i+1].positions[j] - trajectory_in.trajectory.points[i].positions[j];
00095 
00096       double v1 = dx1/dt1;
00097       double v2 = dx2/dt2;
00098 
00099       trajectory_out.trajectory.points[i].velocities[j] = 0.5*(v1 + v2);
00100     }
00101   }
00102 
00103   // all accelerations are 0 for now:
00104   for (int i=0; i<size; i++)
00105     for (int j=0; j<num_traj; j++)
00106       trajectory_out.trajectory.points[i].accelerations[j] = 0.0;
00107 
00108   return success;
00109 }
00110 }
00111 
00112 #endif /* NUMERICAL_DIFFERENTIATION_SPLINE_SMOOTHER_H_ */
numerical_differentiation_spline_smoother.h
