OmniWheel.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; tab-width: 4; -*-
00002 #include "ConfigurationSpace.h"
00003 #include "PathPlanner.h"
00004 #include "OmniWheel.h"
00005 #define _USE_MATH_DEFINES // for MSVC
00006 #include <math.h>
00007 
00008 using namespace PathEngine;
00009 
00010 inline double theta_limit(double theta)
00011 {
00012     while (theta >= 2*M_PI) theta -= 2*M_PI;
00013     while (theta < 0) theta += 2*M_PI;
00014     return theta;
00015 }
00016 
00017 Configuration OmniWheel::interpolate(const Configuration& from, 
00018                                      const Configuration& to,
00019                                      double ratio) const
00020 {
00021     ConfigurationSpace *cspace = planner_->getConfigurationSpace();
00022     Configuration cfg(cspace->size());
00023     for (unsigned int i=0; i<cfg.size(); i++){
00024         if (cspace->unboundedRotation(i)){
00025             double dth = to.value(i) - from.value(i);
00026             dth = theta_limit(dth);
00027             if (dth > M_PI){
00028                 dth = dth - 2*M_PI;
00029             }
00030             cfg.value(i) = from.value(i) + ratio*dth;
00031         }else{
00032             cfg.value(i) = (1-ratio)*from.value(i) + ratio*to.value(i);
00033         }
00034     }
00035     
00036     return cfg;
00037 }
00038 
00039 double OmniWheel::distance(const Configuration& from, const Configuration& to) const
00040 {
00041     ConfigurationSpace *cspace = planner_->getConfigurationSpace();
00042     double v=0, d;
00043     for (unsigned int i=0; i<cspace->size(); i++){
00044         if (cspace->unboundedRotation(i)){
00045             double dth = theta_limit(to.value(i) - from.value(i));
00046             if (dth > M_PI) dth = 2*M_PI - dth;
00047             d = cspace->weight(i)*dth;
00048         }else{
00049             d = cspace->weight(i)*(to.value(i) - from.value(i));
00050         }
00051         v += d*d;
00052     }
00053     return sqrt(v);
00054 }
00055 
00056 
00057 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:55