19 #ifndef OP3_ONLINE_WALKING_MODULE_ONLINE_WALKING_MODULE_H_ 20 #define OP3_ONLINE_WALKING_MODULE_ONLINE_WALKING_MODULE_H_ 26 #include <std_msgs/Bool.h> 27 #include <std_msgs/Int16.h> 28 #include <std_msgs/Float64.h> 29 #include <std_msgs/String.h> 30 #include <sensor_msgs/Imu.h> 31 #include <geometry_msgs/Pose.h> 32 #include <geometry_msgs/WrenchStamped.h> 33 #include <geometry_msgs/PoseStamped.h> 34 #include <boost/thread.hpp> 35 #include <eigen3/Eigen/Eigen> 36 #include <yaml-cpp/yaml.h> 43 #include "robotis_controller_msgs/JointCtrlModule.h" 44 #include "robotis_controller_msgs/StatusMsg.h" 51 #include "op3_online_walking_module_msgs/JointPose.h" 52 #include "op3_online_walking_module_msgs/KinematicsPose.h" 53 #include "op3_online_walking_module_msgs/FootStepCommand.h" 54 #include "op3_online_walking_module_msgs/PreviewRequest.h" 55 #include "op3_online_walking_module_msgs/PreviewResponse.h" 56 #include "op3_online_walking_module_msgs/WalkingParam.h" 58 #include "op3_online_walking_module_msgs/GetJointPose.h" 59 #include "op3_online_walking_module_msgs/GetKinematicsPose.h" 60 #include "op3_online_walking_module_msgs/GetPreviewMatrix.h" 62 #include "op3_online_walking_module_msgs/Step2D.h" 63 #include "op3_online_walking_module_msgs/Step2DArray.h" 107 op3_online_walking_module_msgs::GetJointPose::Response &res);
109 op3_online_walking_module_msgs::GetKinematicsPose::Response &res);
115 void process(std::map<std::string, robotis_framework::Dynamixel *> dxls, std::map<std::string, double> sensors);
150 void sensoryFeedback(
const double &rlGyroErr,
const double &fbGyroErr,
double *balance_angle);
void setWholebodyBalanceMsgCallback(const std_msgs::String::ConstPtr &msg)
double foot_roll_angle_d_gain_
std::vector< double_t > des_body_pos_
robotis_framework::MinimumJerk * joint_tra_
void setBodyOffsetCallback(const geometry_msgs::Pose::ConstPtr &msg)
std::vector< double_t > des_body_accel_
double foot_z_force_p_gain_
std::vector< double_t > des_joint_accel_
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)
double foot_pitch_angle_p_gain_
op3_online_walking_module_msgs::WalkingParam walking_param_
geometry_msgs::Wrench l_foot_ft_data_msg_
void setFeedforwardControl()
CONTROL_TYPE control_type_
std::vector< std::string > joint_name_
ros::Publisher goal_joint_state_pub_
std::vector< double_t > des_r_leg_vel_
std::vector< double_t > preview_response_K_
void walkingParamCallback(const op3_online_walking_module_msgs::WalkingParam &msg)
std::vector< double_t > des_r_arm_accel_
std::vector< double_t > des_l_arm_accel_
void setTargetForceTorque()
double balance_l_foot_torque_x_
std::vector< double_t > des_l_leg_accel_
ros::Publisher pelvis_pose_pub_
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_
std_msgs::String movement_done_msg_
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_
std::vector< double_t > des_r_arm_pos_
double foot_x_force_cut_off_frequency_
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_
std::vector< double_t > des_l_leg_Q_
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)
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)
WholebodyControl * wholebody_control_
std::vector< double_t > des_l_arm_pos_
void setFootDistanceCallback(const std_msgs::Float64::ConstPtr &msg)
double balance_l_foot_torque_y_
bool getPreviewMatrix(op3_online_walking_module_msgs::PreviewRequest msg)
ros::Publisher movement_done_pub_
std::vector< double_t > goal_balance_gain_ratio_
double balance_l_foot_force_y_
Eigen::MatrixXd g_to_l_leg_
int preview_response_P_row_
void footStep2DCallback(const op3_online_walking_module_msgs::Step2DArray &msg)
void calcWalkingControl()
Eigen::MatrixXd g_to_r_leg_
void goalJointPoseCallback(const op3_online_walking_module_msgs::JointPose &msg)
void calcWholebodyControl()
std::vector< double_t > des_body_vel_
std::vector< double_t > preview_response_P_
double foot_pitch_torque_cut_off_frequency_
double balance_r_foot_force_x_
double foot_roll_torque_p_gain_
op3_online_walking_module_msgs::PreviewResponse preview_response_
std::vector< double_t > joint_feedforward_gain_
boost::mutex imu_data_mutex_lock_
double balance_r_foot_force_z_
void calcBalanceControl()
void setResetBodyCallback(const std_msgs::Bool::ConstPtr &msg)
geometry_msgs::Pose wholebody_goal_msg_
std::vector< double_t > des_l_arm_Q_
std::vector< double_t > curr_joint_accel_
void rightFootForceTorqueOutputCallback(const geometry_msgs::WrenchStamped::ConstPtr &msg)
double foot_pitch_angle_d_gain_
std::vector< double_t > des_balance_gain_ratio_
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_
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)
double pitch_gyro_cut_off_frequency_
bool joint_control_initialize_
void parseJointFeedforwardGainData(const std::string &path)
std::vector< double_t > des_l_leg_pos_
sensor_msgs::Imu imu_data_msg_
std::vector< double_t > des_joint_pos_
std::vector< double_t > des_l_leg_vel_
double foot_x_force_p_gain_
std::vector< double_t > curr_joint_vel_
std::vector< double_t > des_r_leg_accel_
double balance_l_foot_force_z_
op3_online_walking_module_msgs::Step2DArray foot_step_2d_
robotis_framework::MinimumJerk * body_offset_tra_
bool wholebody_initialize_
std::vector< double_t > des_r_arm_Q_
WalkingControl * walking_control_
void initFeedforwardControl()
std::vector< double_t > des_r_arm_vel_
void setFeedbackControl()
std::vector< double_t > y_lipm_
double foot_x_force_d_gain_
void initWholebodyControl()
std::string wholegbody_control_group_
double foot_pitch_gyro_p_gain_
double balance_ankle_pitch_gain_
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()
std::vector< double_t > des_joint_feedback_
std::vector< double_t > goal_joint_accel_
void initBalanceControl()
double roll_gyro_cut_off_frequency_
double balance_ankle_roll_gain_
double pitch_angle_cut_off_frequency_
std::vector< double_t > x_lipm_
double balance_l_foot_torque_z_
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_
std::vector< double_t > des_joint_feedforward_
double balance_r_foot_torque_z_
double foot_pitch_gyro_d_gain_
geometry_msgs::Wrench r_foot_ft_data_msg_
bool getKinematicsPoseCallback(op3_online_walking_module_msgs::GetKinematicsPose::Request &req, op3_online_walking_module_msgs::GetKinematicsPose::Response &res)
boost::mutex queue_mutex_
void parseJointFeedbackGainData(const std::string &path)
std::vector< double_t > des_l_arm_vel_
void initialize(const int control_cycle_msec, robotis_framework::Robot *robot)