Cheetah Software  1.0
JPosInitializer.cpp
Go to the documentation of this file.
1 #include "JPosInitializer.h"
3 #include <ParamHandler/ParamHandler.hpp>
4 #include <Configuration.h>
5 
6 template <typename T>
8  : _b_first_visit(true),
9  _end_time(end_time),
10  _curr_time(0.),
11  _dt(0.001),
12  _ini_jpos(cheetah::num_act_joint) {
13  _UpdateParam();
14 }
15 
16 template <typename T>
18 
19 template <typename T>
21  _curr_time += _dt;
22  // Initial Setup
23  if (_b_first_visit) {
24  _UpdateInitial(ctrl);
25  _b_first_visit = false;
26  }
27 
28  // Smooth movement
29  if (_curr_time < _end_time) {
30  T jpos[cheetah::num_act_joint];
32 
33  // pretty_print(jpos, "jpos_cmd", cheetah::num_act_joint);
34 
35  for (size_t leg(0); leg < cheetah::num_leg; ++leg) {
36  for (size_t jidx(0); jidx < cheetah::num_leg_joint; ++jidx) {
37  ctrl->commands[leg].tauFeedForward[jidx] = 0.;
38  ctrl->commands[leg].qDes[jidx] = jpos[3 * leg + jidx];
39  ctrl->commands[leg].qdDes[jidx] = 0.;
40  }
41  }
42  return false;
43  }
44  return true;
45 }
46 
47 template <typename T>
49  T ini[3 * cheetah::num_act_joint];
50  T fin[3 * cheetah::num_act_joint];
51  T** mid = new T*[1];
52  mid[0] = new T[cheetah::num_act_joint];
53 
54  for (size_t i(cheetah::num_act_joint); i < 3 * cheetah::num_act_joint; ++i) {
55  ini[i] = 0.;
56  fin[i] = 0.;
57  }
58 
59  for (int leg(0); leg < 4; ++leg) {
60  for (int jidx(0); jidx < 3; ++jidx) {
61  ini[3 * leg + jidx] = ctrl->datas[leg].q[jidx];
62  _ini_jpos[3 * leg + jidx] = ctrl->datas[leg].q[jidx];
63  }
64  }
65 
66  for (size_t i(0); i < cheetah::num_act_joint; ++i) {
67  fin[i] = _target_jpos[i];
68  mid[0][i] = _mid_jpos[i];
69  }
70  _jpos_trj.SetParam(ini, fin, mid, _end_time);
71 
72  delete[] mid[0];
73  delete[] mid;
74 }
75 
76 template <typename T>
78  ParamHandler handler(THIS_COM "config/initial_jpos_ctrl.yaml");
79  handler.getVector<T>("target_jpos", _target_jpos);
80  handler.getVector<T>("mid_jpos", _mid_jpos);
81 }
82 
83 template class JPosInitializer<double>;
84 template class JPosInitializer<float>;
bool IsInitialized(LegController< T > *)
std::vector< T > _ini_jpos
BS_Basic< T, cheetah::num_act_joint, 3, 1, 2, 2 > _jpos_trj
constexpr size_t num_act_joint
Definition: Quadruped.h:22
LegControllerData< T > datas[4]
Definition: LegController.h:70
constexpr size_t num_leg_joint
Definition: Quadruped.h:26
bool getCurvePoint(T u, T *ret)
Definition: BSplineBasic.h:78
JPosInitializer(T end_time)
std::vector< T > _target_jpos
bool SetParam(T *init, T *fin, T **middle_pt, T fin_time)
Definition: BSplineBasic.h:63
std::vector< T > _mid_jpos
void _UpdateInitial(const LegController< T > *ctrl)
constexpr size_t num_leg
Definition: Quadruped.h:25
LegControllerCommand< T > commands[4]
Definition: LegController.h:69