| 348 | } |
| 349 | |
| 350 | void Control() { |
| 351 | // set rpy of observation; for projected gravity later |
| 352 | |
| 353 | MotorCommand motor_command_tmp; |
| 354 | const std::shared_ptr<const MotorState> ms = motor_state_buffer_.GetData(); |
| 355 | |
| 356 | for (int i = 0; i < G1_NUM_MOTOR; ++i) { |
| 357 | motor_command_tmp.tau_ff.at(i) = 0.0; |
| 358 | motor_command_tmp.q_target.at(i) = 0.0; |
| 359 | motor_command_tmp.dq_target.at(i) = 0.0; |
| 360 | motor_command_tmp.kp.at(i) = 0.0; |
| 361 | motor_command_tmp.kd.at(i) = 0.0; |
| 362 | } |
| 363 | |
| 364 | if (ms) { |
| 365 | time_ += control_dt_; |
| 366 | if (time_ < duration_) { |
| 367 | // [Stage 1]: set robot to zero posture |
| 368 | for (int i = 0; i < G1_NUM_MOTOR; ++i) { |
| 369 | double ratio = std::clamp(time_ / duration_, 0.0, 1.0); |
| 370 | |
| 371 | double q_des = 0; |
| 372 | motor_command_tmp.tau_ff.at(i) = 0.0; |
| 373 | motor_command_tmp.q_target.at(i) = |
| 374 | (q_des - ms->q.at(i)) * ratio + ms->q.at(i); |
| 375 | motor_command_tmp.dq_target.at(i) = 0.0; |
| 376 | motor_command_tmp.kp.at(i) = Kp[i]; |
| 377 | motor_command_tmp.kd.at(i) = Kd[i]; |
| 378 | } |
| 379 | } else { |
| 380 | const std::shared_ptr<const ImuState> imu_tmp_ptr = imu_state_buffer_.GetData(); |
| 381 | if (imu_tmp_ptr) { |
| 382 | for (int i = 0; i < 3; i++){ |
| 383 | body_state_simple.rpy[i] = imu_tmp_ptr->rpy.at(i); |
| 384 | body_state_simple.omegaBody[i] = imu_tmp_ptr->omega.at(i); |
| 385 | body_state_simple.aBody[i] = imu_tmp_ptr->abody.at(i); |
| 386 | } |
| 387 | } |
| 388 | |
| 389 | const std::shared_ptr<const MotorState> ms = motor_state_buffer_.GetData(); |
| 390 | if (ms){ |
| 391 | for (int i = 0; i < G1_NUM_MOTOR; ++i){ |
| 392 | joint_state_simple.q[i] = ms->q.at(i); |
| 393 | joint_state_simple.qd[i] = ms->dq.at(i); |
| 394 | } |
| 395 | } |
| 396 | _simpleLCM.publish("state_estimator_data", &body_state_simple); |
| 397 | _simpleLCM.publish("body_control_data", &joint_state_simple); |
| 398 | |
| 399 | |
| 400 | |
| 401 | for (int i = 0; i < 15; ++i){ |
| 402 | motor_command_tmp.q_target.at(i) = joint_command_simple.q_des[i]; |
| 403 | motor_command_tmp.dq_target.at(i) = 0.0; |
| 404 | motor_command_tmp.kp.at(i) = Kp[i]; |
| 405 | motor_command_tmp.kd.at(i) = Kd[i]; |
| 406 | |
| 407 | } |