17 #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_
18 #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_
20 #include "admittance_controller/admittance_rule.hpp"
25 #include <control_toolbox/filters.hpp>
26 #include <tf2_eigen/tf2_eigen.hpp>
28 #include "rclcpp/duration.hpp"
33 constexpr
auto NUM_CARTESIAN_DOF = 6;
37 const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node,
const size_t num_joints)
39 num_joints_ = num_joints;
45 if (!parameters_.kinematics.plugin_name.empty())
50 if (kinematics_loader_)
55 std::make_shared<pluginlib::ClassLoader<kinematics_interface::KinematicsInterface>>(
56 parameters_.kinematics.plugin_package,
"kinematics_interface::KinematicsInterface");
57 kinematics_ = std::unique_ptr<kinematics_interface::KinematicsInterface>(
58 kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name));
60 if (!kinematics_->initialize(
61 node->get_node_parameters_interface(), parameters_.kinematics.tip))
63 return controller_interface::return_type::ERROR;
66 catch (pluginlib::PluginlibException & ex)
69 rclcpp::get_logger(
"AdmittanceRule"),
"Exception while loading the IK plugin '%s': '%s'",
70 parameters_.kinematics.plugin_name.c_str(), ex.what());
71 return controller_interface::return_type::ERROR;
77 rclcpp::get_logger(
"AdmittanceRule"),
78 "A differential IK plugin name was not specified in the config file.");
79 return controller_interface::return_type::ERROR;
82 return controller_interface::return_type::OK;
88 state_message_.joint_state.name.assign(num_joints,
"");
89 state_message_.joint_state.position.assign(num_joints, 0);
90 state_message_.joint_state.velocity.assign(num_joints, 0);
91 state_message_.joint_state.effort.assign(num_joints, 0);
92 for (
size_t i = 0; i < parameters_.joints.size(); ++i)
94 state_message_.joint_state.name = parameters_.joints;
96 state_message_.mass.data.resize(NUM_CARTESIAN_DOF, 0.0);
97 state_message_.selected_axes.data.resize(NUM_CARTESIAN_DOF, 0);
98 state_message_.damping.data.resize(NUM_CARTESIAN_DOF, 0);
99 state_message_.stiffness.data.resize(NUM_CARTESIAN_DOF, 0);
100 state_message_.wrench_base.header.frame_id = parameters_.kinematics.base;
101 state_message_.admittance_velocity.header.frame_id = parameters_.kinematics.base;
102 state_message_.admittance_acceleration.header.frame_id = parameters_.kinematics.base;
103 state_message_.admittance_position.header.frame_id = parameters_.kinematics.base;
104 state_message_.admittance_position.child_frame_id =
"admittance_offset";
113 wrench_world_.setZero();
114 end_effector_weight_.setZero();
119 return controller_interface::return_type::OK;
124 if (parameter_handler_->is_old(parameters_))
126 parameters_ = parameter_handler_->get_params();
129 end_effector_weight_[2] = -parameters_.gravity_compensation.CoG.force;
130 vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_pos_);
131 vec_to_eigen(parameters_.admittance.mass, admittance_state_.mass);
132 vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness);
133 vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes);
135 for (
size_t i = 0; i < NUM_CARTESIAN_DOF; ++i)
137 admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i];
138 admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 *
139 sqrt(admittance_state_.mass[i] * admittance_state_.stiffness[i]);
144 const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
145 const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state)
148 bool success = kinematics_->calculate_link_transform(
149 reference_joint_state.positions, parameters_.ft_sensor.frame.id,
150 admittance_transforms_.ref_base_ft_);
153 success &= kinematics_->calculate_link_transform(
154 current_joint_state.positions, parameters_.ft_sensor.frame.id, admittance_transforms_.base_ft_);
155 success &= kinematics_->calculate_link_transform(
156 current_joint_state.positions, parameters_.kinematics.tip, admittance_transforms_.base_tip_);
157 success &= kinematics_->calculate_link_transform(
158 current_joint_state.positions, parameters_.fixed_world_frame.frame.id,
159 admittance_transforms_.world_base_);
160 success &= kinematics_->calculate_link_transform(
161 current_joint_state.positions, parameters_.gravity_compensation.frame.id,
162 admittance_transforms_.base_cog_);
163 success &= kinematics_->calculate_link_transform(
164 current_joint_state.positions, parameters_.control.frame.id,
165 admittance_transforms_.base_control_);
172 const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
173 const geometry_msgs::msg::Wrench & measured_wrench,
174 const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state,
175 const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state)
177 const double dt = period.seconds();
179 if (parameters_.enable_parameter_update_without_reactivation)
187 Eigen::Matrix<double, 3, 3> rot_world_sensor =
188 admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_ft_.rotation();
189 Eigen::Matrix<double, 3, 3> rot_world_cog =
190 admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_cog_.rotation();
194 admittance_state_.wrench_base.block<3, 1>(0, 0) =
195 admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(0, 0);
196 admittance_state_.wrench_base.block<3, 1>(3, 0) =
197 admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(3, 0);
200 vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos);
201 admittance_state_.rot_base_control = admittance_transforms_.base_control_.rotation();
202 admittance_state_.ref_trans_base_ft = admittance_transforms_.ref_base_ft_;
203 admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id;
210 desired_joint_state = reference_joint_state;
211 return controller_interface::return_type::ERROR;
215 for (
size_t i = 0; i < num_joints_; ++i)
217 desired_joint_state.positions[i] =
218 reference_joint_state.positions[i] + admittance_state_.joint_pos[i];
219 desired_joint_state.velocities[i] =
220 reference_joint_state.velocities[i] + admittance_state_.joint_vel[i];
221 desired_joint_state.accelerations[i] =
222 reference_joint_state.accelerations[i] + admittance_state_.joint_acc[i];
225 return controller_interface::return_type::OK;
232 auto rot_base_control = admittance_state.rot_base_control;
233 Eigen::Matrix<double, 6, 6> K = Eigen::Matrix<double, 6, 6>::Zero();
234 Eigen::Matrix<double, 3, 3> K_pos = Eigen::Matrix<double, 3, 3>::Zero();
235 Eigen::Matrix<double, 3, 3> K_rot = Eigen::Matrix<double, 3, 3>::Zero();
236 K_pos.diagonal() = admittance_state.stiffness.block<3, 1>(0, 0);
237 K_rot.diagonal() = admittance_state.stiffness.block<3, 1>(3, 0);
242 K_pos = rot_base_control * K_pos * rot_base_control.transpose();
243 K_rot = rot_base_control * K_rot * rot_base_control.transpose();
244 K.block<3, 3>(0, 0) = K_pos;
245 K.block<3, 3>(3, 3) = K_rot;
248 Eigen::Matrix<double, 6, 6> D = Eigen::Matrix<double, 6, 6>::Zero();
249 Eigen::Matrix<double, 3, 3> D_pos = Eigen::Matrix<double, 3, 3>::Zero();
250 Eigen::Matrix<double, 3, 3> D_rot = Eigen::Matrix<double, 3, 3>::Zero();
251 D_pos.diagonal() = admittance_state.damping.block<3, 1>(0, 0);
252 D_rot.diagonal() = admittance_state.damping.block<3, 1>(3, 0);
253 D_pos = rot_base_control * D_pos * rot_base_control.transpose();
254 D_rot = rot_base_control * D_rot * rot_base_control.transpose();
255 D.block<3, 3>(0, 0) = D_pos;
256 D.block<3, 3>(3, 3) = D_rot;
259 Eigen::Isometry3d desired_trans_base_ft;
260 kinematics_->calculate_link_transform(
261 admittance_state.current_joint_pos, admittance_state.ft_sensor_frame, desired_trans_base_ft);
262 Eigen::Matrix<double, 6, 1> X;
263 X.block<3, 1>(0, 0) =
264 desired_trans_base_ft.translation() - admittance_state.ref_trans_base_ft.translation();
265 auto R_ref = admittance_state.ref_trans_base_ft.rotation();
266 auto R_desired = desired_trans_base_ft.rotation();
267 auto R = R_desired * R_ref.transpose();
268 auto angle_axis = Eigen::AngleAxisd(R);
269 X.block<3, 1>(3, 0) = angle_axis.angle() * angle_axis.axis();
272 auto X_dot = Eigen::Matrix<double, 6, 1>(admittance_state.admittance_velocity.data());
275 auto F_base = admittance_state.wrench_base;
278 Eigen::Matrix<double, 6, 1> F_control;
279 F_control.block<3, 1>(0, 0) = rot_base_control.transpose() * F_base.block<3, 1>(0, 0);
280 F_control.block<3, 1>(3, 0) = rot_base_control.transpose() * F_base.block<3, 1>(3, 0);
281 F_control = F_control.cwiseProduct(admittance_state.selected_axes);
282 F_base.block<3, 1>(0, 0) = rot_base_control * F_control.block<3, 1>(0, 0);
283 F_base.block<3, 1>(3, 0) = rot_base_control * F_control.block<3, 1>(3, 0);
286 Eigen::Matrix<double, 6, 1> X_ddot =
287 admittance_state.mass_inv.cwiseProduct(F_base - D * X_dot - K * X);
288 bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas(
289 admittance_state.current_joint_pos, X_ddot, admittance_state.ft_sensor_frame,
290 admittance_state.joint_acc);
293 for (int64_t i = 0; i < admittance_state.joint_acc.size(); ++i)
295 admittance_state.joint_acc[i] -=
296 parameters_.admittance.joint_damping * admittance_state.joint_vel[i];
300 admittance_state.joint_vel += (admittance_state.joint_acc) * dt;
301 admittance_state.joint_pos += admittance_state.joint_vel * dt;
304 success &= kinematics_->convert_joint_deltas_to_cartesian_deltas(
305 admittance_state.current_joint_pos, admittance_state.joint_vel,
306 admittance_state.ft_sensor_frame, admittance_state.admittance_velocity);
307 success &= kinematics_->convert_joint_deltas_to_cartesian_deltas(
308 admittance_state.current_joint_pos, admittance_state.joint_acc,
309 admittance_state.ft_sensor_frame, admittance_state.admittance_acceleration);
315 const geometry_msgs::msg::Wrench & measured_wrench,
316 const Eigen::Matrix<double, 3, 3> & sensor_world_rot,
317 const Eigen::Matrix<double, 3, 3> & cog_world_rot)
319 Eigen::Matrix<double, 3, 2, Eigen::ColMajor> new_wrench;
320 new_wrench(0, 0) = measured_wrench.force.x;
321 new_wrench(1, 0) = measured_wrench.force.y;
322 new_wrench(2, 0) = measured_wrench.force.z;
323 new_wrench(0, 1) = measured_wrench.torque.x;
324 new_wrench(1, 1) = measured_wrench.torque.y;
325 new_wrench(2, 1) = measured_wrench.torque.z;
328 Eigen::Matrix<double, 3, 2> new_wrench_base = sensor_world_rot * new_wrench;
331 new_wrench_base(2, 0) -= end_effector_weight_[2];
332 new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_pos_).cross(end_effector_weight_);
335 for (
size_t i = 0; i < 6; ++i)
337 wrench_world_(i) = filters::exponentialSmoothing(
338 new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient);
344 for (
size_t i = 0; i < NUM_CARTESIAN_DOF; ++i)
346 state_message_.stiffness.data[i] = admittance_state_.stiffness[i];
347 state_message_.damping.data[i] = admittance_state_.damping[i];
348 state_message_.selected_axes.data[i] =
static_cast<bool>(admittance_state_.selected_axes[i]);
349 state_message_.mass.data[i] = admittance_state_.mass[i];
352 for (
size_t i = 0; i < parameters_.joints.size(); ++i)
354 state_message_.joint_state.name[i] = parameters_.joints[i];
355 state_message_.joint_state.position[i] = admittance_state_.joint_pos[i];
356 state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i];
357 state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i];
360 state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0];
361 state_message_.wrench_base.wrench.force.y = admittance_state_.wrench_base[1];
362 state_message_.wrench_base.wrench.force.z = admittance_state_.wrench_base[2];
363 state_message_.wrench_base.wrench.torque.x = admittance_state_.wrench_base[3];
364 state_message_.wrench_base.wrench.torque.y = admittance_state_.wrench_base[4];
365 state_message_.wrench_base.wrench.torque.z = admittance_state_.wrench_base[5];
367 state_message_.admittance_velocity.twist.linear.x = admittance_state_.admittance_velocity[0];
368 state_message_.admittance_velocity.twist.linear.y = admittance_state_.admittance_velocity[1];
369 state_message_.admittance_velocity.twist.linear.z = admittance_state_.admittance_velocity[2];
370 state_message_.admittance_velocity.twist.angular.x = admittance_state_.admittance_velocity[3];
371 state_message_.admittance_velocity.twist.angular.y = admittance_state_.admittance_velocity[4];
372 state_message_.admittance_velocity.twist.angular.z = admittance_state_.admittance_velocity[5];
374 state_message_.admittance_acceleration.twist.linear.x =
375 admittance_state_.admittance_acceleration[0];
376 state_message_.admittance_acceleration.twist.linear.y =
377 admittance_state_.admittance_acceleration[1];
378 state_message_.admittance_acceleration.twist.linear.z =
379 admittance_state_.admittance_acceleration[2];
380 state_message_.admittance_acceleration.twist.angular.x =
381 admittance_state_.admittance_acceleration[3];
382 state_message_.admittance_acceleration.twist.angular.y =
383 admittance_state_.admittance_acceleration[4];
384 state_message_.admittance_acceleration.twist.angular.z =
385 admittance_state_.admittance_acceleration[5];
387 state_message_.admittance_position = tf2::eigenToTransform(admittance_state_.admittance_position);
389 state_message_.ref_trans_base_ft.header.frame_id = parameters_.kinematics.base;
390 state_message_.ref_trans_base_ft.header.frame_id =
"ft_reference";
391 state_message_.ref_trans_base_ft = tf2::eigenToTransform(admittance_state_.ref_trans_base_ft);
393 Eigen::Quaterniond quat(admittance_state_.rot_base_control);
394 state_message_.rot_base_control.w = quat.w();
395 state_message_.rot_base_control.x = quat.x();
396 state_message_.rot_base_control.y = quat.y();
397 state_message_.rot_base_control.z = quat.z();
399 state_message_.ft_sensor_frame.data =
400 admittance_state_.ft_sensor_frame;
402 return state_message_;
405 template <
typename T1,
typename T2>
406 void AdmittanceRule::vec_to_eigen(
const std::vector<T1> & data, T2 & matrix)
408 for (
auto col = 0; col < matrix.cols(); col++)
410 for (
auto row = 0; row < matrix.rows(); row++)
412 matrix(row, col) = data[row + col * matrix.rows()];
bool calculate_admittance_rule(AdmittanceState &admittance_state, double dt)
Definition: admittance_rule_impl.hpp:228
bool get_all_transforms(const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state)
Definition: admittance_rule_impl.hpp:143
controller_interface::return_type reset(const size_t num_joints)
Reset all values back to default.
Definition: admittance_rule_impl.hpp:85
void apply_parameters_update()
Definition: admittance_rule_impl.hpp:122
const control_msgs::msg::AdmittanceControllerState & get_controller_state()
Definition: admittance_rule_impl.hpp:342
controller_interface::return_type configure(const std::shared_ptr< rclcpp_lifecycle::LifecycleNode > &node, const size_t num_joint)
Configure admittance rule memory using number of joints.
Definition: admittance_rule_impl.hpp:36
controller_interface::return_type update(const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, const geometry_msgs::msg::Wrench &measured_wrench, const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state, const rclcpp::Duration &period, trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_states)
Definition: admittance_rule_impl.hpp:171
void process_wrench_measurements(const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix< double, 3, 3 > &sensor_world_rot, const Eigen::Matrix< double, 3, 3 > &cog_world_rot)
Definition: admittance_rule_impl.hpp:314
Definition: admittance_controller.hpp:40
Definition: admittance_rule.hpp:56