op3_kdl.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>
20 #include "op3_online_walking_module/op3_kdl.h"
21 
22 OP3Kinematics::OP3Kinematics()
23 {
24  rleg_joint_position_.resize(LEG_JOINT_NUM);
25 
26  for (int i=0; i<LEG_JOINT_NUM; i++)
27  rleg_joint_position_(i) = 0.0;
28 }
29 
30 OP3Kinematics::~OP3Kinematics()
31 {
32 
33 }
34 
35 void OP3Kinematics::initialize(Eigen::MatrixXd pelvis_position, Eigen::MatrixXd pelvis_orientation)
36 {
37  KDL::Chain rleg_chain, lleg_chain;
38 
39  double pelvis_x = pelvis_position.coeff(0,0);
40  double pelvis_y = pelvis_position.coeff(1,0);
41  double pelvis_z = pelvis_position.coeff(2,0);
42 
43  double pelvis_Xx = pelvis_orientation.coeff(0,0);
44  double pelvis_Yx = pelvis_orientation.coeff(0,1);
45  double pelvis_Zx = pelvis_orientation.coeff(0,2);
46 
47  double pelvis_Xy = pelvis_orientation.coeff(1,0);
48  double pelvis_Yy = pelvis_orientation.coeff(1,1);
49  double pelvis_Zy = pelvis_orientation.coeff(1,2);
50 
51  double pelvis_Xz = pelvis_orientation.coeff(2,0);
52  double pelvis_Yz = pelvis_orientation.coeff(2,1);
53  double pelvis_Zz = pelvis_orientation.coeff(2,2);
54 
55  // Set Kinematics Tree
56 
57  // Right Leg Chain
58  rleg_chain.addSegment(KDL::Segment("base",
59  KDL::Joint(KDL::Joint::None),
60  KDL::Frame(KDL::Rotation(pelvis_Xx, pelvis_Yx, pelvis_Zx,
61  pelvis_Xy, pelvis_Yy, pelvis_Zy,
62  pelvis_Xz, pelvis_Yz, pelvis_Zz),
63  KDL::Vector(pelvis_x , pelvis_y , pelvis_z)),
64  KDL::RigidBodyInertia(0.0,
65  KDL::Vector(0.0, 0.0, 0.0),
66  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
67  )
68  )
69  );
70  rleg_chain.addSegment(KDL::Segment("pelvis",
71  KDL::Joint(KDL::Joint::None),
72  KDL::Frame(KDL::Vector(0.0, -0.035, -0.0907)),
73 // KDL::Frame(KDL::Vector(-0.005, -0.035, -0.0907)),
74  KDL::RigidBodyInertia(0.72235,
75  KDL::Vector(0.0, 0.0, 0.0),
76  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
77  )
78  )
79  );
80  rleg_chain.addSegment(KDL::Segment("r_hip_yaw",
81  KDL::Joint("minus_RotZ", KDL::Vector(0,0,0), KDL::Vector(0,0,-1), KDL::Joint::RotAxis),
82  KDL::Frame(KDL::Vector(0.000, 0.000, -0.0285)),
83  KDL::RigidBodyInertia(0.01181,
84  KDL::Vector(0.0, 0.0, 0.0),
85  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
86  )
87  )
88  );
89  rleg_chain.addSegment(KDL::Segment("r_leg_hip_r",
90  KDL::Joint("minus_RotX", KDL::Vector(0,0,0), KDL::Vector(-1,0,0), KDL::Joint::RotAxis),
91  KDL::Frame(KDL::Vector(0.0, 0.0, 0.0)),
92  KDL::RigidBodyInertia(0.17886,
93  KDL::Vector(0.0, 0.0, 0.0),
94  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
95  )
96  )
97  );
98  rleg_chain.addSegment(KDL::Segment("r_leg_hip_p",
99  KDL::Joint("minus_RotY", KDL::Vector(0,0,0), KDL::Vector(0,-1,0), KDL::Joint::RotAxis),
100  KDL::Frame(KDL::Vector(0.0, 0.0, -0.11)),
101  KDL::RigidBodyInertia(0.11543,
102  KDL::Vector(0.0, 0.0, 0.0),
103  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
104  )
105  )
106  );
107  rleg_chain.addSegment(KDL::Segment("r_leg_kn_p",
108  KDL::Joint("minus_RotY", KDL::Vector(0,0,0), KDL::Vector(0,-1,0), KDL::Joint::RotAxis),
109  KDL::Frame(KDL::Vector(0.0, 0.0, -0.11)),
110  KDL::RigidBodyInertia(0.04015,
111  KDL::Vector(0.0, 0.0, 0.0),
112  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
113  )
114  )
115  );
116  rleg_chain.addSegment(KDL::Segment("r_leg_an_p",
117  KDL::Joint(KDL::Joint::RotY),
118  KDL::Frame(KDL::Vector(0.0, 0.0, 0.0)),
119  KDL::RigidBodyInertia(0.17886,
120  KDL::Vector(0.0, 0.0, 0.0),
121  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
122  )
123  )
124  );
125  rleg_chain.addSegment(KDL::Segment("r_leg_an_r",
126  KDL::Joint(KDL::Joint::RotX),
127  KDL::Frame(KDL::Vector(0.0, 0.0, -0.0305)),
128  KDL::RigidBodyInertia(0.06934,
129  KDL::Vector(0.0, 0.0, 0.0),
130  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
131  )
132  )
133  );
134  rleg_chain.addSegment(KDL::Segment("r_leg_end",
135  KDL::Joint(KDL::Joint::None),
136  KDL::Frame(KDL::Vector(0.0 , 0.0 , 0.0)),
137  KDL::RigidBodyInertia(0.0,
138  KDL::Vector(0.0, 0.0, 0.0),
139  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
140  )
141  )
142  );
143 
144  // Left Leg Chain
145  lleg_chain.addSegment(KDL::Segment("base",
146  KDL::Joint(KDL::Joint::None),
147  KDL::Frame(KDL::Rotation(pelvis_Xx, pelvis_Yx, pelvis_Zx,
148  pelvis_Xy, pelvis_Yy, pelvis_Zy,
149  pelvis_Xz, pelvis_Yz, pelvis_Zz),
150  KDL::Vector(pelvis_x , pelvis_y , pelvis_z)),
151  KDL::RigidBodyInertia(0.0,
152  KDL::Vector(0.0, 0.0, 0.0),
153  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
154  )
155  )
156  );
157  lleg_chain.addSegment(KDL::Segment("pelvis",
158  KDL::Joint(KDL::Joint::None),
159  KDL::Frame(KDL::Vector(0.0, 0.035, -0.0907)),
160 // KDL::Frame(KDL::Vector(-0.005, 0.035, -0.0907)),
161  KDL::RigidBodyInertia(0.72235,
162  KDL::Vector(0.0, 0.0, 0.0),
163  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
164  )
165  )
166  );
167  lleg_chain.addSegment(KDL::Segment("l_leg_hip_y",
168  KDL::Joint("minus_RotZ", KDL::Vector(0,0,0), KDL::Vector(0,0,-1), KDL::Joint::RotAxis),
169  KDL::Frame(KDL::Vector(0.000, 0.000, -0.0285)),
170  KDL::RigidBodyInertia(0.01181,
171  KDL::Vector(0.0, 0.0, 0.0),
172  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
173  )
174  )
175  );
176  lleg_chain.addSegment(KDL::Segment("l_leg_hip_r",
177  KDL::Joint("minus_RotX", KDL::Vector(0,0,0), KDL::Vector(-1,0,0), KDL::Joint::RotAxis),
178  KDL::Frame(KDL::Vector(0.0, 0.0, 0.0)),
179  KDL::RigidBodyInertia(0.17886,
180  KDL::Vector(0.0, 0.0, 0.0),
181  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
182  )
183  )
184  );
185  lleg_chain.addSegment(KDL::Segment("l_leg_hip_p",
186  KDL::Joint(KDL::Joint::RotY),
187  KDL::Frame(KDL::Vector(0.0, 0.0, -0.11)),
188  KDL::RigidBodyInertia(0.11543,
189  KDL::Vector(0.0, 0.0, 0.0),
190  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
191  )
192  )
193  );
194  lleg_chain.addSegment(KDL::Segment("l_leg_kn_p",
195  KDL::Joint(KDL::Joint::RotY),
196  KDL::Frame(KDL::Vector(0.0, 0.0, -0.11)),
197  KDL::RigidBodyInertia(0.04015,
198  KDL::Vector(0.0, 0.0, 0.0),
199  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
200  )
201  )
202  );
203  lleg_chain.addSegment(KDL::Segment("l_leg_an_p",
204  KDL::Joint("minus_RotY", KDL::Vector(0,0,0), KDL::Vector(0,-1,0), KDL::Joint::RotAxis),
205  KDL::Frame(KDL::Vector(0.0, 0.0, 0.0)),
206  KDL::RigidBodyInertia(0.17886,
207  KDL::Vector(0.0, 0.0, 0.0),
208  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
209  )
210  )
211  );
212  lleg_chain.addSegment(KDL::Segment("l_leg_an_r",
213  KDL::Joint(KDL::Joint::RotX),
214  KDL::Frame(KDL::Vector(0.0, 0.0, -0.0305)),
215  KDL::RigidBodyInertia(0.06934,
216  KDL::Vector(0.0, 0.0, 0.0),
217  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
218  )
219  )
220  );
221  lleg_chain.addSegment(KDL::Segment("l_leg_end",
222  KDL::Joint(KDL::Joint::None),
223  KDL::Frame(KDL::Vector(0.0 , 0.0 , 0.0)),
224  KDL::RigidBodyInertia(0.0,
225  KDL::Vector(0.0, 0.0, 0.0),
226  KDL::RotationalInertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
227  )
228  )
229  );
230 
231  // Set Joint Limits
232  std::vector<double> min_position_limit, max_position_limit;
233  min_position_limit.push_back(-180.0); max_position_limit.push_back(180.0); // r_leg_hip_y
234  min_position_limit.push_back(-180.0); max_position_limit.push_back(180.0); // r_leg_hip_r
235  min_position_limit.push_back(-180.0); max_position_limit.push_back(180.0); // r_leg_hip_p
236  min_position_limit.push_back(-180.0); max_position_limit.push_back(180.0); // r_leg_kn_p
237  min_position_limit.push_back(-180.0); max_position_limit.push_back(180.0); // r_leg_an_p
238  min_position_limit.push_back(-180.0); max_position_limit.push_back(180.0); // r_leg_an_r
239 
240  KDL::JntArray min_joint_position_limit(LEG_JOINT_NUM), max_joint_position_limit(LEG_JOINT_NUM);
241  for (int index=0; index<LEG_JOINT_NUM; index++)
242  {
243  min_joint_position_limit(index) = min_position_limit[index]*D2R;
244  max_joint_position_limit(index) = max_position_limit[index]*D2R;
245  }
246 
247  /* KDL Solver Initialization */
248  // rleg_dyn_param_ = new KDL::ChainDynParam(rleg_chain_, KDL::Vector(0.0, 0.0, -9.81)); // kinematics & dynamics parameter
249  // rleg_jacobian_solver_ = new KDL::ChainJntToJacSolver(rleg_chain__); // jabocian solver
250  rleg_fk_solver_ = new KDL::ChainFkSolverPos_recursive(rleg_chain); // forward kinematics solver
251 
252  // inverse kinematics solver
253  rleg_ik_vel_solver_ = new KDL::ChainIkSolverVel_pinv(rleg_chain);
254  rleg_ik_pos_solver_ = new KDL::ChainIkSolverPos_NR_JL(rleg_chain,
255  min_joint_position_limit, max_joint_position_limit,
256  *rleg_fk_solver_,
257  *rleg_ik_vel_solver_);
258 
259  // lleg_dyn_param_ = new KDL::ChainDynParam(lleg_chain_, KDL::Vector(0.0, 0.0, -9.81)); // kinematics & dynamics parameter
260  // lleg_jacobian_solver_ = new KDL::ChainJntToJacSolver(lleg_chain__); // jabocian solver
261  lleg_fk_solver_ = new KDL::ChainFkSolverPos_recursive(lleg_chain); // forward kinematics solver
262 
263  // inverse kinematics solver
264  lleg_ik_vel_solver_ = new KDL::ChainIkSolverVel_pinv(lleg_chain);
265  lleg_ik_pos_solver_ = new KDL::ChainIkSolverPos_NR_JL(lleg_chain,
266  min_joint_position_limit, max_joint_position_limit,
267  *lleg_fk_solver_,
268  *lleg_ik_vel_solver_);
269 }
270 
271 void OP3Kinematics::setJointPosition(Eigen::VectorXd rleg_joint_position, Eigen::VectorXd lleg_joint_position)
272 {
273  rleg_joint_position_ = rleg_joint_position;
274  lleg_joint_position_ = lleg_joint_position;
275 
276  // for (int i=0; i<LEG_JOINT_NUM; i++)
277  // ROS_INFO("rleg_joint_position_(%d): %f", i, rleg_joint_position_(i));
278 
279  // for (int i=0; i<LEG_JOINT_NUM; i++)
280  // ROS_INFO("lleg_joint_position_(%d): %f", i, lleg_joint_position_(i));
281 }
282 
283 void OP3Kinematics::solveForwardKinematics(std::vector<double_t> &rleg_position, std::vector<double_t> &rleg_orientation,
284  std::vector<double_t> &lleg_position, std::vector<double_t> &lleg_orientation)
285 {
286  // rleg
287  KDL::JntArray rleg_joint_position;
288  rleg_joint_position.data = rleg_joint_position_;
289 
290  KDL::Frame rleg_pose;
291  rleg_fk_solver_->JntToCart(rleg_joint_position, rleg_pose);
292 
293  rleg_pose_.position.x = rleg_pose.p.x();
294  rleg_pose_.position.y = rleg_pose.p.y();
295  rleg_pose_.position.z = rleg_pose.p.z();
296 
297  rleg_pose.M.GetQuaternion(rleg_pose_.orientation.x,
298  rleg_pose_.orientation.y,
299  rleg_pose_.orientation.z,
300  rleg_pose_.orientation.w);
301 
302 // ROS_INFO("rleg position x : %f y: %f, z: %f", rleg_pose_.position.x, rleg_pose_.position.y, rleg_pose_.position.z);
303 
304  rleg_position.resize(3,0.0);
305  rleg_position[0] = rleg_pose_.position.x;
306  rleg_position[1] = rleg_pose_.position.y;
307  rleg_position[2] = rleg_pose_.position.z;
308 
309  rleg_orientation.resize(4,0.0);
310  rleg_orientation[0] = rleg_pose_.orientation.x;
311  rleg_orientation[1] = rleg_pose_.orientation.y;
312  rleg_orientation[2] = rleg_pose_.orientation.z;
313  rleg_orientation[3] = rleg_pose_.orientation.w;
314 
315  // lleg
316  KDL::JntArray lleg_joint_position;
317  lleg_joint_position.data = lleg_joint_position_;
318 
319  KDL::Frame lleg_pose;
320  lleg_fk_solver_->JntToCart(lleg_joint_position, lleg_pose);
321 
322  lleg_pose_.position.x = lleg_pose.p.x();
323  lleg_pose_.position.y = lleg_pose.p.y();
324  lleg_pose_.position.z = lleg_pose.p.z();
325 
326  lleg_pose.M.GetQuaternion(lleg_pose_.orientation.x,
327  lleg_pose_.orientation.y,
328  lleg_pose_.orientation.z,
329  lleg_pose_.orientation.w);
330 
331 // ROS_INFO("lleg position x : %f y: %f, z: %f", lleg_pose_.position.x, lleg_pose_.position.y, lleg_pose_.position.z);
332 
333  lleg_position.resize(3,0.0);
334  lleg_position[0] = lleg_pose_.position.x;
335  lleg_position[1] = lleg_pose_.position.y;
336  lleg_position[2] = lleg_pose_.position.z;
337 
338  lleg_orientation.resize(4,0.0);
339  lleg_orientation[0] = lleg_pose_.orientation.x;
340  lleg_orientation[1] = lleg_pose_.orientation.y;
341  lleg_orientation[2] = lleg_pose_.orientation.z;
342  lleg_orientation[3] = lleg_pose_.orientation.w;
343 }
344 
345 bool OP3Kinematics::solveInverseKinematics(std::vector<double_t> &rleg_output,
346  Eigen::MatrixXd rleg_target_position, Eigen::Quaterniond rleg_target_orientation,
347  std::vector<double_t> &lleg_output,
348  Eigen::MatrixXd lleg_target_position, Eigen::Quaterniond lleg_target_orientation)
349 {
350  // ROS_INFO("right x: %f, y: %f, z: %f", rleg_target_position(0), rleg_target_position(1), rleg_target_position(2));
351  // ROS_INFO("left x: %f, y: %f, z: %f", lleg_target_position(0), lleg_target_position(1), lleg_target_position(2));
352 
353  // rleg
354  KDL::JntArray rleg_joint_position;
355  rleg_joint_position.data = rleg_joint_position_;
356 
357  KDL::Frame rleg_desired_pose;
358  rleg_desired_pose.p.x(rleg_target_position.coeff(0,0));
359  rleg_desired_pose.p.y(rleg_target_position.coeff(1,0));
360  rleg_desired_pose.p.z(rleg_target_position.coeff(2,0));
361 
362  rleg_desired_pose.M = KDL::Rotation::Quaternion(rleg_target_orientation.x(),
363  rleg_target_orientation.y(),
364  rleg_target_orientation.z(),
365  rleg_target_orientation.w());
366 
367  KDL::JntArray rleg_desired_joint_position;
368  rleg_desired_joint_position.resize(LEG_JOINT_NUM);
369 
370  int rleg_err = rleg_ik_pos_solver_->CartToJnt(rleg_joint_position, rleg_desired_pose, rleg_desired_joint_position);
371 
372  if (rleg_err < 0)
373  {
374  ROS_WARN("RLEG IK ERR : %d", rleg_err);
375  return false;
376  }
377 
378  // lleg
379  KDL::JntArray lleg_joint_position;
380  lleg_joint_position.data = lleg_joint_position_;
381 
382  KDL::Frame lleg_desired_pose;
383  lleg_desired_pose.p.x(lleg_target_position.coeff(0,0));
384  lleg_desired_pose.p.y(lleg_target_position.coeff(1,0));
385  lleg_desired_pose.p.z(lleg_target_position.coeff(2,0));
386 
387  lleg_desired_pose.M = KDL::Rotation::Quaternion(lleg_target_orientation.x(),
388  lleg_target_orientation.y(),
389  lleg_target_orientation.z(),
390  lleg_target_orientation.w());
391 
392  KDL::JntArray lleg_desired_joint_position;
393  lleg_desired_joint_position.resize(LEG_JOINT_NUM);
394 
395  int lleg_err = lleg_ik_pos_solver_->CartToJnt(lleg_joint_position, lleg_desired_pose, lleg_desired_joint_position);
396 
397  if (lleg_err < 0)
398  {
399  ROS_WARN("LLEG IK ERR : %d", lleg_err);
400  return false;
401  }
402 
403  // output
404  rleg_output.resize(LEG_JOINT_NUM);
405  lleg_output.resize(LEG_JOINT_NUM);
406 
407  for (int i=0; i<LEG_JOINT_NUM; i++)
408  {
409  rleg_output[i] = rleg_desired_joint_position(i);
410  lleg_output[i] = lleg_desired_joint_position(i);
411  }
412 
413  return true;
414 }
415 
416 void OP3Kinematics::finalize()
417 {
418  // delete rleg_chain_;
419  // delete rleg_dyn_param_;
420  // delete rleg_jacobian_solver_;
421  delete rleg_fk_solver_;
422  delete rleg_ik_vel_solver_;
423  delete rleg_ik_pos_solver_;
424 
425  // delete lleg_chain_;
426  // delete lleg_dyn_param_;
427  // delete lleg_jacobian_solver_;
428  delete lleg_fk_solver_;
429  delete lleg_ik_vel_solver_;
430  delete lleg_ik_pos_solver_;
431 }
OP3Kinematics::initialize
void initialize(Eigen::MatrixXd pelvis_position, Eigen::MatrixXd pelvis_orientation)
Definition: op3_kdl.cpp:35
OP3Kinematics::lleg_fk_solver_
KDL::ChainFkSolverPos_recursive * lleg_fk_solver_
Definition: op3_kdl.h:75
KDL::ChainFkSolverPos_recursive::JntToCart
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
KDL::ChainFkSolverPos_recursive
OP3Kinematics::finalize
void finalize()
Definition: op3_kdl.cpp:416
op3_kdl.h
KDL::Chain::addSegment
void addSegment(const Segment &segment)
KDL::Rotation::Quaternion
static Rotation Quaternion(double x, double y, double z, double w)
OP3Kinematics::rleg_joint_position_
Eigen::VectorXd rleg_joint_position_
Definition: op3_kdl.h:82
ROS_WARN
#define ROS_WARN(...)
KDL::Vector::z
double z() const
KDL::Frame::M
Rotation M
OP3Kinematics::solveForwardKinematics
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)
Definition: op3_kdl.cpp:283
KDL::Rotation::GetQuaternion
void GetQuaternion(double &x, double &y, double &z, double &w) const
KDL::Vector::y
double y() const
D2R
#define D2R
Definition: op3_kdl.h:45
KDL::ChainIkSolverPos_NR_JL
KDL::Vector::x
double x() const
KDL::JntArray::data
Eigen::VectorXd data
KDL::ChainIkSolverVel_pinv
OP3Kinematics::rleg_fk_solver_
KDL::ChainFkSolverPos_recursive * rleg_fk_solver_
Definition: op3_kdl.h:68
OP3Kinematics::setJointPosition
void setJointPosition(Eigen::VectorXd rleg_joint_position, Eigen::VectorXd lleg_joint_position)
Definition: op3_kdl.cpp:271
OP3Kinematics::solveInverseKinematics
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)
Definition: op3_kdl.cpp:345
OP3Kinematics::lleg_joint_position_
Eigen::VectorXd lleg_joint_position_
Definition: op3_kdl.h:82
KDL::JntArray::resize
void resize(unsigned int newSize)
OP3Kinematics::lleg_ik_vel_solver_
KDL::ChainIkSolverVel_pinv * lleg_ik_vel_solver_
Definition: op3_kdl.h:76
OP3Kinematics::lleg_pose_
geometry_msgs::Pose lleg_pose_
Definition: op3_kdl.h:83
OP3Kinematics::rleg_pose_
geometry_msgs::Pose rleg_pose_
Definition: op3_kdl.h:83
OP3Kinematics::rleg_ik_vel_solver_
KDL::ChainIkSolverVel_pinv * rleg_ik_vel_solver_
Definition: op3_kdl.h:69
OP3Kinematics::rleg_ik_pos_solver_
KDL::ChainIkSolverPos_NR_JL * rleg_ik_pos_solver_
Definition: op3_kdl.h:70
OP3Kinematics::lleg_ik_pos_solver_
KDL::ChainIkSolverPos_NR_JL * lleg_ik_pos_solver_
Definition: op3_kdl.h:77
KDL::ChainIkSolverPos_NR_JL::CartToJnt
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)
LEG_JOINT_NUM
#define LEG_JOINT_NUM
Definition: op3_kdl.h:44
OP3Kinematics::~OP3Kinematics
virtual ~OP3Kinematics()
Definition: op3_kdl.cpp:30

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