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.

bool get_all_transforms(const trajectory_msgs::msg::JointTrajectoryPoint &current_joint_state, const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state)

Calculate all transforms needed for admittance control using the loader kinematics plugin. If the transform does not exist in the kinematics model, then TF will be used for lookup. The return value is true if all transformation are calculated without an error

Parameters:

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

  • reference_joint_state – [in] input joint state reference

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

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’.

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}
AdmittanceTransforms admittance_transforms_
Eigen::Vector3d cog_pos_
Eigen::Vector3d end_effector_weight_
control_msgs::msg::AdmittanceControllerState state_message_