23 double init_time,
double fin_time,
24 geometry_msgs::Pose goal_msg)
83 std::vector<double_t> init_r_foot_pos, std::vector<double_t> init_r_foot_Q,
84 std::vector<double_t> init_l_foot_pos, std::vector<double_t> init_l_foot_Q)
89 Eigen::Quaterniond body_Q(init_body_rot[3],init_body_rot[0],init_body_rot[1],init_body_rot[2]);
99 Eigen::Quaterniond l_foot_Q(init_l_foot_Q[3],init_l_foot_Q[0],init_l_foot_Q[1],init_l_foot_Q[2]);
103 Eigen::Quaterniond r_foot_Q(init_r_foot_Q[3],init_r_foot_Q[0],init_r_foot_Q[1],init_r_foot_Q[2]);
202 std::vector<double_t> &r_foot_pos,
203 std::vector<double_t> &body_pos)
221 std::vector<double_t> &r_foot_Q,
222 std::vector<double_t> &body_Q)
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 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 > getPosition(double time)
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)
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_
WholebodyControl(std::string control_group, double init_time, double fin_time, geometry_msgs::Pose goal_msg)