24 : control_cycle_sec_(0.008),
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),
152 std::string balance_gain_path =
ros::package::getPath(
"op3_online_walking_module") +
"/config/balance_gain.yaml";
155 std::string joint_feedback_gain_path =
ros::package::getPath(
"op3_online_walking_module") +
"/config/joint_feedback_gain.yaml";
158 std::string joint_feedforward_gain_path =
ros::package::getPath(
"op3_online_walking_module") +
"/config/joint_feedforward_gain.yaml";
276 doc = YAML::LoadFile(path.c_str());
278 catch (
const std::exception& e)
333 doc = YAML::LoadFile(path.c_str());
335 catch (
const std::exception& e)
374 doc = YAML::LoadFile(path.c_str());
376 catch (
const std::exception& e)
402 std::string balance_gain_path =
ros::package::getPath(
"op3_online_walking_module") +
"/config/balance_gain.yaml";
405 std::string joint_feedback_gain_path =
ros::package::getPath(
"op3_online_walking_module") +
"/config/joint_feedback_gain.yaml";
408 std::string joint_feedforward_gain_path =
ros::package::getPath(
"op3_online_walking_module") +
"/config/joint_feedforward_gain.yaml";
411 if (msg->data ==
"balance_on")
413 else if(msg->data ==
"balance_off")
428 double ini_time = 0.0;
429 double mov_time = 1.0;
434 std::vector<double_t> balance_zero;
435 balance_zero.resize(1, 0.0);
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;
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;
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);
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;
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;
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);
566 if (msg->data ==
true)
586 size_t joint_size = msg.pose.name.size();
592 for (
size_t i = 0; i < msg.pose.name.size(); i++)
594 std::string joint_name = msg.pose.name[i];
604 ROS_WARN(
"[WARN] Control type is different!");
614 double ini_time = 0.0;
683 ROS_WARN(
"[WARN] Control type is different!");
703 double ini_time = 0.0;
704 double mov_time = 1.0;
709 std::vector<double_t> offset_zero;
710 offset_zero.resize(3, 0.0);
770 ROS_WARN(
"[WARN] Control group is different!");
782 ROS_WARN(
"[WARN] Control type is different!");
792 double ini_time = 0.0;
811 ROS_INFO(
"[START] Wholebody Control");
843 ROS_INFO(
"[END] Wholebody Control");
864 Eigen::MatrixXd body_T = Eigen::MatrixXd::Identity(4,4);
865 body_T.block(0,0,3,3) = body_R;
869 op3_online_walking_module_msgs::Step2DArray foot_step_msg;
871 int old_size = msg.footsteps_2d.size();
872 int new_size = old_size + 3;
874 op3_online_walking_module_msgs::Step2D first_msg;
875 op3_online_walking_module_msgs::Step2D second_msg;
877 first_msg.moving_foot = msg.footsteps_2d[0].moving_foot - 1;
878 second_msg.moving_foot = first_msg.moving_foot + 1;
880 if (first_msg.moving_foot ==
LEFT_LEG)
884 first_msg.step2d.theta = body_rpy.coeff(2,0);
888 second_msg.step2d.theta = body_rpy.coeff(2,0);
890 else if (first_msg.moving_foot ==
RIGHT_LEG)
894 first_msg.step2d.theta = body_rpy.coeff(2,0);
898 second_msg.step2d.theta = body_rpy.coeff(2,0);
901 foot_step_msg.footsteps_2d.push_back(first_msg);
902 foot_step_msg.footsteps_2d.push_back(second_msg);
904 double step_final_theta;
908 for (
int i=0; i<old_size; i++)
910 op3_online_walking_module_msgs::Step2D step_msg = msg.footsteps_2d[i];
911 step_msg.moving_foot -= 1;
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;
919 Eigen::MatrixXd step_T_new = body_T*step_T;
920 Eigen::MatrixXd step_R_new = step_T_new.block(0,0,3,3);
922 double step_new_x = step_T_new.coeff(0,3);
923 double step_new_y = step_T_new.coeff(1,3);
925 double step_new_theta = step_new_rpy.coeff(2,0);
927 step_msg.step2d.x = step_new_x;
928 step_msg.step2d.y = step_new_y;
929 step_msg.step2d.theta = step_new_theta;
932 step_final_theta = step_new_theta;
934 foot_step_msg.footsteps_2d.push_back(step_msg);
944 op3_online_walking_module_msgs::Step2D step_msg = msg.footsteps_2d[old_size-1];
946 if (step_msg.moving_foot - 1 ==
LEFT_LEG)
951 first_msg.step2d.x = 0.0;
952 first_msg.step2d.y = 0.0;
953 first_msg.step2d.theta = step_final_theta;
955 foot_step_msg.footsteps_2d.push_back(first_msg);
957 for (
int i=0; i<new_size; i++)
959 op3_online_walking_module_msgs::Step2D step_msg = foot_step_msg.footsteps_2d[i];
980 ROS_WARN(
"[WARN] Previous task is alive!");
983 ROS_WARN(
"[WARN] Control type is different!");
1012 ROS_WARN(
"[WARN] Previous task is alive!");
1015 ROS_WARN(
"[WARN] Control type is different!");
1037 bool get_preview_matrix =
false;
1041 if (get_preview_matrix ==
true)
1086 ROS_WARN(
"[FAIL] Cannot get preview matrix");
1163 std::vector<double_t> zero_vector;
1164 zero_vector.resize(1,0.0);
1166 std::vector<double_t> via_pos;
1167 via_pos.resize(3, 0.0);
1170 double init_time = 0.0;
1172 double via_time = 0.5 * (init_time + fin_time);
1177 zero_vector, zero_vector, zero_vector,
1178 zero_vector, zero_vector, zero_vector,
1179 via_pos, zero_vector, zero_vector);
1184 Eigen::MatrixXd des_body_pos = Eigen::MatrixXd::Zero(3,1);
1195 Eigen::VectorXd r_leg_joint_pos, l_leg_joint_pos;
1197 r_leg_joint_pos.resize(6);
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];
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];
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);
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);
1224 l_leg_pos, l_leg_Q);
1226 Eigen::Quaterniond curr_r_leg_Q(r_leg_Q[3],r_leg_Q[0],r_leg_Q[1],r_leg_Q[2]);
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];
1235 Eigen::Quaterniond curr_l_leg_Q(l_leg_Q[3],l_leg_Q[0],l_leg_Q[1],l_leg_Q[2]);
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];
1362 bool ik_success =
true;
1365 Eigen::MatrixXd des_body_pos = Eigen::MatrixXd::Zero(3,1);
1375 Eigen::MatrixXd des_r_foot_pos = Eigen::MatrixXd::Zero(3,1);
1384 Eigen::MatrixXd des_l_foot_pos = Eigen::MatrixXd::Zero(3,1);
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;
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;
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;
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;
1424 Eigen::Quaterniond imu_quaternion(
imu_data_msg_.orientation.w,
1428 Eigen::MatrixXd imu_rpy =
1456 Eigen::MatrixXd robot_to_r_foot_force =
1460 Eigen::MatrixXd robot_to_r_foot_torque =
1464 Eigen::MatrixXd robot_to_l_foot_force =
1468 Eigen::MatrixXd robot_to_l_foot_torque =
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));
1514 Eigen::MatrixXd robot_to_body_mod, robot_to_r_foot_mod, robot_to_l_foot_mod;
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;
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);
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);
1538 Eigen::VectorXd r_leg_joint_pos, l_leg_joint_pos;
1540 r_leg_joint_pos.resize(6);
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];
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];
1558 std::vector<double_t> r_leg_output, l_leg_output;
1564 des_r_foot_pos_mod,des_r_foot_Q_mod,
1566 des_l_foot_pos_mod,des_l_foot_Q_mod);
1570 if (ik_success ==
true)
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];
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];
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];
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];
1610 feed_forward_value[0] = 0.0;
1612 std::vector<double_t> support_leg_gain;
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;
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;
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;
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;
1658 double internal_gain = 0.05;
1662 balance_angle[joint_name_to_id_[
"l_hip_roll"]-1] =
1665 balance_angle[joint_name_to_id_[
"r_knee"]-1] =
1667 balance_angle[joint_name_to_id_[
"l_knee"]-1] =
1670 balance_angle[joint_name_to_id_[
"r_ank_pitch"]-1] =
1672 balance_angle[joint_name_to_id_[
"l_ank_pitch"]-1] =
1675 balance_angle[joint_name_to_id_[
"r_ank_roll"]-1] =
1677 balance_angle[joint_name_to_id_[
"l_ank_roll"]-1] =
1683 std::map<std::string, double> sensors)
1691 balance_angle[i] = 0.0;
1693 double rl_gyro_err = 0.0 - sensors[
"gyro_x"];
1694 double fb_gyro_err = 0.0 - sensors[
"gyro_y"];
1699 for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter =
result_.begin();
1700 state_iter !=
result_.end(); state_iter++)
1702 std::string joint_name = state_iter->first;
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;
1767 ROS_INFO(
"[FAIL] Task Space Control");
1773 if (time_duration.
toSec() > 0.003)
1774 ROS_INFO(
"[Wholebody Module] Calc Time: %f", time_duration.
toSec());
1781 sensor_msgs::JointState goal_joint_msg;
1782 geometry_msgs::PoseStamped pelvis_pose_msg;
1789 pelvis_pose_msg.pose.position.z =
des_body_pos_[2] - 0.0907;
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];
1797 for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter =
result_.begin();
1798 state_iter !=
result_.end(); state_iter++)
1800 std::string joint_name = state_iter->first;
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]);
1843 robotis_controller_msgs::StatusMsg status;
1846 status.module_name =
"Wholebody";
1847 status.status_msg = msg;
1853 op3_online_walking_module_msgs::GetJointPose::Response &res)
1865 op3_online_walking_module_msgs::GetKinematicsPose::Response &res)
1867 std::string group_name = req.name;
1869 geometry_msgs::Pose msg;
1871 if (group_name ==
"body")
1882 else if (group_name ==
"left_leg")
1893 else if (group_name ==
"right_leg")
1905 res.pose.pose = msg;
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);
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);
void setWholebodyBalanceMsgCallback(const std_msgs::String::ConstPtr &msg)
double foot_roll_angle_d_gain_
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_
double foot_z_force_p_gain_
std::vector< double_t > des_joint_accel_
BalanceLowPassFilter roll_gyro_lpf_
std::vector< double_t > des_body_Q_
double balance_r_foot_torque_x_
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)
double foot_pitch_angle_p_gain_
op3_online_walking_module_msgs::WalkingParam walking_param_
void initialize(Eigen::MatrixXd pelvis_position, Eigen::MatrixXd pelvis_orientation)
std::vector< double_t > getAcceleration(double time)
geometry_msgs::Wrench l_foot_ft_data_msg_
void setFeedforwardControl()
void setDesiredPose(const Eigen::MatrixXd &robot_to_cob, const Eigen::MatrixXd &robot_to_right_foot, const Eigen::MatrixXd &robot_to_left_foot)
CONTROL_TYPE control_type_
std::map< std::string, DynamixelState * > result_
std::vector< std::string > joint_name_
ros::Publisher goal_joint_state_pub_
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)
void setTargetForceTorque()
double balance_l_foot_torque_x_
std::vector< double_t > des_l_leg_accel_
ros::Publisher pelvis_pose_pub_
void publish(const boost::shared_ptr< M > &message) const
double getFeedBack(double present_sensor_output)
std::vector< double_t > des_r_leg_pos_
double foot_roll_torque_cut_off_frequency_
bool balance_control_initialize_
boost::thread queue_thread_
double foot_z_force_d_gain_
BalanceLowPassFilter roll_angle_lpf_
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 control_cycle_sec_
double foot_roll_gyro_d_gain_
double foot_roll_gyro_p_gain_
double foot_pitch_torque_p_gain_
double foot_x_force_cut_off_frequency_
BalancePDController right_foot_force_y_ctrl_
BalancePDController left_foot_force_y_ctrl_
void footStepCommandCallback(const op3_online_walking_module_msgs::FootStepCommand &msg)
double foot_y_force_d_gain_
virtual ~OnlineWalkingModule()
int preview_response_K_col_
std::vector< double_t > curr_joint_pos_
void setForceTorqueBalanceEnable(bool enable)
BalancePDController foot_roll_angle_ctrl_
std::vector< double_t > des_l_leg_Q_
BalanceLowPassFilter right_foot_torque_pitch_lpf_
BalanceLowPassFilter right_foot_force_z_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)
BalanceLowPassFilter left_foot_force_z_lpf_
bool definePreviewMatrix()
std::map< std::string, int > joint_name_to_id_
double balance_r_foot_torque_y_
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)
WholebodyControl * wholebody_control_
void getTaskPosition(std::vector< double_t > &l_foot_pos, std::vector< double_t > &r_foot_pos, std::vector< double_t > &body_pos)
BalancePDController left_foot_torque_pitch_ctrl_
void setFootDistanceCallback(const std_msgs::Float64::ConstPtr &msg)
Eigen::Vector3d convertQuaternionToRPY(const Eigen::Quaterniond &quaternion)
ControlMode control_mode_
BalanceLowPassFilter left_foot_force_x_lpf_
double balance_l_foot_torque_y_
ros::Publisher movement_done_pub_
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)
BalanceLowPassFilter right_foot_force_x_lpf_
double balance_l_foot_force_y_
Eigen::Vector3d getTransitionXYZ(double position_x, double position_y, double position_z)
int preview_response_P_row_
void footStep2DCallback(const op3_online_walking_module_msgs::Step2DArray &msg)
void calcWalkingControl()
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)
void goalJointPoseCallback(const op3_online_walking_module_msgs::JointPose &msg)
void setOrientationBalanceEnable(bool enable)
void calcWholebodyControl()
std::vector< double_t > des_body_vel_
BalanceLowPassFilter pitch_angle_lpf_
std::vector< double_t > preview_response_P_
std::vector< double_t > getPosition(double time)
double foot_pitch_torque_cut_off_frequency_
double balance_r_foot_force_x_
double foot_roll_torque_p_gain_
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_
boost::mutex imu_data_mutex_lock_
Eigen::Matrix3d convertRPYToRotation(double roll, double pitch, double yaw)
double balance_r_foot_force_z_
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 calcBalanceControl()
void setResetBodyCallback(const std_msgs::Bool::ConstPtr &msg)
geometry_msgs::Pose wholebody_goal_msg_
std::vector< double_t > curr_joint_accel_
void rightFootForceTorqueOutputCallback(const geometry_msgs::WrenchStamped::ConstPtr &msg)
double foot_pitch_angle_d_gain_
BalancePDController left_foot_force_z_ctrl_
std::vector< double_t > des_balance_gain_ratio_
BalancePDController foot_pitch_gyro_ctrl_
robotis_framework::MinimumJerkViaPoint * feed_forward_tra_
double balance_l_foot_force_x_
BALANCE_TYPE balance_type_
void sensoryFeedback(const double &rlGyroErr, const double &fbGyroErr, double *balance_angle)
bool body_offset_initialize_
double balance_r_foot_force_y_
void setCallbackQueue(CallbackQueueInterface *queue)
int preview_response_K_row_
double balance_knee_gain_
op3_online_walking_module_msgs::FootStepCommand foot_step_command_
double foot_roll_angle_p_gain_
void leftFootForceTorqueOutputCallback(const geometry_msgs::WrenchStamped::ConstPtr &msg)
double balance_hip_roll_gain_
double foot_roll_torque_d_gain_
double foot_y_force_p_gain_
BalancePDController joint_feedback_[12]
robotis_framework::MinimumJerk * balance_tra_
double foot_z_force_cut_off_frequency_
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)
double pitch_gyro_cut_off_frequency_
Eigen::Vector3d convertRotationToRPY(const Eigen::Matrix3d &rotation)
bool joint_control_initialize_
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_
BalancePDController right_foot_torque_roll_ctrl_
sensor_msgs::Imu imu_data_msg_
BalancePDController foot_roll_gyro_ctrl_
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)
BalancePDController foot_pitch_angle_ctrl_
std::vector< double_t > des_joint_pos_
std::vector< double_t > des_l_leg_vel_
double foot_x_force_p_gain_
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_
BalancePDController right_foot_torque_pitch_ctrl_
double balance_l_foot_force_z_
BalanceLowPassFilter left_foot_force_y_lpf_
op3_online_walking_module_msgs::Step2DArray foot_step_2d_
std::vector< double_t > getPosition(double time)
robotis_framework::MinimumJerk * body_offset_tra_
bool wholebody_initialize_
void initialize(const int control_cycle_msec)
WalkingControl * walking_control_
void initFeedforwardControl()
DynamixelState * dxl_state_
void setFeedbackControl()
std::vector< double_t > y_lipm_
BalanceLowPassFilter right_foot_force_y_lpf_
void setGyroBalanceEnable(bool enable)
BalanceLowPassFilter right_foot_torque_roll_lpf_
double foot_x_force_d_gain_
void initWholebodyControl()
BalancePDController right_foot_force_x_ctrl_
BalanceLowPassFilter pitch_gyro_lpf_
std::string wholegbody_control_group_
void setDesiredCOBGyro(double gyro_roll, double gyro_pitch)
double foot_pitch_gyro_p_gain_
double balance_ankle_pitch_gain_
std::vector< double_t > getVelocity(double time)
void parseBalanceGainData(const std::string &path)
std::vector< double_t > des_joint_vel_
ros::Publisher status_msg_pub_
double foot_pitch_torque_d_gain_
void publishStatusMsg(unsigned int type, std::string msg)
double foot_y_force_cut_off_frequency_
void setBalanceControlGain()
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_
BalancePDController left_foot_torque_roll_ctrl_
std::vector< double_t > goal_joint_accel_
void getLIPM(std::vector< double_t > &x_lipm, std::vector< double_t > &y_lipm)
void initBalanceControl()
double roll_gyro_cut_off_frequency_
void set(double time, int step, bool foot_step_2d)
double balance_ankle_roll_gain_
double pitch_angle_cut_off_frequency_
std::vector< double_t > x_lipm_
double balance_l_foot_torque_z_
void setCurrentGyroSensorOutput(double gyro_roll, double gyro_pitch)
int preview_response_P_col_
std::vector< double_t > goal_body_offset_
std::vector< double_t > des_r_leg_Q_
std::vector< double_t > goal_joint_vel_
std::vector< double_t > des_body_offset_
void initWalkingControl()
std::vector< double_t > goal_joint_pos_
double roll_angle_cut_off_frequency_
BalancePDController left_foot_force_x_ctrl_
BalanceLowPassFilter left_foot_torque_pitch_lpf_
std::vector< double_t > des_joint_feedforward_
double balance_r_foot_torque_z_
BalanceLowPassFilter left_foot_torque_roll_lpf_
double foot_pitch_gyro_d_gain_
BalancePDController right_foot_force_z_ctrl_
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)
boost::mutex queue_mutex_
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)