wholebody_control.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 
19 #include <stdio.h>
21 
22 WholebodyControl::WholebodyControl(std::string control_group,
23  double init_time, double fin_time,
24  geometry_msgs::Pose goal_msg)
25 {
26  control_group_ = control_group;
27 
28  init_time_ = init_time;
29  fin_time_ = fin_time;
30 
31  goal_msg_ = goal_msg;
32 
33  // Initialization
34  init_body_pos_.resize(3, 0.0);
35  init_body_vel_.resize(3, 0.0);
36  init_body_accel_.resize(3, 0.0);
37  des_body_pos_.resize(3, 0.0);
38  des_body_vel_.resize(3, 0.0);
39  des_body_accel_.resize(3, 0.0);
40  goal_body_pos_.resize(3, 0.0);
41  goal_body_vel_.resize(3, 0.0);
42  goal_body_accel_.resize(3, 0.0);
43 
44  init_l_foot_pos_.resize(3, 0.0);
45  init_l_foot_vel_.resize(3, 0.0);
46  init_l_foot_accel_.resize(3, 0.0);
47  des_l_foot_pos_.resize(3, 0.0);
48  des_l_foot_vel_.resize(3, 0.0);
49  des_l_foot_accel_.resize(3, 0.0);
50  goal_l_foot_pos_.resize(3, 0.0);
51  goal_l_foot_vel_.resize(3, 0.0);
52  goal_l_foot_accel_.resize(3, 0.0);
53 
54  init_r_foot_pos_.resize(3, 0.0);
55  init_r_foot_vel_.resize(3, 0.0);
56  init_r_foot_accel_.resize(3, 0.0);
57  des_r_foot_pos_.resize(3, 0.0);
58  des_r_foot_vel_.resize(3, 0.0);
59  des_r_foot_accel_.resize(3, 0.0);
60  goal_r_foot_pos_.resize(3, 0.0);
61  goal_r_foot_vel_.resize(3, 0.0);
62  goal_r_foot_accel_.resize(3, 0.0);
63 
64  goal_task_pos_.resize(3, 0.0);
65  goal_task_vel_.resize(3, 0.0);
66  goal_task_accel_.resize(3, 0.0);
67 
68  goal_task_pos_[0] = goal_msg_.position.x;
69  goal_task_pos_[1] = goal_msg_.position.y;
70  goal_task_pos_[2] = goal_msg_.position.z;
71 
72  Eigen::Quaterniond goal_task_Q(goal_msg_.orientation.w,goal_msg_.orientation.x,
73  goal_msg_.orientation.y,goal_msg_.orientation.z);
74  goal_task_Q_ = goal_task_Q;
75 }
76 
78 {
79 
80 }
81 
82 void WholebodyControl::initialize(std::vector<double_t> init_body_pos, std::vector<double_t> init_body_rot,
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)
85 {
86  init_body_pos_ = init_body_pos;
87  des_body_pos_ = init_body_pos;
88 
89  Eigen::Quaterniond body_Q(init_body_rot[3],init_body_rot[0],init_body_rot[1],init_body_rot[2]);
90  init_body_Q_ = body_Q;
91  des_body_Q_ = body_Q;
92 
93  init_r_foot_pos_ = init_r_foot_pos;
94  init_l_foot_pos_ = init_l_foot_pos;
95 
98 
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]);
100  init_l_foot_Q_ = l_foot_Q;
101  des_l_foot_Q_ = l_foot_Q;
102 
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]);
104  init_r_foot_Q_ = r_foot_Q;
105  des_r_foot_Q_ = r_foot_Q;
106 
107  if (control_group_ == "body")
108  {
113  init_task_Q_ = body_Q;
114  }
115  else if (control_group_ == "right_leg")
116  {
121  init_task_Q_ = r_foot_Q;
122  }
123  else if (control_group_ == "left_leg")
124  {
129  init_task_Q_ = l_foot_Q;
130  }
131 }
132 
134 {
135 
136 }
137 
139 {
140  delete task_trajectory_;
141 }
142 
143 void WholebodyControl::set(double time)
144 {
145  std::vector<double_t> des_task_pos = task_trajectory_->getPosition(time);
146 
147  double count = time / fin_time_;
148  des_task_Q_ = init_task_Q_.slerp(count, goal_task_Q_);
149 
150  if (control_group_ == "left_leg")
151  {
154 
155  des_l_foot_pos_ = des_task_pos;
157 
160  }
161  else if (control_group_ == "right_leg")
162  {
165 
168 
169  des_r_foot_pos_ = des_task_pos;
171  }
172  else if (control_group_ == "body")
173  {
174  des_body_pos_ = des_task_pos;
176 
179 
182  }
183 
184 }
185 
186 std::vector<double_t> WholebodyControl::getJointPosition(double time)
187 {
188 
189 }
190 
191 std::vector<double_t> WholebodyControl::getJointVelocity(double time)
192 {
193 
194 }
195 
196 std::vector<double_t> WholebodyControl::getJointAcceleration(double time)
197 {
198 
199 }
200 
201 void WholebodyControl::getTaskPosition(std::vector<double_t> &l_foot_pos,
202  std::vector<double_t> &r_foot_pos,
203  std::vector<double_t> &body_pos)
204 {
205  l_foot_pos = des_l_foot_pos_;
206  r_foot_pos = des_r_foot_pos_;
207  body_pos = des_body_pos_;
208 }
209 
210 std::vector<double_t> WholebodyControl::getTaskVelocity(double time)
211 {
212 
213 }
214 
215 std::vector<double_t> WholebodyControl::getTaskAcceleration(double time)
216 {
217 
218 }
219 
220 void WholebodyControl::getTaskOrientation(std::vector<double_t> &l_foot_Q,
221  std::vector<double_t> &r_foot_Q,
222  std::vector<double_t> &body_Q)
223 {
224  l_foot_Q[0] = des_l_foot_Q_.x();
225  l_foot_Q[1] = des_l_foot_Q_.y();
226  l_foot_Q[2] = des_l_foot_Q_.z();
227  l_foot_Q[3] = des_l_foot_Q_.w();
228 
229  r_foot_Q[0] = des_r_foot_Q_.x();
230  r_foot_Q[1] = des_r_foot_Q_.y();
231  r_foot_Q[2] = des_r_foot_Q_.z();
232  r_foot_Q[3] = des_r_foot_Q_.w();
233 
234  body_Q[0] = des_body_Q_.x();
235  body_Q[1] = des_body_Q_.y();
236  body_Q[2] = des_body_Q_.z();
237  body_Q[3] = des_body_Q_.w();
238 }
239 void WholebodyControl::getGroupPose(std::string name, geometry_msgs::Pose *msg)
240 {
241 
242 }
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_
Eigen::Quaterniond init_r_foot_Q_
void set(double time)
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)


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