34 (std::vector<int16_t>& _strokes,
const std::vector<double>& _angles)
36 static const float rad2Deg = 180.0 / M_PI;
37 static const float scale = 100.0;
52 _strokes[0] =
static_cast<int16_t
>(scale * rad2Deg * _angles[0]);
53 _strokes[1] =
static_cast<int16_t
>(scale * waist.
one);
54 _strokes[2] =
static_cast<int16_t
>(scale * waist.
two);
60 _strokes[5] =
static_cast<int16_t
>(-scale * rad2Deg * _angles[5]);
63 _strokes[7] =
static_cast<int16_t
>(-scale * rad2Deg * _angles[7]);
64 _strokes[8] =
static_cast<int16_t
>(scale * l_wrist.
one);
65 _strokes[9] =
static_cast<int16_t
>(scale * l_wrist.
two);
66 _strokes[10] =
static_cast<int16_t
>(scale * (rad2Deg * _angles[10] + 50.0) * 0.18);
68 _strokes[11] =
static_cast<int16_t
>(scale * rad2Deg * _angles[11]);
69 _strokes[12] =
static_cast<int16_t
>(scale * neck.
two);
70 _strokes[13] =
static_cast<int16_t
>(scale * neck.
one);
76 _strokes[16] =
static_cast<int16_t
>(-scale * rad2Deg * _angles[16]);
79 _strokes[18] =
static_cast<int16_t
>(-scale * rad2Deg * _angles[18]);
80 _strokes[19] =
static_cast<int16_t
>(scale * r_wrist.
two);
81 _strokes[20] =
static_cast<int16_t
>(scale * r_wrist.
one);
82 _strokes[21] =
static_cast<int16_t
>(-scale * (rad2Deg * _angles[21] - 50.0) * 0.18);
91 (std::vector<double>& _angles,
const std::vector<int16_t>& _strokes)
93 static const float deg2Rad = M_PI / 180.0;
94 static const float scale_inv = 0.01;
96 _angles[0] = deg2Rad * scale_inv * _strokes[0];
103 _angles[5] = -deg2Rad * scale_inv * _strokes[5];
105 _angles[7] = -deg2Rad * scale_inv * _strokes[7];
110 _angles[10] = deg2Rad * (scale_inv * _strokes[10] * 5.556 - 50.0);
112 _angles[11] = deg2Rad * scale_inv * _strokes[11];
120 _angles[16] = -deg2Rad * scale_inv * _strokes[16];
122 _angles[18] = -deg2Rad * scale_inv * _strokes[18];
127 _angles[21] = -deg2Rad * (scale_inv * _strokes[21] * 5.556 - 50.0);
std::vector< StrokeMap > table
bool makeTable(std::vector< StrokeMap > &_table, const std::string _file_name)
float setStrokeToAngle(const float _stroke, const std::vector< StrokeMap > &_inv_table)
const std::string lifter_csv_dir
const std::string upper_csv_dir
std::vector< StrokeMap > inv_table
void Angle2Stroke(std::vector< int16_t > &_strokes, const std::vector< double > &_angles)
void Stroke2Angle(std::vector< double > &_angles, const std::vector< int16_t > &_strokes)
float setAngleToStroke(const float _angle, const std::vector< StrokeMap > &_table)
DiffJoint setDualAngleToStroke(const float _r_angle, const float _p_angle, const std::vector< StrokeMap > &_r_table, const std::vector< StrokeMap > &_p_table, const bool _is_pitch=false)
PLUGINLIB_EXPORT_CLASS(transmission_interface::BiDirectionalEffortJointInterfaceProvider, transmission_interface::RequisiteProvider)
void makeInvTable(std::vector< StrokeMap > &_inv_table, const std::vector< StrokeMap > &_table)