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