TGT.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 "TGT.h"
5 #define _USE_MATH_DEFINES // for MSVC
6 #include <math.h>
7 
8 using namespace PathEngine;
9 
10 inline double theta_diff(double from, double to)
11 {
12  double diff = to - from;
13  if (diff > M_PI){
14  return diff - 2*M_PI;
15  }else if (diff < -M_PI){
16  return diff + 2*M_PI;
17  }else{
18  return diff;
19  }
20 }
21 
22 #if 0
23 std::vector<Configuration> TGT::getPath(const Configuration &from, const Configuration &to) const
24 {
25  std::vector<Configuration> path;
26 
27  double dx = to.value(0) - from.value(0);
28  double dy = to.value(1) - from.value(1);
29  double theta = atan2(dy, dx);
30 
31  Configuration changedFrom(from.value(0), from.value(1), theta);
32  Configuration changedTo(to.value(0), to.value(1), theta);
33 
34  path.push_back(from);
35 
36  unsigned int n;
37  Configuration pos;
38 
39  std::cout << 1 << std::endl;
40  // from - changedFrom
41  n = (unsigned int)(distance(from, changedFrom)/interpolationDistance())+1;
42  for (unsigned int i=1; i<=n; i++){
43  pos = interpolate(from, changedFrom, ((double)i)/n);
44  std::cout << pos << std::endl;
45  path.push_back(pos);
46  }
47 
48  std::cout << 2 << std::endl;
49  // changedFrom - changedTo
50  n = (unsigned int)(distance(changedFrom, changedTo)/interpolationDistance())+1;
51  for (unsigned int i=1; i<=n; i++){
52  pos = interpolate(changedFrom, changedTo, ((double)i)/n);
53  std::cout << pos << std::endl;
54  path.push_back(pos);
55  }
56 
57  std::cout << 3 << std::endl;
58  // changedTo - to
59  n = (unsigned int)(distance(changedTo, to)/interpolationDistance())+1;
60  for (unsigned int i=1; i<=n; i++){
61  pos = interpolate(changedTo, to, ((double)i)/n);
62  std::cout << pos << std::endl;
63  path.push_back(pos);
64  }
65  getchar();
66 
67  return path;
68 }
69 #endif
70 
72  double ratio) const
73 {
75  double dx = to.value(0) - from.value(0);
76  double dy = to.value(1) - from.value(1);
77 
78  if (dx == 0 && dy == 0){
79  double dth = theta_diff(from.value(2), to.value(2));
80  Configuration cfg(cspace->size());
81  cfg.value(0) = from.value(0);
82  cfg.value(1) = from.value(1);
83  cfg.value(2) = from.value(2)+ratio*dth;
84  return cfg;
85  }else{
86 
87  double theta = atan2(dy, dx);
88 
89  double dth1 = theta_diff(from.value(2), theta);
90  double d1 = cspace->weight(2)*fabs(dth1);
91 
92  dx *= cspace->weight(0);
93  dy *= cspace->weight(1);
94  double d2 = sqrt(dx*dx + dy*dy);
95 
96  double dth2 = theta_diff(theta, to.value(2));
97  double d3 = cspace->weight(2) * fabs(dth2);
98 
99  double d = d1 + d2 + d3;
100 
101 #if 0
102  std::cout << "theta = " << theta << ", dth1 = " << dth1
103  << ", dth2 = " << dth2 << std::endl;
104  std::cout << "d1:" << d1 << ", d2:" << d2 << ", d3:" << d3 << std::endl;
105 #endif
106 
107  if (d == 0){
108  return from;
109  }
110 
111  if (ratio >= 0 && ratio*d < d1){
112  Configuration cfg(cspace->size());
113  cfg.value(0) = from.value(0);
114  cfg.value(1) = from.value(1);
115  cfg.value(2) = from.value(2) + ratio*d/d1*dth1;
116  return cfg;
117  }else if (ratio*d >= d1 && ratio*d < (d1+d2)){
118  double r = (ratio*d - d1)/d2;
119  Configuration cfg(cspace->size());
120  cfg.value(0) = (1-r)*from.value(0) + r*to.value(0);
121  cfg.value(1) = (1-r)*from.value(1) + r*to.value(1);
122  cfg.value(2) = theta;
123  return cfg;
124  }else if (ratio*d >= (d1+d2) && ratio <= 1.0){
125  Configuration cfg(cspace->size());
126  cfg.value(0) = to.value(0);
127  cfg.value(1) = to.value(1);
128  cfg.value(2) = theta + (ratio*d-d1-d2)/d3*dth2;
129  return cfg;
130  }else{
131  std::cout << "TGT::interpolate() : invalid ratio(" << ratio << ")"
132  << std::endl;
133  abort ();
134  }
135  }
136 }
137 
138 double TGT::distance(const Configuration& from, const Configuration& to) const
139 {
141  double dx = to.value(0) - from.value(0);
142  double dy = to.value(1) - from.value(1);
143  double theta = atan2(dy, dx);
144 
145  dx *= cspace->weight(0);
146  dy *= cspace->weight(1);
147 
148  if (dx == 0 && dy == 0) {
149  return cspace->weight(2)*fabs(theta_diff(from.value(2), to.value(2)));
150  }
151 
152 
153  double dth1 = fabs(theta_diff(from.value(2), theta));
154  dth1 *= cspace->weight(2);
155 
156  double dth2 = fabs(theta_diff(theta, to.value(2)));
157  dth2 *= cspace->weight(2);
158 
159  //std::cout << "d = " << sqrt(dx*dx + dy*dy) << " + " << dth1 << " + " << dth2 << std::endl;
160  return sqrt(dx*dx + dy*dy) + dth1 + dth2;
161 }
unsigned int size()
get the number of degrees of freedom
static double interpolationDistance()
補間時の隣接する2点間の最大距離を取得する
Definition: Mobility.h:90
ConfigurationSpace * getConfigurationSpace()
コンフィギュレーション空間設定を取得する
PathPlanner * planner_
計画経路エンジン
Definition: Mobility.h:28
png_uint_32 i
Definition: png.h:2735
double distance(const Configuration &from, const Configuration &to) const
親クラスのドキュメントを参照
Definition: TGT.cpp:138
virtual bool getPath(Configuration &from, Configuration &to, std::vector< Configuration > &o_path) const
開始位置から目標地点への移動を補間して生成された姿勢列を取得する。
Definition: Mobility.cpp:26
double & weight(unsigned int i_rank)
get weight for i_rank th element
path
double theta_diff(double from, double to)
Definition: TGT.cpp:10
#define M_PI
typedef int
Definition: png.h:1113
const double value(unsigned int i_rank) const
Configuration interpolate(const Configuration &from, const Configuration &to, double ratio) const
親クラスのドキュメントを参照
Definition: TGT.cpp:71


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:05