岡本 陸
test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: Walk/Walk.cpp
- Revision:
- 29:7d8b8011a88d
- Parent:
- 27:79b4b932a6dd
- Child:
- 32:dc684a0b8448
--- a/Walk/Walk.cpp Wed Feb 27 01:25:01 2019 +0000
+++ b/Walk/Walk.cpp Wed Feb 27 12:16:18 2019 +0000
@@ -6,145 +6,6 @@
#include "pi.h"
//Orbitは足毎の軌道をあらわす。
const float kGravity = 9.8;
-void EllipseOrbit::SetParam(float stridetime_s, float risetime_s,
- float stride_m, float height_m, float ground_m,
- float ellipsecenter_x_m, float ellipsecenter_y_m)
-{
- stridetime_s_ = stridetime_s;
- risetime_s_ = risetime_s;
- stride_m_ = stride_m;
- height_m_ = height_m;
- ground_m_ = ground_m;
- ellipsecenter_x_m_ = ellipsecenter_x_m;
- ellipsecenter_y_m_ = ellipsecenter_y_m;
-}
-
-//着地中の動き.直線軌道.等速
-int EllipseOrbit::StrideLine_(OneLeg &leg, float phasetime_s)
-{
- float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + ellipsecenter_x_m_;
- float y_m = ellipsecenter_y_m_;
- return leg.SetXY_m(x_m, y_m);
-}
-int EllipseOrbit::StrideLineAccel_(OneLeg &leg, float phasetime_s)
-{
- ///////////x,yを計算
- float s0 = stride_m_ * 0.5 + ellipsecenter_x_m_;
- float s1 = -stride_m_ * 0.5 + ellipsecenter_x_m_;
- float g_h = sqrtf(kGravity / ground_m_);
- float t = phasetime_s / stridetime_s_;
- float denominator = expf(g_h) - expf(-g_h); //分母
-
- float x_m = -(s0 * expf(-g_h) - s1) * expf(g_h * t) / denominator + (s0 * expf(g_h) - s1) * expf(-g_h * t) / denominator;
- float y_m = ground_m_;
- //x,yを代入
- return leg.SetXY_m(x_m, y_m);
-}
-//空中の動き.半分にきれいに切れる楕円軌道
-int EllipseOrbit::RiseEllipse_(OneLeg &leg, float phasetime_s)
-{
- float rad = M_PI * (phasetime_s - stridetime_s_) / risetime_s_ + M_PI;
- float x_m = stride_m_ * 0.5 * cosf(rad) + ellipsecenter_x_m_;
- float y_m = height_m_ * sinf(rad) + ellipsecenter_y_m_;
- return leg.SetXY_m(x_m, y_m);
-}
-//空中の動き.完全に半分には切れない楕円軌道
-int EllipseOrbit::RiseEllipse2_(OneLeg &leg, float phasetime_s)
-{
- ///////////x,yを計算
- float ellipselong_m = stride_m_ * 0.5 / cosf(asinf(ground_m_ - ellipsecenter_y_m_));
- float ellipseshort_m = 2 * (height_m_ - ground_m_ + ellipsecenter_y_m_);
- float theta1 = acosf(stride_m_ / ellipselong_m * 0.5); //stride の始まりの位置の角度（数学基準）
- float theta2 = acosf(-stride_m_ / ellipselong_m * 0.5); //stride の終わりの位置の角度（数学基準）
- float omega = (2 * M_PI - theta2 + theta1) / risetime_s_; //楕円軌道における角速度、一定の値とする（適当）
- float rad = (phasetime_s - stridetime_s_) * omega + theta2; //角度の基準はtheta1とする（適当）
- float x_m = ellipselong_m * cos(rad) + ellipsecenter_x_m_;
- float y_m = ellipseshort_m * sin(rad) + ellipsecenter_y_m_;
- //x,yを代入
- return leg.SetXY_m(x_m, y_m);
-}
-//楕円軌道
-int EllipseOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
-{
- if (phasetime_s < stridetime_s_)
- return StrideLine_(leg, phasetime_s);
- else
- return RiseEllipse_(leg, phasetime_s);
-}
-float EllipseOrbit::GetOneWalkTime()
-{
- return stridetime_s_ + risetime_s_;
-}
-void EllipseOrbit::ChangeOneParam(EllipseParams param, float val)
-{
- switch (param)
- {
- case STRIDE_M:
- stride_m_ = val;
- break;
- case HEIGHT_M:
- height_m_ = val;
- break;
- case GROUND_M:
- ground_m_ = val;
- break;
- case ELLIPSE_CENTER_X_M:
- ellipsecenter_x_m_ = val;
- break;
- case ELLIPSE_CENTER_Y_M:
- ellipsecenter_y_m_ = val;
- break;
- }
-}
-void FreeLineOrbit::SetFreeLinesParam(LineParam lineparams[], int point_num)
-{
- point_num_ = point_num;
- if (point_num_ == 1)
- {
- printf("error:point_num = 1. You should put >=2 to make line.");
- while (1)
- ;
- }
- for (int i = 0; i < point_num_; i++)
- lineparams_[i] = lineparams[i];
-}
-int FreeLineOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
-{
- ;
- //現在の最終到達pointを決定
- int arrived_point;
- float sum_time = 0;
- for (arrived_point = 0; arrived_point < point_num_ - 2; arrived_point++) //arrived_point = point_num-1(これが配列の最大index)の状態で終わらないように-2している
- {
- sum_time += lineparams_[arrived_point].time_s;
- if (phasetime_s < sum_time)
- break;
- }
- //x,yを計算
- float x_m, y_m;
- x_m = lineparams_[arrived_point].x_m +
- (lineparams_[arrived_point + 1].x_m - lineparams_[arrived_point].x_m) * phasetime_s / lineparams_[arrived_point].time_s;
- y_m = lineparams_[arrived_point].y_m +
- (lineparams_[arrived_point + 1].y_m - lineparams_[arrived_point].y_m) * phasetime_s / lineparams_[arrived_point].time_s;
- return leg.SetXY_m(x_m, y_m);
-}
-float FreeLineOrbit::GetOneWalkTime() //足一周の時間
-{
- float sum_time = 0;
- for (int i = 0; i < point_num_; i++)
- sum_time += lineparams_[i].time_s;
- return sum_time;
-}
-void FreeLineOrbit::SetStandParam(float x_m, float y_m, float time_s)
-{
- point_num_ = 1;
- lineparams_[0].x_m = x_m;
- lineparams_[0].y_m = y_m;
- lineparams_[0].time_s = time_s;
- lineparams_[1].x_m = x_m;
- lineparams_[1].y_m = y_m;
- lineparams_[1].time_s = 0;
-}
Orbit::Orbit(OrbitPattern pattern)
{
@@ -155,12 +16,8 @@
float time_s;
switch (pattern_)
{
- case FREELINES:
- time_s = FreeLineOrbit::GetOneWalkTime();
- break;
-
default:
- time_s = EllipseOrbit::GetOneWalkTime();
+ time_s = TriangleOrbit::GetOneWalkTime();
break;
}
return time_s;
@@ -170,11 +27,8 @@
int ret;
switch (pattern_)
{
- case FREELINES:
- ret = FreeLineOrbit::GetOrbit(leg, phasetime_s);
- break;
default:
- ret = EllipseOrbit::GetOrbit(leg, phasetime_s);
+ ret = TriangleOrbit::GetOrbit(leg, phasetime_s);
break;
}
return ret;
@@ -186,22 +40,28 @@
offset_multi_[i] = 0;
}
float Walk::calctime_s_;
-void Walk::Cal4LegsPosi(OneLeg leg[4])
+int Walk::Cal4LegsPosi(OneLeg leg[4]) //失敗したら1を返す。成功なら0
{
+ int is_out = 0;
for (int i = 0; i < 4; i++)
{
float one_walk_time = orbit_[i].GetOneWalkTime();
phasetime_s_[i] += calctime_s_;
while (phasetime_s_[i] > one_walk_time)
phasetime_s_[i] -= one_walk_time;
- orbit_[i].GetOrbit(leg[i], phasetime_s_[i]);
+ if (orbit_[i].GetOrbit(leg[i], phasetime_s_[i]) == 1)
+ {
+ printf("error:leg %d in Cal4LegsPosi\r\n", i);
+ is_out = 1;
+ }
}
+ return is_out;
}
float Walk::GetOneWalkTime()
{
return orbit_[0].GetOneWalkTime(); //4足全て同じ時間のはずなので一例としてorbit_[0]のものを返している.
};
-void Walk::SetOffset(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3)
+void Walk::SetOffsetTime(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3)
{
offset_multi_[0] = offset_multi0;
offset_multi_[1] = offset_multi1;
@@ -219,7 +79,10 @@
for (float j = 0; j < one_walk_time; j += step)
{
if (orbit_[i].GetOrbit(templateleg, j) == 1)
+ {
+ printf("error:leg %d, time %f", i, j);
return 1; //解が出ないときは1を返す
+ }
}
phasetime_s_[i] = one_walk_time * offset_multi_[i];
}
@@ -234,21 +97,86 @@
for (int i = 0; i < 4; i++)
SetOneOrbit(i, orbit);
}
-
-void Walk::ChangeOneParam(int legnum, EllipseParams param, float val)
+void Walk::SetOneLegStandParam(int legnum,float x_m, float y_m, float time_s)
+{
+ Orbit triangle(TRIANGLE);
+ triangle.SetTriangleParam(x_m, y_m, 0, 0, 0, time_s, 0,0);
+ SetOneOrbit(legnum, triangle);
+}
+void Walk::SetAllLegStandParam(float x_m, float y_m, float time_s)
{
- orbit_[legnum].ChangeOneParam(param, val);
+ for (int i = 0; i < 4; i ++)
+ SetOneLegStandParam(i, x_m, y_m, time_s);
}
-void Walk::SetAllLegEllipseParam(float stridetime_s, float risetime_s,
- float stride_m, float height_m, float ground_m,
- float ellipsecenter_x_m, float ellipsecenter_y_m)
+void Walk::SetOneLegTriangleParam(int legnum, float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m,
+ float stridetime_s, float toptime_s, float buffer_time_s)
+{
+ Orbit triangle(TRIANGLE);
+ triangle.SetTriangleParam(start_x_m, start_y_m, stride_m, height_m, buffer_height_m,
+ stridetime_s, toptime_s, buffer_time_s);
+ SetOneOrbit(legnum, triangle);
+}
+void Walk::SetAllLegTriangleParam(float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m,
+ float stridetime_s, float toptime_s, float buffer_time_s)
{
for (int i = 0; i < 4; i++)
- orbit_[i].SetParam(stridetime_s, risetime_s, stride_m, height_m, ground_m, ellipsecenter_x_m, ellipsecenter_y_m);
+ SetOneLegTriangleParam(i, start_x_m, start_y_m, stride_m, height_m, buffer_height_m,
+ stridetime_s, toptime_s, buffer_time_s);
}
-void Walk::SetAllLegStandParam(float x_m, float y_m, float time_s)
+
+void TriangleOrbit::SetTriangleParam(float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m,
+ float stridetime_s, float toptime_s, float buffer_time_s)
+{
+ reverse_tanbeta_ = 1.0 / tan(81.0 / 180.0 * M_PI); //論文よりこれが最適らしい
+ start_x_m_ = start_x_m;
+ start_y_m_ = start_y_m;
+ stride_m_ = stride_m;
+ height_m_ = height_m; //足上げ幅
+ buffer_height_m_ = buffer_height_m; //着地直前で止める高さ
+ stridetime_s_ = stridetime_s;
+ toptime_s_ = toptime_s; //頂点に行くまでの時間
+ buffer_time_s_ = buffer_time_s; //頂点から一時停止点までの時間.
+
+ //事前に計算しておく
+ top_x_m_ = start_x_m_ - height_m * reverse_tanbeta_;
+ top_y_m_ = -height_m_ + start_y_m_;
+ buffer_x_m_ = start_x_m_ - buffer_height_m_ * reverse_tanbeta_;
+ buffer_y_m_ = -buffer_height_m_ + start_y_m_;
+}
+//足一周の時間
+float TriangleOrbit::GetOneWalkTime()
+{
+ return stridetime_s_ + toptime_s_ + buffer_time_s_;
+};
+int TriangleOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
{
- Orbit stand(FREELINES);
- stand.SetStandParam(x_m, y_m, time_s); //defaultとなる軌道設定
- SetAllOrbit(stand); //4足にコピー
+ int ret = 0;
+ if (phasetime_s < stridetime_s_)
+ ret = StrideLineAccel_(leg, phasetime_s);
+ else if (phasetime_s < stridetime_s_ + toptime_s_)
+ ret = leg.SetXY_m(top_x_m_, top_y_m_);
+ else
+ ret = leg.SetXY_m(buffer_x_m_, buffer_y_m_);
+ return ret;
+};
+int TriangleOrbit::StrideLine_(OneLeg &leg, float phasetime_s)
+{
+ float x_m = start_x_m_ - stride_m_ * phasetime_s / stridetime_s_;
+ float y_m = start_y_m_;
+ return leg.SetXY_m(x_m, y_m);
}
+
+int TriangleOrbit::StrideLineAccel_(OneLeg &leg, float phasetime_s)
+{
+ ///////////x,yを計算
+ float s0 = start_x_m_;
+ float s1 = start_x_m_ - stride_m_;
+ float g_h = sqrtf(kGravity / start_y_m_);
+ float t = phasetime_s / stridetime_s_;
+ float denominator = expf(g_h) - expf(-g_h); //分母
+
+ float x_m = -(s0 * expf(-g_h) - s1) * expf(g_h * t) / denominator + (s0 * expf(g_h) - s1) * expf(-g_h * t) / denominator;
+ float y_m = start_y_m_;
+ //x,yを代入
+ return leg.SetXY_m(x_m, y_m);
+}
\ No newline at end of file