Class AdmittanceRule

Class Documentation

class AdmittanceRule

Public Functions

inline explicit AdmittanceRule(const std::shared_ptr<admittance_controller::ParamListener> &parameter_handler)
controller_interface::return_type configure(const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> &node, const size_t num_joint, const std::string &robot_description)

Configure admittance rule memory using number of joints.

Configure admittance rule memory for num joints and load kinematics interface.

controller_interface::return_type reset(const size_t num_joints)

Reset all values back to default.

void apply_parameters_update()

Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field members (end_effector_weight_, cog_pos_, mass_, mass_inv_ stiffness, selected_axes, damping_) are also updated

controller_interface::return_type update(const trajectory_msgs::msg::JointTrajectoryPoint &current_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)

Calculate ‘desired joint states’ based on the ‘measured force’, ‘reference joint state’, and ‘current_joint_state’.

All transforms (e.g., world to base, sensor to base, CoG to base) are now computed directly in this function and stored in admittance_state_, removing the need for an intermediate transform struct.

Parameters:

  • current_joint_state – [in] current joint state of the robot

  • measured_wrench – [in] most recent measured wrench from force torque sensor

  • reference_joint_state – [in] input joint state reference

  • period – [in] time in seconds since last controller update

  • desired_joint_state – [out] joint state reference after the admittance offset is applied to the input reference

const control_msgs::msg::AdmittanceControllerState &get_controller_state()

Set fields of state_message from current admittance controller state.

Parameters:

state_message – [out] message containing target position/vel/accel, wrench, and actual robot state, among other things

Public Members

std::shared_ptr<admittance_controller::ParamListener> parameter_handler_
admittance_controller::Params parameters_

Protected Functions

bool calculate_admittance_rule(AdmittanceState &admittance_state, double dt)

Calculates the admittance rule from given the robot’s current joint angles. The admittance controller state input is updated with the new calculated values. A boolean value is returned indicating if any of the kinematics plugin calls failed.

Parameters:

  • admittance_state – [in] contains all the information needed to calculate the admittance offset

  • dt – [in] controller period

  • success – [out] true if no calls to the kinematics interface fail

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)

Updates internal estimate of wrench in world frame wrench_world_ given the new measurement measured_wrench, the sensor to base frame rotation sensor_world_rot, and the center of gravity frame to base frame rotation cog_world_rot. The wrench_world_ estimate includes gravity compensation

Parameters:

  • measured_wrench – [in] most recent measured wrench from force torque sensor

  • sensor_world_rot – [in] rotation matrix from world frame to sensor frame

  • cog_world_rot – [in] rotation matrix from world frame to center of gravity frame

template<typename T1, typename T2>
void vec_to_eigen(const std::vector<T1> &data, T2 &matrix)

Protected Attributes

size_t num_joints_
std::shared_ptr<pluginlib::ClassLoader<kinematics_interface::KinematicsInterface>> kinematics_loader_
std::unique_ptr<kinematics_interface::KinematicsInterface> kinematics_
Eigen::Matrix<double, 6, 1> wrench_world_
AdmittanceState admittance_state_ = {0}
Eigen::Vector3d cog_pos_
Eigen::Vector3d end_effector_weight_
control_msgs::msg::AdmittanceControllerState state_message_