1 // **************************************************************************
2 // FILENAME: Robot.cpp
3 // AUTHOR:   Kenji Suzuki
4 // DATE:     15 June 1993
5 // **************************************************************************
6 
7 #include "Robot.hpp"
8 
9 // **************************************************************************
10 // Robot Class Member Function
11 // Constructor
12 // **************************************************************************
Robot()13 Robot::Robot()
14 {
15   body = Body();
16   leg1 = Leg(1);
17   leg2 = Leg(2);
18   leg3 = Leg(3);
19   leg4 = Leg(4);
20   leg5 = Leg(5);
21   leg6 = Leg(6);
22   H_matrix_B2W = MatrixMy(4, 4);
23   H_matrix_B2W.Set_H_Matrix(body.W_body_pos.x,
24                             body.W_body_pos.y,
25                             body.W_body_pos.z,
26                             body.W_body_pos.roll,
27                             body.W_body_pos.elevation,
28                             body.W_body_pos.azimuth);
29   H_matrix_W2B = MatrixMy(4, 4);
30   H_matrix_W2B = H_matrix_B2W.Inverse();
31 
32   R_matrix_B2W = MatrixMy(3, 3);
33   R_matrix_B2W.Set_Hr_Matrix(body.W_body_pos.roll,
34                              body.W_body_pos.elevation,
35                              body.W_body_pos.azimuth);
36   R_matrix_W2B = MatrixMy(3, 3);
37   R_matrix_W2B = R_matrix_B2W.Inverse();
38   kinematic_phase_variable = 0.0;
39   kinematic_period = 5.0;
40 //  support_period = kinematic_period*duty_factor;
41 //  transfer_period = kinematic_period - support_period;
42 }
43 
44 
45 
46 // **************************************************************************
47 // Robot Class Member Function
48 // Constructor
49 // **************************************************************************
Robot(double xx,double yy,double zz,double rl,double el,double az)50 Robot::Robot(double xx, double yy, double zz,
51              double rl, double el, double az)
52 {
53   body = Body(xx, yy, zz, rl, el, az);
54   leg1 = Leg(1);
55   leg2 = Leg(2);
56   leg3 = Leg(3);
57   leg4 = Leg(4);
58   leg5 = Leg(5);
59   leg6 = Leg(6);
60   H_matrix_B2W = MatrixMy(4, 4);
61   H_matrix_B2W.Set_H_Matrix(body.W_body_pos.x,
62                             body.W_body_pos.y,
63                             body.W_body_pos.z,
64                             body.W_body_pos.roll,
65                             body.W_body_pos.elevation,
66                             body.W_body_pos.azimuth);
67   H_matrix_W2B = MatrixMy(4, 4);
68   H_matrix_W2B = H_matrix_B2W.Inverse();
69   R_matrix_B2W = MatrixMy(3, 3);
70   R_matrix_B2W.Set_Hr_Matrix(body.W_body_pos.roll,
71                              body.W_body_pos.elevation,
72                              body.W_body_pos.azimuth);
73   R_matrix_W2B = MatrixMy(3, 3);
74   R_matrix_W2B = R_matrix_B2W.Inverse();
75   kinematic_phase_variable = 0.0;
76   kinematic_period = 5.0;
77 //   support_period = kinematic_period*duty_factor;
78 //   transfer_period = kinematic_period - support_period;
79 
80 } //   End of Robot::Robot()
81 
82 
83 
84 // **************************************************************************
85 // FUNCTION: Resize_CWV()
86 // AUTHOR:   Kan Yoneda and Kenji Suzuki.
87 // DATE:     16 Aug 1993.
88 // **************************************************************************
Resize_CWV()89 void Robot::Resize_CWV()
90 {
91   double body_speed;
92   double v0 = 10.0; // velocity constant.[cm/sec]
93   body_speed = sqrt( sqr(body.B_body_vel.x) + sqr(body.B_body_vel.y));
94 
95   leg1.cwv_radius =
96   leg2.cwv_radius =
97   leg3.cwv_radius =
98   leg4.cwv_radius =
99   leg5.cwv_radius =
100   leg6.cwv_radius = 0.8*R_CWV*(1.0 - exp(-body_speed/v0)) + 0.2*R_CWV;
101 
102 
103 } // End of Resize_CWV()
104 
105 
106 
107 // **************************************************************************
108 // FUNCTION: Find_Duty_Factor().
109 // AUTHOR:   Kan Yoneda and Kenji Suzuki.
110 // DATE:     09 Aug 1993.
111 // **************************************************************************
Find_Duty_Factor()112 void Robot::Find_Duty_Factor()
113 {
114   double body_speed; // scaler body velocity
115   double leg_arrange_radius = STANCE;
116   double max_leg_transfer_vel = MAX_TRANSFER_B_FOOT_VEL;
117 
118 
119   // Compute scaler body speed
120   body_speed = sqrt( sqr(body.B_body_vel.x) + sqr(body.B_body_vel.y) ) +
121                leg_arrange_radius*fabs(body.B_body_vel.azimuth);
122 
123   // Compute duty factor
124   duty_factor = max_leg_transfer_vel/(max_leg_transfer_vel + body_speed);
125 
126   leg1.duty_factor = duty_factor;
127   leg2.duty_factor = duty_factor;
128   leg3.duty_factor = duty_factor;
129   leg4.duty_factor = duty_factor;
130   leg5.duty_factor = duty_factor;
131   leg6.duty_factor = duty_factor;
132 
133 } // End of Find_duty_factor()
134 
135 
136 
137 
138 // **************************************************************************
139 // FUNCTION: void Find_Relative_Phase().
140 // AUTHOR:   Kan Yoneda and Kenji Suzuki.
141 // DATE:     09 Aug 1993.
142 // **************************************************************************
Find_Relative_Phase()143 void Robot::Find_Relative_Phase()
144 {
145   double crab_angle;
146   double relative_alpha; // crab angle from each leg
147   double relative_phase[7];
148   double f23;            //relative_pahse_beta_2of3
149   int lg;
150   double beta = leg1.duty_factor;
151 
152 
153   for (lg = 1; lg <= 6; lg++) {
154 
155     // Find CrabAngle
156     if ((body.B_body_vel.x == 0.0) || (body.B_body_vel.y == 0.0)) {
157       crab_angle = 0.0;
158     }
159     else {
160       crab_angle = atan2( body.B_body_vel.y, body.B_body_vel.x );
161     }
162 
163     relative_alpha = 2.0*PI*
164                      mod1( (crab_angle - double(lg - 1)*PI/3.0)/(2.0*PI) );
165 
166     // Find RelativePhase
167     if( relative_alpha < PI*(1.0/6.0) ) {
168       f23 = 0.0;
169     }
170     else if( relative_alpha < PI*(5.0/6.0) ) {
171       f23 = 0.5*((relative_alpha/PI) - (1.0/6.0));
172     }
173     else if( relative_alpha < PI*(7.0/6.0) ) {
174       f23 = 1.0/3.0;
175     }
176     else if( relative_alpha < PI*(11.0/6.0) ) {
177       f23 = (1.0/3.0) - 0.5*((relative_alpha/PI) - (7.0/6.0));
178     }
179     else {
180       f23 = 0.0;
181     }
182 
183     //
184     relative_phase[lg] =
185       (6.0*f23 - (3.0/2.0))*(beta - 0.5) + (1.0/4.0);
186 
187     // shift relative phase 0.5
188     if ( (lg == 2) || (lg == 4) || (lg == 6) ) {
189       relative_phase[lg] -= 0.5;
190     }
191 
192     relative_phase[lg] = mod1( relative_phase[lg] );
193   }
194 
195   leg1.relative_phase = relative_phase[1];
196   leg2.relative_phase = relative_phase[2];
197   leg3.relative_phase = relative_phase[3];
198   leg4.relative_phase = relative_phase[4];
199   leg5.relative_phase = relative_phase[5];
200   leg6.relative_phase = relative_phase[6];
201 
202 
203 // relative phase
204 //    |
205 //    |
206 //    |         _____
207 //    |        /     \            /
208 //    |       /       \          /
209 //    |      /         \        /
210 //    |  ___/           \______/
211 //    |
212 //    ----------------------------------> CrabAngle
213 //   0     (1/6)PI   ...          2PI
214 //
215 //  Ask Yoneda for detail.
216 
217 
218 } // End of find_relative_phase( void )
219 
220 
221 
222 // **************************************************************************
223 // void Robot::Support_Phase_Block()
224 // **************************************************************************
Support_Phase_Block(double sampling_time)225 void Robot::Support_Phase_Block(double sampling_time)
226 {
227 
228   // Find Maximum Instantaneous Support Period
229   leg1.Find_Max_Instantaneous_Support_Period_Sequence(body.B_body_vel);
230   leg2.Find_Max_Instantaneous_Support_Period_Sequence(body.B_body_vel);
231   leg3.Find_Max_Instantaneous_Support_Period_Sequence(body.B_body_vel);
232   leg4.Find_Max_Instantaneous_Support_Period_Sequence(body.B_body_vel);
233   leg5.Find_Max_Instantaneous_Support_Period_Sequence(body.B_body_vel);
234   leg6.Find_Max_Instantaneous_Support_Period_Sequence(body.B_body_vel);
235 
236 
237   // Update CWV Intersection Position(Predicted liftoff point)
238   // (BODY->WORLD coordinates).
239   leg1.W_foothold = H_matrix_B2W*leg1.B_foothold;
240   leg2.W_foothold = H_matrix_B2W*leg2.B_foothold;
241   leg3.W_foothold = H_matrix_B2W*leg3.B_foothold;
242   leg4.W_foothold = H_matrix_B2W*leg4.B_foothold;
243   leg5.W_foothold = H_matrix_B2W*leg5.B_foothold;
244   leg6.W_foothold = H_matrix_B2W*leg6.B_foothold;
245 
246 
247   // Find Kinematic Period
248   Find_Kinematic_Period();
249 
250 
251   // Find Kinematic Phase Variable
252   kinematic_phase_variable =
253     fmod(kinematic_phase_variable + sampling_time/kinematic_period, 1.0);
254 
255 
256   // Find Leg Phase Variable
257   leg1.Find_Leg_Phase_Var(kinematic_phase_variable);
258   leg2.Find_Leg_Phase_Var(kinematic_phase_variable);
259   leg3.Find_Leg_Phase_Var(kinematic_phase_variable);
260   leg4.Find_Leg_Phase_Var(kinematic_phase_variable);
261   leg5.Find_Leg_Phase_Var(kinematic_phase_variable);
262   leg6.Find_Leg_Phase_Var(kinematic_phase_variable);
263 
264 
265   // Find Support Phase Variable
266   leg1.Find_Support_Phase_Var(kinematic_phase_variable);
267   leg2.Find_Support_Phase_Var(kinematic_phase_variable);
268   leg3.Find_Support_Phase_Var(kinematic_phase_variable);
269   leg4.Find_Support_Phase_Var(kinematic_phase_variable);
270   leg5.Find_Support_Phase_Var(kinematic_phase_variable);
271   leg6.Find_Support_Phase_Var(kinematic_phase_variable);
272 
273 
274   // Compute Foot Velocity (WORLD coordinates)
275   leg1.W_foot_vel = Vector(0.0, 0.0, 0.0);
276   leg2.W_foot_vel = Vector(0.0, 0.0, 0.0);
277   leg3.W_foot_vel = Vector(0.0, 0.0, 0.0);
278   leg4.W_foot_vel = Vector(0.0, 0.0, 0.0);
279   leg5.W_foot_vel = Vector(0.0, 0.0, 0.0);
280   leg6.W_foot_vel = Vector(0.0, 0.0, 0.0);
281 
282 
283 } // End of Robot::Support_Phase_Block()
284 
285 
286 
287 // **************************************************************************
288 // FUNCTION: Find_Kinematic_Period()
289 // **************************************************************************
Find_Kinematic_Period()290 void Robot::Find_Kinematic_Period()
291 {
292   kinematic_period
293     = aqmin(leg1.max_instantaneous_support_period/
294           leg1.duty_factor,
295           aqmin(leg2.max_instantaneous_support_period/
296               leg2.duty_factor,
297               aqmin(leg3.max_instantaneous_support_period/
298                   leg3.duty_factor,
299                   aqmin(leg4.max_instantaneous_support_period/
300                       leg4.duty_factor,
301                       aqmin(leg5.max_instantaneous_support_period/
302                           leg5.duty_factor,
303                           leg6.max_instantaneous_support_period/
304                           leg6.duty_factor)
305                       )
306                   )
307               )
308           );
309 
310 } // End of Find Kinematic_Period()
311 
312 
313 
314 // **************************************************************************
315 // void Robot::Transfer_Phase_Block()
316 // **************************************************************************
Transfer_Phase_Block()317 void Robot::Transfer_Phase_Block()
318 {
319   leg1.Transfer_Phase_Sequence(body.B_body_vel,
320                                R_matrix_B2W,
321                                R_matrix_W2B,
322                                kinematic_period);
323 
324   leg2.Transfer_Phase_Sequence(body.B_body_vel,
325                                R_matrix_B2W,
326                                R_matrix_W2B,
327                                kinematic_period);
328 
329   leg3.Transfer_Phase_Sequence(body.B_body_vel,
330                                R_matrix_B2W,
331                                R_matrix_W2B,
332                                kinematic_period);
333 
334   leg4.Transfer_Phase_Sequence(body.B_body_vel,
335                                R_matrix_B2W,
336                                R_matrix_W2B,
337                                kinematic_period);
338 
339   leg5.Transfer_Phase_Sequence(body.B_body_vel,
340                                R_matrix_B2W,
341                                R_matrix_W2B,
342                                kinematic_period);
343 
344   leg6.Transfer_Phase_Sequence(body.B_body_vel,
345                                R_matrix_B2W,
346                                R_matrix_W2B,
347                                kinematic_period);
348 
349 } // End of Robot::Transfer_Phase_Block()
350 
351 
352 
353 // **************************************************************************
354 // FUNCTION: Update_H_matrix()
355 // **************************************************************************
Update_H_Matrix()356 void Robot::Update_H_Matrix()
357 {
358   H_matrix_B2W.Set_H_Matrix(body.W_body_pos.x,
359                             body.W_body_pos.y,
360                             body.W_body_pos.z,
361                             body.W_body_pos.roll,
362                             body.W_body_pos.elevation,
363                             body.W_body_pos.azimuth);
364 
365   H_matrix_W2B = H_matrix_B2W.Inverse();
366 
367   R_matrix_B2W.Set_Hr_Matrix(body.W_body_pos.roll,
368                              body.W_body_pos.elevation,
369                              body.W_body_pos.azimuth);
370 
371   R_matrix_W2B = R_matrix_B2W.Inverse();
372 
373 } // End of Robot::Update_H_Matrix()
374 
375 
376 
377 
378 // **************************************************************************
379 // void Robot::Update_Body_Posture_Block()
380 // **************************************************************************
Update_Body_Posture_Block(double sampling_time)381 void Robot::Update_Body_Posture_Block(double sampling_time)
382 {
383   Vector W_body_position(body.W_body_pos.x,
384                          body.W_body_pos.y,
385                          body.W_body_pos.z);
386   Vector W_body_eulerang(body.W_body_pos.roll,
387                          body.W_body_pos.elevation,
388                          body.W_body_pos.azimuth);
389 
390   Vector B_body_trans_rate(body.B_body_vel.x,
391                            body.B_body_vel.y,
392                            body.B_body_vel.z);
393   Vector B_body_euler_rate(body.B_body_vel.roll,
394                            body.B_body_vel.elevation,
395                            body.B_body_vel.azimuth);
396 
397   Vector W_body_trans_rate;
398   Vector W_body_euler_rate;
399 
400   W_body_trans_rate = R_matrix_B2W*B_body_trans_rate;
401   W_body_euler_rate = R_matrix_B2W*B_body_euler_rate;
402 
403   body.W_body_vel = Posture(W_body_trans_rate.x,
404                             W_body_trans_rate.y,
405                             W_body_trans_rate.z,
406                             W_body_euler_rate.x,
407                             W_body_euler_rate.y,
408                             W_body_euler_rate.z);
409 
410 
411   // Update body posture(WORLD frame)
412   W_body_position = W_body_position + W_body_trans_rate*sampling_time;
413   W_body_eulerang = W_body_eulerang + W_body_euler_rate*sampling_time;
414   body.W_body_pos = Posture(W_body_position.x,
415                             W_body_position.y,
416                             W_body_position.z,
417                             W_body_eulerang.x,
418                             W_body_eulerang.y,
419                             W_body_eulerang.z);
420 
421 } // End of Robot::Update_Body_Posture()
422 
423 
424 
425 // **************************************************************************
426 // void Robot::Update_CWV_Position_Block()
427 // **************************************************************************
Update_CWV_Position_Block()428 void Robot::Update_CWV_Position_Block()
429 {
430   leg1.W_cwv_pos = H_matrix_B2W*leg1.B_cwv_pos;
431   leg2.W_cwv_pos = H_matrix_B2W*leg2.B_cwv_pos;
432   leg3.W_cwv_pos = H_matrix_B2W*leg3.B_cwv_pos;
433   leg4.W_cwv_pos = H_matrix_B2W*leg4.B_cwv_pos;
434   leg5.W_cwv_pos = H_matrix_B2W*leg5.B_cwv_pos;
435   leg6.W_cwv_pos = H_matrix_B2W*leg6.B_cwv_pos;
436 
437 } // End of Robot::Update_CWV_Position()
438 
439 
440 
441 // **************************************************************************
442 // FUNCTION: Robot::Update_Foot_Position_Block(double sampling_time)
443 // **************************************************************************
Update_Foot_Position_Block(double sampling_time)444 void Robot::Update_Foot_Position_Block(double sampling_time)
445 {
446   leg1.Update_Foot_Position(H_matrix_W2B, sampling_time);
447   leg2.Update_Foot_Position(H_matrix_W2B, sampling_time);
448   leg3.Update_Foot_Position(H_matrix_W2B, sampling_time);
449   leg4.Update_Foot_Position(H_matrix_W2B, sampling_time);
450   leg5.Update_Foot_Position(H_matrix_W2B, sampling_time);
451   leg6.Update_Foot_Position(H_matrix_W2B, sampling_time);
452 
453 } // End of Update_Foot_Position_Block()
454 
455 
456 
457 // **************************************************************************
458 // FUNCTION:Inverse_Jacobian_Block()
459 // **************************************************************************
Inverse_Jacobian_Block()460 void Robot::Inverse_Jacobian_Block()
461 {
462   leg1.Inv_Jacobian();
463   leg2.Inv_Jacobian();
464   leg3.Inv_Jacobian();
465   leg4.Inv_Jacobian();
466   leg5.Inv_Jacobian();
467   leg6.Inv_Jacobian();
468 
469 } // End of Inverse_Jacobian_Block()
470 
471 
472 
473 // **************************************************************************
474 // FUNCTION:Inverse_Kinematics_Block()
475 // **************************************************************************
Inverse_Kinematics_Block()476 void Robot::Inverse_Kinematics_Block()
477 {
478   leg1.Inv_Kinematics();
479   leg1.Update_Joint_Position(H_matrix_B2W);
480 
481   leg2.Inv_Kinematics();
482   leg2.Update_Joint_Position(H_matrix_B2W);
483 
484   leg3.Inv_Kinematics();
485   leg3.Update_Joint_Position(H_matrix_B2W);
486 
487   leg4.Inv_Kinematics();
488   leg4.Update_Joint_Position(H_matrix_B2W);
489 
490   leg5.Inv_Kinematics();
491   leg5.Update_Joint_Position(H_matrix_B2W);
492 
493   leg6.Inv_Kinematics();
494   leg6.Update_Joint_Position(H_matrix_B2W);
495 
496 } // End of Inverse_Kinematics_Block()
497 
498 
499 
500 // **************************************************************************
501 // FUNCTION:Update_Joint_Angle_Block(double sampling_time)
502 // **************************************************************************
Update_Joint_Angle_Block(double sampling_time)503 void Robot::Update_Joint_Angle_Block(double sampling_time)
504 {
505   leg1.Update_Joint_Angle(H_matrix_B2W, sampling_time);
506   leg2.Update_Joint_Angle(H_matrix_B2W, sampling_time);
507   leg3.Update_Joint_Angle(H_matrix_B2W, sampling_time);
508   leg4.Update_Joint_Angle(H_matrix_B2W, sampling_time);
509   leg5.Update_Joint_Angle(H_matrix_B2W, sampling_time);
510   leg6.Update_Joint_Angle(H_matrix_B2W, sampling_time);
511 
512 } // End of Update_Joint_Angle_Block()
513 
514 
515 // EOF
516