OmniWheel.cpp
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; tab-width: 4; -*-
2 #include "ConfigurationSpace.h"
3 #include "PathPlanner.h"
4 #include "OmniWheel.h"
5 #define _USE_MATH_DEFINES // for MSVC
6 #include <math.h>
7 
8 using namespace PathEngine;
9 
10 inline double theta_limit(double theta)
11 {
12  while (theta >= 2*M_PI) theta -= 2*M_PI;
13  while (theta < 0) theta += 2*M_PI;
14  return theta;
15 }
16 
18  const Configuration& to,
19  double ratio) const
20 {
22  Configuration cfg(cspace->size());
23  for (unsigned int i=0; i<cfg.size(); i++){
24  if (cspace->unboundedRotation(i)){
25  double dth = to.value(i) - from.value(i);
26  dth = theta_limit(dth);
27  if (dth > M_PI){
28  dth = dth - 2*M_PI;
29  }
30  cfg.value(i) = from.value(i) + ratio*dth;
31  }else{
32  cfg.value(i) = (1-ratio)*from.value(i) + ratio*to.value(i);
33  }
34  }
35 
36  return cfg;
37 }
38 
39 double OmniWheel::distance(const Configuration& from, const Configuration& to) const
40 {
42  double v=0, d;
43  for (unsigned int i=0; i<cspace->size(); i++){
44  if (cspace->unboundedRotation(i)){
45  double dth = theta_limit(to.value(i) - from.value(i));
46  if (dth > M_PI) dth = 2*M_PI - dth;
47  d = cspace->weight(i)*dth;
48  }else{
49  d = cspace->weight(i)*(to.value(i) - from.value(i));
50  }
51  v += d*d;
52  }
53  return sqrt(v);
54 }
55 
56 
57 
double theta_limit(double theta)
Definition: OmniWheel.cpp:10
unsigned int size()
get the number of degrees of freedom
ConfigurationSpace * getConfigurationSpace()
コンフィギュレーション空間設定を取得する
PathPlanner * planner_
計画経路エンジン
Definition: Mobility.h:28
Configuration interpolate(const Configuration &from, const Configuration &to, double ratio) const
親クラスのドキュメントを参照
Definition: OmniWheel.cpp:17
png_uint_32 i
Definition: png.h:2735
void unboundedRotation(unsigned int i_rank, bool i_flag)
specify i th degree of freedom is unbounded rotaion or not. default is false
double distance(const Configuration &from, const Configuration &to) const
親クラスのドキュメントを参照
Definition: OmniWheel.cpp:39
double & weight(unsigned int i_rank)
get weight for i_rank th element
#define M_PI
const double value(unsigned int i_rank) const


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:39