Skip to content

Commit

Permalink
added filters and pid to client
Browse files Browse the repository at this point in the history
  • Loading branch information
@mhubii
mhubii committed Aug 9, 2023
1 parent 38230ef commit cd00107
Show file tree
Hide file tree
Showing 2 changed files with 78 additions and 70 deletions.
35 changes: 21 additions & 14 deletions lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include "lbr_fri_msgs/msg/lbr_command.hpp"
#include "lbr_fri_msgs/msg/lbr_state.hpp"
#include "lbr_fri_ros2/lbr_command_guard.hpp"
#include "lbr_fri_ros2/utils.hpp"

namespace lbr_fri_ros2 {
/**
Expand Down Expand Up @@ -89,6 +90,14 @@ class LBRClient : public KUKA::FRI::LBRClient {
*/
void init_topics_();

/**
* @brief Initialize filters, that is #external_torque_filter_, #measured_torque_filter_ and
* #joint_position_pid_. This method may only be called after #robotState is accessible since
* robots's sample time is required.
*
*/
void init_filters_();

/**
* @brief Declare parameters for #node_ that are utilized within LBRClient.
*
Expand All @@ -115,21 +124,22 @@ class LBRClient : public KUKA::FRI::LBRClient {
*/
void on_lbr_command_(const lbr_fri_msgs::msg::LBRCommand::SharedPtr lbr_command);

/**
* @brief Calculate the command for the robot using the current state and the desired command.
*
* @param[in] lbr_command_target The desired command
* @param[in] lbr_state The current state
* @param[out] lbr_command The command to be written to the robot
*/
void lbr_command_pid_(const lbr_fri_msgs::msg::LBRCommand &lbr_command_target,
const lbr_fri_msgs::msg::LBRState &lbr_state,
lbr_fri_msgs::msg::LBRCommand &lbr_command);

rclcpp::Node::SharedPtr node_; /**< Shared pointer to node.*/
std::unique_ptr<LBRCommandGuard>
lbr_command_guard_; /**< Validating commands prior to writing them to #robotCommand.*/

JointExponentialFilterArrayROS
external_torque_filter_; /**< JointExponentialFilterArrayROS for smoothing external torques.*/
JointExponentialFilterArrayROS
measured_torque_filter_; /**< JointExponentialFilterArrayROS for smoothing measured torques.*/
JointPIDArrayROS
joint_position_pid_; /**< JointPIDArrayROS for smoothing joint positions commands.*/

bool topics_init_; /**< Indicates whether topics were initialized. Necessary since #robotState
sample time required.*/
bool filters_init_; /**< Indicates whether filters were initialized. Necessary since #robotState
sample time required.*/

uint32_t missed_deadlines_pub_; /**< Counter for states that were not published within specified
deadline.*/
uint32_t missed_deadlines_sub_; /**< Counter for commands that were not received within specified
Expand All @@ -139,9 +149,6 @@ class LBRClient : public KUKA::FRI::LBRClient {
lbr_fri_msgs::msg::LBRCommand lbr_command_; /**< Command buffer.*/
lbr_fri_msgs::msg::LBRState lbr_state_; /**< State buffer.*/

std::array<control_toolbox::PidROS, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>
position_pid_controllers_; /**< Array of PidROS controllers for each joint.*/

bool open_loop_; /**< Open loop control if true. Best way to command the LBRs.*/

rclcpp::Publisher<lbr_fri_msgs::msg::LBRState>::SharedPtr
Expand Down
113 changes: 57 additions & 56 deletions lbr_fri_ros2/src/lbr_client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,10 @@ namespace lbr_fri_ros2 {
LBRClient::LBRClient(const rclcpp::Node::SharedPtr node,
std::unique_ptr<LBRCommandGuard> lbr_command_guard)
: node_(node), lbr_command_guard_(std::move(lbr_command_guard)),
position_pid_controllers_(
{control_toolbox::PidROS{node, "A1"}, control_toolbox::PidROS{node, "A2"},
control_toolbox::PidROS{node, "A3"}, control_toolbox::PidROS{node, "A4"},
control_toolbox::PidROS{node, "A5"}, control_toolbox::PidROS{node, "A6"},
control_toolbox::PidROS{node, "A7"}}) {
std::for_each(position_pid_controllers_.begin(), position_pid_controllers_.end(),
[](auto &pid) { pid.initPid(0.02, 0.0, 0.0, 0.0, 0.0, false); });

external_torque_filter_(node, "external_torque"),
measured_torque_filter_(node, "measured_torque"),
joint_position_pid_(node, {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}), topics_init_(false),
filters_init_(false) {
missed_deadlines_pub_ = 0;
missed_deadlines_sub_ = 0;

Expand All @@ -29,6 +25,7 @@ void LBRClient::log_status() {
void LBRClient::onStateChange(KUKA::FRI::ESessionState old_state,
KUKA::FRI::ESessionState new_state) {
init_topics_();
init_filters_();
RCLCPP_INFO(node_->get_logger(), "LBR switched from %s to %s.",
KUKA_FRI_STATE_MAP[old_state].c_str(), KUKA_FRI_STATE_MAP[new_state].c_str());
}
Expand All @@ -55,7 +52,11 @@ void LBRClient::command() {
pub_lbr_state_();

// compute command
lbr_command_pid_(lbr_command_target_, lbr_state_, lbr_command_);
joint_position_pid_.compute(lbr_command_target_.joint_position,
lbr_state_.measured_joint_position,
rclcpp::Duration(std::chrono::milliseconds(
static_cast<int64_t>(robotState().getSampleTime() * 1e3))),
lbr_command_.joint_position);

// validate command
if (!lbr_command_guard_->is_valid_command(lbr_command_, robotState())) {
Expand Down Expand Up @@ -90,38 +91,54 @@ void LBRClient::init_lbr_command_() {
}

void LBRClient::init_topics_() {
if (!lbr_state_pub_) {
auto pub_options = rclcpp::PublisherOptions();
pub_options.event_callbacks.deadline_callback = [this](rclcpp::QOSDeadlineOfferedInfo &) {
missed_deadlines_pub_++;
};

lbr_state_pub_ = node_->create_publisher<lbr_fri_msgs::msg::LBRState>(
"~/state", rclcpp::QoS(1)
.deadline(std::chrono::milliseconds(
static_cast<int64_t>(robotState().getSampleTime() * 1e3)))
.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE));
if (topics_init_) {
return;
}

if (!lbr_command_sub_) {
auto memory_strategy =
rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy<
lbr_fri_msgs::msg::LBRCommand, 1>::make_shared();

auto sub_options = rclcpp::SubscriptionOptions();
sub_options.event_callbacks.deadline_callback = [this](rclcpp::QOSDeadlineRequestedInfo &) {
missed_deadlines_sub_++;
};

lbr_command_sub_ = node_->create_subscription<lbr_fri_msgs::msg::LBRCommand>(
"~/command",
rclcpp::QoS(1)
.deadline(
std::chrono::milliseconds(static_cast<int64_t>(robotState().getSampleTime() * 1e3)))
.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE),
std::bind(&LBRClient::on_lbr_command_, this, std::placeholders::_1), sub_options,
memory_strategy);
auto pub_options = rclcpp::PublisherOptions();
pub_options.event_callbacks.deadline_callback = [this](rclcpp::QOSDeadlineOfferedInfo &) {
missed_deadlines_pub_++;
};

lbr_state_pub_ = node_->create_publisher<lbr_fri_msgs::msg::LBRState>(
"~/state", rclcpp::QoS(1)
.deadline(std::chrono::milliseconds(
static_cast<int64_t>(robotState().getSampleTime() * 1e3)))
.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE));

auto memory_strategy =
rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy<
lbr_fri_msgs::msg::LBRCommand, 1>::make_shared();

auto sub_options = rclcpp::SubscriptionOptions();
sub_options.event_callbacks.deadline_callback = [this](rclcpp::QOSDeadlineRequestedInfo &) {
missed_deadlines_sub_++;
};

lbr_command_sub_ = node_->create_subscription<lbr_fri_msgs::msg::LBRCommand>(
"~/command",
rclcpp::QoS(1)
.deadline(
std::chrono::milliseconds(static_cast<int64_t>(robotState().getSampleTime() * 1e3)))
.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE),
std::bind(&LBRClient::on_lbr_command_, this, std::placeholders::_1), sub_options,
memory_strategy);

topics_init_ = true;
}

void LBRClient::init_filters_() {
if (filters_init_) {
return;
}
// initialize PID
joint_position_pid_.init(2. * robotState().getSampleTime(), 0., 0., 0., 0., false);

// initialize torque filters
external_torque_filter_.init(50, robotState().getSampleTime());
measured_torque_filter_.init(50, robotState().getSampleTime());

filters_init_ = true;
}

void LBRClient::declare_parameters_() {
Expand All @@ -141,8 +158,7 @@ void LBRClient::pub_lbr_state_() {
lbr_state_.connection_quality = robotState().getConnectionQuality();
lbr_state_.control_mode = robotState().getControlMode();
lbr_state_.drive_state = robotState().getDriveState();
std::memcpy(lbr_state_.external_torque.data(), robotState().getExternalTorque(),
sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
external_torque_filter_.compute(robotState().getExternalTorque(), lbr_state_.external_torque);
if (robotState().getSessionState() == KUKA::FRI::ESessionState::COMMANDING_WAIT ||
robotState().getSessionState() == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) {
std::memcpy(lbr_state_.ipo_joint_position.data(), robotState().getIpoJointPosition(),
Expand All @@ -155,8 +171,7 @@ void LBRClient::pub_lbr_state_() {
std::memcpy(lbr_state_.measured_joint_position.data(), robotState().getMeasuredJointPosition(),
sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
}
std::memcpy(lbr_state_.measured_torque.data(), robotState().getMeasuredTorque(),
sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
measured_torque_filter_.compute(robotState().getMeasuredTorque(), lbr_state_.measured_torque);
lbr_state_.operation_mode = robotState().getOperationMode();
lbr_state_.overlay_type = robotState().getOverlayType();
lbr_state_.safety_state = robotState().getSafetyState();
Expand All @@ -173,18 +188,4 @@ void LBRClient::pub_lbr_state_() {
void LBRClient::on_lbr_command_(const lbr_fri_msgs::msg::LBRCommand::SharedPtr lbr_command) {
lbr_command_target_ = *lbr_command;
}

void LBRClient::lbr_command_pid_(const lbr_fri_msgs::msg::LBRCommand &lbr_command_target,
const lbr_fri_msgs::msg::LBRState &lbr_state,
lbr_fri_msgs::msg::LBRCommand &lbr_command) {
int i = 0;
std::for_each(lbr_command.joint_position.begin(), lbr_command.joint_position.end(),
[&](double &joint_position) {
joint_position += position_pid_controllers_[i].computeCommand(
lbr_command_target.joint_position[i] - lbr_state.measured_joint_position[i],
rclcpp::Duration(std::chrono::milliseconds(
static_cast<int64_t>(robotState().getSampleTime() * 1e3))));
++i;
});
}
} // end of namespace lbr_fri_ros2

0 comments on commit cd00107

Please sign in to comment.