19 #ifndef OP3_ONLINE_WALKING_MODULE_WHOLEBODY_CONTROL_ 20 #define OP3_ONLINE_WALKING_MODULE_WHOLEBODY_CONTROL_ 29 #include <geometry_msgs/Pose.h> 30 #include <eigen3/Eigen/Eigen> 37 double init_time,
double fin_time,
38 geometry_msgs::Pose goal_msg);
41 void initialize(std::vector<double_t> init_body_pos, std::vector<double_t> init_body_rot,
42 std::vector<double_t> init_r_foot_pos, std::vector<double_t> init_r_foot_Q,
43 std::vector<double_t> init_l_foot_pos, std::vector<double_t> init_l_foot_Q);
47 void set(
double time);
54 std::vector<double_t> &r_foot_pos,
55 std::vector<double_t> &body_pos);
59 std::vector<double_t> &r_foot_Q,
60 std::vector<double_t> &body_Q);
62 void getGroupPose(std::string name, geometry_msgs::Pose *msg);
std::vector< double_t > des_l_foot_pos_
std::vector< double_t > goal_body_pos_
std::vector< double_t > init_body_vel_
std::vector< double_t > goal_task_accel_
std::vector< double_t > init_l_foot_pos_
Eigen::Quaterniond des_body_Q_
Eigen::Quaterniond init_l_foot_Q_
std::vector< double_t > des_r_foot_vel_
std::vector< double_t > goal_task_pos_
std::vector< double_t > des_l_foot_vel_
std::vector< double_t > goal_body_vel_
std::vector< double_t > init_r_foot_vel_
Eigen::Quaterniond goal_body_Q_
Eigen::Quaterniond init_body_Q_
virtual ~WholebodyControl()
Eigen::Quaterniond init_r_foot_Q_
std::vector< double_t > des_body_pos_
std::vector< double_t > goal_task_vel_
std::vector< double_t > init_r_foot_pos_
std::string control_group_
void getTaskPosition(std::vector< double_t > &l_foot_pos, std::vector< double_t > &r_foot_pos, std::vector< double_t > &body_pos)
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)
std::vector< double_t > getJointPosition(double time)
std::vector< double_t > getJointVelocity(double time)
std::vector< double_t > goal_r_foot_vel_
std::vector< double_t > des_r_foot_accel_
std::vector< double_t > goal_r_foot_accel_
Eigen::Quaterniond des_l_foot_Q_
std::vector< double_t > goal_l_foot_pos_
Eigen::Quaterniond des_task_Q_
Eigen::Quaterniond init_task_Q_
std::vector< double_t > init_r_foot_accel_
robotis_framework::MinimumJerk * task_trajectory_
std::vector< double_t > goal_r_foot_pos_
std::vector< double_t > init_l_foot_vel_
geometry_msgs::Pose goal_msg_
std::vector< double_t > getJointAcceleration(double time)
Eigen::Quaterniond goal_l_foot_Q_
std::vector< double_t > init_body_accel_
std::vector< double_t > init_l_foot_accel_
std::vector< double_t > des_l_foot_accel_
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 > goal_l_foot_vel_
std::vector< double_t > des_r_foot_pos_
std::vector< double_t > goal_body_accel_
std::vector< double_t > getTaskAcceleration(double time)
std::vector< double_t > init_body_pos_
std::vector< double_t > goal_l_foot_accel_
std::vector< double_t > getTaskVelocity(double time)
std::vector< double_t > des_body_vel_
void getGroupPose(std::string name, geometry_msgs::Pose *msg)
std::vector< double_t > des_body_accel_
Eigen::Quaterniond goal_task_Q_
Eigen::Quaterniond des_r_foot_Q_
Eigen::Quaterniond goal_r_foot_Q_
WholebodyControl(std::string control_group, double init_time, double fin_time, geometry_msgs::Pose goal_msg)