joint_position_integrator.cpp
Go to the documentation of this file.
2 
4 {
6 {
7  if (!init())
8  {
9  throw std::logic_error(
10  "Missing parameters for the JointPositionIntegrator");
11  }
12 
13  reset();
14 }
15 
17 
19  const KDL::JntArray& q, float dt)
20 {
21  if (!got_state_)
22  {
23  got_state_ = true;
24  virt_q_ = q;
25  }
26 
27  KDL::JntArray out = virt_q_;
28 
29  out.data += q_dot.data * dt;
30 
31  for (int i = 0; i < out.rows(); i++)
32  {
33  if (std::abs(out(i) - q(i)) > max_joint_error_)
34  {
35  return virt_q_;
36  }
37  }
38 
39  virt_q_ = out;
40  return virt_q_;
41 }
42 
43 Eigen::VectorXd JointPositionIntegrator::update(const Eigen::VectorXd& q_dot,
44  const Eigen::VectorXd& q,
45  float dt)
46 {
47  KDL::JntArray qdotk(q_dot.rows()), qk(q.rows()), outk;
48  Eigen::VectorXd out;
49 
50  for (unsigned int i = 0; i < q_dot.rows(); i++)
51  {
52  qdotk(i) = q_dot[i];
53  qk(i) = q[i];
54  }
55 
56  outk = update(qdotk, qk, dt);
57  out.resize(outk.rows());
58 
59  for (unsigned int i = 0; i < outk.rows(); i++)
60  {
61  out[i] = outk(i);
62  }
63 
64  return out;
65 }
66 
68 {
69  if (!nh_.getParam("max_joint_error", max_joint_error_))
70  {
71  ROS_ERROR("Missing parameter max_joint_error");
72  return false;
73  }
74 
75  return true;
76 }
77 } // namespace generic_control_toolbox
KDL::JntArray update(const KDL::JntArray &q_dot, const KDL::JntArray &q, float dt)
#define ROS_ERROR(...)
unsigned int rows() const
bool getParam(const std::string &key, std::string &s) const
Eigen::VectorXd data


generic_control_toolbox
Author(s): diogo
autogenerated on Mon Feb 28 2022 22:24:37