From 07e413885020dee4393d7647a8bbae80811dfc3f Mon Sep 17 00:00:00 2001 From: matthias-mayr Date: Tue, 31 Oct 2023 14:37:40 +0000 Subject: [PATCH] deploy: 18e2a3fd5d2126af123079aa510664f00b7c05b9 --- README_8md.html | 2 +- README_8md_source.html | 2 +- annotated.html | 2 +- cartesian__impedance__controller_8cpp.html | 2 +- ...an__impedance__controller_8cpp_source.html | 2 +- cartesian__impedance__controller_8h.html | 2 +- ...sian__impedance__controller_8h_source.html | 2 +- ...sian__impedance__controller__ros_8cpp.html | 2 +- ...mpedance__controller__ros_8cpp_source.html | 56 +++++++++---------- cartesian__impedance__controller__ros_8h.html | 2 +- ..._impedance__controller__ros_8h_source.html | 54 +++++++++--------- ...1CartesianImpedanceController-members.html | 2 +- ...oller_1_1CartesianImpedanceController.html | 2 +- ...rtesianImpedanceControllerRos-members.html | 2 +- ...er_1_1CartesianImpedanceControllerRos.html | 54 +++++++++--------- classes.html | 2 +- classrbdyn__wrapper-members.html | 2 +- classrbdyn__wrapper.html | 2 +- dir_000000_000001.html | 2 +- dir_594d4c40386ea469b7c9e60075edfcd9.html | 2 +- dir_68267d1309a1af8e8297ef4c3efbcdba.html | 2 +- dir_d44c64559bbebec7f509842c48db8b23.html | 2 +- files.html | 2 +- functions.html | 2 +- functions_func.html | 2 +- functions_vars.html | 2 +- globals.html | 2 +- globals_func.html | 2 +- graph_legend.html | 2 +- hierarchy.html | 2 +- index-msg.html | 2 +- index.html | 2 +- inherits.html | 2 +- md_res_paper.html | 2 +- msg/ControllerConfig.html | 2 +- msg/ControllerState.html | 2 +- ...spacecartesian__impedance__controller.html | 2 +- namespacemembers.html | 2 +- namespacemembers_func.html | 2 +- namespaces.html | 2 +- pages.html | 2 +- paper_8md.html | 2 +- paper_8md_source.html | 2 +- pseudo__inversion_8h.html | 2 +- pseudo__inversion_8h_source.html | 2 +- rbdyn__wrapper_8h.html | 2 +- rbdyn__wrapper_8h_source.html | 2 +- structrbdyn__wrapper_1_1EefState-members.html | 2 +- structrbdyn__wrapper_1_1EefState.html | 2 +- 49 files changed, 128 insertions(+), 128 deletions(-) diff --git a/README_8md.html b/README_8md.html index b2bf0c5..a928ca7 100644 --- a/README_8md.html +++ b/README_8md.html @@ -28,7 +28,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/README_8md_source.html b/README_8md_source.html index 772d560..ed71b8f 100644 --- a/README_8md_source.html +++ b/README_8md_source.html @@ -26,7 +26,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/annotated.html b/annotated.html index 606ffb9..c843825 100644 --- a/annotated.html +++ b/annotated.html @@ -35,7 +35,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/cartesian__impedance__controller_8cpp.html b/cartesian__impedance__controller_8cpp.html index 41ab720..53a86e8 100644 --- a/cartesian__impedance__controller_8cpp.html +++ b/cartesian__impedance__controller_8cpp.html @@ -73,7 +73,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/cartesian__impedance__controller_8cpp_source.html b/cartesian__impedance__controller_8cpp_source.html index 164fb12..98bfa67 100644 --- a/cartesian__impedance__controller_8cpp_source.html +++ b/cartesian__impedance__controller_8cpp_source.html @@ -94,7 +94,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/cartesian__impedance__controller_8h.html b/cartesian__impedance__controller_8h.html index e07bc9c..1c7a55a 100644 --- a/cartesian__impedance__controller_8h.html +++ b/cartesian__impedance__controller_8h.html @@ -65,7 +65,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/cartesian__impedance__controller_8h_source.html b/cartesian__impedance__controller_8h_source.html index 24e21a0..b454d18 100644 --- a/cartesian__impedance__controller_8h_source.html +++ b/cartesian__impedance__controller_8h_source.html @@ -88,7 +88,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/cartesian__impedance__controller__ros_8cpp.html b/cartesian__impedance__controller__ros_8cpp.html index 7d0cdd0..5f96230 100644 --- a/cartesian__impedance__controller__ros_8cpp.html +++ b/cartesian__impedance__controller__ros_8cpp.html @@ -63,7 +63,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/cartesian__impedance__controller__ros_8cpp_source.html b/cartesian__impedance__controller__ros_8cpp_source.html index 08b3101..0722b59 100644 --- a/cartesian__impedance__controller__ros_8cpp_source.html +++ b/cartesian__impedance__controller__ros_8cpp_source.html @@ -24,17 +24,17 @@
cartesian_impedance_controller_ros.cpp
-Go to the documentation of this file.
1 #include <cartesian_impedance_controller/cartesian_impedance_controller_ros.h>
2 
3 #include <eigen_conversions/eigen_msg.h>
4 #include <tf_conversions/tf_eigen.h>
5 
6 namespace cartesian_impedance_controller
7 {
15  double saturateValue(double x, double x_min, double x_max)
16  {
17  return std::min(std::max(x, x_min), x_max);
18  }
19 
26  void EigenVectorToWrench(const Eigen::Matrix<double, 6, 1> &v, geometry_msgs::Wrench *wrench)
27  {
28  wrench->force.x = v(0);
29  wrench->force.y = v(1);
30  wrench->force.z = v(2);
31  wrench->torque.x = v(3);
32  wrench->torque.y = v(4);
33  wrench->torque.z = v(5);
34  }
35 
36  bool CartesianImpedanceControllerRos::initDynamicReconfigure(const ros::NodeHandle &nh)
37  {
38  this->dynamic_server_compliance_param_ = std::make_unique<dynamic_reconfigure::Server<cartesian_impedance_controller::stiffnessConfig>>(ros::NodeHandle(std::string(nh.getNamespace() + "/stiffness_reconfigure")));
39  this->dynamic_server_compliance_param_->setCallback(
40  boost::bind(&CartesianImpedanceControllerRos::dynamicStiffnessCb, this, _1, _2));
41 
42  this->dynamic_server_damping_param_ = std::make_unique<dynamic_reconfigure::Server<cartesian_impedance_controller::dampingConfig>>(ros::NodeHandle(std::string(nh.getNamespace() + "/damping_factors_reconfigure")));
43  dynamic_server_damping_param_->setCallback(
44  boost::bind(&CartesianImpedanceControllerRos::dynamicDampingCb, this, _1, _2));
45 
46  this->dynamic_server_wrench_param_ = std::make_unique<dynamic_reconfigure::Server<cartesian_impedance_controller::wrenchConfig>>(ros::NodeHandle(std::string(nh.getNamespace() + "/cartesian_wrench_reconfigure")));
47  dynamic_server_wrench_param_->setCallback(
48  boost::bind(&CartesianImpedanceControllerRos::dynamicWrenchCb, this, _1, _2));
49  return true;
50  }
51 
52  bool CartesianImpedanceControllerRos::initJointHandles(hardware_interface::EffortJointInterface *hw, const ros::NodeHandle &nh)
53  {
54  std::vector<std::string> joint_names;
55  if (!nh.getParam("joints", joint_names))
56  {
57  ROS_ERROR("Invalid or no joint_names parameters provided, aborting controller init!");
58  return false;
59  }
60  for (size_t i = 0; i < joint_names.size(); ++i)
61  {
62  try
63  {
64  this->joint_handles_.push_back(hw->getHandle(joint_names[i]));
65  }
66  catch (const hardware_interface::HardwareInterfaceException &ex)
67  {
68  ROS_ERROR_STREAM("Exception getting joint handles: " << ex.what());
69  return false;
70  }
71  }
72  ROS_INFO_STREAM("Number of joints specified in parameters: " << joint_names.size());
73  return true;
74  }
75 
76  bool CartesianImpedanceControllerRos::initMessaging(ros::NodeHandle *nh)
77  {
78  // Queue size of 1 since we are only interested in the last message
79  this->sub_cart_stiffness_ = nh->subscribe("set_cartesian_stiffness", 1,
80  &CartesianImpedanceControllerRos::cartesianStiffnessCb, this);
81  this->sub_cart_wrench_ = nh->subscribe("set_cartesian_wrench", 1,
82  &CartesianImpedanceControllerRos::wrenchCommandCb, this);
83  this->sub_damping_factors_ = nh->subscribe("set_damping_factors", 1,
84  &CartesianImpedanceControllerRos::cartesianDampingFactorCb, this);
85  this->sub_controller_config_ =
86  nh->subscribe("set_config", 1, &CartesianImpedanceControllerRos::controllerConfigCb, this);
87  this->sub_reference_pose_ = nh->subscribe("reference_pose", 1, &CartesianImpedanceControllerRos::referencePoseCb, this);
88 
89  // Initializing the realtime publisher and the message
90  this->pub_torques_.init(*nh, "commanded_torques", 20);
91  this->pub_torques_.msg_.layout.dim.resize(1);
92  this->pub_torques_.msg_.layout.data_offset = 0;
93  this->pub_torques_.msg_.layout.dim[0].size = this->n_joints_;
94  this->pub_torques_.msg_.layout.dim[0].stride = 0;
95  this->pub_torques_.msg_.data.resize(this->n_joints_);
96 
97  std::vector<std::string> joint_names;
98  nh->getParam("joints", joint_names);
99  this->pub_state_.init(*nh, "controller_state", 10);
100  this->pub_state_.msg_.header.seq = 0;
101  for (size_t i = 0; i < this->n_joints_; i++)
102  {
103  this->pub_state_.msg_.joint_state.name.push_back(joint_names.at(i));
104  }
105  this->pub_state_.msg_.joint_state.position = std::vector<double>(this->n_joints_);
106  this->pub_state_.msg_.joint_state.velocity = std::vector<double>(this->n_joints_);
107  this->pub_state_.msg_.joint_state.effort = std::vector<double>(this->n_joints_);
108  this->pub_state_.msg_.commanded_torques = std::vector<double>(this->n_joints_);
109  this->pub_state_.msg_.nullspace_config = std::vector<double>(this->n_joints_);
110  return true;
111  }
112 
113  bool CartesianImpedanceControllerRos::initRBDyn(const ros::NodeHandle &nh)
114  {
115  // Get the URDF XML from the parameter server. Wait if needed.
116  std::string urdf_string;
117  nh.param<std::string>("robot_description", robot_description_, "/robot_description");
118  while (!nh.getParam(robot_description_, urdf_string))
119  {
120  ROS_INFO_ONCE("Waiting for robot description in parameter %s on the ROS param server.",
121  robot_description_.c_str());
122  usleep(100000);
123  }
124  try
125  {
126  this->rbdyn_wrapper_.init_rbdyn(urdf_string, end_effector_);
127  }
128  catch (std::runtime_error e)
129  {
130  ROS_ERROR("Error when intializing RBDyn: %s", e.what());
131  return false;
132  }
133  ROS_INFO_STREAM("Number of joints found in urdf: " << this->rbdyn_wrapper_.n_joints());
134  if (this->rbdyn_wrapper_.n_joints() < this->n_joints_)
135  {
136  ROS_ERROR("Number of joints in the URDF is smaller than supplied number of joints. %i < %zu", this->rbdyn_wrapper_.n_joints(), this->n_joints_);
137  return false;
138  }
139  else if (this->rbdyn_wrapper_.n_joints() > this->n_joints_)
140  {
141  ROS_WARN("Number of joints in the URDF is greater than supplied number of joints: %i > %zu. Assuming that the actuated joints come first.", this->rbdyn_wrapper_.n_joints(), this->n_joints_);
142  }
143  return true;
144  }
145 
146  bool CartesianImpedanceControllerRos::initTrajectories(ros::NodeHandle *nh)
147  {
148  this->sub_trajectory_ = nh->subscribe("joint_trajectory", 1, &CartesianImpedanceControllerRos::trajCb, this);
149  this->traj_as_ = std::unique_ptr<actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>>(
150  new actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>(
151  *nh, std::string("follow_joint_trajectory"), false));
152  this->traj_as_->registerGoalCallback(boost::bind(&CartesianImpedanceControllerRos::trajGoalCb, this));
153  this->traj_as_->registerPreemptCallback(boost::bind(&CartesianImpedanceControllerRos::trajPreemptCb, this));
154  this->traj_as_->start();
155  return true;
156  }
157 
158  bool CartesianImpedanceControllerRos::init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &node_handle)
159  {
160  ROS_INFO("Initializing Cartesian impedance controller in namespace: %s", node_handle.getNamespace().c_str());
161 
162  // Fetch parameters
163  node_handle.param<std::string>("end_effector", this->end_effector_, "iiwa_link_ee");
164  ROS_INFO_STREAM("End effektor link is: " << this->end_effector_);
165  // Frame for applying commanded Cartesian wrenches
166  node_handle.param<std::string>("wrench_ee_frame", this->wrench_ee_frame_, this->end_effector_);
167  bool dynamic_reconfigure{true};
168  node_handle.param<bool>("dynamic_reconfigure", dynamic_reconfigure, true);
169  bool enable_trajectories{true};
170  node_handle.param<bool>("handle_trajectories", enable_trajectories, true);
171  node_handle.param<double>("delta_tau_max", this->delta_tau_max_, 1.);
172  node_handle.param<double>("update_frequency", this->update_frequency_, 500.);
173  node_handle.param<double>("filtering/nullspace_config", this->filter_params_nullspace_config_, 0.1);
174  node_handle.param<double>("filtering/stiffness", this->filter_params_stiffness_, 0.1);
175  node_handle.param<double>("filtering/pose", this->filter_params_pose_, 0.1);
176  node_handle.param<double>("filtering/wrench", this->filter_params_wrench_, 0.1);
177  node_handle.param<bool>("verbosity/verbose_print", this->verbose_print_, false);
178  node_handle.param<bool>("verbosity/state_msgs", this->verbose_state_, false);
179  node_handle.param<bool>("verbosity/tf_frames", this->verbose_tf_, false);
180 
181  if (!this->initJointHandles(hw, node_handle) || !this->initMessaging(&node_handle) || !this->initRBDyn(node_handle))
182  {
183  return false;
184  }
185  if (enable_trajectories && !this->initTrajectories(&node_handle))
186  {
187  return false;
188  }
189  this->root_frame_ = this->rbdyn_wrapper_.root_link();
190  node_handle.setParam("root_frame", this->root_frame_);
191 
192  // Initialize base_tools and member variables
193  this->setNumberOfJoints(this->joint_handles_.size());
194  if (this->n_joints_ < 6)
195  {
196  ROS_WARN("Number of joints is below 6. Functions might be limited.");
197  }
198  if (this->n_joints_ < 7)
199  {
200  ROS_WARN("Number of joints is below 7. No redundant joint for nullspace.");
201  }
202  this->tau_m_ = Eigen::VectorXd(this->n_joints_);
203 
204  // Needs to be after base_tools init since the wrench callback calls it
205  if (dynamic_reconfigure && !this->initDynamicReconfigure(node_handle))
206  {
207  return false;
208  }
209 
210  ROS_INFO("Finished initialization.");
211  return true;
212  }
213 
214  void CartesianImpedanceControllerRos::starting(const ros::Time & /*time*/)
215  {
216  this->updateState();
217 
218  // Set reference pose to current pose and q_d_nullspace
219  this->initDesiredPose(this->position_, this->orientation_);
220  this->initNullspaceConfig(this->q_);
221  ROS_INFO("Started Cartesian Impedance Controller");
222  }
223 
224  void CartesianImpedanceControllerRos::update(const ros::Time & /*time*/, const ros::Duration &period /*period*/)
225  {
226  if (this->traj_running_)
227  {
228  trajUpdate();
229  }
230 
231  this->updateState();
232 
233  // Apply control law in base library
234  this->calculateCommandedTorques();
235 
236  // Write commands
237  for (size_t i = 0; i < this->n_joints_; ++i)
238  {
239  this->joint_handles_[i].setCommand(this->tau_c_(i));
240  }
241 
242  publishMsgsAndTf();
243  }
244 
245  bool CartesianImpedanceControllerRos::getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position,
246  Eigen::Quaterniond *orientation) const
247  {
248  rbdyn_wrapper::EefState ee_state;
249  // If the URDF contains more joints than there are controlled, only the state of the controlled ones are known
250  if (this->rbdyn_wrapper_.n_joints() != this->n_joints_)
251  {
252  Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints());
253  q_rb.head(q.size()) = q;
254  ee_state = this->rbdyn_wrapper_.perform_fk(q_rb);
255  }
256  else
257  {
258  ee_state = this->rbdyn_wrapper_.perform_fk(q);
259  }
260  *position = ee_state.translation;
261  *orientation = ee_state.orientation;
262  return true;
263  }
264 
265  bool CartesianImpedanceControllerRos::getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq,
266  Eigen::MatrixXd *jacobian)
267  {
268  // If the URDF contains more joints than there are controlled, only the state of the controlled ones are known
269  if (this->rbdyn_wrapper_.n_joints() != this->n_joints_)
270  {
271  Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints());
272  q_rb.head(q.size()) = q;
273  Eigen::VectorXd dq_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints());
274  dq_rb.head(dq.size()) = dq;
275  *jacobian = this->rbdyn_wrapper_.jacobian(q_rb, dq_rb);
276  }
277  else
278  {
279  *jacobian = this->rbdyn_wrapper_.jacobian(q, dq);
280  }
281  *jacobian = jacobian_perm_ * *jacobian;
282  return true;
283  }
284 
285  void CartesianImpedanceControllerRos::updateState()
286  {
287  for (size_t i = 0; i < this->n_joints_; ++i)
288  {
289  this->q_[i] = this->joint_handles_[i].getPosition();
290  this->dq_[i] = this->joint_handles_[i].getVelocity();
291  this->tau_m_[i] = this->joint_handles_[i].getEffort();
292  }
293  getJacobian(this->q_, this->dq_, &this->jacobian_);
294  getFk(this->q_, &this->position_, &this->orientation_);
295  }
296 
297  void CartesianImpedanceControllerRos::controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg)
298  {
299  this->setStiffness(msg->cartesian_stiffness, msg->nullspace_stiffness, false);
300  this->setDampingFactors(msg->cartesian_damping_factors, msg->nullspace_damping_factor);
301 
302  if (msg->q_d_nullspace.size() == this->n_joints_)
303  {
304  Eigen::VectorXd q_d_nullspace(this->n_joints_);
305  for (size_t i = 0; i < this->n_joints_; i++)
306  {
307  q_d_nullspace(i) = msg->q_d_nullspace.at(i);
308  }
309  this->setNullspaceConfig(q_d_nullspace);
310  }
311  else
312  {
313  ROS_WARN_STREAM("Nullspace configuration does not have the correct amount of entries. Got " << msg->q_d_nullspace.size() << " expected " << this->n_joints_ << ". Ignoring.");
314  }
315  }
316 
317  void CartesianImpedanceControllerRos::cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg)
318  {
319  this->setDampingFactors(*msg, this->damping_factors_[6]);
320  }
321 
322  void CartesianImpedanceControllerRos::referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg)
323  {
324  if (!msg->header.frame_id.empty() && msg->header.frame_id != this->root_frame_)
325  {
326  ROS_WARN_STREAM("Reference poses need to be in the root frame '" << this->root_frame_ << "'. Ignoring.");
327  return;
328  }
329  Eigen::Vector3d position_d;
330  position_d << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
331  const Eigen::Quaterniond last_orientation_d_target(this->orientation_d_);
332  Eigen::Quaterniond orientation_d;
333  orientation_d.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z,
334  msg->pose.orientation.w;
335  if (last_orientation_d_target.coeffs().dot(this->orientation_d_.coeffs()) < 0.0)
336  {
337  this->orientation_d_.coeffs() << -this->orientation_d_.coeffs();
338  }
339  this->setReferencePose(position_d, orientation_d);
340  }
341 
342  void CartesianImpedanceControllerRos::cartesianStiffnessCb(const geometry_msgs::WrenchStampedConstPtr &msg)
343  {
344  this->setStiffness(msg->wrench, this->nullspace_stiffness_target_);
345  }
346 
347  void CartesianImpedanceControllerRos::setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace)
348  {
349  CartesianImpedanceController::setDampingFactors(saturateValue(cart_damping.force.x, dmp_factor_min_, dmp_factor_max_),
350  saturateValue(cart_damping.force.y, dmp_factor_min_, dmp_factor_max_),
351  saturateValue(cart_damping.force.z, dmp_factor_min_, dmp_factor_max_),
352  saturateValue(cart_damping.torque.x, dmp_factor_min_, dmp_factor_max_),
353  saturateValue(cart_damping.torque.y, dmp_factor_min_, dmp_factor_max_),
354  saturateValue(cart_damping.torque.z, dmp_factor_min_, dmp_factor_max_),
355  saturateValue(nullspace, dmp_factor_min_, dmp_factor_max_));
356  }
357 
358  void CartesianImpedanceControllerRos::setStiffness(const geometry_msgs::Wrench &cart_stiffness, double nullspace, bool auto_damping)
359  {
360  CartesianImpedanceController::setStiffness(saturateValue(cart_stiffness.force.x, trans_stf_min_, trans_stf_max_),
361  saturateValue(cart_stiffness.force.y, trans_stf_min_, trans_stf_max_),
362  saturateValue(cart_stiffness.force.z, trans_stf_min_, trans_stf_max_),
363  saturateValue(cart_stiffness.torque.x, rot_stf_min_, rot_stf_max_),
364  saturateValue(cart_stiffness.torque.y, rot_stf_min_, rot_stf_max_),
365  saturateValue(cart_stiffness.torque.z, rot_stf_min_, rot_stf_max_),
366  saturateValue(nullspace, ns_min_, ns_max_), auto_damping);
367  }
368 
369  void CartesianImpedanceControllerRos::wrenchCommandCb(const geometry_msgs::WrenchStampedConstPtr &msg)
370  {
371  Eigen::Matrix<double, 6, 1> F;
372  F << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, msg->wrench.torque.x, msg->wrench.torque.y,
373  msg->wrench.torque.z;
374 
375  if (!msg->header.frame_id.empty() && msg->header.frame_id != this->root_frame_)
376  {
377  if (!transformWrench(&F, msg->header.frame_id, this->root_frame_))
378  {
379  ROS_ERROR("Could not transform wrench. Not applying it.");
380  return;
381  }
382  }
383  else if (msg->header.frame_id.empty())
384  {
385  if (!transformWrench(&F, this->wrench_ee_frame_, this->root_frame_))
386  {
387  ROS_ERROR("Could not transform wrench. Not applying it.");
388  return;
389  }
390  }
391  this->applyWrench(F);
392  }
393 
394  bool CartesianImpedanceControllerRos::transformWrench(Eigen::Matrix<double, 6, 1> *cartesian_wrench,
395  const std::string &from_frame, const std::string &to_frame) const
396  {
397  try
398  {
399  tf::StampedTransform transform;
400  tf_listener_.lookupTransform(to_frame, from_frame, ros::Time(0), transform);
401  tf::Vector3 v_f(cartesian_wrench->operator()(0), cartesian_wrench->operator()(1), cartesian_wrench->operator()(2));
402  tf::Vector3 v_t(cartesian_wrench->operator()(3), cartesian_wrench->operator()(4), cartesian_wrench->operator()(5));
403  tf::Vector3 v_f_rot = tf::quatRotate(transform.getRotation(), v_f);
404  tf::Vector3 v_t_rot = tf::quatRotate(transform.getRotation(), v_t);
405  *cartesian_wrench << v_f_rot[0], v_f_rot[1], v_f_rot[2], v_t_rot[0], v_t_rot[1], v_t_rot[2];
406  return true;
407  }
408  catch (const tf::TransformException &ex)
409  {
410  ROS_ERROR_THROTTLE(1, "%s", ex.what());
411  return false;
412  }
413  }
414 
415  void CartesianImpedanceControllerRos::publishMsgsAndTf()
416  {
417  // publish commanded torques
418  if (this->pub_torques_.trylock())
419  {
420  for (size_t i = 0; i < this->n_joints_; i++)
421  {
422  this->pub_torques_.msg_.data[i] = this->tau_c_[i];
423  }
424  this->pub_torques_.unlockAndPublish();
425  }
426 
427  const Eigen::Matrix<double, 6, 1> error{this->getPoseError()};
428 
429  if (this->verbose_print_)
430  {
431  ROS_INFO_STREAM_THROTTLE(0.1, "\nCartesian Position:\n"
432  << this->position_ << "\nError:\n"
433  << error << "\nCartesian Stiffness:\n"
434  << this->cartesian_stiffness_ << "\nCartesian damping:\n"
435  << this->cartesian_damping_ << "\nNullspace stiffness:\n"
436  << this->nullspace_stiffness_ << "\nq_d_nullspace:\n"
437  << this->q_d_nullspace_ << "\ntau_d:\n"
438  << this->tau_c_);
439  }
440  if (this->verbose_tf_ && ros::Time::now() > this->tf_last_time_)
441  {
442  // Publish result of forward kinematics
443  tf::vectorEigenToTF(this->position_, this->tf_pos_);
444  this->tf_br_transform_.setOrigin(this->tf_pos_);
445  tf::quaternionEigenToTF(this->orientation_, this->tf_rot_);
446  this->tf_br_transform_.setRotation(this->tf_rot_);
447  tf_br_.sendTransform(tf::StampedTransform(this->tf_br_transform_, ros::Time::now(), this->root_frame_, this->end_effector_ + "_ee_fk"));
448  // Publish tf to the reference pose
449  tf::vectorEigenToTF(this->position_d_, this->tf_pos_);
450  this->tf_br_transform_.setOrigin(this->tf_pos_);
451  tf::quaternionEigenToTF(this->orientation_d_, this->tf_rot_);
452  this->tf_br_transform_.setRotation(this->tf_rot_);
453  tf_br_.sendTransform(tf::StampedTransform(this->tf_br_transform_, ros::Time::now(), this->root_frame_, this->end_effector_ + "_ee_ref_pose"));
454  this->tf_last_time_ = ros::Time::now();
455  }
456  if (this->verbose_state_ && this->pub_state_.trylock())
457  {
458  this->pub_state_.msg_.header.stamp = ros::Time::now();
459  tf::pointEigenToMsg(this->position_, this->pub_state_.msg_.current_pose.position);
460  tf::quaternionEigenToMsg(this->orientation_, this->pub_state_.msg_.current_pose.orientation);
461  tf::pointEigenToMsg(this->position_d_, this->pub_state_.msg_.reference_pose.position);
462  tf::quaternionEigenToMsg(this->orientation_d_, this->pub_state_.msg_.reference_pose.orientation);
463  tf::pointEigenToMsg(error.head(3), this->pub_state_.msg_.pose_error.position);
464  Eigen::Quaterniond q = Eigen::AngleAxisd(error(3), Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(error(4), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(error(5), Eigen::Vector3d::UnitZ());
465  tf::quaternionEigenToMsg(q, this->pub_state_.msg_.pose_error.orientation);
466 
467  EigenVectorToWrench(this->cartesian_stiffness_.diagonal(), &this->pub_state_.msg_.cartesian_stiffness);
468  EigenVectorToWrench(this->cartesian_damping_.diagonal(), &this->pub_state_.msg_.cartesian_damping);
469  EigenVectorToWrench(this->getAppliedWrench(), &this->pub_state_.msg_.commanded_wrench);
470 
471  for (size_t i = 0; i < this->n_joints_; i++)
472  {
473  this->pub_state_.msg_.joint_state.position.at(i) = this->q_(i);
474  this->pub_state_.msg_.joint_state.velocity.at(i) = this->dq_(i);
475  this->pub_state_.msg_.joint_state.effort.at(i) = this->tau_m_(i);
476  this->pub_state_.msg_.nullspace_config.at(i) = this->q_d_nullspace_(i);
477  this->pub_state_.msg_.commanded_torques.at(i) = this->tau_c_(i);
478  }
479  this->pub_state_.msg_.nullspace_stiffness = this->nullspace_stiffness_;
480  this->pub_state_.msg_.nullspace_damping = this->nullspace_damping_;
481  const Eigen::Matrix<double, 6, 1> dx = this->jacobian_ * this->dq_;
482  this->pub_state_.msg_.cartesian_velocity = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2));
483 
484  this->pub_state_.unlockAndPublish();
485  this->pub_state_.msg_.header.seq++;
486  }
487  }
488 
489  // Dynamic reconfigure
490  // --------------------------------------------------------------------------------------------------------------------------------------
491  void CartesianImpedanceControllerRos::dynamicStiffnessCb(
492  cartesian_impedance_controller::stiffnessConfig &config, uint32_t level)
493  {
494  if (config.update_stiffness)
495  {
496  CartesianImpedanceController::setStiffness(saturateValue(config.translation_x, trans_stf_min_, trans_stf_max_),
497  saturateValue(config.translation_y, trans_stf_min_, trans_stf_max_),
498  saturateValue(config.translation_z, trans_stf_min_, trans_stf_max_),
499  saturateValue(config.rotation_x, trans_stf_min_, trans_stf_max_),
500  saturateValue(config.rotation_y, trans_stf_min_, trans_stf_max_),
501  saturateValue(config.rotation_z, trans_stf_min_, trans_stf_max_), config.nullspace_stiffness);
502  }
503  }
504 
505  void CartesianImpedanceControllerRos::dynamicDampingCb(
506  cartesian_impedance_controller::dampingConfig &config, uint32_t level)
507  {
508  if (config.update_damping_factors)
509  {
510  CartesianImpedanceController::setDampingFactors(
511  config.translation_x, config.translation_y, config.translation_z, config.rotation_x, config.rotation_y, config.rotation_z, config.nullspace_damping);
512  }
513  }
514 
515  void CartesianImpedanceControllerRos::dynamicWrenchCb(cartesian_impedance_controller::wrenchConfig &config,
516  uint32_t level)
517  {
518  Eigen::Vector6d F{Eigen::Vector6d::Zero()};
519  if (config.apply_wrench)
520  {
521  F << config.f_x, config.f_y, config.f_z, config.tau_x, config.tau_y, config.tau_z;
522  if (!transformWrench(&F, this->wrench_ee_frame_, this->root_frame_))
523  {
524  ROS_ERROR("Could not transform wrench. Not applying it.");
525  return;
526  }
527  }
528  this->applyWrench(F);
529  }
530 
531  void CartesianImpedanceControllerRos::trajCb(const trajectory_msgs::JointTrajectoryConstPtr &msg)
532  {
533  ROS_INFO("Got trajectory msg from trajectory topic.");
534  if (this->traj_as_->isActive())
535  {
536  this->traj_as_->setPreempted();
537  ROS_INFO("Preempted running action server goal.");
538  }
539  trajStart(*msg);
540  }
541 
542  void CartesianImpedanceControllerRos::trajGoalCb()
543  {
544  this->traj_as_goal_ = this->traj_as_->acceptNewGoal();
545  ROS_INFO("Accepted new goal from action server.");
546  trajStart(this->traj_as_goal_->trajectory);
547  }
548 
549  void CartesianImpedanceControllerRos::trajPreemptCb()
550  {
551  ROS_INFO("Actionserver got preempted.");
552  this->traj_as_->setPreempted();
553  }
554 
555  void CartesianImpedanceControllerRos::trajStart(const trajectory_msgs::JointTrajectory &trajectory)
556  {
557  this->traj_duration_ = trajectory.points[trajectory.points.size() - 1].time_from_start;
558  ROS_INFO_STREAM("Starting a new trajectory with " << trajectory.points.size() << " points that takes " << this->traj_duration_ << "s.");
559  this->trajectory_ = trajectory;
560  this->traj_running_ = true;
561  this->traj_start_ = ros::Time::now();
562  this->traj_index_ = 0;
563  trajUpdate();
564  if (this->nullspace_stiffness_ < 5.)
565  {
566  ROS_WARN("Nullspace stiffness is low. The joints might not follow the planned path.");
567  }
568  }
569 
570  void CartesianImpedanceControllerRos::trajUpdate()
571  {
572  if (ros::Time::now() > (this->traj_start_ + trajectory_.points.at(this->traj_index_).time_from_start))
573  {
574  // Get end effector pose
575  Eigen::VectorXd q = Eigen::VectorXd::Map(trajectory_.points.at(this->traj_index_).positions.data(),
576  trajectory_.points.at(this->traj_index_).positions.size());
577  if (this->verbose_print_)
578  {
579  ROS_INFO_STREAM("Index " << this->traj_index_ << " q_nullspace: " << q.transpose());
580  }
581  // Update end-effector pose and nullspace
582  getFk(q, &this->position_d_target_, &this->orientation_d_target_);
583  this->setNullspaceConfig(q);
584  this->traj_index_++;
585  }
586 
587  if (ros::Time::now() > (this->traj_start_ + this->traj_duration_))
588  {
589  ROS_INFO_STREAM("Finished executing trajectory.");
590  if (this->traj_as_->isActive())
591  {
592  this->traj_as_->setSucceeded();
593  }
594  this->traj_running_ = false;
595  }
596  }
597 } // namespace cartesian_impedance_controller
cartesian_impedance_controller::CartesianImpedanceControllerRos::sub_cart_wrench_
ros::Subscriber sub_cart_wrench_
Cartesian wrench subscriber.
Definition: cartesian_impedance_controller_ros.h:291
+Go to the documentation of this file.
1 #include <cartesian_impedance_controller/cartesian_impedance_controller_ros.h>
2 
3 #include <eigen_conversions/eigen_msg.h>
4 #include <tf_conversions/tf_eigen.h>
5 
6 namespace cartesian_impedance_controller
7 {
15  double saturateValue(double x, double x_min, double x_max)
16  {
17  return std::min(std::max(x, x_min), x_max);
18  }
19 
26  void EigenVectorToWrench(const Eigen::Matrix<double, 6, 1> &v, geometry_msgs::Wrench *wrench)
27  {
28  wrench->force.x = v(0);
29  wrench->force.y = v(1);
30  wrench->force.z = v(2);
31  wrench->torque.x = v(3);
32  wrench->torque.y = v(4);
33  wrench->torque.z = v(5);
34  }
35 
36  bool CartesianImpedanceControllerRos::initDynamicReconfigure(const ros::NodeHandle &nh)
37  {
38  this->dynamic_server_compliance_param_ = std::make_unique<dynamic_reconfigure::Server<cartesian_impedance_controller::stiffnessConfig>>(ros::NodeHandle(std::string(nh.getNamespace() + "/stiffness_reconfigure")));
39  this->dynamic_server_compliance_param_->setCallback(
40  boost::bind(&CartesianImpedanceControllerRos::dynamicStiffnessCb, this, _1, _2));
41 
42  this->dynamic_server_damping_param_ = std::make_unique<dynamic_reconfigure::Server<cartesian_impedance_controller::dampingConfig>>(ros::NodeHandle(std::string(nh.getNamespace() + "/damping_factors_reconfigure")));
43  dynamic_server_damping_param_->setCallback(
44  boost::bind(&CartesianImpedanceControllerRos::dynamicDampingCb, this, _1, _2));
45 
46  this->dynamic_server_wrench_param_ = std::make_unique<dynamic_reconfigure::Server<cartesian_impedance_controller::wrenchConfig>>(ros::NodeHandle(std::string(nh.getNamespace() + "/cartesian_wrench_reconfigure")));
47  dynamic_server_wrench_param_->setCallback(
48  boost::bind(&CartesianImpedanceControllerRos::dynamicWrenchCb, this, _1, _2));
49  return true;
50  }
51 
52  bool CartesianImpedanceControllerRos::initJointHandles(hardware_interface::EffortJointInterface *hw, const ros::NodeHandle &nh)
53  {
54  std::vector<std::string> joint_names;
55  if (!nh.getParam("joints", joint_names))
56  {
57  ROS_ERROR("Invalid or no 'joints' parameter provided, aborting controller init!");
58  return false;
59  }
60  for (size_t i = 0; i < joint_names.size(); ++i)
61  {
62  try
63  {
64  this->joint_handles_.push_back(hw->getHandle(joint_names[i]));
65  }
66  catch (const hardware_interface::HardwareInterfaceException &ex)
67  {
68  ROS_ERROR_STREAM("Exception getting joint handles: " << ex.what());
69  return false;
70  }
71  }
72  ROS_INFO_STREAM("Number of joints specified in parameters: " << joint_names.size());
73  this->setNumberOfJoints(joint_names.size());
74  return true;
75  }
76 
77  bool CartesianImpedanceControllerRos::initMessaging(ros::NodeHandle *nh)
78  {
79  // Queue size of 1 since we are only interested in the last message
80  this->sub_cart_stiffness_ = nh->subscribe("set_cartesian_stiffness", 1,
81  &CartesianImpedanceControllerRos::cartesianStiffnessCb, this);
82  this->sub_cart_wrench_ = nh->subscribe("set_cartesian_wrench", 1,
83  &CartesianImpedanceControllerRos::wrenchCommandCb, this);
84  this->sub_damping_factors_ = nh->subscribe("set_damping_factors", 1,
85  &CartesianImpedanceControllerRos::cartesianDampingFactorCb, this);
86  this->sub_controller_config_ =
87  nh->subscribe("set_config", 1, &CartesianImpedanceControllerRos::controllerConfigCb, this);
88  this->sub_reference_pose_ = nh->subscribe("reference_pose", 1, &CartesianImpedanceControllerRos::referencePoseCb, this);
89 
90  // Initializing the realtime publisher and the message
91  this->pub_torques_.init(*nh, "commanded_torques", 20);
92  this->pub_torques_.msg_.layout.dim.resize(1);
93  this->pub_torques_.msg_.layout.data_offset = 0;
94  this->pub_torques_.msg_.layout.dim[0].size = this->n_joints_;
95  this->pub_torques_.msg_.layout.dim[0].stride = 0;
96  this->pub_torques_.msg_.data.resize(this->n_joints_);
97 
98  std::vector<std::string> joint_names;
99  nh->getParam("joints", joint_names);
100  this->pub_state_.init(*nh, "controller_state", 10);
101  this->pub_state_.msg_.header.seq = 0;
102  for (size_t i = 0; i < this->n_joints_; i++)
103  {
104  this->pub_state_.msg_.joint_state.name.push_back(joint_names.at(i));
105  }
106  this->pub_state_.msg_.joint_state.position = std::vector<double>(this->n_joints_);
107  this->pub_state_.msg_.joint_state.velocity = std::vector<double>(this->n_joints_);
108  this->pub_state_.msg_.joint_state.effort = std::vector<double>(this->n_joints_);
109  this->pub_state_.msg_.commanded_torques = std::vector<double>(this->n_joints_);
110  this->pub_state_.msg_.nullspace_config = std::vector<double>(this->n_joints_);
111  return true;
112  }
113 
114  bool CartesianImpedanceControllerRos::initRBDyn(const ros::NodeHandle &nh)
115  {
116  // Get the URDF XML from the parameter server. Wait if needed.
117  std::string urdf_string;
118  nh.param<std::string>("robot_description", robot_description_, "/robot_description");
119  while (!nh.getParam(robot_description_, urdf_string))
120  {
121  ROS_INFO_ONCE("Waiting for robot description in parameter %s on the ROS param server.",
122  robot_description_.c_str());
123  usleep(100000);
124  }
125  try
126  {
127  this->rbdyn_wrapper_.init_rbdyn(urdf_string, end_effector_);
128  }
129  catch (std::runtime_error e)
130  {
131  ROS_ERROR("Error when intializing RBDyn: %s", e.what());
132  return false;
133  }
134  ROS_INFO_STREAM("Number of joints found in urdf: " << this->rbdyn_wrapper_.n_joints());
135  if (this->rbdyn_wrapper_.n_joints() < this->n_joints_)
136  {
137  ROS_ERROR("Number of joints in the URDF is smaller than supplied number of joints. %i < %zu", this->rbdyn_wrapper_.n_joints(), this->n_joints_);
138  return false;
139  }
140  else if (this->rbdyn_wrapper_.n_joints() > this->n_joints_)
141  {
142  ROS_WARN("Number of joints in the URDF is greater than supplied number of joints: %i > %zu. Assuming that the actuated joints come first.", this->rbdyn_wrapper_.n_joints(), this->n_joints_);
143  }
144  return true;
145  }
146 
147  bool CartesianImpedanceControllerRos::initTrajectories(ros::NodeHandle *nh)
148  {
149  this->sub_trajectory_ = nh->subscribe("joint_trajectory", 1, &CartesianImpedanceControllerRos::trajCb, this);
150  this->traj_as_ = std::unique_ptr<actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>>(
151  new actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>(
152  *nh, std::string("follow_joint_trajectory"), false));
153  this->traj_as_->registerGoalCallback(boost::bind(&CartesianImpedanceControllerRos::trajGoalCb, this));
154  this->traj_as_->registerPreemptCallback(boost::bind(&CartesianImpedanceControllerRos::trajPreemptCb, this));
155  this->traj_as_->start();
156  return true;
157  }
158 
159  bool CartesianImpedanceControllerRos::init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &node_handle)
160  {
161  ROS_INFO("Initializing Cartesian impedance controller in namespace: %s", node_handle.getNamespace().c_str());
162 
163  // Fetch parameters
164  node_handle.param<std::string>("end_effector", this->end_effector_, "iiwa_link_ee");
165  ROS_INFO_STREAM("End effektor link is: " << this->end_effector_);
166  // Frame for applying commanded Cartesian wrenches
167  node_handle.param<std::string>("wrench_ee_frame", this->wrench_ee_frame_, this->end_effector_);
168  bool dynamic_reconfigure{true};
169  node_handle.param<bool>("dynamic_reconfigure", dynamic_reconfigure, true);
170  bool enable_trajectories{true};
171  node_handle.param<bool>("handle_trajectories", enable_trajectories, true);
172  node_handle.param<double>("delta_tau_max", this->delta_tau_max_, 1.);
173  node_handle.param<double>("update_frequency", this->update_frequency_, 500.);
174  node_handle.param<double>("filtering/nullspace_config", this->filter_params_nullspace_config_, 0.1);
175  node_handle.param<double>("filtering/stiffness", this->filter_params_stiffness_, 0.1);
176  node_handle.param<double>("filtering/pose", this->filter_params_pose_, 0.1);
177  node_handle.param<double>("filtering/wrench", this->filter_params_wrench_, 0.1);
178  node_handle.param<bool>("verbosity/verbose_print", this->verbose_print_, false);
179  node_handle.param<bool>("verbosity/state_msgs", this->verbose_state_, false);
180  node_handle.param<bool>("verbosity/tf_frames", this->verbose_tf_, false);
181 
182  if (!this->initJointHandles(hw, node_handle) || !this->initMessaging(&node_handle) || !this->initRBDyn(node_handle))
183  {
184  return false;
185  }
186  if (enable_trajectories && !this->initTrajectories(&node_handle))
187  {
188  return false;
189  }
190  this->root_frame_ = this->rbdyn_wrapper_.root_link();
191  node_handle.setParam("root_frame", this->root_frame_);
192 
193  // Initialize base_tools and member variables
194  this->setNumberOfJoints(this->joint_handles_.size());
195  if (this->n_joints_ < 6)
196  {
197  ROS_WARN("Number of joints is below 6. Functions might be limited.");
198  }
199  if (this->n_joints_ < 7)
200  {
201  ROS_WARN("Number of joints is below 7. No redundant joint for nullspace.");
202  }
203  this->tau_m_ = Eigen::VectorXd(this->n_joints_);
204 
205  // Needs to be after base_tools init since the wrench callback calls it
206  if (dynamic_reconfigure && !this->initDynamicReconfigure(node_handle))
207  {
208  return false;
209  }
210 
211  ROS_INFO("Finished initialization.");
212  return true;
213  }
214 
215  void CartesianImpedanceControllerRos::starting(const ros::Time & /*time*/)
216  {
217  this->updateState();
218 
219  // Set reference pose to current pose and q_d_nullspace
220  this->initDesiredPose(this->position_, this->orientation_);
221  this->initNullspaceConfig(this->q_);
222  ROS_INFO("Started Cartesian Impedance Controller");
223  }
224 
225  void CartesianImpedanceControllerRos::update(const ros::Time & /*time*/, const ros::Duration &period /*period*/)
226  {
227  if (this->traj_running_)
228  {
229  trajUpdate();
230  }
231 
232  this->updateState();
233 
234  // Apply control law in base library
235  this->calculateCommandedTorques();
236 
237  // Write commands
238  for (size_t i = 0; i < this->n_joints_; ++i)
239  {
240  this->joint_handles_[i].setCommand(this->tau_c_(i));
241  }
242 
243  publishMsgsAndTf();
244  }
245 
246  bool CartesianImpedanceControllerRos::getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position,
247  Eigen::Quaterniond *orientation) const
248  {
249  rbdyn_wrapper::EefState ee_state;
250  // If the URDF contains more joints than there are controlled, only the state of the controlled ones are known
251  if (this->rbdyn_wrapper_.n_joints() != this->n_joints_)
252  {
253  Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints());
254  q_rb.head(q.size()) = q;
255  ee_state = this->rbdyn_wrapper_.perform_fk(q_rb);
256  }
257  else
258  {
259  ee_state = this->rbdyn_wrapper_.perform_fk(q);
260  }
261  *position = ee_state.translation;
262  *orientation = ee_state.orientation;
263  return true;
264  }
265 
266  bool CartesianImpedanceControllerRos::getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq,
267  Eigen::MatrixXd *jacobian)
268  {
269  // If the URDF contains more joints than there are controlled, only the state of the controlled ones are known
270  if (this->rbdyn_wrapper_.n_joints() != this->n_joints_)
271  {
272  Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints());
273  q_rb.head(q.size()) = q;
274  Eigen::VectorXd dq_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints());
275  dq_rb.head(dq.size()) = dq;
276  *jacobian = this->rbdyn_wrapper_.jacobian(q_rb, dq_rb);
277  }
278  else
279  {
280  *jacobian = this->rbdyn_wrapper_.jacobian(q, dq);
281  }
282  *jacobian = jacobian_perm_ * *jacobian;
283  return true;
284  }
285 
286  void CartesianImpedanceControllerRos::updateState()
287  {
288  for (size_t i = 0; i < this->n_joints_; ++i)
289  {
290  this->q_[i] = this->joint_handles_[i].getPosition();
291  this->dq_[i] = this->joint_handles_[i].getVelocity();
292  this->tau_m_[i] = this->joint_handles_[i].getEffort();
293  }
294  getJacobian(this->q_, this->dq_, &this->jacobian_);
295  getFk(this->q_, &this->position_, &this->orientation_);
296  }
297 
298  void CartesianImpedanceControllerRos::controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg)
299  {
300  this->setStiffness(msg->cartesian_stiffness, msg->nullspace_stiffness, false);
301  this->setDampingFactors(msg->cartesian_damping_factors, msg->nullspace_damping_factor);
302 
303  if (msg->q_d_nullspace.size() == this->n_joints_)
304  {
305  Eigen::VectorXd q_d_nullspace(this->n_joints_);
306  for (size_t i = 0; i < this->n_joints_; i++)
307  {
308  q_d_nullspace(i) = msg->q_d_nullspace.at(i);
309  }
310  this->setNullspaceConfig(q_d_nullspace);
311  }
312  else
313  {
314  ROS_WARN_STREAM("Nullspace configuration does not have the correct amount of entries. Got " << msg->q_d_nullspace.size() << " expected " << this->n_joints_ << ". Ignoring.");
315  }
316  }
317 
318  void CartesianImpedanceControllerRos::cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg)
319  {
320  this->setDampingFactors(*msg, this->damping_factors_[6]);
321  }
322 
323  void CartesianImpedanceControllerRos::referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg)
324  {
325  if (!msg->header.frame_id.empty() && msg->header.frame_id != this->root_frame_)
326  {
327  ROS_WARN_STREAM("Reference poses need to be in the root frame '" << this->root_frame_ << "'. Ignoring.");
328  return;
329  }
330  Eigen::Vector3d position_d;
331  position_d << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
332  const Eigen::Quaterniond last_orientation_d_target(this->orientation_d_);
333  Eigen::Quaterniond orientation_d;
334  orientation_d.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z,
335  msg->pose.orientation.w;
336  if (last_orientation_d_target.coeffs().dot(this->orientation_d_.coeffs()) < 0.0)
337  {
338  this->orientation_d_.coeffs() << -this->orientation_d_.coeffs();
339  }
340  this->setReferencePose(position_d, orientation_d);
341  }
342 
343  void CartesianImpedanceControllerRos::cartesianStiffnessCb(const geometry_msgs::WrenchStampedConstPtr &msg)
344  {
345  this->setStiffness(msg->wrench, this->nullspace_stiffness_target_);
346  }
347 
348  void CartesianImpedanceControllerRos::setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace)
349  {
350  CartesianImpedanceController::setDampingFactors(saturateValue(cart_damping.force.x, dmp_factor_min_, dmp_factor_max_),
351  saturateValue(cart_damping.force.y, dmp_factor_min_, dmp_factor_max_),
352  saturateValue(cart_damping.force.z, dmp_factor_min_, dmp_factor_max_),
353  saturateValue(cart_damping.torque.x, dmp_factor_min_, dmp_factor_max_),
354  saturateValue(cart_damping.torque.y, dmp_factor_min_, dmp_factor_max_),
355  saturateValue(cart_damping.torque.z, dmp_factor_min_, dmp_factor_max_),
356  saturateValue(nullspace, dmp_factor_min_, dmp_factor_max_));
357  }
358 
359  void CartesianImpedanceControllerRos::setStiffness(const geometry_msgs::Wrench &cart_stiffness, double nullspace, bool auto_damping)
360  {
361  CartesianImpedanceController::setStiffness(saturateValue(cart_stiffness.force.x, trans_stf_min_, trans_stf_max_),
362  saturateValue(cart_stiffness.force.y, trans_stf_min_, trans_stf_max_),
363  saturateValue(cart_stiffness.force.z, trans_stf_min_, trans_stf_max_),
364  saturateValue(cart_stiffness.torque.x, rot_stf_min_, rot_stf_max_),
365  saturateValue(cart_stiffness.torque.y, rot_stf_min_, rot_stf_max_),
366  saturateValue(cart_stiffness.torque.z, rot_stf_min_, rot_stf_max_),
367  saturateValue(nullspace, ns_min_, ns_max_), auto_damping);
368  }
369 
370  void CartesianImpedanceControllerRos::wrenchCommandCb(const geometry_msgs::WrenchStampedConstPtr &msg)
371  {
372  Eigen::Matrix<double, 6, 1> F;
373  F << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, msg->wrench.torque.x, msg->wrench.torque.y,
374  msg->wrench.torque.z;
375 
376  if (!msg->header.frame_id.empty() && msg->header.frame_id != this->root_frame_)
377  {
378  if (!transformWrench(&F, msg->header.frame_id, this->root_frame_))
379  {
380  ROS_ERROR("Could not transform wrench. Not applying it.");
381  return;
382  }
383  }
384  else if (msg->header.frame_id.empty())
385  {
386  if (!transformWrench(&F, this->wrench_ee_frame_, this->root_frame_))
387  {
388  ROS_ERROR("Could not transform wrench. Not applying it.");
389  return;
390  }
391  }
392  this->applyWrench(F);
393  }
394 
395  bool CartesianImpedanceControllerRos::transformWrench(Eigen::Matrix<double, 6, 1> *cartesian_wrench,
396  const std::string &from_frame, const std::string &to_frame) const
397  {
398  try
399  {
400  tf::StampedTransform transform;
401  tf_listener_.lookupTransform(to_frame, from_frame, ros::Time(0), transform);
402  tf::Vector3 v_f(cartesian_wrench->operator()(0), cartesian_wrench->operator()(1), cartesian_wrench->operator()(2));
403  tf::Vector3 v_t(cartesian_wrench->operator()(3), cartesian_wrench->operator()(4), cartesian_wrench->operator()(5));
404  tf::Vector3 v_f_rot = tf::quatRotate(transform.getRotation(), v_f);
405  tf::Vector3 v_t_rot = tf::quatRotate(transform.getRotation(), v_t);
406  *cartesian_wrench << v_f_rot[0], v_f_rot[1], v_f_rot[2], v_t_rot[0], v_t_rot[1], v_t_rot[2];
407  return true;
408  }
409  catch (const tf::TransformException &ex)
410  {
411  ROS_ERROR_THROTTLE(1, "%s", ex.what());
412  return false;
413  }
414  }
415 
416  void CartesianImpedanceControllerRos::publishMsgsAndTf()
417  {
418  // publish commanded torques
419  if (this->pub_torques_.trylock())
420  {
421  for (size_t i = 0; i < this->n_joints_; i++)
422  {
423  this->pub_torques_.msg_.data[i] = this->tau_c_[i];
424  }
425  this->pub_torques_.unlockAndPublish();
426  }
427 
428  const Eigen::Matrix<double, 6, 1> error{this->getPoseError()};
429 
430  if (this->verbose_print_)
431  {
432  ROS_INFO_STREAM_THROTTLE(0.1, "\nCartesian Position:\n"
433  << this->position_ << "\nError:\n"
434  << error << "\nCartesian Stiffness:\n"
435  << this->cartesian_stiffness_ << "\nCartesian damping:\n"
436  << this->cartesian_damping_ << "\nNullspace stiffness:\n"
437  << this->nullspace_stiffness_ << "\nq_d_nullspace:\n"
438  << this->q_d_nullspace_ << "\ntau_d:\n"
439  << this->tau_c_);
440  }
441  if (this->verbose_tf_ && ros::Time::now() > this->tf_last_time_)
442  {
443  // Publish result of forward kinematics
444  tf::vectorEigenToTF(this->position_, this->tf_pos_);
445  this->tf_br_transform_.setOrigin(this->tf_pos_);
446  tf::quaternionEigenToTF(this->orientation_, this->tf_rot_);
447  this->tf_br_transform_.setRotation(this->tf_rot_);
448  tf_br_.sendTransform(tf::StampedTransform(this->tf_br_transform_, ros::Time::now(), this->root_frame_, this->end_effector_ + "_ee_fk"));
449  // Publish tf to the reference pose
450  tf::vectorEigenToTF(this->position_d_, this->tf_pos_);
451  this->tf_br_transform_.setOrigin(this->tf_pos_);
452  tf::quaternionEigenToTF(this->orientation_d_, this->tf_rot_);
453  this->tf_br_transform_.setRotation(this->tf_rot_);
454  tf_br_.sendTransform(tf::StampedTransform(this->tf_br_transform_, ros::Time::now(), this->root_frame_, this->end_effector_ + "_ee_ref_pose"));
455  this->tf_last_time_ = ros::Time::now();
456  }
457  if (this->verbose_state_ && this->pub_state_.trylock())
458  {
459  this->pub_state_.msg_.header.stamp = ros::Time::now();
460  tf::pointEigenToMsg(this->position_, this->pub_state_.msg_.current_pose.position);
461  tf::quaternionEigenToMsg(this->orientation_, this->pub_state_.msg_.current_pose.orientation);
462  tf::pointEigenToMsg(this->position_d_, this->pub_state_.msg_.reference_pose.position);
463  tf::quaternionEigenToMsg(this->orientation_d_, this->pub_state_.msg_.reference_pose.orientation);
464  tf::pointEigenToMsg(error.head(3), this->pub_state_.msg_.pose_error.position);
465  Eigen::Quaterniond q = Eigen::AngleAxisd(error(3), Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(error(4), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(error(5), Eigen::Vector3d::UnitZ());
466  tf::quaternionEigenToMsg(q, this->pub_state_.msg_.pose_error.orientation);
467 
468  EigenVectorToWrench(this->cartesian_stiffness_.diagonal(), &this->pub_state_.msg_.cartesian_stiffness);
469  EigenVectorToWrench(this->cartesian_damping_.diagonal(), &this->pub_state_.msg_.cartesian_damping);
470  EigenVectorToWrench(this->getAppliedWrench(), &this->pub_state_.msg_.commanded_wrench);
471 
472  for (size_t i = 0; i < this->n_joints_; i++)
473  {
474  this->pub_state_.msg_.joint_state.position.at(i) = this->q_(i);
475  this->pub_state_.msg_.joint_state.velocity.at(i) = this->dq_(i);
476  this->pub_state_.msg_.joint_state.effort.at(i) = this->tau_m_(i);
477  this->pub_state_.msg_.nullspace_config.at(i) = this->q_d_nullspace_(i);
478  this->pub_state_.msg_.commanded_torques.at(i) = this->tau_c_(i);
479  }
480  this->pub_state_.msg_.nullspace_stiffness = this->nullspace_stiffness_;
481  this->pub_state_.msg_.nullspace_damping = this->nullspace_damping_;
482  const Eigen::Matrix<double, 6, 1> dx = this->jacobian_ * this->dq_;
483  this->pub_state_.msg_.cartesian_velocity = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2));
484 
485  this->pub_state_.unlockAndPublish();
486  this->pub_state_.msg_.header.seq++;
487  }
488  }
489 
490  // Dynamic reconfigure
491  // --------------------------------------------------------------------------------------------------------------------------------------
492  void CartesianImpedanceControllerRos::dynamicStiffnessCb(
493  cartesian_impedance_controller::stiffnessConfig &config, uint32_t level)
494  {
495  if (config.update_stiffness)
496  {
497  CartesianImpedanceController::setStiffness(saturateValue(config.translation_x, trans_stf_min_, trans_stf_max_),
498  saturateValue(config.translation_y, trans_stf_min_, trans_stf_max_),
499  saturateValue(config.translation_z, trans_stf_min_, trans_stf_max_),
500  saturateValue(config.rotation_x, trans_stf_min_, trans_stf_max_),
501  saturateValue(config.rotation_y, trans_stf_min_, trans_stf_max_),
502  saturateValue(config.rotation_z, trans_stf_min_, trans_stf_max_), config.nullspace_stiffness);
503  }
504  }
505 
506  void CartesianImpedanceControllerRos::dynamicDampingCb(
507  cartesian_impedance_controller::dampingConfig &config, uint32_t level)
508  {
509  if (config.update_damping_factors)
510  {
511  CartesianImpedanceController::setDampingFactors(
512  config.translation_x, config.translation_y, config.translation_z, config.rotation_x, config.rotation_y, config.rotation_z, config.nullspace_damping);
513  }
514  }
515 
516  void CartesianImpedanceControllerRos::dynamicWrenchCb(cartesian_impedance_controller::wrenchConfig &config,
517  uint32_t level)
518  {
519  Eigen::Vector6d F{Eigen::Vector6d::Zero()};
520  if (config.apply_wrench)
521  {
522  F << config.f_x, config.f_y, config.f_z, config.tau_x, config.tau_y, config.tau_z;
523  if (!transformWrench(&F, this->wrench_ee_frame_, this->root_frame_))
524  {
525  ROS_ERROR("Could not transform wrench. Not applying it.");
526  return;
527  }
528  }
529  this->applyWrench(F);
530  }
531 
532  void CartesianImpedanceControllerRos::trajCb(const trajectory_msgs::JointTrajectoryConstPtr &msg)
533  {
534  ROS_INFO("Got trajectory msg from trajectory topic.");
535  if (this->traj_as_->isActive())
536  {
537  this->traj_as_->setPreempted();
538  ROS_INFO("Preempted running action server goal.");
539  }
540  trajStart(*msg);
541  }
542 
543  void CartesianImpedanceControllerRos::trajGoalCb()
544  {
545  this->traj_as_goal_ = this->traj_as_->acceptNewGoal();
546  ROS_INFO("Accepted new goal from action server.");
547  trajStart(this->traj_as_goal_->trajectory);
548  }
549 
550  void CartesianImpedanceControllerRos::trajPreemptCb()
551  {
552  ROS_INFO("Actionserver got preempted.");
553  this->traj_as_->setPreempted();
554  }
555 
556  void CartesianImpedanceControllerRos::trajStart(const trajectory_msgs::JointTrajectory &trajectory)
557  {
558  this->traj_duration_ = trajectory.points[trajectory.points.size() - 1].time_from_start;
559  ROS_INFO_STREAM("Starting a new trajectory with " << trajectory.points.size() << " points that takes " << this->traj_duration_ << "s.");
560  this->trajectory_ = trajectory;
561  this->traj_running_ = true;
562  this->traj_start_ = ros::Time::now();
563  this->traj_index_ = 0;
564  trajUpdate();
565  if (this->nullspace_stiffness_ < 5.)
566  {
567  ROS_WARN("Nullspace stiffness is low. The joints might not follow the planned path.");
568  }
569  }
570 
571  void CartesianImpedanceControllerRos::trajUpdate()
572  {
573  if (ros::Time::now() > (this->traj_start_ + trajectory_.points.at(this->traj_index_).time_from_start))
574  {
575  // Get end effector pose
576  Eigen::VectorXd q = Eigen::VectorXd::Map(trajectory_.points.at(this->traj_index_).positions.data(),
577  trajectory_.points.at(this->traj_index_).positions.size());
578  if (this->verbose_print_)
579  {
580  ROS_INFO_STREAM("Index " << this->traj_index_ << " q_nullspace: " << q.transpose());
581  }
582  // Update end-effector pose and nullspace
583  getFk(q, &this->position_d_target_, &this->orientation_d_target_);
584  this->setNullspaceConfig(q);
585  this->traj_index_++;
586  }
587 
588  if (ros::Time::now() > (this->traj_start_ + this->traj_duration_))
589  {
590  ROS_INFO_STREAM("Finished executing trajectory.");
591  if (this->traj_as_->isActive())
592  {
593  this->traj_as_->setSucceeded();
594  }
595  this->traj_running_ = false;
596  }
597  }
598 } // namespace cartesian_impedance_controller
cartesian_impedance_controller::CartesianImpedanceControllerRos::sub_cart_wrench_
ros::Subscriber sub_cart_wrench_
Cartesian wrench subscriber.
Definition: cartesian_impedance_controller_ros.h:291
cartesian_impedance_controller::CartesianImpedanceControllerRos::tf_rot_
tf::Quaternion tf_rot_
tf orientation for publishing
Definition: cartesian_impedance_controller_ros.h:342
cartesian_impedance_controller::CartesianImpedanceController::position_d_
Eigen::Vector3d position_d_
Current end-effector reference position.
Definition: cartesian_impedance_controller.h:221
cartesian_impedance_controller::CartesianImpedanceControllerRos::jacobian_perm_
const Eigen::PermutationMatrix< Eigen::Dynamic, 6 > jacobian_perm_
Permutation matrix to switch position and orientation entries.
Definition: cartesian_impedance_controller_ros.h:312
cartesian_impedance_controller::CartesianImpedanceControllerRos::tf_br_transform_
tf::Transform tf_br_transform_
tf transform for publishing
Definition: cartesian_impedance_controller_ros.h:340
cartesian_impedance_controller::CartesianImpedanceControllerRos::traj_start_
ros::Time traj_start_
Time the current trajectory is started.
Definition: cartesian_impedance_controller_ros.h:328
cartesian_impedance_controller::CartesianImpedanceController::jacobian_
Eigen::MatrixXd jacobian_
Jacobian. Row format: 3 translations, 3 rotation.
Definition: cartesian_impedance_controller.h:216
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::init
bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &node_handle) override
Initializes the controller.
Definition: cartesian_impedance_controller_ros.cpp:158
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::publishMsgsAndTf
void publishMsgsAndTf()
Verbose printing; publishes ROS messages and tf frames.
Definition: cartesian_impedance_controller_ros.cpp:415
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::setStiffness
void setStiffness(const geometry_msgs::Wrench &cart_stiffness, double nullspace, bool auto_damping=true)
Sets Cartesian and nullspace stiffness.
Definition: cartesian_impedance_controller_ros.cpp:358
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajUpdate
void trajUpdate()
Updates the trajectory.
Definition: cartesian_impedance_controller_ros.cpp:570
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::init
bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &node_handle) override
Initializes the controller.
Definition: cartesian_impedance_controller_ros.cpp:159
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::publishMsgsAndTf
void publishMsgsAndTf()
Verbose printing; publishes ROS messages and tf frames.
Definition: cartesian_impedance_controller_ros.cpp:416
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::setStiffness
void setStiffness(const geometry_msgs::Wrench &cart_stiffness, double nullspace, bool auto_damping=true)
Sets Cartesian and nullspace stiffness.
Definition: cartesian_impedance_controller_ros.cpp:359
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajUpdate
void trajUpdate()
Updates the trajectory.
Definition: cartesian_impedance_controller_ros.cpp:571
cartesian_impedance_controller::CartesianImpedanceControllerRos::initDynamicReconfigure
bool initDynamicReconfigure(const ros::NodeHandle &nh)
Initializes dynamic reconfigure.
Definition: cartesian_impedance_controller_ros.cpp:36
rbdyn_wrapper::perform_fk
EefState perform_fk(const Eigen::VectorXd &q) const
Definition: rbdyn_wrapper.h:63
rbdyn_wrapper::root_link
std::string root_link() const
Definition: rbdyn_wrapper.h:111
@@ -42,49 +42,49 @@
cartesian_impedance_controller::CartesianImpedanceControllerRos::traj_as_
std::unique_ptr< actionlib::SimpleActionServer< control_msgs::FollowJointTrajectoryAction > > traj_as_
Trajectory action server.
Definition: cartesian_impedance_controller_ros.h:325
rbdyn_wrapper::EefState::orientation
Eigen::Quaterniond orientation
Definition: rbdyn_wrapper.h:18
cartesian_impedance_controller::CartesianImpedanceControllerRos::initJointHandles
bool initJointHandles(hardware_interface::EffortJointInterface *hw, const ros::NodeHandle &nh)
Initializes the joint handles.
Definition: cartesian_impedance_controller_ros.cpp:52
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::controllerConfigCb
void controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg)
Message callback for the whole controller configuration.
Definition: cartesian_impedance_controller_ros.cpp:297
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::controllerConfigCb
void controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg)
Message callback for the whole controller configuration.
Definition: cartesian_impedance_controller_ros.cpp:298
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamic_server_damping_param_
std::unique_ptr< dynamic_reconfigure::Server< cartesian_impedance_controller::dampingConfig > > dynamic_server_damping_param_
Dynamic reconfigure server for damping.
Definition: cartesian_impedance_controller_ros.h:319
cartesian_impedance_controller::EigenVectorToWrench
void EigenVectorToWrench(const Eigen::Matrix< double, 6, 1 > &v, geometry_msgs::Wrench *wrench)
Populates a wrench msg with value from Eigen vector.
Definition: cartesian_impedance_controller_ros.cpp:26
cartesian_impedance_controller::CartesianImpedanceControllerRos::tf_pos_
tf::Vector3 tf_pos_
tf position for publishing
Definition: cartesian_impedance_controller_ros.h:341
cartesian_impedance_controller::CartesianImpedanceControllerRos::ns_min_
const double ns_min_
Minimum nullspace stiffness.
Definition: cartesian_impedance_controller_ros.h:304
cartesian_impedance_controller
Definition: cartesian_impedance_controller.h:6
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajPreemptCb
void trajPreemptCb()
Preempt function of the action server.
Definition: cartesian_impedance_controller_ros.cpp:549
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajPreemptCb
void trajPreemptCb()
Preempt function of the action server.
Definition: cartesian_impedance_controller_ros.cpp:550
cartesian_impedance_controller::CartesianImpedanceControllerRos::trans_stf_max_
const double trans_stf_max_
Maximum translational stiffness.
Definition: cartesian_impedance_controller_ros.h:301
cartesian_impedance_controller::CartesianImpedanceController::cartesian_stiffness_
Eigen::Matrix< double, 6, 6 > cartesian_stiffness_
Cartesian stiffness matrix.
Definition: cartesian_impedance_controller.h:199
cartesian_impedance_controller::CartesianImpedanceController::getAppliedWrench
Eigen::Matrix< double, 6, 1 > getAppliedWrench() const
Get the currently applied Cartesian wrench.
Definition: cartesian_impedance_controller.cpp:296
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::update
void update(const ros::Time &, const ros::Duration &period) override
Periodically called update function.
Definition: cartesian_impedance_controller_ros.cpp:224
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::update
void update(const ros::Time &, const ros::Duration &period) override
Periodically called update function.
Definition: cartesian_impedance_controller_ros.cpp:225
cartesian_impedance_controller::CartesianImpedanceController::setStiffness
void setStiffness(const Eigen::Matrix< double, 7, 1 > &stiffness, bool auto_damping=true)
Set the desired diagonal stiffnessess + nullspace stiffness.
Definition: cartesian_impedance_controller.cpp:115
cartesian_impedance_controller::CartesianImpedanceControllerRos::tf_last_time_
ros::Time tf_last_time_
Last published tf message.
Definition: cartesian_impedance_controller_ros.h:343
cartesian_impedance_controller::CartesianImpedanceControllerRos::trans_stf_min_
const double trans_stf_min_
Minimum translational stiffness.
Definition: cartesian_impedance_controller_ros.h:300
cartesian_impedance_controller::CartesianImpedanceControllerRos::verbose_tf_
bool verbose_tf_
Verbose tf pubishing enabled.
Definition: cartesian_impedance_controller_ros.h:336
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::wrenchCommandCb
void wrenchCommandCb(const geometry_msgs::WrenchStampedConstPtr &msg)
Message callback for Cartesian wrench messages.
Definition: cartesian_impedance_controller_ros.cpp:369
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::wrenchCommandCb
void wrenchCommandCb(const geometry_msgs::WrenchStampedConstPtr &msg)
Message callback for Cartesian wrench messages.
Definition: cartesian_impedance_controller_ros.cpp:370
cartesian_impedance_controller::CartesianImpedanceController::q_
Eigen::VectorXd q_
Joint positions.
Definition: cartesian_impedance_controller.h:213
cartesian_impedance_controller::CartesianImpedanceController::nullspace_damping_
double nullspace_damping_
Current nullspace damping.
Definition: cartesian_impedance_controller.h:206
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::getJacobian
bool getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, Eigen::MatrixXd *jacobian)
Get Jacobian from RBDyn.
Definition: cartesian_impedance_controller_ros.cpp:265
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamicWrenchCb
void dynamicWrenchCb(cartesian_impedance_controller::wrenchConfig &config, uint32_t level)
Callback for wrench dynamic reconfigure.
Definition: cartesian_impedance_controller_ros.cpp:515
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::referencePoseCb
void referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg)
Message callback for a Cartesian reference pose.
Definition: cartesian_impedance_controller_ros.cpp:322
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::getJacobian
bool getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, Eigen::MatrixXd *jacobian)
Get Jacobian from RBDyn.
Definition: cartesian_impedance_controller_ros.cpp:266
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamicWrenchCb
void dynamicWrenchCb(cartesian_impedance_controller::wrenchConfig &config, uint32_t level)
Callback for wrench dynamic reconfigure.
Definition: cartesian_impedance_controller_ros.cpp:516
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::referencePoseCb
void referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg)
Message callback for a Cartesian reference pose.
Definition: cartesian_impedance_controller_ros.cpp:323
cartesian_impedance_controller::CartesianImpedanceController::delta_tau_max_
double delta_tau_max_
Maximum allowed torque change per time step.
Definition: cartesian_impedance_controller.h:240
cartesian_impedance_controller::CartesianImpedanceControllerRos::robot_description_
std::string robot_description_
URDF of the robot.
Definition: cartesian_impedance_controller_ros.h:285
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::getFk
bool getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, Eigen::Quaterniond *rotation) const
Get forward kinematics solution.
Definition: cartesian_impedance_controller_ros.cpp:245
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::getFk
bool getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, Eigen::Quaterniond *rotation) const
Get forward kinematics solution.
Definition: cartesian_impedance_controller_ros.cpp:246
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamic_server_wrench_param_
std::unique_ptr< dynamic_reconfigure::Server< cartesian_impedance_controller::wrenchConfig > > dynamic_server_wrench_param_
Dynamic reconfigure server for commanded wrench.
Definition: cartesian_impedance_controller_ros.h:321
cartesian_impedance_controller::CartesianImpedanceController::q_d_nullspace_
Eigen::VectorXd q_d_nullspace_
Current nullspace reference pose.
Definition: cartesian_impedance_controller.h:202
cartesian_impedance_controller::CartesianImpedanceController::orientation_
Eigen::Quaterniond orientation_
Current end-effector orientation.
Definition: cartesian_impedance_controller.h:224
cartesian_impedance_controller::CartesianImpedanceController::applyWrench
void applyWrench(const Eigen::Matrix< double, 6, 1 > &cartesian_wrench)
Apply a virtual Cartesian wrench in the root frame (often "world")
Definition: cartesian_impedance_controller.cpp:210
cartesian_impedance_controller::CartesianImpedanceControllerRos::pub_state_
realtime_tools::RealtimePublisher< cartesian_impedance_controller::ControllerState > pub_state_
Realtime publisher for controller state.
Definition: cartesian_impedance_controller_ros.h:339
cartesian_impedance_controller::CartesianImpedanceController::setNullspaceConfig
void setNullspaceConfig(const Eigen::VectorXd &q_d_nullspace_target)
Sets a new nullspace joint configuration.
Definition: cartesian_impedance_controller.cpp:182
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajGoalCb
void trajGoalCb()
Callback for a trajectory action goal.
Definition: cartesian_impedance_controller_ros.cpp:542
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajGoalCb
void trajGoalCb()
Callback for a trajectory action goal.
Definition: cartesian_impedance_controller_ros.cpp:543
cartesian_impedance_controller::CartesianImpedanceController::cartesian_damping_
Eigen::Matrix< double, 6, 6 > cartesian_damping_
Cartesian damping matrix.
Definition: cartesian_impedance_controller.h:200
cartesian_impedance_controller::CartesianImpedanceController::update_frequency_
double update_frequency_
Update frequency in Hz.
Definition: cartesian_impedance_controller.h:234
cartesian_impedance_controller::CartesianImpedanceController::orientation_d_
Eigen::Quaterniond orientation_d_
Current end-effector target orientation.
Definition: cartesian_impedance_controller.h:225
cartesian_impedance_controller::CartesianImpedanceController::filter_params_wrench_
double filter_params_wrench_
Commanded wrench filtering.
Definition: cartesian_impedance_controller.h:238
cartesian_impedance_controller::CartesianImpedanceControllerRos::verbose_print_
bool verbose_print_
Verbose printing enabled.
Definition: cartesian_impedance_controller_ros.h:334
cartesian_impedance_controller::CartesianImpedanceControllerRos::sub_cart_stiffness_
ros::Subscriber sub_cart_stiffness_
Cartesian stiffness subscriber.
Definition: cartesian_impedance_controller_ros.h:290
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::setDampingFactors
void setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace)
Sets damping for Cartesian space and nullspace.
Definition: cartesian_impedance_controller_ros.cpp:347
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::setDampingFactors
void setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace)
Sets damping for Cartesian space and nullspace.
Definition: cartesian_impedance_controller_ros.cpp:348
cartesian_impedance_controller::CartesianImpedanceControllerRos::dmp_factor_min_
const double dmp_factor_min_
Minimum damping factor.
Definition: cartesian_impedance_controller_ros.h:306
cartesian_impedance_controller::CartesianImpedanceController::dq_
Eigen::VectorXd dq_
Joint velocities.
Definition: cartesian_impedance_controller.h:214
cartesian_impedance_controller::CartesianImpedanceControllerRos::sub_damping_factors_
ros::Subscriber sub_damping_factors_
Damping subscriber.
Definition: cartesian_impedance_controller_ros.h:292
cartesian_impedance_controller::CartesianImpedanceControllerRos::tf_listener_
tf::TransformListener tf_listener_
tf transformation listener
Definition: cartesian_impedance_controller_ros.h:296
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::updateState
void updateState()
Updates the state based on the joint handles.
Definition: cartesian_impedance_controller_ros.cpp:285
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::updateState
void updateState()
Updates the state based on the joint handles.
Definition: cartesian_impedance_controller_ros.cpp:286
cartesian_impedance_controller_ros.h
cartesian_impedance_controller::CartesianImpedanceController::position_d_target_
Eigen::Vector3d position_d_target_
End-effector target position.
Definition: cartesian_impedance_controller.h:222
cartesian_impedance_controller::CartesianImpedanceControllerRos::verbose_state_
bool verbose_state_
Verbose state messages enabled.
Definition: cartesian_impedance_controller_ros.h:335
@@ -102,14 +102,14 @@
rbdyn_wrapper::init_rbdyn
void init_rbdyn(const std::string &urdf_string, const std::string &end_effector)
Definition: rbdyn_wrapper.h:21
cartesian_impedance_controller::CartesianImpedanceController::filter_params_stiffness_
double filter_params_stiffness_
Cartesian stiffness filtering.
Definition: cartesian_impedance_controller.h:236
cartesian_impedance_controller::CartesianImpedanceController::n_joints_
size_t n_joints_
Number of joints to control.
Definition: cartesian_impedance_controller.h:197
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamicStiffnessCb
void dynamicStiffnessCb(cartesian_impedance_controller::stiffnessConfig &config, uint32_t level)
Callback for stiffness dynamic reconfigure.
Definition: cartesian_impedance_controller_ros.cpp:491
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamicStiffnessCb
void dynamicStiffnessCb(cartesian_impedance_controller::stiffnessConfig &config, uint32_t level)
Callback for stiffness dynamic reconfigure.
Definition: cartesian_impedance_controller_ros.cpp:492
cartesian_impedance_controller::CartesianImpedanceControllerRos::tf_br_
tf::TransformBroadcaster tf_br_
tf transform broadcaster for verbose tf
Definition: cartesian_impedance_controller_ros.h:337
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamicDampingCb
void dynamicDampingCb(cartesian_impedance_controller::dampingConfig &config, uint32_t level)
Callback for damping dynamic reconfigure.
Definition: cartesian_impedance_controller_ros.cpp:505
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajCb
void trajCb(const trajectory_msgs::JointTrajectoryConstPtr &msg)
Callback for a joint trajectory message.
Definition: cartesian_impedance_controller_ros.cpp:531
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamicDampingCb
void dynamicDampingCb(cartesian_impedance_controller::dampingConfig &config, uint32_t level)
Callback for damping dynamic reconfigure.
Definition: cartesian_impedance_controller_ros.cpp:506
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajCb
void trajCb(const trajectory_msgs::JointTrajectoryConstPtr &msg)
Callback for a joint trajectory message.
Definition: cartesian_impedance_controller_ros.cpp:532
cartesian_impedance_controller::CartesianImpedanceController::nullspace_stiffness_
double nullspace_stiffness_
Current nullspace stiffness.
Definition: cartesian_impedance_controller.h:204
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::initRBDyn
bool initRBDyn(const ros::NodeHandle &nh)
Initializes RBDyn.
Definition: cartesian_impedance_controller_ros.cpp:113
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::cartesianStiffnessCb
void cartesianStiffnessCb(const geometry_msgs::WrenchStampedConstPtr &msg)
Message callback for Cartesian stiffness.
Definition: cartesian_impedance_controller_ros.cpp:342
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajStart
void trajStart(const trajectory_msgs::JointTrajectory &trajectory)
Starts the trajectory.
Definition: cartesian_impedance_controller_ros.cpp:555
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::initRBDyn
bool initRBDyn(const ros::NodeHandle &nh)
Initializes RBDyn.
Definition: cartesian_impedance_controller_ros.cpp:114
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::cartesianStiffnessCb
void cartesianStiffnessCb(const geometry_msgs::WrenchStampedConstPtr &msg)
Message callback for Cartesian stiffness.
Definition: cartesian_impedance_controller_ros.cpp:343
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajStart
void trajStart(const trajectory_msgs::JointTrajectory &trajectory)
Starts the trajectory.
Definition: cartesian_impedance_controller_ros.cpp:556
cartesian_impedance_controller::CartesianImpedanceControllerRos::pub_torques_
realtime_tools::RealtimePublisher< std_msgs::Float64MultiArray > pub_torques_
Realtime publisher for commanded torques.
Definition: cartesian_impedance_controller_ros.h:338
cartesian_impedance_controller::CartesianImpedanceControllerRos::root_frame_
std::string root_frame_
Base frame obtained from URDF.
Definition: cartesian_impedance_controller_ros.h:286
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajectory_
trajectory_msgs::JointTrajectory trajectory_
Currently played trajectory.
Definition: cartesian_impedance_controller_ros.h:327
@@ -121,13 +121,13 @@
cartesian_impedance_controller::CartesianImpedanceController::filter_params_nullspace_config_
double filter_params_nullspace_config_
Nullspace filtering.
Definition: cartesian_impedance_controller.h:235
cartesian_impedance_controller::CartesianImpedanceControllerRos::ns_max_
const double ns_max_
Maximum nullspace stiffness.
Definition: cartesian_impedance_controller_ros.h:305
cartesian_impedance_controller::CartesianImpedanceControllerRos::wrench_ee_frame_
std::string wrench_ee_frame_
Frame for the application of the commanded wrench.
Definition: cartesian_impedance_controller_ros.h:297
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::initTrajectories
bool initTrajectories(ros::NodeHandle *nh)
Initializes trajectory handling.
Definition: cartesian_impedance_controller_ros.cpp:146
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::initTrajectories
bool initTrajectories(ros::NodeHandle *nh)
Initializes trajectory handling.
Definition: cartesian_impedance_controller_ros.cpp:147
cartesian_impedance_controller::CartesianImpedanceController::tau_c_
Eigen::VectorXd tau_c_
Last commanded torques.
Definition: cartesian_impedance_controller.h:232
cartesian_impedance_controller::CartesianImpedanceController::orientation_d_target_
Eigen::Quaterniond orientation_d_target_
End-effector orientation target.
Definition: cartesian_impedance_controller.h:226
cartesian_impedance_controller::CartesianImpedanceController::setDampingFactors
void setDampingFactors(double d_x, double d_y, double d_z, double d_a, double d_b, double d_c, double d_n)
Set the desired damping factors.
Definition: cartesian_impedance_controller.cpp:146
cartesian_impedance_controller::CartesianImpedanceControllerRos::traj_duration_
ros::Duration traj_duration_
Duration of the current trajectory.
Definition: cartesian_impedance_controller_ros.h:329
cartesian_impedance_controller::CartesianImpedanceControllerRos::tau_m_
Eigen::VectorXd tau_m_
Measured joint torques.
Definition: cartesian_impedance_controller_ros.h:288
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::starting
void starting(const ros::Time &) override
Starts the controller.
Definition: cartesian_impedance_controller_ros.cpp:214
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::starting
void starting(const ros::Time &) override
Starts the controller.
Definition: cartesian_impedance_controller_ros.cpp:215
cartesian_impedance_controller::CartesianImpedanceController::damping_factors_
Eigen::Matrix< double, 7, 1 > damping_factors_
Damping factors.
Definition: cartesian_impedance_controller.h:211
cartesian_impedance_controller::CartesianImpedanceControllerRos::dmp_factor_max_
const double dmp_factor_max_
Maximum damping factor.
Definition: cartesian_impedance_controller_ros.h:307
cartesian_impedance_controller::CartesianImpedanceController::filter_params_pose_
double filter_params_pose_
Reference pose filtering.
Definition: cartesian_impedance_controller.h:237
@@ -135,11 +135,11 @@
cartesian_impedance_controller::CartesianImpedanceControllerRos::sub_trajectory_
ros::Subscriber sub_trajectory_
Subscriber for a single trajectory.
Definition: cartesian_impedance_controller_ros.h:324
cartesian_impedance_controller::CartesianImpedanceControllerRos::traj_running_
bool traj_running_
True when running a trajectory.
Definition: cartesian_impedance_controller_ros.h:331
cartesian_impedance_controller::CartesianImpedanceController::position_
Eigen::Vector3d position_
Current end-effector position.
Definition: cartesian_impedance_controller.h:220
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::transformWrench
bool transformWrench(Eigen::Matrix< double, 6, 1 > *cartesian_wrench, const std::string &from_frame, const std::string &to_frame) const
Transforms the wrench in a target frame.
Definition: cartesian_impedance_controller_ros.cpp:394
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::transformWrench
bool transformWrench(Eigen::Matrix< double, 6, 1 > *cartesian_wrench, const std::string &from_frame, const std::string &to_frame) const
Transforms the wrench in a target frame.
Definition: cartesian_impedance_controller_ros.cpp:395
cartesian_impedance_controller::CartesianImpedanceController::setNumberOfJoints
void setNumberOfJoints(size_t n_joints)
Sets the number of joints.
Definition: cartesian_impedance_controller.cpp:104
rbdyn_wrapper::EefState::translation
Eigen::Vector3d translation
Definition: rbdyn_wrapper.h:17
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::cartesianDampingFactorCb
void cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg)
Message callback for Cartesian damping.
Definition: cartesian_impedance_controller_ros.cpp:317
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::initMessaging
bool initMessaging(ros::NodeHandle *nh)
Initializes messaging.
Definition: cartesian_impedance_controller_ros.cpp:76
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::cartesianDampingFactorCb
void cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg)
Message callback for Cartesian damping.
Definition: cartesian_impedance_controller_ros.cpp:318
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::initMessaging
bool initMessaging(ros::NodeHandle *nh)
Initializes messaging.
Definition: cartesian_impedance_controller_ros.cpp:77
cartesian_impedance_controller::CartesianImpedanceControllerRos::traj_index_
unsigned int traj_index_
Index of the current trajectory point.
Definition: cartesian_impedance_controller_ros.h:330
@@ -147,7 +147,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/cartesian__impedance__controller__ros_8h.html b/cartesian__impedance__controller__ros_8h.html index af5607b..07708bd 100644 --- a/cartesian__impedance__controller__ros_8h.html +++ b/cartesian__impedance__controller__ros_8h.html @@ -97,7 +97,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/cartesian__impedance__controller__ros_8h_source.html b/cartesian__impedance__controller__ros_8h_source.html index dd9bb90..7d5b236 100644 --- a/cartesian__impedance__controller__ros_8h_source.html +++ b/cartesian__impedance__controller__ros_8h_source.html @@ -29,43 +29,43 @@
cartesian_impedance_controller::CartesianImpedanceControllerRos::jacobian_perm_
const Eigen::PermutationMatrix< Eigen::Dynamic, 6 > jacobian_perm_
Permutation matrix to switch position and orientation entries.
Definition: cartesian_impedance_controller_ros.h:312
cartesian_impedance_controller::CartesianImpedanceControllerRos::tf_br_transform_
tf::Transform tf_br_transform_
tf transform for publishing
Definition: cartesian_impedance_controller_ros.h:340
cartesian_impedance_controller::CartesianImpedanceControllerRos::traj_start_
ros::Time traj_start_
Time the current trajectory is started.
Definition: cartesian_impedance_controller_ros.h:328
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::init
bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &node_handle) override
Initializes the controller.
Definition: cartesian_impedance_controller_ros.cpp:158
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::publishMsgsAndTf
void publishMsgsAndTf()
Verbose printing; publishes ROS messages and tf frames.
Definition: cartesian_impedance_controller_ros.cpp:415
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::setStiffness
void setStiffness(const geometry_msgs::Wrench &cart_stiffness, double nullspace, bool auto_damping=true)
Sets Cartesian and nullspace stiffness.
Definition: cartesian_impedance_controller_ros.cpp:358
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajUpdate
void trajUpdate()
Updates the trajectory.
Definition: cartesian_impedance_controller_ros.cpp:570
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::init
bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &node_handle) override
Initializes the controller.
Definition: cartesian_impedance_controller_ros.cpp:159
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::publishMsgsAndTf
void publishMsgsAndTf()
Verbose printing; publishes ROS messages and tf frames.
Definition: cartesian_impedance_controller_ros.cpp:416
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::setStiffness
void setStiffness(const geometry_msgs::Wrench &cart_stiffness, double nullspace, bool auto_damping=true)
Sets Cartesian and nullspace stiffness.
Definition: cartesian_impedance_controller_ros.cpp:359
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajUpdate
void trajUpdate()
Updates the trajectory.
Definition: cartesian_impedance_controller_ros.cpp:571
cartesian_impedance_controller::CartesianImpedanceControllerRos::initDynamicReconfigure
bool initDynamicReconfigure(const ros::NodeHandle &nh)
Initializes dynamic reconfigure.
Definition: cartesian_impedance_controller_ros.cpp:36
cartesian_impedance_controller::CartesianImpedanceControllerRos::traj_as_
std::unique_ptr< actionlib::SimpleActionServer< control_msgs::FollowJointTrajectoryAction > > traj_as_
Trajectory action server.
Definition: cartesian_impedance_controller_ros.h:325
cartesian_impedance_controller::CartesianImpedanceControllerRos::initJointHandles
bool initJointHandles(hardware_interface::EffortJointInterface *hw, const ros::NodeHandle &nh)
Initializes the joint handles.
Definition: cartesian_impedance_controller_ros.cpp:52
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::controllerConfigCb
void controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg)
Message callback for the whole controller configuration.
Definition: cartesian_impedance_controller_ros.cpp:297
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::controllerConfigCb
void controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg)
Message callback for the whole controller configuration.
Definition: cartesian_impedance_controller_ros.cpp:298
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamic_server_damping_param_
std::unique_ptr< dynamic_reconfigure::Server< cartesian_impedance_controller::dampingConfig > > dynamic_server_damping_param_
Dynamic reconfigure server for damping.
Definition: cartesian_impedance_controller_ros.h:319
cartesian_impedance_controller::CartesianImpedanceControllerRos::tf_pos_
tf::Vector3 tf_pos_
tf position for publishing
Definition: cartesian_impedance_controller_ros.h:341
cartesian_impedance_controller::CartesianImpedanceControllerRos::ns_min_
const double ns_min_
Minimum nullspace stiffness.
Definition: cartesian_impedance_controller_ros.h:304
cartesian_impedance_controller
Definition: cartesian_impedance_controller.h:6
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajPreemptCb
void trajPreemptCb()
Preempt function of the action server.
Definition: cartesian_impedance_controller_ros.cpp:549
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajPreemptCb
void trajPreemptCb()
Preempt function of the action server.
Definition: cartesian_impedance_controller_ros.cpp:550
cartesian_impedance_controller::CartesianImpedanceControllerRos::trans_stf_max_
const double trans_stf_max_
Maximum translational stiffness.
Definition: cartesian_impedance_controller_ros.h:301
rbdyn_wrapper
Definition: rbdyn_wrapper.h:12
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::update
void update(const ros::Time &, const ros::Duration &period) override
Periodically called update function.
Definition: cartesian_impedance_controller_ros.cpp:224
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::update
void update(const ros::Time &, const ros::Duration &period) override
Periodically called update function.
Definition: cartesian_impedance_controller_ros.cpp:225
cartesian_impedance_controller::CartesianImpedanceControllerRos::tf_last_time_
ros::Time tf_last_time_
Last published tf message.
Definition: cartesian_impedance_controller_ros.h:343
cartesian_impedance_controller::CartesianImpedanceControllerRos::trans_stf_min_
const double trans_stf_min_
Minimum translational stiffness.
Definition: cartesian_impedance_controller_ros.h:300
cartesian_impedance_controller::CartesianImpedanceControllerRos::verbose_tf_
bool verbose_tf_
Verbose tf pubishing enabled.
Definition: cartesian_impedance_controller_ros.h:336
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::wrenchCommandCb
void wrenchCommandCb(const geometry_msgs::WrenchStampedConstPtr &msg)
Message callback for Cartesian wrench messages.
Definition: cartesian_impedance_controller_ros.cpp:369
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::wrenchCommandCb
void wrenchCommandCb(const geometry_msgs::WrenchStampedConstPtr &msg)
Message callback for Cartesian wrench messages.
Definition: cartesian_impedance_controller_ros.cpp:370
cartesian_impedance_controller.h
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::getJacobian
bool getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, Eigen::MatrixXd *jacobian)
Get Jacobian from RBDyn.
Definition: cartesian_impedance_controller_ros.cpp:265
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamicWrenchCb
void dynamicWrenchCb(cartesian_impedance_controller::wrenchConfig &config, uint32_t level)
Callback for wrench dynamic reconfigure.
Definition: cartesian_impedance_controller_ros.cpp:515
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::referencePoseCb
void referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg)
Message callback for a Cartesian reference pose.
Definition: cartesian_impedance_controller_ros.cpp:322
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::getJacobian
bool getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, Eigen::MatrixXd *jacobian)
Get Jacobian from RBDyn.
Definition: cartesian_impedance_controller_ros.cpp:266
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamicWrenchCb
void dynamicWrenchCb(cartesian_impedance_controller::wrenchConfig &config, uint32_t level)
Callback for wrench dynamic reconfigure.
Definition: cartesian_impedance_controller_ros.cpp:516
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::referencePoseCb
void referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg)
Message callback for a Cartesian reference pose.
Definition: cartesian_impedance_controller_ros.cpp:323
cartesian_impedance_controller::CartesianImpedanceControllerRos::robot_description_
std::string robot_description_
URDF of the robot.
Definition: cartesian_impedance_controller_ros.h:285
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::getFk
bool getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, Eigen::Quaterniond *rotation) const
Get forward kinematics solution.
Definition: cartesian_impedance_controller_ros.cpp:245
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::getFk
bool getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, Eigen::Quaterniond *rotation) const
Get forward kinematics solution.
Definition: cartesian_impedance_controller_ros.cpp:246
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamic_server_wrench_param_
std::unique_ptr< dynamic_reconfigure::Server< cartesian_impedance_controller::wrenchConfig > > dynamic_server_wrench_param_
Dynamic reconfigure server for commanded wrench.
Definition: cartesian_impedance_controller_ros.h:321
cartesian_impedance_controller::CartesianImpedanceControllerRos::pub_state_
realtime_tools::RealtimePublisher< cartesian_impedance_controller::ControllerState > pub_state_
Realtime publisher for controller state.
Definition: cartesian_impedance_controller_ros.h:339
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajGoalCb
void trajGoalCb()
Callback for a trajectory action goal.
Definition: cartesian_impedance_controller_ros.cpp:542
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajGoalCb
void trajGoalCb()
Callback for a trajectory action goal.
Definition: cartesian_impedance_controller_ros.cpp:543
cartesian_impedance_controller::CartesianImpedanceControllerRos::verbose_print_
bool verbose_print_
Verbose printing enabled.
Definition: cartesian_impedance_controller_ros.h:334
rbdyn_wrapper.h
cartesian_impedance_controller::CartesianImpedanceControllerRos::sub_cart_stiffness_
ros::Subscriber sub_cart_stiffness_
Cartesian stiffness subscriber.
Definition: cartesian_impedance_controller_ros.h:290
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::setDampingFactors
void setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace)
Sets damping for Cartesian space and nullspace.
Definition: cartesian_impedance_controller_ros.cpp:347
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::setDampingFactors
void setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace)
Sets damping for Cartesian space and nullspace.
Definition: cartesian_impedance_controller_ros.cpp:348
cartesian_impedance_controller::CartesianImpedanceControllerRos::dmp_factor_min_
const double dmp_factor_min_
Minimum damping factor.
Definition: cartesian_impedance_controller_ros.h:306
cartesian_impedance_controller::CartesianImpedanceControllerRos::sub_damping_factors_
ros::Subscriber sub_damping_factors_
Damping subscriber.
Definition: cartesian_impedance_controller_ros.h:292
cartesian_impedance_controller::CartesianImpedanceControllerRos::tf_listener_
tf::TransformListener tf_listener_
tf transformation listener
Definition: cartesian_impedance_controller_ros.h:296
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::updateState
void updateState()
Updates the state based on the joint handles.
Definition: cartesian_impedance_controller_ros.cpp:285
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::updateState
void updateState()
Updates the state based on the joint handles.
Definition: cartesian_impedance_controller_ros.cpp:286
cartesian_impedance_controller::CartesianImpedanceControllerRos::verbose_state_
bool verbose_state_
Verbose state messages enabled.
Definition: cartesian_impedance_controller_ros.h:335
cartesian_impedance_controller::CartesianImpedanceControllerRos::rot_stf_min_
const double rot_stf_min_
Minimum rotational stiffness.
Definition: cartesian_impedance_controller_ros.h:302
cartesian_impedance_controller::CartesianImpedanceControllerRos::traj_as_goal_
boost::shared_ptr< const control_msgs::FollowJointTrajectoryGoal > traj_as_goal_
Trajectory action server goal.
Definition: cartesian_impedance_controller_ros.h:326
@@ -74,13 +74,13 @@
cartesian_impedance_controller::CartesianImpedanceControllerRos::rot_stf_max_
const double rot_stf_max_
Maximum rotational stiffness.
Definition: cartesian_impedance_controller_ros.h:303
cartesian_impedance_controller::CartesianImpedanceControllerRos::sub_controller_config_
ros::Subscriber sub_controller_config_
Controller configuration subscriber.
Definition: cartesian_impedance_controller_ros.h:293
cartesian_impedance_controller::CartesianImpedanceControllerRos::perm_indices_
const Eigen::VectorXi perm_indices_
Permutation indices to switch position and orientation.
Definition: cartesian_impedance_controller_ros.h:310
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamicStiffnessCb
void dynamicStiffnessCb(cartesian_impedance_controller::stiffnessConfig &config, uint32_t level)
Callback for stiffness dynamic reconfigure.
Definition: cartesian_impedance_controller_ros.cpp:491
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamicStiffnessCb
void dynamicStiffnessCb(cartesian_impedance_controller::stiffnessConfig &config, uint32_t level)
Callback for stiffness dynamic reconfigure.
Definition: cartesian_impedance_controller_ros.cpp:492
cartesian_impedance_controller::CartesianImpedanceControllerRos::tf_br_
tf::TransformBroadcaster tf_br_
tf transform broadcaster for verbose tf
Definition: cartesian_impedance_controller_ros.h:337
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamicDampingCb
void dynamicDampingCb(cartesian_impedance_controller::dampingConfig &config, uint32_t level)
Callback for damping dynamic reconfigure.
Definition: cartesian_impedance_controller_ros.cpp:505
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajCb
void trajCb(const trajectory_msgs::JointTrajectoryConstPtr &msg)
Callback for a joint trajectory message.
Definition: cartesian_impedance_controller_ros.cpp:531
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::initRBDyn
bool initRBDyn(const ros::NodeHandle &nh)
Initializes RBDyn.
Definition: cartesian_impedance_controller_ros.cpp:113
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::cartesianStiffnessCb
void cartesianStiffnessCb(const geometry_msgs::WrenchStampedConstPtr &msg)
Message callback for Cartesian stiffness.
Definition: cartesian_impedance_controller_ros.cpp:342
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajStart
void trajStart(const trajectory_msgs::JointTrajectory &trajectory)
Starts the trajectory.
Definition: cartesian_impedance_controller_ros.cpp:555
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamicDampingCb
void dynamicDampingCb(cartesian_impedance_controller::dampingConfig &config, uint32_t level)
Callback for damping dynamic reconfigure.
Definition: cartesian_impedance_controller_ros.cpp:506
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajCb
void trajCb(const trajectory_msgs::JointTrajectoryConstPtr &msg)
Callback for a joint trajectory message.
Definition: cartesian_impedance_controller_ros.cpp:532
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::initRBDyn
bool initRBDyn(const ros::NodeHandle &nh)
Initializes RBDyn.
Definition: cartesian_impedance_controller_ros.cpp:114
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::cartesianStiffnessCb
void cartesianStiffnessCb(const geometry_msgs::WrenchStampedConstPtr &msg)
Message callback for Cartesian stiffness.
Definition: cartesian_impedance_controller_ros.cpp:343
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajStart
void trajStart(const trajectory_msgs::JointTrajectory &trajectory)
Starts the trajectory.
Definition: cartesian_impedance_controller_ros.cpp:556
cartesian_impedance_controller::CartesianImpedanceControllerRos::pub_torques_
realtime_tools::RealtimePublisher< std_msgs::Float64MultiArray > pub_torques_
Realtime publisher for commanded torques.
Definition: cartesian_impedance_controller_ros.h:338
cartesian_impedance_controller::CartesianImpedanceControllerRos::root_frame_
std::string root_frame_
Base frame obtained from URDF.
Definition: cartesian_impedance_controller_ros.h:286
cartesian_impedance_controller::CartesianImpedanceControllerRos::trajectory_
trajectory_msgs::JointTrajectory trajectory_
Currently played trajectory.
Definition: cartesian_impedance_controller_ros.h:327
@@ -91,17 +91,17 @@
cartesian_impedance_controller::CartesianImpedanceControllerRos::dynamic_server_compliance_param_
std::unique_ptr< dynamic_reconfigure::Server< cartesian_impedance_controller::stiffnessConfig > > dynamic_server_compliance_param_
Dybanic reconfigure server for stiffness.
Definition: cartesian_impedance_controller_ros.h:317
cartesian_impedance_controller::CartesianImpedanceControllerRos::ns_max_
const double ns_max_
Maximum nullspace stiffness.
Definition: cartesian_impedance_controller_ros.h:305
cartesian_impedance_controller::CartesianImpedanceControllerRos::wrench_ee_frame_
std::string wrench_ee_frame_
Frame for the application of the commanded wrench.
Definition: cartesian_impedance_controller_ros.h:297
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::initTrajectories
bool initTrajectories(ros::NodeHandle *nh)
Initializes trajectory handling.
Definition: cartesian_impedance_controller_ros.cpp:146
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::initTrajectories
bool initTrajectories(ros::NodeHandle *nh)
Initializes trajectory handling.
Definition: cartesian_impedance_controller_ros.cpp:147
cartesian_impedance_controller::CartesianImpedanceControllerRos::traj_duration_
ros::Duration traj_duration_
Duration of the current trajectory.
Definition: cartesian_impedance_controller_ros.h:329
cartesian_impedance_controller::CartesianImpedanceControllerRos::tau_m_
Eigen::VectorXd tau_m_
Measured joint torques.
Definition: cartesian_impedance_controller_ros.h:288
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::starting
void starting(const ros::Time &) override
Starts the controller.
Definition: cartesian_impedance_controller_ros.cpp:214
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::starting
void starting(const ros::Time &) override
Starts the controller.
Definition: cartesian_impedance_controller_ros.cpp:215
cartesian_impedance_controller::PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(cartesian_impedance_controller::CartesianImpedanceControllerRos, controller_interface::ControllerBase)
cartesian_impedance_controller::CartesianImpedanceControllerRos::dmp_factor_max_
const double dmp_factor_max_
Maximum damping factor.
Definition: cartesian_impedance_controller_ros.h:307
cartesian_impedance_controller::CartesianImpedanceControllerRos::sub_trajectory_
ros::Subscriber sub_trajectory_
Subscriber for a single trajectory.
Definition: cartesian_impedance_controller_ros.h:324
cartesian_impedance_controller::CartesianImpedanceControllerRos::traj_running_
bool traj_running_
True when running a trajectory.
Definition: cartesian_impedance_controller_ros.h:331
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::transformWrench
bool transformWrench(Eigen::Matrix< double, 6, 1 > *cartesian_wrench, const std::string &from_frame, const std::string &to_frame) const
Transforms the wrench in a target frame.
Definition: cartesian_impedance_controller_ros.cpp:394
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::cartesianDampingFactorCb
void cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg)
Message callback for Cartesian damping.
Definition: cartesian_impedance_controller_ros.cpp:317
-
cartesian_impedance_controller::CartesianImpedanceControllerRos::initMessaging
bool initMessaging(ros::NodeHandle *nh)
Initializes messaging.
Definition: cartesian_impedance_controller_ros.cpp:76
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::transformWrench
bool transformWrench(Eigen::Matrix< double, 6, 1 > *cartesian_wrench, const std::string &from_frame, const std::string &to_frame) const
Transforms the wrench in a target frame.
Definition: cartesian_impedance_controller_ros.cpp:395
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::cartesianDampingFactorCb
void cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg)
Message callback for Cartesian damping.
Definition: cartesian_impedance_controller_ros.cpp:318
+
cartesian_impedance_controller::CartesianImpedanceControllerRos::initMessaging
bool initMessaging(ros::NodeHandle *nh)
Initializes messaging.
Definition: cartesian_impedance_controller_ros.cpp:77
cartesian_impedance_controller::CartesianImpedanceControllerRos::traj_index_
unsigned int traj_index_
Index of the current trajectory point.
Definition: cartesian_impedance_controller_ros.h:330
@@ -109,7 +109,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/classcartesian__impedance__controller_1_1CartesianImpedanceController-members.html b/classcartesian__impedance__controller_1_1CartesianImpedanceController-members.html index 6529d06..b90f67f 100644 --- a/classcartesian__impedance__controller_1_1CartesianImpedanceController-members.html +++ b/classcartesian__impedance__controller_1_1CartesianImpedanceController-members.html @@ -94,7 +94,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/classcartesian__impedance__controller_1_1CartesianImpedanceController.html b/classcartesian__impedance__controller_1_1CartesianImpedanceController.html index db0eb87..1296673 100644 --- a/classcartesian__impedance__controller_1_1CartesianImpedanceController.html +++ b/classcartesian__impedance__controller_1_1CartesianImpedanceController.html @@ -2255,7 +2255,7 @@

workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/classcartesian__impedance__controller_1_1CartesianImpedanceControllerRos-members.html b/classcartesian__impedance__controller_1_1CartesianImpedanceControllerRos-members.html index cf48fb5..de78827 100644 --- a/classcartesian__impedance__controller_1_1CartesianImpedanceControllerRos-members.html +++ b/classcartesian__impedance__controller_1_1CartesianImpedanceControllerRos-members.html @@ -158,7 +158,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/classcartesian__impedance__controller_1_1CartesianImpedanceControllerRos.html b/classcartesian__impedance__controller_1_1CartesianImpedanceControllerRos.html index ee73051..af341f2 100644 --- a/classcartesian__impedance__controller_1_1CartesianImpedanceControllerRos.html +++ b/classcartesian__impedance__controller_1_1CartesianImpedanceControllerRos.html @@ -469,7 +469,7 @@

Definition at line 317 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 318 of file cartesian_impedance_controller_ros.cpp.

@@ -506,7 +506,7 @@

Definition at line 342 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 343 of file cartesian_impedance_controller_ros.cpp.

@@ -543,7 +543,7 @@

Definition at line 297 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 298 of file cartesian_impedance_controller_ros.cpp.

@@ -589,7 +589,7 @@

Definition at line 505 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 506 of file cartesian_impedance_controller_ros.cpp.

@@ -635,7 +635,7 @@

Definition at line 491 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 492 of file cartesian_impedance_controller_ros.cpp.

@@ -681,7 +681,7 @@

Definition at line 515 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 516 of file cartesian_impedance_controller_ros.cpp.

@@ -736,7 +736,7 @@

Returns
Always true.
-

Definition at line 245 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 246 of file cartesian_impedance_controller_ros.cpp.

@@ -791,7 +791,7 @@

Returns
True on success, false on failure.
-

Definition at line 265 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 266 of file cartesian_impedance_controller_ros.cpp.

@@ -850,7 +850,7 @@

Definition at line 158 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 159 of file cartesian_impedance_controller_ros.cpp.

@@ -972,7 +972,7 @@

Returns
True on success, false on failure.
-

Definition at line 76 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 77 of file cartesian_impedance_controller_ros.cpp.

@@ -1009,7 +1009,7 @@

Returns
True on success, false on failure.
-

Definition at line 113 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 114 of file cartesian_impedance_controller_ros.cpp.

@@ -1046,7 +1046,7 @@

Returns
Always true.
-

Definition at line 146 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 147 of file cartesian_impedance_controller_ros.cpp.

@@ -1076,7 +1076,7 @@

Definition at line 415 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 416 of file cartesian_impedance_controller_ros.cpp.

@@ -1113,7 +1113,7 @@

Definition at line 322 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 323 of file cartesian_impedance_controller_ros.cpp.

@@ -1160,7 +1160,7 @@

Definition at line 347 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 348 of file cartesian_impedance_controller_ros.cpp.

@@ -1214,7 +1214,7 @@

Definition at line 358 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 359 of file cartesian_impedance_controller_ros.cpp.

@@ -1250,7 +1250,7 @@

Definition at line 214 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 215 of file cartesian_impedance_controller_ros.cpp.

@@ -1286,7 +1286,7 @@

Definition at line 531 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 532 of file cartesian_impedance_controller_ros.cpp.

@@ -1316,7 +1316,7 @@

Definition at line 542 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 543 of file cartesian_impedance_controller_ros.cpp.

@@ -1346,7 +1346,7 @@

Definition at line 549 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 550 of file cartesian_impedance_controller_ros.cpp.

@@ -1377,7 +1377,7 @@

Definition at line 555 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 556 of file cartesian_impedance_controller_ros.cpp.

@@ -1407,7 +1407,7 @@

Definition at line 570 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 571 of file cartesian_impedance_controller_ros.cpp.

@@ -1464,7 +1464,7 @@

Returns
True on success, false on failure.
-

Definition at line 394 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 395 of file cartesian_impedance_controller_ros.cpp.

@@ -1511,7 +1511,7 @@

Definition at line 224 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 225 of file cartesian_impedance_controller_ros.cpp.

@@ -1541,7 +1541,7 @@

Definition at line 285 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 286 of file cartesian_impedance_controller_ros.cpp.

@@ -1578,7 +1578,7 @@

Definition at line 369 of file cartesian_impedance_controller_ros.cpp.

+

Definition at line 370 of file cartesian_impedance_controller_ros.cpp.

@@ -2737,7 +2737,7 @@

workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/classes.html b/classes.html index ec4d6e7..b0d4b64 100644 --- a/classes.html +++ b/classes.html @@ -38,7 +38,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/classrbdyn__wrapper-members.html b/classrbdyn__wrapper-members.html index 4f49546..57f0075 100644 --- a/classrbdyn__wrapper-members.html +++ b/classrbdyn__wrapper-members.html @@ -39,7 +39,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/classrbdyn__wrapper.html b/classrbdyn__wrapper.html index 43e5564..1ea1e07 100644 --- a/classrbdyn__wrapper.html +++ b/classrbdyn__wrapper.html @@ -379,7 +379,7 @@

workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/dir_000000_000001.html b/dir_000000_000001.html index 6a0fa1d..6c50b96 100644 --- a/dir_000000_000001.html +++ b/dir_000000_000001.html @@ -26,7 +26,7 @@

src → include Relation

workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/dir_594d4c40386ea469b7c9e60075edfcd9.html b/dir_594d4c40386ea469b7c9e60075edfcd9.html index a43dd5b..b567afb 100644 --- a/dir_594d4c40386ea469b7c9e60075edfcd9.html +++ b/dir_594d4c40386ea469b7c9e60075edfcd9.html @@ -40,7 +40,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/dir_68267d1309a1af8e8297ef4c3efbcdba.html b/dir_68267d1309a1af8e8297ef4c3efbcdba.html index 923d44b..90e517e 100644 --- a/dir_68267d1309a1af8e8297ef4c3efbcdba.html +++ b/dir_68267d1309a1af8e8297ef4c3efbcdba.html @@ -50,7 +50,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/dir_d44c64559bbebec7f509842c48db8b23.html b/dir_d44c64559bbebec7f509842c48db8b23.html index 6b53d0f..adc2ccd 100644 --- a/dir_d44c64559bbebec7f509842c48db8b23.html +++ b/dir_d44c64559bbebec7f509842c48db8b23.html @@ -36,7 +36,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/files.html b/files.html index 638e50b..5fe1fde 100644 --- a/files.html +++ b/files.html @@ -36,7 +36,7 @@
workspace
Author(s): Matthias Mayr, Oussama Chouman
-autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
diff --git a/functions.html b/functions.html index 2f1ed62..b8c3353 100644 --- a/functions.html +++ b/functions.html @@ -518,7 +518,7 @@

- ~ -

    workspace
    Author(s): Matthias Mayr, Oussama Chouman
    -autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
    diff --git a/functions_func.html b/functions_func.html index 62e49dc..1030c02 100644 --- a/functions_func.html +++ b/functions_func.html @@ -258,7 +258,7 @@

    - ~ -

      workspace
      Author(s): Matthias Mayr, Oussama Chouman
      -autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
      diff --git a/functions_vars.html b/functions_vars.html index bee606a..2f52ed1 100644 --- a/functions_vars.html +++ b/functions_vars.html @@ -326,7 +326,7 @@

      - w -

        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/globals.html b/globals.html index e07a1a3..220091d 100644 --- a/globals.html +++ b/globals.html @@ -27,7 +27,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/globals_func.html b/globals_func.html index 816fe66..6ab3095 100644 --- a/globals_func.html +++ b/globals_func.html @@ -27,7 +27,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/graph_legend.html b/graph_legend.html index efa9805..cc77596 100644 --- a/graph_legend.html +++ b/graph_legend.html @@ -55,7 +55,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/hierarchy.html b/hierarchy.html index 63f48dd..388f819 100644 --- a/hierarchy.html +++ b/hierarchy.html @@ -38,7 +38,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/index-msg.html b/index-msg.html index 78b777d..7e8f790 100644 --- a/index-msg.html +++ b/index-msg.html @@ -31,7 +31,7 @@

        Message types

        -

        autogenerated on Thu, 26 Oct 2023 17:45:15

        +

        autogenerated on Tue, 31 Oct 2023 14:37:32

        diff --git a/index.html b/index.html index 7830795..a48aaf9 100644 --- a/index.html +++ b/index.html @@ -131,7 +131,7 @@

        Controller crashes

        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/inherits.html b/inherits.html index 9b638b3..5348700 100644 --- a/inherits.html +++ b/inherits.html @@ -46,7 +46,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/md_res_paper.html b/md_res_paper.html index c263515..8d93667 100644 --- a/md_res_paper.html +++ b/md_res_paper.html @@ -84,7 +84,7 @@

        Control Implementation

        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/msg/ControllerConfig.html b/msg/ControllerConfig.html index 1326108..89f844f 100644 --- a/msg/ControllerConfig.html +++ b/msg/ControllerConfig.html @@ -26,7 +26,7 @@

        Compact Message Definition

        -

        autogenerated on Thu, 26 Oct 2023 17:45:15

        +

        autogenerated on Tue, 31 Oct 2023 14:37:32

        diff --git a/msg/ControllerState.html b/msg/ControllerState.html index 388a4f8..84418ff 100644 --- a/msg/ControllerState.html +++ b/msg/ControllerState.html @@ -26,7 +26,7 @@

        Compact Message Definition

        -

        autogenerated on Thu, 26 Oct 2023 17:45:15

        +

        autogenerated on Tue, 31 Oct 2023 14:37:32

        diff --git a/namespacecartesian__impedance__controller.html b/namespacecartesian__impedance__controller.html index 29ff5e3..565a9c0 100644 --- a/namespacecartesian__impedance__controller.html +++ b/namespacecartesian__impedance__controller.html @@ -382,7 +382,7 @@

        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/namespacemembers.html b/namespacemembers.html index 00a0749..87c86b8 100644 --- a/namespacemembers.html +++ b/namespacemembers.html @@ -45,7 +45,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/namespacemembers_func.html b/namespacemembers_func.html index a345d7f..3e7110a 100644 --- a/namespacemembers_func.html +++ b/namespacemembers_func.html @@ -45,7 +45,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/namespaces.html b/namespaces.html index 5c67d37..8deb5ba 100644 --- a/namespaces.html +++ b/namespaces.html @@ -31,7 +31,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/pages.html b/pages.html index 63e205b..a625096 100644 --- a/pages.html +++ b/pages.html @@ -31,7 +31,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:15 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/paper_8md.html b/paper_8md.html index a603079..ce174a0 100644 --- a/paper_8md.html +++ b/paper_8md.html @@ -28,7 +28,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/paper_8md_source.html b/paper_8md_source.html index 2c870ab..b6a253f 100644 --- a/paper_8md_source.html +++ b/paper_8md_source.html @@ -26,7 +26,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/pseudo__inversion_8h.html b/pseudo__inversion_8h.html index f5afd8d..05d43ef 100644 --- a/pseudo__inversion_8h.html +++ b/pseudo__inversion_8h.html @@ -103,7 +103,7 @@

        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/pseudo__inversion_8h_source.html b/pseudo__inversion_8h_source.html index ce1d675..4cf014d 100644 --- a/pseudo__inversion_8h_source.html +++ b/pseudo__inversion_8h_source.html @@ -31,7 +31,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/rbdyn__wrapper_8h.html b/rbdyn__wrapper_8h.html index db05a50..6f04931 100644 --- a/rbdyn__wrapper_8h.html +++ b/rbdyn__wrapper_8h.html @@ -65,7 +65,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/rbdyn__wrapper_8h_source.html b/rbdyn__wrapper_8h_source.html index 068d995..b14ce5c 100644 --- a/rbdyn__wrapper_8h_source.html +++ b/rbdyn__wrapper_8h_source.html @@ -44,7 +44,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/structrbdyn__wrapper_1_1EefState-members.html b/structrbdyn__wrapper_1_1EefState-members.html index 147471e..262daba 100644 --- a/structrbdyn__wrapper_1_1EefState-members.html +++ b/structrbdyn__wrapper_1_1EefState-members.html @@ -35,7 +35,7 @@
        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31
        diff --git a/structrbdyn__wrapper_1_1EefState.html b/structrbdyn__wrapper_1_1EefState.html index fdf6bc8..52f80c5 100644 --- a/structrbdyn__wrapper_1_1EefState.html +++ b/structrbdyn__wrapper_1_1EefState.html @@ -82,7 +82,7 @@

        workspace
        Author(s): Matthias Mayr, Oussama Chouman
        -autogenerated on Thu Oct 26 2023 17:45:14 +autogenerated on Tue Oct 31 2023 14:37:31