ImpedanceOutputGenerator.h
Go to the documentation of this file.
1 #ifndef IMPEDANCETRGETGENERATOR_H
2 #define IMPEDANCETRGETGENERATOR_H
3 
4 #include "RatsMatrix.h"
5 
6 struct ImpedanceOutputGenerator
7 {
8  // target : Target pos and rot from SequencePlayer and StateHolder
9  // current : Current ee pos and rot (IK result)
10  // output : Output from ImpedanceOutput which response is characterized by MDK
11  // index 0 : t+dt. Values in current onExecute.
12  // index 1 : t. Values in previous onExecute.
13  // index 2 : t-dt. Values in previous previous onExecute.
14  hrp::Vector3 target_p0, target_p1, target_p2, current_p1, output_p1, output_p2;
15  hrp::Matrix33 target_r0, target_r1, target_r2, current_r1, output_r1, output_r2;
16  double M_p, D_p, K_p;
17  double M_r, D_r, K_r;
18  hrp::Matrix33 force_gain, moment_gain;
19 
20  ImpedanceOutputGenerator ()
21  : target_p0(hrp::Vector3::Zero()), target_p1(hrp::Vector3::Zero()), target_p2(hrp::Vector3::Zero()),
22  current_p1(hrp::Vector3::Zero()), output_p1(hrp::Vector3::Zero()), output_p2(hrp::Vector3::Zero()),
23  target_r0(hrp::Matrix33::Identity()), target_r1(hrp::Matrix33::Identity()), target_r2(hrp::Matrix33::Identity()),
24  current_r1(hrp::Matrix33::Identity()), output_r1(hrp::Matrix33::Identity()), output_r2(hrp::Matrix33::Identity()),
25  M_p(10), D_p(200), K_p(400), M_r(5), D_r(100), K_r(200),
26  force_gain(hrp::Matrix33::Identity()), moment_gain(hrp::Matrix33::Identity())
27  {};
28  const hrp::Matrix33& getOutputRot () { return output_r1; };
29  const hrp::Vector3& getOutputPos () { return output_p1; };
30  void resetPreviousTargetParam ()
31  {
32  target_p1 = target_p0;
33  target_p2 = target_p1;
34  target_r1 = target_r0;
35  target_r2 = target_r1;
36  };
37  void resetPreviousCurrentParam ()
38  {
39  output_p1 = current_p1;
40  output_p2 = output_p1;
41  output_r1 = current_r1;
42  output_r2 = output_r1;
43  };
44  void calcTargetVelocityOrg (hrp::Vector3& vel_p, hrp::Vector3& vel_r,
45  const hrp::Matrix33& eeR,
46  const hrp::Vector3& force_diff, const hrp::Vector3& moment_diff,
47  const double _dt, const bool printp = false, const std::string& print_str = "", const std::string& ee_name = "")
48  {
49  if ( printp ) {
50  std::cerr << "[" << print_str << "] impedance calc [" << ee_name << "] (Old version)" << std::endl;
51  std::cerr << "[" << print_str << "] cur1 = " << current_p1.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
52  std::cerr << "[" << print_str << "] out1 = " << output_p1.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
53  std::cerr << "[" << print_str << "] out2 = " << output_p2.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
54  std::cerr << "[" << print_str << "] tgt0 = " << target_p0.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
55  std::cerr << "[" << print_str << "] tgt1 = " << target_p1.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
56  }
57 
58  hrp::Vector3 dif_output_pos, dif_target_pos, vel_target_pos, err_pos;
59  hrp::Vector3 dif_output_rot, dif_target_rot, vel_target_rot, err_rot;
60 
61  dif_output_pos = output_p1 - output_p2;
62  dif_target_pos = target_p0 - target_p1;
63  vel_target_pos = target_p0 - current_p1;
64  err_pos = output_p1 - current_p1;
65 
66  rats::difference_rotation(dif_output_rot, output_r2, output_r1);
67  rats::difference_rotation(dif_target_rot, target_r1, target_r0);
68  rats::difference_rotation(vel_target_rot, current_r1, target_r0);
69  rats::difference_rotation(err_rot, current_r1, output_r1);
70 
71  if ( printp ) {
72  std::cerr << "[" << print_str << "] dif_out_p = " << dif_output_pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
73  std::cerr << "[" << print_str << "] dif_tgt_p = " << dif_target_pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
74  std::cerr << "[" << print_str << "] vel_tgt_p = " << vel_target_pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
75  std::cerr << "[" << print_str << "] err_pos = " << err_pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
76  std::cerr << "[" << print_str << "] dif_out_r = " << dif_output_rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[rad]" << std::endl;
77  std::cerr << "[" << print_str << "] dif_tgt_r = " << dif_target_rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[rad]" << std::endl;
78  std::cerr << "[" << print_str << "] vel_tgt_r = " << vel_target_rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[rad]" << std::endl;
79  std::cerr << "[" << print_str << "] err_rot = " << err_rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[rad]" << std::endl;
80  }
81  vel_p = ( eeR * (force_gain * (eeR.transpose() * force_diff)) * _dt * _dt
82  + M_p * ( dif_output_pos + err_pos )
83  + D_p * ( dif_target_pos + err_pos ) * _dt
84  + K_p * ( vel_target_pos * _dt * _dt ) ) /
85  (M_p + (D_p * _dt) + (K_p * _dt * _dt));
86  vel_r = ( eeR * (moment_gain * (eeR.transpose() * moment_diff)) * _dt * _dt
87  + M_r * ( dif_output_rot + err_rot )
88  + D_r * ( dif_target_rot + err_rot ) * _dt
89  + K_r * ( vel_target_rot * _dt * _dt ) ) /
90  (M_r + (D_r * _dt) + (K_r * _dt * _dt));
91  // generate smooth motion just after impedance started
92  if ( printp ) {
93  std::cerr << "[" << print_str << "] vel_p = " << vel_p.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
94  std::cerr << "[" << print_str << "] vel_r = " << vel_r.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[rad]" << std::endl;
95  }
96 
97  // Spin time-related parameter
98  output_p2 = output_p1;
99  output_r2 = output_r1;
100 
101  output_p1 = current_p1 + vel_p;
102  // if ( std::fabs(vel_r.norm() - 0.0) < ::std::numeric_limits<double>::epsilon() ) {
103  if ( vel_r.norm() != 0.0 ) {
104  hrp::Matrix33 tmpm;
105  Eigen::AngleAxis<double> tmpr(vel_r.norm(), vel_r.normalized());
106  rats::rotm3times(tmpm, tmpr.toRotationMatrix(), current_r1);
107  output_r1 = tmpm;
108  } else {
109  output_r1 = current_r1;
110  }
111 
112  target_p1 = target_p0;
113  target_r1 = target_r0;
114  };
115  void calcTargetVelocity (hrp::Vector3& vel_p, hrp::Vector3& vel_r,
116  const hrp::Matrix33& eeR,
117  const hrp::Vector3& force_diff, const hrp::Vector3& moment_diff,
118  const double _dt, const bool printp = false, const std::string& print_str = "", const std::string& ee_name = "")
119  {
120  if ( printp ) {
121  std::cerr << "[" << print_str << "] impedance calc [" << ee_name << "] (New version)" << std::endl;
122  std::cerr << "[" << print_str << "] cur1 = " << current_p1.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
123  std::cerr << "[" << print_str << "] out1 = " << output_p1.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
124  std::cerr << "[" << print_str << "] out2 = " << output_p2.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
125  std::cerr << "[" << print_str << "] tgt0 = " << target_p0.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
126  std::cerr << "[" << print_str << "] tgt1 = " << target_p1.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
127  std::cerr << "[" << print_str << "] tgt2 = " << target_p2.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
128  }
129  hrp::Vector3 dif_pos1, dif_rot1, dif_pos2, dif_rot2, vel_target_pos, vel_target_rot;
130  dif_pos1 = output_p1 - target_p1;
131  dif_pos2 = output_p2 - target_p2;
132  vel_target_pos = target_p0 - current_p1;
133  rats::difference_rotation(dif_rot1, target_r1, output_r1);
134  rats::difference_rotation(dif_rot2, target_r2, output_r2);
135  rats::difference_rotation(vel_target_rot, current_r1, target_r0);
136  vel_p = ( eeR * (force_gain * (eeR.transpose() * force_diff)) * _dt * _dt
137  + (2 * M_p + D_p * _dt) * dif_pos1
138  - M_p * dif_pos2) /
139  (M_p + (D_p * _dt) + (K_p * _dt * _dt))
140  + vel_target_pos;
141  vel_r = ( eeR * (moment_gain * (eeR.transpose() * moment_diff)) * _dt * _dt
142  + (2 * M_r + D_r * _dt) * dif_rot1
143  - M_r * dif_rot2) /
144  (M_r + (D_r * _dt) + (K_r * _dt * _dt))
145  + vel_target_rot;
146  // generate smooth motion just after impedance started
147  if ( printp ) {
148  std::cerr << "[" << print_str << "] vel_p = " << vel_p.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
149  std::cerr << "[" << print_str << "] vel_r = " << vel_r.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[rad]" << std::endl;
150  }
151 
152  // Spin time-related parameter
153  output_p2 = output_p1;
154  output_r2 = output_r1;
155 
156  output_p1 = current_p1 + vel_p;
157  // if ( std::fabs(vel_r.norm() - 0.0) < ::std::numeric_limits<double>::epsilon() ) {
158  if ( vel_r.norm() != 0.0 ) {
159  hrp::Matrix33 tmpm;
160  Eigen::AngleAxis<double> tmpr(vel_r.norm(), vel_r.normalized());
161  rats::rotm3times(tmpm, tmpr.toRotationMatrix(), current_r1);
162  output_r1 = tmpm;
163  } else {
164  output_r1 = current_r1;
165  }
166 
167  target_p2 = target_p1;
168  target_r2 = target_r1;
169  target_p1 = target_p0;
170  target_r1 = target_r0;
171  };
172 };
173 #endif // IMPEDANCETRGETGENERATOR_H
ImpedanceOutputGenerator::calcTargetVelocityOrg
void calcTargetVelocityOrg(hrp::Vector3 &vel_p, hrp::Vector3 &vel_r, const hrp::Matrix33 &eeR, const hrp::Vector3 &force_diff, const hrp::Vector3 &moment_diff, const double _dt, const bool printp=false, const std::string &print_str="", const std::string &ee_name="")
Definition: ImpedanceOutputGenerator.h:44
rats::difference_rotation
void difference_rotation(hrp::Vector3 &ret_dif_rot, const hrp::Matrix33 &self_rot, const hrp::Matrix33 &target_rot)
Definition: RatsMatrix.cpp:40
ImpedanceOutputGenerator::getOutputPos
const hrp::Vector3 & getOutputPos()
Definition: ImpedanceOutputGenerator.h:29
Matrix33
OpenHRP::matrix33 Matrix33
hrp::Vector3
Eigen::Vector3d Vector3
hrp::Matrix33
Eigen::Matrix3d Matrix33
Vector3
OpenHRP::vector3 Vector3
rats::rotm3times
void rotm3times(hrp::Matrix33 &m12, const hrp::Matrix33 &m1, const hrp::Matrix33 &m2)
Definition: RatsMatrix.cpp:31
ImpedanceOutputGenerator::getOutputRot
const hrp::Matrix33 & getOutputRot()
Definition: ImpedanceOutputGenerator.h:28
ImpedanceOutputGenerator::calcTargetVelocity
void calcTargetVelocity(hrp::Vector3 &vel_p, hrp::Vector3 &vel_r, const hrp::Matrix33 &eeR, const hrp::Vector3 &force_diff, const hrp::Vector3 &moment_diff, const double _dt, const bool printp=false, const std::string &print_str="", const std::string &ee_name="")
Definition: ImpedanceOutputGenerator.h:115

hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:50