17 #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_
18 #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_
26 #include "admittance_controller_parameters.hpp"
27 #include "control_msgs/msg/admittance_controller_state.hpp"
28 #include "controller_interface/controller_interface_base.hpp"
29 #include "kinematics_interface/kinematics_interface.hpp"
30 #include "pluginlib/class_loader.hpp"
31 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
38 Eigen::Isometry3d ref_base_ft_;
41 Eigen::Isometry3d base_ft_;
44 Eigen::Isometry3d base_control_;
47 Eigen::Isometry3d base_tip_;
50 Eigen::Isometry3d base_cog_;
52 Eigen::Isometry3d world_base_;
59 admittance_velocity.setZero();
60 admittance_acceleration.setZero();
65 selected_axes.setZero();
66 current_joint_pos = Eigen::VectorXd::Zero(num_joints);
67 joint_pos = Eigen::VectorXd::Zero(num_joints);
68 joint_vel = Eigen::VectorXd::Zero(num_joints);
69 joint_acc = Eigen::VectorXd::Zero(num_joints);
72 Eigen::VectorXd current_joint_pos;
73 Eigen::VectorXd joint_pos;
74 Eigen::VectorXd joint_vel;
75 Eigen::VectorXd joint_acc;
76 Eigen::Matrix<double, 6, 1> damping;
77 Eigen::Matrix<double, 6, 1> mass;
78 Eigen::Matrix<double, 6, 1> mass_inv;
79 Eigen::Matrix<double, 6, 1> selected_axes;
80 Eigen::Matrix<double, 6, 1> stiffness;
81 Eigen::Matrix<double, 6, 1> wrench_base;
82 Eigen::Matrix<double, 6, 1> admittance_acceleration;
83 Eigen::Matrix<double, 6, 1> admittance_velocity;
84 Eigen::Isometry3d admittance_position;
85 Eigen::Matrix<double, 3, 3> rot_base_control;
86 Eigen::Isometry3d ref_trans_base_ft;
87 std::string ft_sensor_frame;
94 const std::shared_ptr<admittance_controller::ParamListener> & parameter_handler)
96 parameter_handler_ = parameter_handler;
97 parameters_ = parameter_handler_->get_params();
98 num_joints_ = parameters_.joints.size();
104 controller_interface::return_type
configure(
105 const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node,
const size_t num_joint);
108 controller_interface::return_type
reset(
const size_t num_joints);
118 const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
119 const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state);
139 controller_interface::return_type
update(
140 const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
141 const geometry_msgs::msg::Wrench & measured_wrench,
142 const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state,
143 const rclcpp::Duration & period,
144 trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states);
156 std::shared_ptr<admittance_controller::ParamListener> parameter_handler_;
157 admittance_controller::Params parameters_;
178 const geometry_msgs::msg::Wrench & measured_wrench,
179 const Eigen::Matrix<double, 3, 3> & sensor_world_rot,
180 const Eigen::Matrix<double, 3, 3> & cog_world_rot);
182 template <
typename T1,
typename T2>
183 void vec_to_eigen(
const std::vector<T1> & data, T2 & matrix);
189 std::shared_ptr<pluginlib::ClassLoader<kinematics_interface::KinematicsInterface>>
191 std::unique_ptr<kinematics_interface::KinematicsInterface> kinematics_;
194 Eigen::Matrix<double, 6, 1> wrench_world_;
203 Eigen::Vector3d cog_pos_;
206 Eigen::Vector3d end_effector_weight_;
209 control_msgs::msg::AdmittanceControllerState state_message_;
Definition: admittance_rule.hpp:91
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