ros2_control - rolling
Loading...
Searching...
No Matches
admittance_controller::AdmittanceRule Class Reference
Collaboration diagram for admittance_controller::AdmittanceRule:

Public Member Functions

 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.
controller_interface::return_type reset (const size_t num_joints)
 Reset all values back to default.
void apply_parameters_update ()
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)
const control_msgs::msg::AdmittanceControllerState & get_controller_state ()

Public Attributes

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

Protected Member Functions

bool calculate_admittance_rule (AdmittanceState &admittance_state, double dt)
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)
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_

Member Function Documentation

◆ apply_parameters_update()

void admittance_controller::AdmittanceRule::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

◆ calculate_admittance_rule()

bool admittance_controller::AdmittanceRule::calculate_admittance_rule ( AdmittanceState & admittance_state,
double dt )
protected

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
[in]admittance_statecontains all the information needed to calculate the admittance offset
[in]dtcontroller period
[out]successtrue if no calls to the kinematics interface fail

◆ configure()

controller_interface::return_type admittance_controller::AdmittanceRule::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.

◆ get_controller_state()

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

Set fields of state_message from current admittance controller state.

Parameters
[out]state_messagemessage containing target position/vel/accel, wrench, and actual robot state, among other things

◆ process_wrench_measurements()

void admittance_controller::AdmittanceRule::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 )
protected

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
[in]measured_wrenchmost recent measured wrench from force torque sensor
[in]sensor_world_rotrotation matrix from world frame to sensor frame
[in]cog_world_rotrotation matrix from world frame to center of gravity frame

◆ update()

controller_interface::return_type admittance_controller::AdmittanceRule::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
[in]current_joint_statecurrent joint state of the robot
[in]measured_wrenchmost recent measured wrench from force torque sensor
[in]reference_joint_stateinput joint state reference
[in]periodtime in seconds since last controller update
[out]desired_joint_statejoint state reference after the admittance offset is applied to the input reference

The documentation for this class was generated from the following files: