online_walking_module.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2017 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 /* Author: SCH */
18 
20 
21 using namespace robotis_op;
22 
24  : control_cycle_sec_(0.008),
25  is_moving_(false),
26  is_balancing_(false),
27  is_offset_updating_(false),
28  goal_initialize_(false),
29  balance_control_initialize_(false),
30  body_offset_initialize_(false),
31  joint_control_initialize_(false),
32  wholebody_initialize_(false),
33  walking_initialize_(false),
34  is_foot_step_2d_(false),
35  walking_phase_(DSP),
36  total_mass_(3.5),
37  foot_distance_(0.07)
38 {
39  enable_ = false;
40  module_name_ = "online_walking_module";
44 
45  op3_kdl_ = new OP3Kinematics();
46 
47  /* leg */
48  result_["r_hip_yaw"] = new robotis_framework::DynamixelState();
49  result_["r_hip_roll"] = new robotis_framework::DynamixelState();
50  result_["r_hip_pitch"] = new robotis_framework::DynamixelState();
52  result_["r_ank_pitch"] = new robotis_framework::DynamixelState();
53  result_["r_ank_roll"] = new robotis_framework::DynamixelState();
54  result_["l_hip_yaw"] = new robotis_framework::DynamixelState();
55  result_["l_hip_roll"] = new robotis_framework::DynamixelState();
56  result_["l_hip_pitch"] = new robotis_framework::DynamixelState();
58  result_["l_ank_pitch"] = new robotis_framework::DynamixelState();
59  result_["l_ank_roll"] = new robotis_framework::DynamixelState();
60 
61  /* leg */
62  joint_name_to_id_["r_hip_yaw"] = 1;
63  joint_name_to_id_["l_hip_yaw"] = 2;
64  joint_name_to_id_["r_hip_roll"] = 3;
65  joint_name_to_id_["l_hip_roll"] = 4;
66  joint_name_to_id_["r_hip_pitch"] = 5;
67  joint_name_to_id_["l_hip_pitch"] = 6;
68  joint_name_to_id_["r_knee"] = 7;
69  joint_name_to_id_["l_knee"] = 8;
70  joint_name_to_id_["r_ank_pitch"] = 9;
71  joint_name_to_id_["l_ank_pitch"] = 10;
72  joint_name_to_id_["r_ank_roll"] = 11;
73  joint_name_to_id_["l_ank_roll"] = 12;
74 
75  /* parameter */
76  number_of_joints_ = 12;
77 
81 
83  des_joint_vel_.resize(number_of_joints_, 0.0);
84  des_joint_pos_.resize(number_of_joints_, 0.0);
85 
89 
93 
95 
96  // body position default
97  des_body_pos_.resize(3, 0.0);
98  des_body_vel_.resize(3, 0.0);
99  des_body_accel_.resize(3, 0.0);
100  des_body_Q_.resize(4, 0.0);
101 
102  // left foot position default
103  des_l_leg_pos_.resize(3, 0.0);
104  des_l_leg_vel_.resize(3, 0.0);
105  des_l_leg_accel_.resize(3, 0.0);
106  des_l_leg_Q_.resize(4, 0.0);
107 
108  // right foot position default
109  des_r_leg_pos_.resize(3, 0.0);
110  des_r_leg_vel_.resize(3, 0.0);
111  des_r_leg_accel_.resize(3, 0.0);
112  des_r_leg_Q_.resize(4, 0.0);
113 
114  x_lipm_.resize(3, 0.0);
115  y_lipm_.resize(3, 0.0);
116 
117  resetBodyPose();
118 
119  // walking parameter default
120  walking_param_.dsp_ratio = 0.2;
121  walking_param_.lipm_height = 0.12;
122  walking_param_.foot_height_max = 0.05;
123  walking_param_.zmp_offset_x = 0.0; // not applied
124  walking_param_.zmp_offset_y = 0.0;
125 
126  des_balance_gain_ratio_.resize(1, 0.0);
127  goal_balance_gain_ratio_.resize(1, 0.0);
128 
130  balance_control_.setGyroBalanceEnable(false); // Gyro
133 
140 
147 
148  // Body Offset
149  des_body_offset_.resize(3, 0.0);
150  goal_body_offset_.resize(3, 0.0);
151 
152  std::string balance_gain_path = ros::package::getPath("op3_online_walking_module") + "/config/balance_gain.yaml";
153  parseBalanceGainData(balance_gain_path);
154 
155  std::string joint_feedback_gain_path = ros::package::getPath("op3_online_walking_module") + "/config/joint_feedback_gain.yaml";
156  parseJointFeedbackGainData(joint_feedback_gain_path);
157 
158  std::string joint_feedforward_gain_path = ros::package::getPath("op3_online_walking_module") + "/config/joint_feedforward_gain.yaml";
159  parseJointFeedforwardGainData(joint_feedforward_gain_path);
160 }
161 
163 {
164  queue_thread_.join();
165 }
166 
167 void OnlineWalkingModule::initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
168 {
169  control_cycle_sec_ = control_cycle_msec * 0.001;
170  queue_thread_ = boost::thread(boost::bind(&OnlineWalkingModule::queueThread, this));
171 
172  ros::NodeHandle ros_node;
173 
174  // Publisher
175  status_msg_pub_ = ros_node.advertise<robotis_controller_msgs::StatusMsg>("/robotis/status", 1);
176  movement_done_pub_ = ros_node.advertise<std_msgs::String>("/robotis/movement_done", 1);
177  goal_joint_state_pub_ = ros_node.advertise<sensor_msgs::JointState>("/robotis/online_walking/goal_joint_states", 1);
178  pelvis_pose_pub_ = ros_node.advertise<geometry_msgs::PoseStamped>("/robotis/pelvis_pose", 1);
179 
180  // Service
181 // get_preview_matrix_client_ = ros_node.serviceClient<op3_online_walking_module_msgs::GetPreviewMatrix>("/robotis/online_walking/get_preview_matrix", 0);
182 }
183 
185 {
186  ros::NodeHandle ros_node;
187  ros::CallbackQueue callback_queue;
188 
189  ros_node.setCallbackQueue(&callback_queue);
190 
191  // Subscriber
192  ros::Subscriber reset_body_sub_ = ros_node.subscribe("/robotis/online_walking/reset_body", 5,
194  ros::Subscriber joint_pose_sub_ = ros_node.subscribe("/robotis/online_walking/goal_joint_pose", 5,
196  ros::Subscriber kinematics_pose_sub_ = ros_node.subscribe("/robotis/online_walking/goal_kinematics_pose", 5,
198  ros::Subscriber foot_step_command_sub_ = ros_node.subscribe("/robotis/online_walking/foot_step_command", 5,
200  ros::Subscriber walking_param_sub_ = ros_node.subscribe("/robotis/online_walking/walking_param", 5,
202  ros::Subscriber wholebody_balance_msg_sub = ros_node.subscribe("/robotis/online_walking/wholebody_balance_msg", 5,
204  ros::Subscriber body_offset_msg_sub = ros_node.subscribe("/robotis/online_walking/body_offset", 5,
206  ros::Subscriber foot_distance_msg_sub = ros_node.subscribe("/robotis/online_walking/foot_distance", 5,
208 
209  ros::Subscriber footsteps_sub = ros_node.subscribe("/robotis/online_walking/footsteps_2d", 5,
211 
212  ros::Subscriber imu_data_sub = ros_node.subscribe("/robotis/sensor/imu/imu", 5,
214  ros::Subscriber l_foot_ft_sub = ros_node.subscribe("/robotis/sensor/l_foot_ft", 3,
216  ros::Subscriber r_foot_ft_sub = ros_node.subscribe("/robotis/sensor/r_foot_ft", 3,
218 
219  // Service
220  ros::ServiceServer get_joint_pose_server = ros_node.advertiseService("/robotis/online_walking/get_joint_pose",
222  ros::ServiceServer get_kinematics_pose_server = ros_node.advertiseService("/robotis/online_walking/get_kinematics_pose",
224 
226  while(ros_node.ok())
227  callback_queue.callAvailable(duration);
228 }
229 
231 {
232  des_body_pos_[0] = 0.0;
233  des_body_pos_[1] = 0.0;
234  des_body_pos_[2] = 0.3402256;
235 
236  des_body_Q_[0] = 0.0;
237  des_body_Q_[1] = 0.0;
238  des_body_Q_[2] = 0.0;
239  des_body_Q_[3] = 1.0;
240 
241  des_r_leg_pos_[0] = 0.0;
242  des_r_leg_pos_[1] = -0.5 * foot_distance_; //-0.045; //-0.035;
243  des_r_leg_pos_[2] = 0.0;
244 
245  des_r_leg_Q_[0] = 0.0;
246  des_r_leg_Q_[1] = 0.0;
247  des_r_leg_Q_[2] = 0.0;
248  des_r_leg_Q_[3] = 1.0;
249 
250  des_l_leg_pos_[0] = 0.0;
251  des_l_leg_pos_[1] = 0.5 * foot_distance_; //0.045; //0.035;
252  des_l_leg_pos_[2] = 0.0;
253 
254  des_l_leg_Q_[0] = 0.0;
255  des_l_leg_Q_[1] = 0.0;
256  des_l_leg_Q_[2] = 0.0;
257  des_l_leg_Q_[3] = 1.0;
258 
259  x_lipm_[0] = des_body_pos_[0];
260  x_lipm_[1] = 0.0;
261  x_lipm_[2] = 0.0;
262 
263  y_lipm_[0] = des_body_pos_[1];
264  y_lipm_[1] = 0.0;
265  y_lipm_[2] = 0.0;
266 
267  walking_param_.zmp_offset_x = des_body_pos_[0];
268 }
269 
270 void OnlineWalkingModule::parseBalanceGainData(const std::string &path)
271 {
272  YAML::Node doc;
273  try
274  {
275  // load yaml
276  doc = YAML::LoadFile(path.c_str());
277  }
278  catch (const std::exception& e)
279  {
280  ROS_ERROR("Fail to load yaml file.");
281  return;
282  }
283 
284  // ROS_INFO("Parse Balance Gain Data");
285 
286  foot_roll_gyro_p_gain_ = doc["foot_roll_gyro_p_gain"].as<double>();
287  foot_roll_gyro_d_gain_ = doc["foot_roll_gyro_d_gain"].as<double>();
288  foot_pitch_gyro_p_gain_ = doc["foot_pitch_gyro_p_gain"].as<double>();
289  foot_pitch_gyro_d_gain_ = doc["foot_pitch_gyro_d_gain"].as<double>();
290 
291  foot_roll_angle_p_gain_ = doc["foot_roll_angle_p_gain"].as<double>();
292  foot_roll_angle_d_gain_ = doc["foot_roll_angle_d_gain"].as<double>();
293  foot_pitch_angle_p_gain_ = doc["foot_pitch_angle_p_gain"].as<double>();
294  foot_pitch_angle_d_gain_ = doc["foot_pitch_angle_d_gain"].as<double>();
295 
296  foot_x_force_p_gain_ = doc["foot_x_force_p_gain"].as<double>();
297  foot_x_force_d_gain_ = doc["foot_x_force_d_gain"].as<double>();
298  foot_y_force_p_gain_ = doc["foot_y_force_p_gain"].as<double>();
299  foot_y_force_d_gain_ = doc["foot_y_force_d_gain"].as<double>();
300  foot_z_force_p_gain_ = doc["foot_z_force_p_gain"].as<double>();
301  foot_z_force_d_gain_ = doc["foot_z_force_d_gain"].as<double>();
302 
303  foot_roll_torque_p_gain_ = doc["foot_roll_torque_p_gain"].as<double>();
304  foot_roll_torque_d_gain_ = doc["foot_roll_torque_d_gain"].as<double>();
305  foot_pitch_torque_p_gain_ = doc["foot_pitch_torque_p_gain"].as<double>();
306  foot_pitch_torque_d_gain_ = doc["foot_pitch_torque_d_gain"].as<double>();
307 
308  roll_gyro_cut_off_frequency_ = doc["roll_gyro_cut_off_frequency"].as<double>();
309  pitch_gyro_cut_off_frequency_ = doc["pitch_gyro_cut_off_frequency"].as<double>();
310 
311  roll_angle_cut_off_frequency_ = doc["roll_angle_cut_off_frequency"].as<double>();
312  pitch_angle_cut_off_frequency_ = doc["pitch_angle_cut_off_frequency"].as<double>();
313 
314  foot_x_force_cut_off_frequency_ = doc["foot_x_force_cut_off_frequency"].as<double>();
315  foot_y_force_cut_off_frequency_ = doc["foot_y_force_cut_off_frequency"].as<double>();
316  foot_z_force_cut_off_frequency_ = doc["foot_z_force_cut_off_frequency"].as<double>();
317 
318  foot_roll_torque_cut_off_frequency_ = doc["foot_roll_torque_cut_off_frequency"].as<double>();
319  foot_pitch_torque_cut_off_frequency_ = doc["foot_pitch_torque_cut_off_frequency"].as<double>();
320 
321  balance_hip_roll_gain_ = doc["balance_hip_roll_gain"].as<double>();
322  balance_knee_gain_ = doc["balance_knee_gain"].as<double>();
323  balance_ankle_roll_gain_ = doc["balance_ankle_roll_gain"].as<double>();
324  balance_ankle_pitch_gain_ = doc["balance_ankle_pitch_gain"].as<double>();
325 }
326 
328 {
329  YAML::Node doc;
330  try
331  {
332  // load yaml
333  doc = YAML::LoadFile(path.c_str());
334  }
335  catch (const std::exception& e)
336  {
337  ROS_ERROR("Fail to load yaml file.");
338  return;
339  }
340 
341  joint_feedback_[joint_name_to_id_["r_hip_yaw"]-1].p_gain_ = doc["r_hip_yaw_p_gain"].as<double>();
342  joint_feedback_[joint_name_to_id_["r_hip_yaw"]-1].d_gain_ = doc["r_hip_yaw_d_gain"].as<double>();
343  joint_feedback_[joint_name_to_id_["r_hip_roll"]-1].p_gain_ = doc["r_hip_roll_p_gain"].as<double>();
344  joint_feedback_[joint_name_to_id_["r_hip_roll"]-1].d_gain_ = doc["r_hip_roll_d_gain"].as<double>();
345  joint_feedback_[joint_name_to_id_["r_hip_pitch"]-1].p_gain_ = doc["r_hip_pitch_p_gain"].as<double>();
346  joint_feedback_[joint_name_to_id_["r_hip_pitch"]-1].d_gain_ = doc["r_hip_pitch_d_gain"].as<double>();
347  joint_feedback_[joint_name_to_id_["r_knee"]-1].p_gain_ = doc["r_knee_p_gain"].as<double>();
348  joint_feedback_[joint_name_to_id_["r_knee"]-1].d_gain_ = doc["r_knee_d_gain"].as<double>();
349  joint_feedback_[joint_name_to_id_["r_ank_pitch"]-1].p_gain_ = doc["r_ank_pitch_p_gain"].as<double>();
350  joint_feedback_[joint_name_to_id_["r_ank_pitch"]-1].d_gain_ = doc["r_ank_pitch_d_gain"].as<double>();
351  joint_feedback_[joint_name_to_id_["r_ank_roll"]-1].p_gain_ = doc["r_ank_roll_p_gain"].as<double>();
352  joint_feedback_[joint_name_to_id_["r_ank_roll"]-1].d_gain_ = doc["r_ank_roll_d_gain"].as<double>();
353 
354  joint_feedback_[joint_name_to_id_["l_hip_yaw"]-1].p_gain_ = doc["l_hip_yaw_p_gain"].as<double>();
355  joint_feedback_[joint_name_to_id_["l_hip_yaw"]-1].d_gain_ = doc["l_hip_yaw_d_gain"].as<double>();
356  joint_feedback_[joint_name_to_id_["l_hip_roll"]-1].p_gain_ = doc["l_hip_roll_p_gain"].as<double>();
357  joint_feedback_[joint_name_to_id_["l_hip_roll"]-1].d_gain_ = doc["l_hip_roll_d_gain"].as<double>();
358  joint_feedback_[joint_name_to_id_["l_hip_pitch"]-1].p_gain_ = doc["l_hip_pitch_p_gain"].as<double>();
359  joint_feedback_[joint_name_to_id_["l_hip_pitch"]-1].d_gain_ = doc["l_hip_pitch_d_gain"].as<double>();
360  joint_feedback_[joint_name_to_id_["l_knee"]-1].p_gain_ = doc["l_knee_p_gain"].as<double>();
361  joint_feedback_[joint_name_to_id_["l_knee"]-1].d_gain_ = doc["l_knee_d_gain"].as<double>();
362  joint_feedback_[joint_name_to_id_["l_ank_pitch"]-1].p_gain_ = doc["l_ank_pitch_p_gain"].as<double>();
363  joint_feedback_[joint_name_to_id_["l_ank_pitch"]-1].d_gain_ = doc["l_ank_pitch_d_gain"].as<double>();
364  joint_feedback_[joint_name_to_id_["l_ank_roll"]-1].p_gain_ = doc["l_ank_roll_p_gain"].as<double>();
365  joint_feedback_[joint_name_to_id_["l_ank_roll"]-1].d_gain_ = doc["l_ank_roll_d_gain"].as<double>();
366 }
367 
369 {
370  YAML::Node doc;
371  try
372  {
373  // load yaml
374  doc = YAML::LoadFile(path.c_str());
375  }
376  catch (const std::exception& e)
377  {
378  ROS_ERROR("Fail to load yaml file.");
379  return;
380  }
381 
382  joint_feedforward_gain_[joint_name_to_id_["r_hip_yaw"]-1] = doc["r_hip_yaw_gain"].as<double>();
383  joint_feedforward_gain_[joint_name_to_id_["r_hip_roll"]-1] = doc["r_hip_roll_gain"].as<double>();
384  joint_feedforward_gain_[joint_name_to_id_["r_hip_pitch"]-1] = doc["r_hip_pitch_gain"].as<double>();
385  joint_feedforward_gain_[joint_name_to_id_["r_knee"]-1] = doc["r_knee_gain"].as<double>();
386  joint_feedforward_gain_[joint_name_to_id_["r_ank_pitch"]-1] = doc["r_ank_pitch_gain"].as<double>();
387  joint_feedforward_gain_[joint_name_to_id_["r_ank_roll"]-1] = doc["r_ank_roll_gain"].as<double>();
388 
389  joint_feedforward_gain_[joint_name_to_id_["l_hip_yaw"]-1] = doc["l_hip_yaw_gain"].as<double>();
390  joint_feedforward_gain_[joint_name_to_id_["l_hip_roll"]-1] = doc["l_hip_roll_gain"].as<double>();
391  joint_feedforward_gain_[joint_name_to_id_["l_hip_pitch"]-1] = doc["l_hip_pitch_gain"].as<double>();
392  joint_feedforward_gain_[joint_name_to_id_["l_knee"]-1] = doc["l_knee_gain"].as<double>();
393  joint_feedforward_gain_[joint_name_to_id_["l_ank_pitch"]-1] = doc["l_ank_pitch_gain"].as<double>();
394  joint_feedforward_gain_[joint_name_to_id_["l_ank_roll"]-1] = doc["l_ank_roll_gain"].as<double>();
395 }
396 
397 void OnlineWalkingModule::setWholebodyBalanceMsgCallback(const std_msgs::String::ConstPtr& msg)
398 {
399  if (enable_ == false)
400  return;
401 
402  std::string balance_gain_path = ros::package::getPath("op3_online_walking_module") + "/config/balance_gain.yaml";
403  parseBalanceGainData(balance_gain_path);
404 
405  std::string joint_feedback_gain_path = ros::package::getPath("op3_online_walking_module") + "/config/joint_feedback_gain.yaml";
406  parseJointFeedbackGainData(joint_feedback_gain_path);
407 
408  std::string joint_feedforward_gain_path = ros::package::getPath("op3_online_walking_module") + "/config/joint_feedforward_gain.yaml";
409  parseJointFeedforwardGainData(joint_feedforward_gain_path);
410 
411  if (msg->data == "balance_on")
412  goal_balance_gain_ratio_[0] = 1.0;
413  else if(msg->data == "balance_off")
414  goal_balance_gain_ratio_[0] = 0.0;
415 
417  balance_type_ = ON;
419 }
420 
422 {
423  if (balance_control_initialize_ == true)
424  return;
425 
427 
428  double ini_time = 0.0;
429  double mov_time = 1.0;
430 
431  balance_step_ = 0;
432  balance_size_ = (int) (mov_time / control_cycle_sec_) + 1;
433 
434  std::vector<double_t> balance_zero;
435  balance_zero.resize(1, 0.0);
436 
437  balance_tra_ =
438  new robotis_framework::MinimumJerk(ini_time, mov_time,
439  des_balance_gain_ratio_, balance_zero, balance_zero,
440  goal_balance_gain_ratio_, balance_zero, balance_zero);
441 
442  if (is_balancing_ == true)
443  ROS_INFO("[UPDATE] Balance Gain");
444  else
445  {
446  is_balancing_ = true;
447  ROS_INFO("[START] Balance Gain");
448  }
449 }
450 
452 {
453  if (is_balancing_ == true)
454  {
455  double cur_time = (double) balance_step_ * control_cycle_sec_;
457 
458  if (balance_step_ == balance_size_-1)
459  {
460  balance_step_ = 0;
461  is_balancing_ = false;
462  delete balance_tra_;
463 
464  if (des_balance_gain_ratio_[0] == 0.0)
465  {
467  balance_type_ = OFF;
468  }
469 
470  ROS_INFO("[END] Balance Gain");
471  }
472  else
473  balance_step_++;
474  }
475 }
476 
477 void OnlineWalkingModule::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg)
478 {
479  imu_data_mutex_lock_.lock();
480 
481  imu_data_msg_ = *msg;
482 
483  imu_data_msg_.angular_velocity.x *= -1.0;
484  imu_data_msg_.angular_velocity.y *= -1.0;
485 
486  imu_data_mutex_lock_.unlock();
487 }
488 
489 void OnlineWalkingModule::leftFootForceTorqueOutputCallback(const geometry_msgs::WrenchStamped::ConstPtr &msg)
490 {
491  Eigen::MatrixXd force = Eigen::MatrixXd::Zero(3,1);
492  force.coeffRef(0,0) = msg->wrench.force.x;
493  force.coeffRef(1,0) = msg->wrench.force.y;
494  force.coeffRef(2,0) = msg->wrench.force.z;
495 
496  Eigen::MatrixXd torque = Eigen::MatrixXd::Zero(3,1);
497  torque.coeffRef(0,0) = msg->wrench.torque.x;
498  torque.coeffRef(1,0) = msg->wrench.torque.y;
499  torque.coeffRef(2,0) = msg->wrench.torque.z;
500 
501  Eigen::MatrixXd force_new = robotis_framework::getRotationX(M_PI)*robotis_framework::getRotationZ(-0.5*M_PI)*force;
502  Eigen::MatrixXd torque_new = robotis_framework::getRotationX(M_PI)*robotis_framework::getRotationZ(-0.5*M_PI)*torque;
503 
504  double l_foot_fx_N = force_new.coeff(0,0);
505  double l_foot_fy_N = force_new.coeff(1,0);
506  double l_foot_fz_N = force_new.coeff(2,0);
507  double l_foot_Tx_Nm = torque_new.coeff(0,0);
508  double l_foot_Ty_Nm = torque_new.coeff(1,0);
509  double l_foot_Tz_Nm = torque_new.coeff(2,0);
510 
511 
512  l_foot_fx_N = robotis_framework::sign(l_foot_fx_N) * fmin( fabs(l_foot_fx_N), 2000.0);
513  l_foot_fy_N = robotis_framework::sign(l_foot_fy_N) * fmin( fabs(l_foot_fy_N), 2000.0);
514  l_foot_fz_N = robotis_framework::sign(l_foot_fz_N) * fmin( fabs(l_foot_fz_N), 2000.0);
515  l_foot_Tx_Nm = robotis_framework::sign(l_foot_Tx_Nm) * fmin(fabs(l_foot_Tx_Nm), 300.0);
516  l_foot_Ty_Nm = robotis_framework::sign(l_foot_Ty_Nm) * fmin(fabs(l_foot_Ty_Nm), 300.0);
517  l_foot_Tz_Nm = robotis_framework::sign(l_foot_Tz_Nm) * fmin(fabs(l_foot_Tz_Nm), 300.0);
518 
519  l_foot_ft_data_msg_.force.x = l_foot_fx_N;
520  l_foot_ft_data_msg_.force.y = l_foot_fy_N;
521  l_foot_ft_data_msg_.force.z = l_foot_fz_N;
522  l_foot_ft_data_msg_.torque.x = l_foot_Tx_Nm;
523  l_foot_ft_data_msg_.torque.y = l_foot_Ty_Nm;
524  l_foot_ft_data_msg_.torque.z = l_foot_Tz_Nm;
525 }
526 
527 void OnlineWalkingModule::rightFootForceTorqueOutputCallback(const geometry_msgs::WrenchStamped::ConstPtr &msg)
528 {
529  Eigen::MatrixXd force = Eigen::MatrixXd::Zero(3,1);
530  force.coeffRef(0,0) = msg->wrench.force.x;
531  force.coeffRef(1,0) = msg->wrench.force.y;
532  force.coeffRef(2,0) = msg->wrench.force.z;
533 
534  Eigen::MatrixXd torque = Eigen::MatrixXd::Zero(3,1);
535  torque.coeffRef(0,0) = msg->wrench.torque.x;
536  torque.coeffRef(1,0) = msg->wrench.torque.y;
537  torque.coeffRef(2,0) = msg->wrench.torque.z;
538 
539  Eigen::MatrixXd force_new = robotis_framework::getRotationX(M_PI)*robotis_framework::getRotationZ(-0.5*M_PI)*force;
540  Eigen::MatrixXd torque_new = robotis_framework::getRotationX(M_PI)*robotis_framework::getRotationZ(-0.5*M_PI)*torque;
541 
542  double r_foot_fx_N = force_new.coeff(0,0);
543  double r_foot_fy_N = force_new.coeff(1,0);
544  double r_foot_fz_N = force_new.coeff(2,0);
545  double r_foot_Tx_Nm = torque_new.coeff(0,0);
546  double r_foot_Ty_Nm = torque_new.coeff(1,0);
547  double r_foot_Tz_Nm = torque_new.coeff(2,0);
548 
549  r_foot_fx_N = robotis_framework::sign(r_foot_fx_N) * fmin( fabs(r_foot_fx_N), 2000.0);
550  r_foot_fy_N = robotis_framework::sign(r_foot_fy_N) * fmin( fabs(r_foot_fy_N), 2000.0);
551  r_foot_fz_N = robotis_framework::sign(r_foot_fz_N) * fmin( fabs(r_foot_fz_N), 2000.0);
552  r_foot_Tx_Nm = robotis_framework::sign(r_foot_Tx_Nm) *fmin(fabs(r_foot_Tx_Nm), 300.0);
553  r_foot_Ty_Nm = robotis_framework::sign(r_foot_Ty_Nm) *fmin(fabs(r_foot_Ty_Nm), 300.0);
554  r_foot_Tz_Nm = robotis_framework::sign(r_foot_Tz_Nm) *fmin(fabs(r_foot_Tz_Nm), 300.0);
555 
556  r_foot_ft_data_msg_.force.x = r_foot_fx_N;
557  r_foot_ft_data_msg_.force.y = r_foot_fy_N;
558  r_foot_ft_data_msg_.force.z = r_foot_fz_N;
559  r_foot_ft_data_msg_.torque.x = r_foot_Tx_Nm;
560  r_foot_ft_data_msg_.torque.y = r_foot_Ty_Nm;
561  r_foot_ft_data_msg_.torque.z = r_foot_Tz_Nm;
562 }
563 
564 void OnlineWalkingModule::setResetBodyCallback(const std_msgs::Bool::ConstPtr& msg)
565 {
566  if (msg->data == true)
567  {
568  des_body_offset_[0] = 0.0;
569  des_body_offset_[1] = 0.0;
570  des_body_offset_[2] = 0.0;
571 
572  resetBodyPose();
573  }
574 }
575 
576 void OnlineWalkingModule::walkingParamCallback(const op3_online_walking_module_msgs::WalkingParam& msg)
577 {
578  walking_param_ = msg;
579 }
580 
581 void OnlineWalkingModule::goalJointPoseCallback(const op3_online_walking_module_msgs::JointPose& msg)
582 {
583  if (enable_ == false)
584  return;
585 
586  size_t joint_size = msg.pose.name.size();
587 
589  {
590  mov_time_ = msg.mov_time;
591 
592  for (size_t i = 0; i < msg.pose.name.size(); i++)
593  {
594  std::string joint_name = msg.pose.name[i];
595  goal_joint_pos_[joint_name_to_id_[joint_name] - 1] = msg.pose.position[i];
596  }
597 
600  balance_type_ = OFF;
601  des_balance_gain_ratio_[0] = 0.0;
602  }
603  else
604  ROS_WARN("[WARN] Control type is different!");
605 }
606 
608 {
609  if (joint_control_initialize_ == true)
610  return;
611 
613 
614  double ini_time = 0.0;
615  double mov_time = mov_time_;
616 
617  mov_step_ = 0;
618  mov_size_ = (int) (mov_time / control_cycle_sec_) + 1;
619 
620  joint_tra_ =
621  new robotis_framework::MinimumJerk(ini_time, mov_time,
624  if (is_moving_ == true)
625  ROS_INFO("[UPDATE] Joint Control");
626  else
627  {
628  is_moving_ = true;
629  ROS_INFO("[START] Joint Control");
630  }
631 }
632 
634 {
635  if (is_moving_ == true)
636  {
637  double cur_time = (double) mov_step_ * control_cycle_sec_;
638 
639  queue_mutex_.lock();
640 
644 
645  queue_mutex_.unlock();
646 
647  if (mov_step_ == mov_size_-1)
648  {
649  mov_step_ = 0;
650  is_moving_ = false;
651  delete joint_tra_;
652 
654 
655  ROS_INFO("[END] Joint Control");
656  }
657  else
658  mov_step_++;
659  }
660 }
661 
662 void OnlineWalkingModule::setBodyOffsetCallback(const geometry_msgs::Pose::ConstPtr& msg)
663 {
664  if (enable_ == false)
665  return;
666 
667  if (balance_type_ == OFF)
668  {
669  ROS_WARN("[WARN] Balance is off!");
670  return;
671  }
672 
674  {
675  goal_body_offset_[0] = msg->position.x;
676  goal_body_offset_[1] = msg->position.y;
677  goal_body_offset_[2] = msg->position.z;
678 
679  body_offset_initialize_ = false;
681  }
682  else
683  ROS_WARN("[WARN] Control type is different!");
684 }
685 
686 void OnlineWalkingModule::setFootDistanceCallback(const std_msgs::Float64::ConstPtr& msg)
687 {
688  if (enable_ == false)
689  return;
690 
691  foot_distance_ = msg->data;
692 
693  resetBodyPose();
694 }
695 
697 {
698  if (body_offset_initialize_ == true)
699  return;
700 
702 
703  double ini_time = 0.0;
704  double mov_time = 1.0;
705 
706  body_offset_step_ = 0;
707  body_offset_size_ = (int) (mov_time / control_cycle_sec_) + 1;
708 
709  std::vector<double_t> offset_zero;
710  offset_zero.resize(3, 0.0);
711 
713  new robotis_framework::MinimumJerk(ini_time, mov_time,
714  des_body_offset_, offset_zero, offset_zero,
715  goal_body_offset_, offset_zero, offset_zero);
716 
717  if (is_moving_ == true)
718  ROS_INFO("[UPDATE] Body Offset");
719  else
720  {
721  is_moving_ = true;
722  ROS_INFO("[START] Body Offset");
723  }
724 }
725 
727 {
728  if (is_moving_ == true)
729  {
730  double cur_time = (double) body_offset_step_ * control_cycle_sec_;
731 
732  queue_mutex_.lock();
733 
735 
736  queue_mutex_.unlock();
737 
738  if (body_offset_step_ == mov_size_-1)
739  {
740  body_offset_step_ = 0;
741  is_moving_ = false;
742  delete body_offset_tra_;
743 
745 
746  ROS_INFO("[END] Body Offset");
747  }
748  else
750  }
751 }
752 
753 void OnlineWalkingModule::goalKinematicsPoseCallback(const op3_online_walking_module_msgs::KinematicsPose& msg)
754 {
755  if (enable_ == false)
756  return;
757 
758  if (balance_type_ == OFF)
759  {
760  ROS_WARN("[WARN] Balance is off!");
761  return;
762  }
763 
765  {
766  if (is_moving_ == true)
767  {
768  if (wholegbody_control_group_!=msg.name)
769  {
770  ROS_WARN("[WARN] Control group is different!");
771  return;
772  }
773  }
774  mov_time_ = msg.mov_time;
775  wholegbody_control_group_ = msg.name;
776  wholebody_goal_msg_ = msg.pose;
777 
778  wholebody_initialize_ = false;
780  }
781  else
782  ROS_WARN("[WARN] Control type is different!");
783 }
784 
786 {
787  if (wholebody_initialize_ == true)
788  return;
789 
790  wholebody_initialize_ = true;
791 
792  double ini_time = 0.0;
793  double mov_time = mov_time_;
794 
795  mov_step_ = 0;
796  mov_size_ = (int) (mov_time / control_cycle_sec_) + 1;
797 
800  ini_time, mov_time,
802 
803  if (is_moving_ == true)
804  {
805  // ROS_INFO("[UPDATE] Wholebody Control");
806  // wholebody_control_->update(desired_body_position_, desired_body_orientation_,
807  // desired_task_position_, desired_task_velocity_, desired_task_acceleration_);
808  }
809  else
810  {
811  ROS_INFO("[START] Wholebody Control");
812 
816  is_moving_ = true;
817  }
818 }
819 
821 {
822  if (is_moving_ == true)
823  {
824  double cur_time = (double) mov_step_ * control_cycle_sec_;
825 
826  wholebody_control_->set(cur_time);
827 
830  des_body_pos_);
832  des_r_leg_Q_,
833  des_body_Q_);
834 
835  if (mov_step_ == mov_size_-1)
836  {
837  mov_step_ = 0;
838  is_moving_ = false;
840 
842 
843  ROS_INFO("[END] Wholebody Control");
844  }
845  else
846  mov_step_++;
847  }
848 }
849 
850 void OnlineWalkingModule::footStep2DCallback(const op3_online_walking_module_msgs::Step2DArray& msg)
851 {
852  if (enable_ == false)
853  return;
854 
855  if (balance_type_ == OFF)
856  {
857  ROS_WARN("[WARN] Balance is off!");
858  return;
859  }
860 
861  Eigen::Quaterniond body_Q(des_body_Q_[3],des_body_Q_[0],des_body_Q_[1],des_body_Q_[2]);
862  Eigen::MatrixXd body_R = robotis_framework::convertQuaternionToRotation(body_Q);
863  Eigen::MatrixXd body_rpy = robotis_framework::convertQuaternionToRPY(body_Q);
864  Eigen::MatrixXd body_T = Eigen::MatrixXd::Identity(4,4);
865  body_T.block(0,0,3,3) = body_R;
866  body_T.coeffRef(0,3) = des_body_pos_[0];
867  body_T.coeffRef(1,3) = des_body_pos_[1];
868 
869  op3_online_walking_module_msgs::Step2DArray foot_step_msg;
870 
871  int old_size = msg.footsteps_2d.size();
872  int new_size = old_size + 3;
873 
874  op3_online_walking_module_msgs::Step2D first_msg;
875  op3_online_walking_module_msgs::Step2D second_msg;
876 
877  first_msg.moving_foot = msg.footsteps_2d[0].moving_foot - 1;
878  second_msg.moving_foot = first_msg.moving_foot + 1;
879 
880  if (first_msg.moving_foot == LEFT_LEG)
881  {
882  first_msg.step2d.x = des_l_leg_pos_[0];
883  first_msg.step2d.y = des_l_leg_pos_[1];
884  first_msg.step2d.theta = body_rpy.coeff(2,0); //0.0;
885 
886  second_msg.step2d.x = des_r_leg_pos_[0];
887  second_msg.step2d.y = des_r_leg_pos_[1];
888  second_msg.step2d.theta = body_rpy.coeff(2,0); //0.0;
889  }
890  else if (first_msg.moving_foot == RIGHT_LEG)
891  {
892  first_msg.step2d.x = des_r_leg_pos_[0];
893  first_msg.step2d.y = des_r_leg_pos_[1];
894  first_msg.step2d.theta = body_rpy.coeff(2,0); //0.0;
895 
896  second_msg.step2d.x = des_l_leg_pos_[0];
897  second_msg.step2d.y = des_l_leg_pos_[1];
898  second_msg.step2d.theta = body_rpy.coeff(2,0); //0.0;
899  }
900 
901  foot_step_msg.footsteps_2d.push_back(first_msg);
902  foot_step_msg.footsteps_2d.push_back(second_msg);
903 
904  double step_final_theta;
905 
907  {
908  for (int i=0; i<old_size; i++)
909  {
910  op3_online_walking_module_msgs::Step2D step_msg = msg.footsteps_2d[i];
911  step_msg.moving_foot -= 1;
912 
913  Eigen::MatrixXd step_R = robotis_framework::convertRPYToRotation(0.0,0.0,step_msg.step2d.theta);
914  Eigen::MatrixXd step_T = Eigen::MatrixXd::Identity(4,4);
915  step_T.block(0,0,3,3) = step_R;
916  step_T.coeffRef(0,3) = step_msg.step2d.x;
917  step_T.coeffRef(1,3) = step_msg.step2d.y;
918 
919  Eigen::MatrixXd step_T_new = body_T*step_T;
920  Eigen::MatrixXd step_R_new = step_T_new.block(0,0,3,3);
921 
922  double step_new_x = step_T_new.coeff(0,3);
923  double step_new_y = step_T_new.coeff(1,3);
924  Eigen::MatrixXd step_new_rpy = robotis_framework::convertRotationToRPY(step_R_new);
925  double step_new_theta = step_new_rpy.coeff(2,0);
926 
927  step_msg.step2d.x = step_new_x;
928  step_msg.step2d.y = step_new_y;
929  step_msg.step2d.theta = step_new_theta;
930 
931  if (i == old_size-1)
932  step_final_theta = step_new_theta;
933 
934  foot_step_msg.footsteps_2d.push_back(step_msg);
935 
936  //ROS_INFO("===== OLD =====");
937  //ROS_INFO("step: %d", i);
938  //ROS_INFO("foot_step_2d_.footsteps_2d[%d].moving_foot: %d", i, step_msg.moving_foot);
939  //ROS_INFO("foot_step_2d_.footsteps_2d[%d].step2d x: %f", i, step_msg.step2d.x);
940  //ROS_INFO("foot_step_2d_.footsteps_2d[%d].step2d y: %f", i, step_msg.step2d.y);
941  //ROS_INFO("foot_step_2d_.footsteps_2d[%d].step2d theta: %f", i, step_msg.step2d.theta);
942  }
943 
944  op3_online_walking_module_msgs::Step2D step_msg = msg.footsteps_2d[old_size-1];
945 
946  if (step_msg.moving_foot - 1 == LEFT_LEG)
947  first_msg.moving_foot = RIGHT_LEG;
948  else
949  first_msg.moving_foot = LEFT_LEG;
950 
951  first_msg.step2d.x = 0.0;
952  first_msg.step2d.y = 0.0;
953  first_msg.step2d.theta = step_final_theta; //step_msg.step2d.theta;
954 
955  foot_step_msg.footsteps_2d.push_back(first_msg);
956 
957  for (int i=0; i<new_size; i++)
958  {
959  op3_online_walking_module_msgs::Step2D step_msg = foot_step_msg.footsteps_2d[i];
960 
961  //ROS_INFO("===== NEW =====");
962  //ROS_INFO("step: %d", i);
963  //ROS_INFO("foot_step_2d_.footsteps_2d[%d].moving_foot: %d", i, step_msg.moving_foot);
964  //ROS_INFO("foot_step_2d_.footsteps_2d[%d].step2d x: %f", i, step_msg.step2d.x);
965  //ROS_INFO("foot_step_2d_.footsteps_2d[%d].step2d y: %f", i, step_msg.step2d.y);
966  //ROS_INFO("foot_step_2d_.footsteps_2d[%d].step2d theta: %f", i, step_msg.step2d.theta);
967  }
968 
969  foot_step_2d_ = foot_step_msg;
970  foot_step_2d_.step_time = msg.step_time;
971 
972  walking_size_ = new_size;
973  mov_time_ = msg.step_time; //1.0;
974  is_foot_step_2d_ = true;
976 
977  if (is_moving_ == false)
979  else
980  ROS_WARN("[WARN] Previous task is alive!");
981  }
982  else
983  ROS_WARN("[WARN] Control type is different!");
984 }
985 
986 void OnlineWalkingModule::footStepCommandCallback(const op3_online_walking_module_msgs::FootStepCommand& msg)
987 {
988  if (enable_ == false)
989  return;
990 
991  if (balance_type_ == OFF)
992  {
993  ROS_WARN("[WARN] Balance is off!");
994  return;
995  }
996 
997  is_foot_step_2d_ = false;
998 
1000  {
1001  walking_size_ = msg.step_num + 3; //msg.step_num + 2;
1002  mov_time_ = msg.step_time;
1003 
1004  foot_step_command_ = msg;
1005  foot_step_command_.step_num = walking_size_;
1006 
1008 
1009  if (is_moving_ == false)
1011  else
1012  ROS_WARN("[WARN] Previous task is alive!");
1013  }
1014  else
1015  ROS_WARN("[WARN] Control type is different!");
1016 }
1017 
1019 {
1020  double mov_time = mov_time_;
1021 
1022  mov_step_ = 0;
1023  mov_size_ = (int) (mov_time / control_cycle_sec_) + 1;
1024 
1025  walking_step_ = 0;
1026 
1028  walking_param_.dsp_ratio, walking_param_.lipm_height, walking_param_.foot_height_max,
1029  walking_param_.zmp_offset_x, walking_param_.zmp_offset_y,
1030  x_lipm_, y_lipm_,
1031  foot_distance_);
1032 
1033  double lipm_height = walking_control_->getLipmHeight();
1034  preview_request_.lipm_height = lipm_height;
1035  preview_request_.control_cycle = control_cycle_sec_;
1036 
1037  bool get_preview_matrix = false;
1038 // get_preview_matrix = getPreviewMatrix(preview_request_);
1039  get_preview_matrix = definePreviewMatrix();
1040 
1041  if (get_preview_matrix == true)
1042  {
1043  if (is_moving_ == true)
1044  {
1045  // Update - TODO
1046  // walking_control_->update(walking_leg_, walking_phase_,
1047  // foot_step_command_,
1048  // des_body_pos_, des_body_Q_,
1049  // des_l_leg_pos_, des_l_leg_vel_, des_l_leg_accel_,
1050  // des_r_leg_pos_, des_r_leg_vel_, des_r_leg_accel_);
1051 
1052  // walking_control_->calcPreviewParam(preview_response_);
1053  // ROS_INFO("[UPDATE] Walking Control (%d/%d)", walking_step_+1, walking_size_);
1054  }
1055  else
1056  {
1057  if (is_foot_step_2d_ == true)
1058  {
1063  }
1064  else
1065  {
1070  }
1071 
1072 // walking_control_->calcPreviewParam(preview_response_);
1075 
1076  is_moving_ = true;
1077 
1079 
1080  ROS_INFO("[START] Walking Control (%d/%d)", walking_step_+1, walking_size_);
1081  }
1082 
1083  walking_initialize_ = true;
1084  }
1085  else
1086  ROS_WARN("[FAIL] Cannot get preview matrix");
1087 }
1088 
1090 {
1091  if (is_moving_ == true)
1092  {
1093  double cur_time = (double) mov_step_ * control_cycle_sec_;
1095 
1096 // ROS_INFO("cur_time: %f", cur_time);
1097 
1098  // queue_mutex_.lock();
1099 
1100  // Set joint position
1101  // desired_joint_position_ = walking_control_->getJointPosition(walking_step_, cur_time);
1102  // desired_joint_velocity_ = walking_control_->getJointVelocity(walking_step_, cur_time);
1103  // desired_joint_acceleration_ = walking_control_->getJointAcceleration(walking_step_, cur_time);
1104 
1105  // queue_mutex_.unlock();
1106 
1109  des_body_pos_);
1110  // walking_control_->getWalkingVelocity(desired_left_foot_velocity_,
1111  // desired_right_foot_velocity_,
1112  // desired_body_velocity_);
1113  // walking_control_->getWalkingAccleration(desired_left_foot_acceleration_,
1114  // desired_right_foot_acceleration_,
1115  // desired_body_acceleration_);
1117  des_r_leg_Q_,
1118  des_body_Q_);
1119 
1120  // ROS_INFO("body x: %f, y: %f, z: %f", desired_body_position_[0], desired_body_position_[1], desired_body_position_[2]);
1121  // ROS_INFO("lfoot x: %f, y: %f, z: %f", desired_left_foot_position_[0], desired_left_foot_position_[1], desired_left_foot_position_[2]);
1122  // ROS_INFO("rfoot x: %f, y: %f, z: %f", desired_right_foot_position_[0], desired_right_foot_position_[1], desired_right_foot_position_[2]);
1123 
1125 
1126  // ROS_INFO("x_lipm_ pos: %f , vel: %f , accel: %f", x_lipm_[0], x_lipm_[1], x_lipm_[2]);
1127  // ROS_INFO("y_lipm_ pos: %f , vel: %f , accel: %f", y_lipm_[0], y_lipm_[1], y_lipm_[2]);
1128 
1130 
1131 
1132  if (mov_step_ == mov_size_-1)
1133  {
1134  ROS_INFO("[END] Walking Control (%d/%d)", walking_step_+1, walking_size_);
1135 
1136  mov_step_ = 0;
1138 
1139  if (walking_step_ == walking_size_-1)
1140  {
1141  is_moving_ = false;
1142  is_foot_step_2d_ = false;
1144 // resetBodyPose();
1145 
1146  control_type_ = NONE;
1147  walking_phase_ = DSP;
1148  }
1149  else
1150  {
1151  walking_step_++;
1152  ROS_INFO("[START] Walking Control (%d/%d)", walking_step_+1, walking_size_);
1153  }
1154  }
1155  else
1156  mov_step_++;
1157  }
1158 }
1159 
1161 {
1162  // feedforward trajectory
1163  std::vector<double_t> zero_vector;
1164  zero_vector.resize(1,0.0);
1165 
1166  std::vector<double_t> via_pos;
1167  via_pos.resize(3, 0.0);
1168  via_pos[0] = 1.0 * DEGREE2RADIAN;
1169 
1170  double init_time = 0.0;
1171  double fin_time = mov_time_;
1172  double via_time = 0.5 * (init_time + fin_time);
1173  double dsp_ratio = walking_param_.dsp_ratio;
1174 
1176  new robotis_framework::MinimumJerkViaPoint(init_time, fin_time, via_time, dsp_ratio,
1177  zero_vector, zero_vector, zero_vector,
1178  zero_vector, zero_vector, zero_vector,
1179  via_pos, zero_vector, zero_vector);
1180 }
1181 
1183 {
1184  Eigen::MatrixXd des_body_pos = Eigen::MatrixXd::Zero(3,1);
1185  des_body_pos.coeffRef(0,0) = des_body_pos_[0];
1186  des_body_pos.coeffRef(1,0) = des_body_pos_[1];
1187  des_body_pos.coeffRef(2,0) = des_body_pos_[2];
1188 
1189  Eigen::Quaterniond des_body_Q(des_body_Q_[3],des_body_Q_[0],des_body_Q_[1],des_body_Q_[2]);
1190  Eigen::MatrixXd des_body_rot = robotis_framework::convertQuaternionToRotation(des_body_Q);
1191 
1192  // Forward Kinematics
1193  op3_kdl_->initialize(des_body_pos, des_body_rot);
1194 
1195  Eigen::VectorXd r_leg_joint_pos, l_leg_joint_pos;
1196 
1197  r_leg_joint_pos.resize(6);
1198  r_leg_joint_pos(0) = des_joint_pos_[joint_name_to_id_["r_hip_yaw"]-1];
1199  r_leg_joint_pos(1) = des_joint_pos_[joint_name_to_id_["r_hip_roll"]-1];
1200  r_leg_joint_pos(2) = des_joint_pos_[joint_name_to_id_["r_hip_pitch"]-1];
1201  r_leg_joint_pos(3) = des_joint_pos_[joint_name_to_id_["r_knee"]-1];
1202  r_leg_joint_pos(4) = des_joint_pos_[joint_name_to_id_["r_ank_pitch"]-1];
1203  r_leg_joint_pos(5) = des_joint_pos_[joint_name_to_id_["r_ank_roll"]-1];
1204 
1205  l_leg_joint_pos.resize(6);
1206  l_leg_joint_pos(0) = des_joint_pos_[joint_name_to_id_["l_hip_yaw"]-1];
1207  l_leg_joint_pos(1) = des_joint_pos_[joint_name_to_id_["l_hip_roll"]-1];
1208  l_leg_joint_pos(2) = des_joint_pos_[joint_name_to_id_["l_hip_pitch"]-1];
1209  l_leg_joint_pos(3) = des_joint_pos_[joint_name_to_id_["l_knee"]-1];
1210  l_leg_joint_pos(4) = des_joint_pos_[joint_name_to_id_["l_ank_pitch"]-1];
1211  l_leg_joint_pos(5) = des_joint_pos_[joint_name_to_id_["l_ank_roll"]-1];
1212 
1213  op3_kdl_->setJointPosition(r_leg_joint_pos, l_leg_joint_pos);
1214 
1215  std::vector<double_t> r_leg_pos, r_leg_Q;
1216  r_leg_pos.resize(3,0.0);
1217  r_leg_Q.resize(4,0.0);
1218 
1219  std::vector<double_t> l_leg_pos, l_leg_Q;
1220  l_leg_pos.resize(3,0.0);
1221  l_leg_Q.resize(4,0.0);
1222 
1223  op3_kdl_->solveForwardKinematics(r_leg_pos, r_leg_Q,
1224  l_leg_pos, l_leg_Q);
1225 
1226  Eigen::Quaterniond curr_r_leg_Q(r_leg_Q[3],r_leg_Q[0],r_leg_Q[1],r_leg_Q[2]);
1227  Eigen::MatrixXd curr_r_leg_rot = robotis_framework::convertQuaternionToRotation(curr_r_leg_Q);
1228 
1229  Eigen::MatrixXd g_to_r_leg = Eigen::MatrixXd::Identity(4,4);
1230  g_to_r_leg.block(0,0,3,3) = curr_r_leg_rot;
1231  g_to_r_leg.coeffRef(0,3) = r_leg_pos[0];
1232  g_to_r_leg.coeffRef(1,3) = r_leg_pos[1];
1233  g_to_r_leg.coeffRef(2,3) = r_leg_pos[2];
1234 
1235  Eigen::Quaterniond curr_l_leg_Q(l_leg_Q[3],l_leg_Q[0],l_leg_Q[1],l_leg_Q[2]);
1236  Eigen::MatrixXd curr_l_leg_rot = robotis_framework::convertQuaternionToRotation(curr_l_leg_Q);
1237 
1238  Eigen::MatrixXd g_to_l_leg = Eigen::MatrixXd::Identity(4,4);
1239  g_to_l_leg.block(0,0,3,3) = curr_l_leg_rot;
1240  g_to_l_leg.coeffRef(0,3) = l_leg_pos[0];
1241  g_to_l_leg.coeffRef(1,3) = l_leg_pos[1];
1242  g_to_l_leg.coeffRef(2,3) = l_leg_pos[2];
1243 
1244  PRINT_MAT(g_to_r_leg);
1245  PRINT_MAT(g_to_l_leg);
1246 
1247  // des_r_leg_pos_ = r_leg_pos;
1248  // des_l_leg_pos_ = l_leg_pos;
1249 
1250  op3_kdl_->finalize();
1251 }
1252 
1254 {
1255  if (walking_phase_ == DSP)
1256  {
1259  balance_r_foot_force_z_ = -0.5 * total_mass_ * 9.81;
1260 
1261  balance_l_foot_force_x_ = -0.5 * total_mass_ * x_lipm_[2];
1262  balance_l_foot_force_y_ = -0.5 * total_mass_ * y_lipm_[2];
1263  balance_l_foot_force_z_ = -0.5 * total_mass_ * 9.81;
1264  }
1265  else if (walking_phase_ == SSP)
1266  {
1267  if (walking_leg_ == LEFT_LEG)
1268  {
1271  balance_r_foot_force_z_ = -1.0 * total_mass_ * 9.81;
1272 
1276  }
1277  else if (walking_leg_ == RIGHT_LEG)
1278  {
1282 
1285  balance_l_foot_force_z_ = -1.0 * total_mass_ * 9.81;
1286  }
1287  }
1288 
1289  // ROS_INFO("r_foot_force x: %f, y: %f, z: %f", balance_r_foot_force_x_, balance_r_foot_force_y_, balance_r_foot_force_z_);
1290  // ROS_INFO("l_foot_force x: %f, y: %f, z: %f", balance_l_foot_force_x_, balance_l_foot_force_y_, balance_l_foot_force_z_);
1291 }
1292 
1294 {
1296  //gyro
1301 
1302  //orientation
1307 
1308  //force torque
1319 
1330 
1336 
1342 
1348 }
1349 
1351 {
1352  // Set Balance Control
1356 
1358 
1361 
1362  bool ik_success = true;
1363 
1364  // Body Pose
1365  Eigen::MatrixXd des_body_pos = Eigen::MatrixXd::Zero(3,1);
1366  des_body_pos.coeffRef(0,0) = des_body_pos_[0];
1367  des_body_pos.coeffRef(1,0) = des_body_pos_[1];
1368  des_body_pos.coeffRef(2,0) = des_body_pos_[2];
1369 
1370  Eigen::Quaterniond des_body_Q(des_body_Q_[3],des_body_Q_[0],des_body_Q_[1],des_body_Q_[2]);
1371  Eigen::MatrixXd des_body_rot = robotis_framework::convertQuaternionToRotation(des_body_Q);
1372  Eigen::MatrixXd des_body_rpy = robotis_framework::convertQuaternionToRPY(des_body_Q);
1373 
1374  // Right Leg Pose
1375  Eigen::MatrixXd des_r_foot_pos = Eigen::MatrixXd::Zero(3,1);
1376  des_r_foot_pos.coeffRef(0,0) = des_r_leg_pos_[0];
1377  des_r_foot_pos.coeffRef(1,0) = des_r_leg_pos_[1];
1378  des_r_foot_pos.coeffRef(2,0) = des_r_leg_pos_[2];
1379 
1380  Eigen::Quaterniond des_r_foot_Q(des_r_leg_Q_[3],des_r_leg_Q_[0],des_r_leg_Q_[1],des_r_leg_Q_[2]);
1381  Eigen::MatrixXd des_r_foot_rot = robotis_framework::convertQuaternionToRotation(des_r_foot_Q);
1382 
1383  // Left Leg Pose
1384  Eigen::MatrixXd des_l_foot_pos = Eigen::MatrixXd::Zero(3,1);
1385  des_l_foot_pos.coeffRef(0,0) = des_l_leg_pos_[0];
1386  des_l_foot_pos.coeffRef(1,0) = des_l_leg_pos_[1];
1387  des_l_foot_pos.coeffRef(2,0) = des_l_leg_pos_[2];
1388 
1389  Eigen::Quaterniond des_l_foot_Q(des_l_leg_Q_[3],des_l_leg_Q_[0],des_l_leg_Q_[1],des_l_leg_Q_[2]);
1390  Eigen::MatrixXd des_l_foot_rot = robotis_framework::convertQuaternionToRotation(des_l_foot_Q);
1391 
1392  // Set Desired Value for Balance Control
1393  Eigen::MatrixXd body_pose = Eigen::MatrixXd::Identity(4,4);
1394  body_pose.block<3,3>(0,0) = des_body_rot;
1395  body_pose.block<3,1>(0,3) = des_body_pos;
1396 
1397  Eigen::MatrixXd l_foot_pose = Eigen::MatrixXd::Identity(4,4);
1398  l_foot_pose.block<3,3>(0,0) = des_l_foot_rot;
1399  l_foot_pose.block<3,1>(0,3) = des_l_foot_pos;
1400 
1401  Eigen::MatrixXd r_foot_pose = Eigen::MatrixXd::Identity(4,4);
1402  r_foot_pose.block<3,3>(0,0) = des_r_foot_rot;
1403  r_foot_pose.block<3,1>(0,3) = des_r_foot_pos;
1404 
1405  // PRINT_MAT(body_pose);
1406  // PRINT_MAT(l_foot_pose);
1407  // PRINT_MAT(r_foot_pose);
1408 
1409  // ===== Transformation =====
1410  Eigen::MatrixXd robot_to_body = Eigen::MatrixXd::Identity(4,4);
1411  Eigen::MatrixXd robot_to_l_foot = body_pose.inverse() * l_foot_pose;
1412  Eigen::MatrixXd robot_to_r_foot = body_pose.inverse() * r_foot_pose;
1413 
1414  // PRINT_MAT(body_pose_trans);
1415  // PRINT_MAT(l_foot_pose_trans);
1416  // PRINT_MAT(r_foot_pose_trans);
1417  // =====
1418 
1419  // Set IMU
1420  imu_data_mutex_lock_.lock();
1421 
1422  balance_control_.setCurrentGyroSensorOutput(imu_data_msg_.angular_velocity.x, imu_data_msg_.angular_velocity.y);
1423 
1424  Eigen::Quaterniond imu_quaternion(imu_data_msg_.orientation.w,
1425  imu_data_msg_.orientation.x,
1426  imu_data_msg_.orientation.y,
1427  imu_data_msg_.orientation.z);
1428  Eigen::MatrixXd imu_rpy =
1430 
1431  imu_data_mutex_lock_.unlock();
1432 
1433  // Set FT
1434  // Eigen::Quaterniond g_to_r_foot_Q(des_r_leg_Q_[3],des_r_leg_Q_[0],des_r_leg_Q_[1],des_r_leg_Q_[2]);
1435  // Eigen::Quaterniond g_to_l_foot_Q(des_l_leg_Q_[3],des_l_leg_Q_[0],des_l_leg_Q_[1],des_l_leg_Q_[2]);
1436 
1437  // Eigen::MatrixXd g_to_r_foot_rot = robotis_framework::convertQuaternionToRotation(g_to_r_foot_Q);
1438  // Eigen::MatrixXd g_to_l_foot_rot = robotis_framework::convertQuaternionToRotation(g_to_l_foot_Q);
1439 
1440  // Eigen::MatrixXd g_to_r_foot_force =
1441  // g_to_r_foot_rot * robotis_framework::getRotationX(M_PI) *
1442  // robotis_framework::getTransitionXYZ(r_foot_ft_data_msg_.force.x, r_foot_ft_data_msg_.force.y, r_foot_ft_data_msg_.force.z);
1443 
1444  // Eigen::MatrixXd g_to_r_foot_torque =
1445  // g_to_r_foot_rot * robotis_framework::getRotationX(M_PI) *
1446  // robotis_framework::getTransitionXYZ(r_foot_ft_data_msg_.torque.x, r_foot_ft_data_msg_.torque.y, r_foot_ft_data_msg_.torque.z);
1447 
1448  // Eigen::MatrixXd g_to_l_foot_force =
1449  // g_to_l_foot_rot * robotis_framework::getRotationX(M_PI) *
1450  // robotis_framework::getTransitionXYZ(l_foot_ft_data_msg_.force.x, l_foot_ft_data_msg_.force.y, l_foot_ft_data_msg_.force.z);
1451 
1452  // Eigen::MatrixXd g_to_l_foot_torque =
1453  // g_to_l_foot_rot * robotis_framework::getRotationX(M_PI) *
1454  // robotis_framework::getTransitionXYZ(l_foot_ft_data_msg_.torque.x, l_foot_ft_data_msg_.torque.y, l_foot_ft_data_msg_.torque.z);
1455 
1456  Eigen::MatrixXd robot_to_r_foot_force =
1457  robot_to_r_foot.block(0,0,3,3) * robotis_framework::getRotationX(M_PI) *
1459 
1460  Eigen::MatrixXd robot_to_r_foot_torque =
1461  robot_to_r_foot.block(0,0,3,3) * robotis_framework::getRotationX(M_PI) *
1463 
1464  Eigen::MatrixXd robot_to_l_foot_force =
1465  robot_to_l_foot.block(0,0,3,3) * robotis_framework::getRotationX(M_PI) *
1467 
1468  Eigen::MatrixXd robot_to_l_foot_torque =
1469  robot_to_l_foot.block(0,0,3,3) * robotis_framework::getRotationX(M_PI) *
1471 
1472  // Eigen::MatrixXd robot_to_r_foot_force =
1473  // robot_to_r_foot.block(0,0,3,3) *
1474  // robotis_framework::getTransitionXYZ(r_foot_ft_data_msg_.force.x, r_foot_ft_data_msg_.force.y, r_foot_ft_data_msg_.force.z);
1475 
1476  // Eigen::MatrixXd robot_to_r_foot_torque =
1477  // robot_to_r_foot.block(0,0,3,3) *
1478  // robotis_framework::getTransitionXYZ(r_foot_ft_data_msg_.torque.x, r_foot_ft_data_msg_.torque.y, r_foot_ft_data_msg_.torque.z);
1479 
1480  // Eigen::MatrixXd robot_to_l_foot_force =
1481  // robot_to_l_foot.block(0,0,3,3) *
1482  // robotis_framework::getTransitionXYZ(l_foot_ft_data_msg_.force.x, l_foot_ft_data_msg_.force.y, l_foot_ft_data_msg_.force.z);
1483 
1484  // Eigen::MatrixXd robot_to_l_foot_torque =
1485  // robot_to_l_foot.block(0,0,3,3) *
1486  // robotis_framework::getTransitionXYZ(l_foot_ft_data_msg_.torque.x, l_foot_ft_data_msg_.torque.y, l_foot_ft_data_msg_.torque.z);
1487 
1488  balance_control_.setCurrentOrientationSensorOutput(imu_rpy.coeff(0,0), imu_rpy.coeff(1,0));
1489 
1490  balance_control_.setCurrentFootForceTorqueSensorOutput(robot_to_r_foot_force.coeff(0,0), robot_to_r_foot_force.coeff(1,0), robot_to_r_foot_force.coeff(2,0),
1491  robot_to_r_foot_torque.coeff(0,0), robot_to_r_foot_torque.coeff(1,0), robot_to_r_foot_torque.coeff(2,0),
1492  robot_to_l_foot_force.coeff(0,0), robot_to_l_foot_force.coeff(1,0), robot_to_l_foot_force.coeff(2,0),
1493  robot_to_l_foot_torque.coeff(0,0), robot_to_l_foot_torque.coeff(1,0), robot_to_l_foot_torque.coeff(2,0));
1494 
1495  // PRINT_MAT(robot_to_r_foot_force);
1496  // PRINT_MAT(robot_to_l_foot_force);
1497 
1499 
1500  // Eigen::Quaterniond g_to_body_Q(des_body_Q_[3],des_body_Q_[0],des_body_Q_[1],des_body_Q_[2]);
1501  // Eigen::MatrixXd g_to_body_rpy = robotis_framework::convertQuaternionToRotation(g_to_body_Q);
1502  // balance_control_.setDesiredCOBOrientation(g_to_body_rpy.coeff(0,0),g_to_body_rpy.coeff(1,0));
1503 
1504  balance_control_.setDesiredCOBOrientation(des_body_rpy.coeff(0,0),des_body_rpy.coeff(1,0));
1505 
1510 
1511  balance_control_.setDesiredPose(robot_to_body, robot_to_r_foot, robot_to_l_foot);
1512 
1513  int error;
1514  Eigen::MatrixXd robot_to_body_mod, robot_to_r_foot_mod, robot_to_l_foot_mod;
1515  balance_control_.process(&error, &robot_to_body_mod, &robot_to_r_foot_mod, &robot_to_l_foot_mod);
1516 
1517  // ===== Transformation =====
1518  Eigen::MatrixXd body_pose_mod = body_pose * robot_to_body_mod;
1519  Eigen::MatrixXd r_foot_pose_mod = body_pose * robot_to_r_foot_mod;
1520  Eigen::MatrixXd l_foot_pose_mod = body_pose * robot_to_l_foot_mod;
1521 
1522  // PRINT_MAT(body_pose_new);
1523  // PRINT_MAT(r_foot_pose_new);
1524  // PRINT_MAT(l_foot_pose_new);
1525  // =====
1526 
1527  Eigen::MatrixXd des_body_rot_mod = body_pose_mod.block<3,3>(0,0);
1528  Eigen::MatrixXd des_body_pos_mod = body_pose_mod.block<3,1>(0,3);
1529 
1530  Eigen::MatrixXd des_r_foot_rot_mod = r_foot_pose_mod.block<3,3>(0,0);
1531  Eigen::MatrixXd des_r_foot_pos_mod = r_foot_pose_mod.block<3,1>(0,3);
1532  Eigen::MatrixXd des_l_foot_rot_mod = l_foot_pose_mod.block<3,3>(0,0);
1533  Eigen::MatrixXd des_l_foot_pos_mod = l_foot_pose_mod.block<3,1>(0,3);
1534 
1535  // ======= ======= //
1536  op3_kdl_->initialize(des_body_pos_mod, des_body_rot_mod);
1537 
1538  Eigen::VectorXd r_leg_joint_pos, l_leg_joint_pos;
1539 
1540  r_leg_joint_pos.resize(6);
1541  r_leg_joint_pos(0) = des_joint_pos_[joint_name_to_id_["r_hip_yaw"]-1];
1542  r_leg_joint_pos(1) = des_joint_pos_[joint_name_to_id_["r_hip_roll"]-1];
1543  r_leg_joint_pos(2) = des_joint_pos_[joint_name_to_id_["r_hip_pitch"]-1];
1544  r_leg_joint_pos(3) = des_joint_pos_[joint_name_to_id_["r_knee"]-1];
1545  r_leg_joint_pos(4) = des_joint_pos_[joint_name_to_id_["r_ank_pitch"]-1];
1546  r_leg_joint_pos(5) = des_joint_pos_[joint_name_to_id_["r_ank_roll"]-1];
1547 
1548  l_leg_joint_pos.resize(6);
1549  l_leg_joint_pos(0) = des_joint_pos_[joint_name_to_id_["l_hip_yaw"]-1];
1550  l_leg_joint_pos(1) = des_joint_pos_[joint_name_to_id_["l_hip_roll"]-1];
1551  l_leg_joint_pos(2) = des_joint_pos_[joint_name_to_id_["l_hip_pitch"]-1];
1552  l_leg_joint_pos(3) = des_joint_pos_[joint_name_to_id_["l_knee"]-1];
1553  l_leg_joint_pos(4) = des_joint_pos_[joint_name_to_id_["l_ank_pitch"]-1];
1554  l_leg_joint_pos(5) = des_joint_pos_[joint_name_to_id_["l_ank_roll"]-1];
1555 
1556  op3_kdl_->setJointPosition(r_leg_joint_pos, l_leg_joint_pos);
1557 
1558  std::vector<double_t> r_leg_output, l_leg_output;
1559 
1560  Eigen::Quaterniond des_r_foot_Q_mod = robotis_framework::convertRotationToQuaternion(des_r_foot_rot_mod);
1561  Eigen::Quaterniond des_l_foot_Q_mod = robotis_framework::convertRotationToQuaternion(des_l_foot_rot_mod);
1562 
1563  ik_success = op3_kdl_->solveInverseKinematics(r_leg_output,
1564  des_r_foot_pos_mod,des_r_foot_Q_mod,
1565  l_leg_output,
1566  des_l_foot_pos_mod,des_l_foot_Q_mod);
1567 
1568  op3_kdl_->finalize();
1569 
1570  if (ik_success == true)
1571  {
1572  des_joint_pos_[joint_name_to_id_["r_hip_yaw"]-1] = r_leg_output[0];
1573  des_joint_pos_[joint_name_to_id_["r_hip_roll"]-1] = r_leg_output[1];
1574  des_joint_pos_[joint_name_to_id_["r_hip_pitch"]-1] = r_leg_output[2];
1575  des_joint_pos_[joint_name_to_id_["r_knee"]-1] = r_leg_output[3];
1576  des_joint_pos_[joint_name_to_id_["r_ank_pitch"]-1] = r_leg_output[4];
1577  des_joint_pos_[joint_name_to_id_["r_ank_roll"]-1] = r_leg_output[5];
1578 
1579  des_joint_pos_[joint_name_to_id_["l_hip_yaw"]-1] = l_leg_output[0];
1580  des_joint_pos_[joint_name_to_id_["l_hip_roll"]-1] = l_leg_output[1];
1581  des_joint_pos_[joint_name_to_id_["l_hip_pitch"]-1] = l_leg_output[2];
1582  des_joint_pos_[joint_name_to_id_["l_knee"]-1] = l_leg_output[3];
1583  des_joint_pos_[joint_name_to_id_["l_ank_pitch"]-1] = l_leg_output[4];
1584  des_joint_pos_[joint_name_to_id_["l_ank_roll"]-1] = l_leg_output[5];
1585  }
1586 
1587  return ik_success;
1588 }
1589 
1591 {
1592  for (int i=0; i<number_of_joints_; i++)
1593  {
1595 
1598 
1600  }
1601 }
1602 
1604 {
1605  double cur_time = (double) mov_step_ * control_cycle_sec_;
1606 
1607  std::vector<double_t> feed_forward_value = feed_forward_tra_->getPosition(cur_time);
1608 
1609  if (walking_phase_ == DSP)
1610  feed_forward_value[0] = 0.0;
1611 
1612  std::vector<double_t> support_leg_gain;
1613  support_leg_gain.resize(number_of_joints_, 0.0);
1614 
1615  if (walking_leg_ == LEFT_LEG)
1616  {
1617  support_leg_gain[joint_name_to_id_["r_hip_yaw"]-1] = 1.0;
1618  support_leg_gain[joint_name_to_id_["r_hip_roll"]-1] = 1.0;
1619  support_leg_gain[joint_name_to_id_["r_hip_pitch"]-1] = 1.0;
1620  support_leg_gain[joint_name_to_id_["r_knee"]-1] = 1.0;
1621  support_leg_gain[joint_name_to_id_["r_ank_pitch"]-1] = 1.0;
1622  support_leg_gain[joint_name_to_id_["r_ank_roll"]-1] = 1.0;
1623 
1624  support_leg_gain[joint_name_to_id_["l_hip_yaw"]-1] = 0.0;
1625  support_leg_gain[joint_name_to_id_["l_hip_roll"]-1] = 0.0;
1626  support_leg_gain[joint_name_to_id_["l_hip_pitch"]-1] = 0.0;
1627  support_leg_gain[joint_name_to_id_["l_knee"]-1] = 0.0;
1628  support_leg_gain[joint_name_to_id_["l_ank_pitch"]-1] = 0.0;
1629  support_leg_gain[joint_name_to_id_["l_ank_roll"]-1] = 0.0;
1630  }
1631  else if (walking_leg_ == RIGHT_LEG)
1632  {
1633  support_leg_gain[joint_name_to_id_["r_hip_yaw"]-1] = 0.0;
1634  support_leg_gain[joint_name_to_id_["r_hip_roll"]-1] = 0.0;
1635  support_leg_gain[joint_name_to_id_["r_hip_pitch"]-1] = 0.0;
1636  support_leg_gain[joint_name_to_id_["r_knee"]-1] = 0.0;
1637  support_leg_gain[joint_name_to_id_["r_ank_pitch"]-1] = 0.0;
1638  support_leg_gain[joint_name_to_id_["r_ank_roll"]-1] = 0.0;
1639 
1640  support_leg_gain[joint_name_to_id_["l_hip_yaw"]-1] = 1.0;
1641  support_leg_gain[joint_name_to_id_["l_hip_roll"]-1] = 1.0;
1642  support_leg_gain[joint_name_to_id_["l_hip_pitch"]-1] = 1.0;
1643  support_leg_gain[joint_name_to_id_["l_knee"]-1] = 1.0;
1644  support_leg_gain[joint_name_to_id_["l_ank_pitch"]-1] = 1.0;
1645  support_leg_gain[joint_name_to_id_["l_ank_roll"]-1] = 1.0;
1646  }
1647 
1648  for (int i=0; i<number_of_joints_; i++)
1649  des_joint_feedforward_[i] = joint_feedforward_gain_[i] * feed_forward_value[0] * support_leg_gain[i];
1650 }
1651 
1652 void OnlineWalkingModule::sensoryFeedback(const double &rlGyroErr, const double &fbGyroErr, double *balance_angle)
1653 {
1654  // adjust balance offset
1655 // if (walking_param_.balance_enable == false)
1656 // return;
1657 
1658  double internal_gain = 0.05;
1659 
1660  balance_angle[joint_name_to_id_["r_hip_roll"]-1] =
1661  -1.0 * internal_gain * rlGyroErr * balance_hip_roll_gain_; // R_HIP_ROLL
1662  balance_angle[joint_name_to_id_["l_hip_roll"]-1] =
1663  -1.0 * internal_gain * rlGyroErr * balance_hip_roll_gain_; // L_HIP_ROLL
1664 
1665  balance_angle[joint_name_to_id_["r_knee"]-1] =
1666  1.0 * internal_gain * fbGyroErr * balance_knee_gain_; // R_KNEE
1667  balance_angle[joint_name_to_id_["l_knee"]-1] =
1668  -1.0 * internal_gain * fbGyroErr * balance_knee_gain_; // L_KNEE
1669 
1670  balance_angle[joint_name_to_id_["r_ank_pitch"]-1] =
1671  -1.0 * internal_gain * fbGyroErr * balance_ankle_pitch_gain_; // R_ANKLE_PITCH
1672  balance_angle[joint_name_to_id_["l_ank_pitch"]-1] =
1673  1.0 * internal_gain * fbGyroErr * balance_ankle_pitch_gain_; // L_ANKLE_PITCH
1674 
1675  balance_angle[joint_name_to_id_["r_ank_roll"]-1] =
1676  -1.0 * internal_gain * rlGyroErr * balance_ankle_roll_gain_; // R_ANKLE_ROLL
1677  balance_angle[joint_name_to_id_["l_ank_roll"]-1] =
1678  -1.0 * internal_gain * rlGyroErr * balance_ankle_roll_gain_; // L_ANKLE_ROLL
1679 }
1680 
1681 
1682 void OnlineWalkingModule::process(std::map<std::string, robotis_framework::Dynamixel *> dxls,
1683  std::map<std::string, double> sensors)
1684 {
1685  if (enable_ == false)
1686  return;
1687 
1688  double balance_angle[number_of_joints_];
1689 
1690  for (int i=0; i<number_of_joints_; i++)
1691  balance_angle[i] = 0.0;
1692 
1693  double rl_gyro_err = 0.0 - sensors["gyro_x"];
1694  double fb_gyro_err = 0.0 - sensors["gyro_y"];
1695 
1696  sensoryFeedback(rl_gyro_err, fb_gyro_err, balance_angle);
1697 
1698  /*----- write curr position -----*/
1699  for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter = result_.begin();
1700  state_iter != result_.end(); state_iter++)
1701  {
1702  std::string joint_name = state_iter->first;
1703 
1704  robotis_framework::Dynamixel *dxl = NULL;
1705  std::map<std::string, robotis_framework::Dynamixel*>::iterator dxl_it = dxls.find(joint_name);
1706  if (dxl_it != dxls.end())
1707  dxl = dxl_it->second;
1708  else
1709  continue;
1710 
1711  double curr_joint_pos = dxl->dxl_state_->present_position_;
1712  double goal_joint_pos = dxl->dxl_state_->goal_position_;
1713 
1714  if (goal_initialize_ == false)
1715  des_joint_pos_[joint_name_to_id_[joint_name]-1] = goal_joint_pos;
1716 
1717  curr_joint_pos_[joint_name_to_id_[joint_name]-1] = curr_joint_pos;
1718  }
1719 
1720  goal_initialize_ = true;
1721 
1722  /* Trajectory Calculation */
1723  ros::Time begin = ros::Time::now();
1724 
1726  {
1727  initJointControl();
1728  calcJointControl();
1729  }
1730  else if (control_type_ == WHOLEBODY_CONTROL)
1731  {
1734  }
1735  else if (control_type_ == WALKING_CONTROL)
1736  {
1737  if(walking_initialize_ == true)
1738  {
1741  }
1742  }
1743  else if (control_type_ == OFFSET_CONTROL)
1744  {
1747  }
1748 
1749  // calcRobotPose();
1750 
1751  if (balance_type_ == ON)
1752  {
1755 
1756  if (setBalanceControl() == false)
1757  {
1758  is_moving_ = false;
1759  is_balancing_ = false;
1760  is_foot_step_2d_ = false;
1761 
1762  balance_type_ = OFF;
1763  control_type_ = NONE;
1764 
1765  resetBodyPose();
1766 
1767  ROS_INFO("[FAIL] Task Space Control");
1768  }
1769  }
1770 
1771  ros::Duration time_duration = ros::Time::now() - begin;
1772 
1773  if (time_duration.toSec() > 0.003)
1774  ROS_INFO("[Wholebody Module] Calc Time: %f", time_duration.toSec());
1775 
1777 
1778  for (int i=0; i<number_of_joints_; i++)
1779  des_joint_pos_to_robot_[i] += balance_angle[i];
1780 
1781  sensor_msgs::JointState goal_joint_msg;
1782  geometry_msgs::PoseStamped pelvis_pose_msg;
1783 
1784  goal_joint_msg.header.stamp = ros::Time::now();
1785  pelvis_pose_msg.header.stamp = ros::Time::now();
1786 
1787  pelvis_pose_msg.pose.position.x = des_body_pos_[0];
1788  pelvis_pose_msg.pose.position.y = des_body_pos_[1];
1789  pelvis_pose_msg.pose.position.z = des_body_pos_[2] - 0.0907;
1790 
1791  pelvis_pose_msg.pose.orientation.x = des_body_Q_[0];
1792  pelvis_pose_msg.pose.orientation.y = des_body_Q_[1];
1793  pelvis_pose_msg.pose.orientation.z = des_body_Q_[2];
1794  pelvis_pose_msg.pose.orientation.w = des_body_Q_[3];
1795 
1796  /*----- set joint data -----*/
1797  for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter = result_.begin();
1798  state_iter != result_.end(); state_iter++)
1799  {
1800  std::string joint_name = state_iter->first;
1801  // result_[joint_name]->goal_position_ = des_joint_pos_[joint_name_to_id_[joint_name]-1];
1802  result_[joint_name]->goal_position_ = des_joint_pos_to_robot_[joint_name_to_id_[joint_name]-1];
1803 
1804  goal_joint_msg.name.push_back(joint_name);
1805  goal_joint_msg.position.push_back(des_joint_pos_[joint_name_to_id_[joint_name]-1]);
1806  }
1807 
1808  pelvis_pose_pub_.publish(pelvis_pose_msg);
1809  goal_joint_state_pub_.publish(goal_joint_msg);
1810 }
1811 
1813 {
1814  for (int i=0; i<number_of_joints_; i++)
1815  {
1816  des_joint_pos_[i] = 0.0;
1817  des_joint_vel_[i] = 0.0;
1818  des_joint_accel_[i] = 0.0;
1819  }
1820 
1821  goal_initialize_ = false;
1822 
1823  is_moving_ = false;
1824  is_balancing_ = false;
1825 
1826  joint_control_initialize_ = false;
1827  wholebody_initialize_ = false;
1828  walking_initialize_ = false;
1830 
1831  control_type_ = NONE;
1832 
1833  return;
1834 }
1835 
1837 {
1838  return is_moving_;
1839 }
1840 
1841 void OnlineWalkingModule::publishStatusMsg(unsigned int type, std::string msg)
1842 {
1843  robotis_controller_msgs::StatusMsg status;
1844  status.header.stamp = ros::Time::now();
1845  status.type = type;
1846  status.module_name = "Wholebody";
1847  status.status_msg = msg;
1848 
1849  status_msg_pub_.publish(status);
1850 }
1851 
1852 bool OnlineWalkingModule::getJointPoseCallback(op3_online_walking_module_msgs::GetJointPose::Request &req,
1853  op3_online_walking_module_msgs::GetJointPose::Response &res)
1854 {
1855  for (int i=0; i<number_of_joints_; i++)
1856  {
1857  res.pose.pose.name.push_back(joint_name_[i]);
1858  res.pose.pose.position.push_back(des_joint_pos_[i]);
1859  }
1860 
1861  return true;
1862 }
1863 
1864 bool OnlineWalkingModule::getKinematicsPoseCallback(op3_online_walking_module_msgs::GetKinematicsPose::Request &req,
1865  op3_online_walking_module_msgs::GetKinematicsPose::Response &res)
1866 {
1867  std::string group_name = req.name;
1868 
1869  geometry_msgs::Pose msg;
1870 
1871  if (group_name == "body")
1872  {
1873  msg.position.x = des_body_pos_[0];
1874  msg.position.y = des_body_pos_[1];
1875  msg.position.z = des_body_pos_[2];
1876 
1877  msg.orientation.x = des_body_Q_[0];
1878  msg.orientation.y = des_body_Q_[1];
1879  msg.orientation.z = des_body_Q_[2];
1880  msg.orientation.w = des_body_Q_[3];
1881  }
1882  else if (group_name == "left_leg")
1883  {
1884  msg.position.x = des_l_leg_pos_[0];
1885  msg.position.y = des_l_leg_pos_[1];
1886  msg.position.z = des_l_leg_pos_[2];
1887 
1888  msg.orientation.x = des_l_leg_Q_[0];
1889  msg.orientation.y = des_l_leg_Q_[1];
1890  msg.orientation.z = des_l_leg_Q_[2];
1891  msg.orientation.w = des_l_leg_Q_[3];
1892  }
1893  else if (group_name == "right_leg")
1894  {
1895  msg.position.x = des_r_leg_pos_[0];
1896  msg.position.y = des_r_leg_pos_[1];
1897  msg.position.z = des_r_leg_pos_[2];
1898 
1899  msg.orientation.x = des_r_leg_Q_[0];
1900  msg.orientation.y = des_r_leg_Q_[1];
1901  msg.orientation.z = des_r_leg_Q_[2];
1902  msg.orientation.w = des_r_leg_Q_[3];
1903  }
1904 
1905  res.pose.pose = msg;
1906 
1907  return true;
1908 }
1909 
1910 //bool OnlineWalkingModule::getPreviewMatrix(op3_online_walking_module_msgs::PreviewRequest msg)
1911 //{
1912 // op3_online_walking_module_msgs::GetPreviewMatrix get_preview_matrix;
1913 
1914 // // request
1915 // get_preview_matrix.request.req.control_cycle = msg.control_cycle;
1916 // get_preview_matrix.request.req.lipm_height = msg.lipm_height;
1917 
1918 // // response
1919 // if ( get_preview_matrix_client_.call( get_preview_matrix ) )
1920 // {
1921 // preview_response_.K = get_preview_matrix.response.res.K;
1922 // preview_response_.K_row = get_preview_matrix.response.res.K_row;
1923 // preview_response_.K_col = get_preview_matrix.response.res.K_col;
1924 
1925 // preview_response_.P = get_preview_matrix.response.res.P;
1926 // preview_response_.P_row = get_preview_matrix.response.res.P_row;
1927 // preview_response_.P_col = get_preview_matrix.response.res.P_col;
1928 
1929 // ROS_INFO("preview_response_.K");
1930 // for (int i=0; i<preview_response_.K.size(); i++)
1931 // {
1932 // ROS_INFO("%f", get_preview_matrix.response.res.K[i]);
1933 // }
1934 
1935 // ROS_INFO("K_row : %d", get_preview_matrix.response.res.K_row);
1936 // ROS_INFO("K_col : %d", get_preview_matrix.response.res.K_col);
1937 
1938 // ROS_INFO("preview_response_.P");
1939 // for (int i=0; i<preview_response_.P.size(); i++)
1940 // {
1941 // ROS_INFO("%f", get_preview_matrix.response.res.P[i]);
1942 // }
1943 
1944 // ROS_INFO("P_row : %d", get_preview_matrix.response.res.P_row);
1945 // ROS_INFO("P_col : %d", get_preview_matrix.response.res.P_col);
1946 
1947 // return true;
1948 // }
1949 // else
1950 // return false;
1951 //}
1952 
1954 {
1955  std::vector<double_t> K;
1956  K.push_back(739.200064);
1957  K.push_back(24489.822984);
1958  K.push_back(3340.410380);
1959  K.push_back(69.798325);
1960 
1961 // preview_response_.K = K;
1962 // preview_response_.K_row = 1;
1963 // preview_response_.K_col = 4;
1964 
1965  preview_response_K_ = K;
1968 
1969  std::vector<double_t> P;
1970  P.push_back(33.130169);
1971  P.push_back(531.738962);
1972  P.push_back(60.201291);
1973  P.push_back(0.327533);
1974  P.push_back(531.738962);
1975  P.push_back(10092.440286);
1976  P.push_back(1108.851055);
1977  P.push_back(7.388990);
1978  P.push_back(60.201291);
1979  P.push_back(1108.851055);
1980  P.push_back(130.194694);
1981  P.push_back(0.922502);
1982  P.push_back(0.327533);
1983  P.push_back(7.388990);
1984  P.push_back(0.922502);
1985  P.push_back(0.012336);
1986 
1987 // preview_response_.P = P;
1988 // preview_response_.P_row = 4;
1989 // preview_response_.P_col = 4;
1990 
1991  preview_response_P_ = P;
1994 
1995  return true;
1996 }
void setWholebodyBalanceMsgCallback(const std_msgs::String::ConstPtr &msg)
std::vector< double_t > des_body_pos_
void setDesiredFootForceTorque(double r_force_x_N, double r_force_y_N, double r_force_z_N, double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm, double l_force_x_N, double l_force_y_N, double l_force_z_N, double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm)
robotis_framework::MinimumJerk * joint_tra_
void getWalkingPosition(std::vector< double_t > &l_foot_pos, std::vector< double_t > &r_foot_pos, std::vector< double_t > &body_pos)
void setBodyOffsetCallback(const geometry_msgs::Pose::ConstPtr &msg)
Eigen::Matrix3d getRotationZ(double angle)
void setCutOffFrequency(double cut_off_frequency)
std::vector< double_t > des_body_accel_
std::vector< double_t > des_joint_accel_
void process(std::map< std::string, robotis_framework::Dynamixel * > dxls, std::map< std::string, double > sensors)
void setCOBManualAdjustment(double cob_x_adjustment_m, double cob_y_adjustment_m, double cob_z_adjustment_m)
op3_online_walking_module_msgs::WalkingParam walking_param_
void initialize(Eigen::MatrixXd pelvis_position, Eigen::MatrixXd pelvis_orientation)
Definition: op3_kdl.cpp:35
std::vector< double_t > getAcceleration(double time)
geometry_msgs::Wrench l_foot_ft_data_msg_
void setDesiredPose(const Eigen::MatrixXd &robot_to_cob, const Eigen::MatrixXd &robot_to_right_foot, const Eigen::MatrixXd &robot_to_left_foot)
std::map< std::string, DynamixelState * > result_
std::vector< std::string > joint_name_
std::vector< double_t > des_r_leg_vel_
void getWalkingState(int &walking_leg, int &walking_phase)
std::vector< double_t > preview_response_K_
void walkingParamCallback(const op3_online_walking_module_msgs::WalkingParam &msg)
void initialize(op3_online_walking_module_msgs::FootStepCommand foot_step_command, std::vector< double_t > init_body_pos, std::vector< double_t > init_body_Q, std::vector< double_t > init_r_foot_pos, std::vector< double_t > init_r_foot_Q, std::vector< double_t > init_l_foot_pos, std::vector< double_t > init_l_foot_Q)
std::vector< double_t > des_l_leg_accel_
void publish(const boost::shared_ptr< M > &message) const
double getFeedBack(double present_sensor_output)
std::vector< double_t > des_r_leg_pos_
void finalize()
Definition: op3_kdl.cpp:416
Eigen::Matrix3d getRotationX(double angle)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
BalanceControlUsingPDController balance_control_
double sign(double x)
void footStepCommandCallback(const op3_online_walking_module_msgs::FootStepCommand &msg)
std::vector< double_t > curr_joint_pos_
std::vector< double_t > des_l_leg_Q_
BalanceLowPassFilter right_foot_torque_pitch_lpf_
op3_online_walking_module_msgs::PreviewRequest preview_request_
std::vector< double_t > des_joint_pos_to_robot_
void imuDataCallback(const sensor_msgs::Imu::ConstPtr &msg)
Eigen::Quaterniond convertRotationToQuaternion(const Eigen::Matrix3d &rotation)
void set(double time)
std::map< std::string, int > joint_name_to_id_
bool getJointPoseCallback(op3_online_walking_module_msgs::GetJointPose::Request &req, op3_online_walking_module_msgs::GetJointPose::Response &res)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void getTaskPosition(std::vector< double_t > &l_foot_pos, std::vector< double_t > &r_foot_pos, std::vector< double_t > &body_pos)
#define ROS_WARN(...)
void setFootDistanceCallback(const std_msgs::Float64::ConstPtr &msg)
Eigen::Vector3d convertQuaternionToRPY(const Eigen::Quaterniond &quaternion)
std::vector< double_t > goal_balance_gain_ratio_
void initialize(std::vector< double_t > init_body_pos, std::vector< double_t > init_body_rot, std::vector< double_t > init_r_foot_pos, std::vector< double_t > init_r_foot_Q, std::vector< double_t > init_l_foot_pos, std::vector< double_t > init_l_foot_Q)
Eigen::Vector3d getTransitionXYZ(double position_x, double position_y, double position_z)
void footStep2DCallback(const op3_online_walking_module_msgs::Step2DArray &msg)
void solveForwardKinematics(std::vector< double_t > &rleg_position, std::vector< double_t > &rleg_orientation, std::vector< double_t > &lleg_position, std::vector< double_t > &lleg_orientation)
Definition: op3_kdl.cpp:283
void goalJointPoseCallback(const op3_online_walking_module_msgs::JointPose &msg)
std::vector< double_t > des_body_vel_
std::vector< double_t > preview_response_P_
std::vector< double_t > getPosition(double time)
void calcPreviewParam(std::vector< double_t > K, int K_row, int K_col, std::vector< double_t > P, int P_row, int P_col)
Eigen::Matrix3d convertQuaternionToRotation(const Eigen::Quaterniond &quaternion)
std::vector< double_t > joint_feedforward_gain_
Eigen::Matrix3d convertRPYToRotation(double roll, double pitch, double yaw)
void setCurrentFootForceTorqueSensorOutput(double r_force_x_N, double r_force_y_N, double r_force_z_N, double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm, double l_force_x_N, double l_force_y_N, double l_force_z_N, double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm)
void setResetBodyCallback(const std_msgs::Bool::ConstPtr &msg)
std::vector< double_t > curr_joint_accel_
void rightFootForceTorqueOutputCallback(const geometry_msgs::WrenchStamped::ConstPtr &msg)
std::vector< double_t > des_balance_gain_ratio_
robotis_framework::MinimumJerkViaPoint * feed_forward_tra_
void sensoryFeedback(const double &rlGyroErr, const double &fbGyroErr, double *balance_angle)
#define ROS_INFO(...)
void setCallbackQueue(CallbackQueueInterface *queue)
#define DEGREE2RADIAN
op3_online_walking_module_msgs::FootStepCommand foot_step_command_
void leftFootForceTorqueOutputCallback(const geometry_msgs::WrenchStamped::ConstPtr &msg)
BalancePDController joint_feedback_[12]
robotis_framework::MinimumJerk * balance_tra_
void goalKinematicsPoseCallback(const op3_online_walking_module_msgs::KinematicsPose &msg)
void process(int *balance_error, Eigen::MatrixXd *robot_to_cob_modified, Eigen::MatrixXd *robot_to_right_foot_modified, Eigen::MatrixXd *robot_to_left_foot_modified)
void setJointPosition(Eigen::VectorXd rleg_joint_position, Eigen::VectorXd lleg_joint_position)
Definition: op3_kdl.cpp:271
Eigen::Vector3d convertRotationToRPY(const Eigen::Matrix3d &rotation)
void parseJointFeedforwardGainData(const std::string &path)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::vector< double_t > des_l_leg_pos_
#define PRINT_MAT(X)
bool solveInverseKinematics(std::vector< double_t > &rleg_output, Eigen::MatrixXd rleg_target_position, Eigen::Quaterniond rleg_target_orientation, std::vector< double_t > &lleg_output, Eigen::MatrixXd lleg_target_position, Eigen::Quaterniond lleg_target_orientation)
Definition: op3_kdl.cpp:345
std::vector< double_t > des_joint_pos_
std::vector< double_t > des_l_leg_vel_
ROSLIB_DECL std::string getPath(const std::string &package_name)
std::vector< double_t > curr_joint_vel_
void getTaskOrientation(std::vector< double_t > &l_foot_Q, std::vector< double_t > &r_foot_Q, std::vector< double_t > &body_Q)
std::vector< double_t > des_r_leg_accel_
op3_online_walking_module_msgs::Step2DArray foot_step_2d_
std::vector< double_t > getPosition(double time)
robotis_framework::MinimumJerk * body_offset_tra_
void initialize(const int control_cycle_msec)
DynamixelState * dxl_state_
BalanceLowPassFilter right_foot_torque_roll_lpf_
static Time now()
void setDesiredCOBGyro(double gyro_roll, double gyro_pitch)
std::vector< double_t > getVelocity(double time)
void parseBalanceGainData(const std::string &path)
std::vector< double_t > des_joint_vel_
void publishStatusMsg(unsigned int type, std::string msg)
bool ok() const
void getWalkingOrientation(std::vector< double_t > &l_foot_Q, std::vector< double_t > &r_foot_Q, std::vector< double_t > &body_Q)
std::vector< double_t > des_joint_feedback_
std::vector< double_t > goal_joint_accel_
void getLIPM(std::vector< double_t > &x_lipm, std::vector< double_t > &y_lipm)
void set(double time, int step, bool foot_step_2d)
void setCurrentGyroSensorOutput(double gyro_roll, double gyro_pitch)
std::vector< double_t > goal_body_offset_
#define ROS_ERROR(...)
std::vector< double_t > des_r_leg_Q_
std::vector< double_t > goal_joint_vel_
std::vector< double_t > des_body_offset_
std::vector< double_t > goal_joint_pos_
BalanceLowPassFilter left_foot_torque_pitch_lpf_
std::vector< double_t > des_joint_feedforward_
geometry_msgs::Wrench r_foot_ft_data_msg_
void setDesiredCOBOrientation(double cob_orientation_roll, double cob_orientation_pitch)
bool getKinematicsPoseCallback(op3_online_walking_module_msgs::GetKinematicsPose::Request &req, op3_online_walking_module_msgs::GetKinematicsPose::Response &res)
void setCurrentOrientationSensorOutput(double cob_orientation_roll, double cob_orientation_pitch)
void parseJointFeedbackGainData(const std::string &path)
void initialize(const int control_cycle_msec, robotis_framework::Robot *robot)


op3_online_walking_module
Author(s): SCH
autogenerated on Mon Jun 10 2019 14:41:22