Skip to content

Commit

Permalink
added minimum functionality
Browse files Browse the repository at this point in the history
  • Loading branch information
@mhubii
mhubii committed Oct 1, 2023
1 parent ad1e7d7 commit 48147cd
Show file tree
Hide file tree
Showing 10 changed files with 280 additions and 158 deletions.
2 changes: 1 addition & 1 deletion lbr_fri_ros2/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ find_package(urdf REQUIRED)
add_library(lbr_fri_ros2
SHARED
src/app.cpp
# src/command_guard.cpp
src/command_guard.cpp
src/command_interface.cpp
src/state_interface.cpp
src/utils.cpp
Expand Down
51 changes: 34 additions & 17 deletions lbr_fri_ros2/include/lbr_fri_ros2/client.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,10 +22,11 @@ namespace lbr_fri_ros2 {
class Client : public KUKA::FRI::LBRClient {
public:
Client() = delete;
Client(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface,
const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface)
: logging_interface_ptr_(logging_interface), parameters_interface_ptr_(parameters_interface),
state_interface_(logging_interface, parameters_interface){};
Client(const rclcpp::Node::SharedPtr node_ptr)
: logging_interface_ptr_(node_ptr->get_node_logging_interface()),
parameters_interface_ptr_(node_ptr->get_node_parameters_interface()),
command_interface_(node_ptr),
state_interface_(logging_interface_ptr_, parameters_interface_ptr_), open_loop_(true){};

inline CommandInterface &get_command_interface() { return command_interface_; }
inline StateInterface &get_state_interface() { return state_interface_; }
Expand All @@ -40,6 +41,7 @@ class Client : public KUKA::FRI::LBRClient {
KUKA::FRI::ESessionState new_state) override {
RCLCPP_INFO(logging_interface_ptr_->get_logger(), "LBR switched from %s to %s.",
KUKA_FRI_STATE_MAP[old_state].c_str(), KUKA_FRI_STATE_MAP[new_state].c_str());
command_interface_.init_command(robotState());
}

/**
Expand All @@ -58,25 +60,40 @@ class Client : public KUKA::FRI::LBRClient {
void waitForCommand() override {
KUKA::FRI::LBRClient::waitForCommand();
state_interface_.set_state(robotState());
command_interface_.get_command(robotCommand(), robotState());

if (robotState().getClientCommandMode() == KUKA::FRI::EClientCommandMode::TORQUE) {
command_interface_.get_torque_command(robotCommand(), robotState());
}

if (robotState().getClientCommandMode() == KUKA::FRI::EClientCommandMode::WRENCH) {
command_interface_.get_wrench_command(robotCommand(), robotState());
}
};
/**
* @brief Called when robot in KUKA::FRI::COMMANDING_ACTIVE state. Publishes state from
* #robotState via #lbr_state_pub_. Writes command from #lbr_command_ to #robotCommand.
*
*/

void command() override {
if (open_loop_) {
state_interface_.set_state(robotState());
state_interface_.set_state_open_loop(robotState(),
command_interface_.get_command().joint_position);
} else {
// state_interface_.set_state_open_loop(robotState(), ); TODO
state_interface_.set_state(robotState());
}
command_interface_.get_command(robotCommand(), robotState());

// set command
// robotCommand().setJointPosition(robotState().getMeasuredJointPosition()); // TODO: replace
// this
// command_interface_.get_command(robotCommand(), robotState());
switch (robotState().getClientCommandMode()) {
case KUKA::FRI::EClientCommandMode::POSITION:
command_interface_.get_joint_position_command(robotCommand(), robotState());
return;
case KUKA::FRI::EClientCommandMode::TORQUE:
command_interface_.get_torque_command(robotCommand(), robotState());
return;
case KUKA::FRI::EClientCommandMode::WRENCH:
command_interface_.get_wrench_command(robotCommand(), robotState());
return;
default:
std::string err =
"Unsupported command mode: " + std::to_string(robotState().getClientCommandMode()) + ".";
RCLCPP_ERROR(logging_interface_ptr_->get_logger(), err.c_str());
throw std::runtime_error(err);
}
};

protected:
Expand Down
68 changes: 38 additions & 30 deletions lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,34 +19,42 @@ namespace lbr_fri_ros2 {
* @brief CommandGuard checks desired commands for limits.
*
*/
template <typename command_type> class CommandGuard {
class CommandGuard {
protected:
using const_command_type_ref = const command_type &;
using JointArray = command_type::_joint_position_type;
// ROS IDL types
using idl_command_t = lbr_fri_msgs::msg::LBRCommand;
using const_idl_command_t_ref = const idl_command_t &;
using joint_array_t = idl_command_t::_joint_position_type;
using const_joint_array_t_ref = const joint_array_t &;

// FRI types
using fri_state_t = KUKA::FRI::LBRState;
using const_fri_state_t_ref = const fri_state_t &;

public:
CommandGuard() = delete;
CommandGuard(
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr);

/**
* @brief Construct a new CommandGuard object.
*
* @param[in] logger_interface Shared node logger interface
* @param[in] logging_interface_ptr Shared node logger interface
* @param[in] min_position Minimum joint position [rad]
* @param[in] max_position Maximum joint position [rad]
* @param[in] max_velocity Maximum joint velocity [rad/s]
* @param[in] max_torque Maximal torque [Nm]
*/
CommandGuard(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
const JointArray &min_position, const JointArray &max_position,
const JointArray &max_velocity, const JointArray &max_torque);
CommandGuard(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr,
const_joint_array_t_ref min_position, const_joint_array_t_ref max_position,
const_joint_array_t_ref max_velocity, const_joint_array_t_ref max_torque);

/**
* @brief Construct a new CommandGuard object.
*
* @param[in] logger_interface Shared node logger interface
* @param[in] logging_interface_ptr Shared node logger interface
* @param robot_description String containing URDF robot rescription
*/
CommandGuard(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
CommandGuard(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr,
const std::string &robot_description);

/**
Expand All @@ -57,8 +65,8 @@ template <typename command_type> class CommandGuard {
* @return true if lbr_command is valid
* @return false if lbr_command is invalid
*/
virtual bool is_valid_command(const_command_type_ref lbr_command,
const KUKA::FRI::LBRState &lbr_state) const;
virtual bool is_valid_command(const_idl_command_t_ref lbr_command,
const_fri_state_t_ref lbr_state) const;

protected:
/**
Expand All @@ -79,8 +87,8 @@ template <typename command_type> class CommandGuard {
* @return true if lbr_command in position limits
* @return false if lbr_command outside position limits
*/
virtual bool command_in_position_limits_(const_command_type_ref lbr_command,
const KUKA::FRI::LBRState & /*lbr_state*/) const;
virtual bool command_in_position_limits_(const_idl_command_t_ref lbr_command,
const_fri_state_t_ref /*lbr_state*/) const;

/**
* @brief Checks for joint velocity limits.
Expand All @@ -90,8 +98,8 @@ template <typename command_type> class CommandGuard {
* @return true if lbr_command in velocity limits
* @return false if lbr_command outside velocity limits
*/
virtual bool command_in_velocity_limits_(const_command_type_ref lbr_command,
const KUKA::FRI::LBRState &lbr_state) const;
virtual bool command_in_velocity_limits_(const_idl_command_t_ref lbr_command,
const_fri_state_t_ref lbr_state) const;

/**
* @brief Checks for joint torque limits.
Expand All @@ -101,16 +109,16 @@ template <typename command_type> class CommandGuard {
* @return true if lbr_command in torque limits
* @return false if lbr_command outside torque limits
*/
virtual bool command_in_torque_limits_(const_command_type_ref lbr_command,
const KUKA::FRI::LBRState &lbr_state) const;
virtual bool command_in_torque_limits_(const_idl_command_t_ref lbr_command,
const_fri_state_t_ref lbr_state) const;

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
logger_interface_; /**< Shared node logger interface.*/
logging_interface_ptr_; /**< Shared node logger interface.*/

JointArray min_position_; /**< Minimum joint position [rad].*/
JointArray max_position_; /**< Maximum joint position [rad].*/
JointArray max_velocity_; /**< Maximum joint velocity [rad/s].*/
JointArray max_torque_; /**< Maximum joint torque [Nm].*/
joint_array_t min_position_; /**< Minimum joint position [rad].*/
joint_array_t max_position_; /**< Maximum joint position [rad].*/
joint_array_t max_velocity_; /**< Maximum joint velocity [rad/s].*/
joint_array_t max_torque_; /**< Maximum joint torque [Nm].*/
};

/**
Expand All @@ -124,13 +132,13 @@ class SafeStopCommandGuard : public CommandGuard {
/**
* @brief Construct a new SafeStopCommandGuard object.
*
* @param[in] logger_interface Shared node logger interface
* @param[in] logging_interface_ptr Shared node logger interface
* @param robot_description String containing URDF robot rescription
*/
SafeStopCommandGuard(
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr,
const std::string &robot_description)
: CommandGuard(logger_interface, robot_description){};
: CommandGuard(logging_interface_ptr, robot_description){};

protected:
/**
Expand All @@ -142,20 +150,20 @@ class SafeStopCommandGuard : public CommandGuard {
* @return true if lbr_command in position limits
* @return false if lbr_command outside position limits
*/
virtual bool command_in_position_limits_(const_command_type_ref lbr_command,
const KUKA::FRI::LBRState &lbr_state) const override;
virtual bool command_in_position_limits_(const_idl_command_t_ref lbr_command,
const_fri_state_t_ref lbr_state) const override;
};

/**
* @brief Creates an CommandGuard object.
*
* @param[in] logger_interface Shared node logger interface
* @param[in] logging_interface_ptr Shared node logger interface
* @param[in] robot_description String containing URDF robot rescription
* @param[in] variant Which variant of CommandGuard to create
* @return std::unique_ptr<CommandGuard> Pointer to CommandGuard object
*/
std::unique_ptr<CommandGuard> command_guard_factory(
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr,
const std::string &robot_description, const std::string &variant);
} // end of namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__COMMAND_GUARDS_HPP_
46 changes: 28 additions & 18 deletions lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,40 +2,50 @@
#define LBR_FRI_ROS2__COMMAND_INTERFACE_HPP_

#include <memory>
#include <stdexcept>
#include <string>

#include "rclcpp/rclcpp.hpp"

#include "friLBRClient.h"

#include "lbr_fri_msgs/msg/lbr_command.hpp"
// #include "lbr_fri_ros2/command_guard.hpp"
#include "lbr_fri_ros2/command_guard.hpp"
#include "lbr_fri_ros2/utils.hpp"

namespace lbr_fri_ros2 {
class CommandInterface {
protected:
// ROS types
using ros_command_type = lbr_fri_msgs::msg::LBRCommand;
using const_ros_command_type_ref = const ros_command_type &;
// ROS IDL types
using idl_command_t = lbr_fri_msgs::msg::LBRCommand;
using const_idl_command_t_ref = const idl_command_t &;

// FRI types
using fri_command_type = KUKA::FRI::LBRCommand;
using fri_command_type_ref = fri_command_type &;
using fri_state_type = KUKA::FRI::LBRState;
using const_fri_state_type_ref = const fri_state_type &;
using fri_command_t = KUKA::FRI::LBRCommand;
using fri_command_t_ref = fri_command_t &;
using fri_state_t = KUKA::FRI::LBRState;
using const_fri_state_t_ref = const fri_state_t &;

public:
CommandInterface() = default;
CommandInterface() = delete;
CommandInterface(const rclcpp::Node::SharedPtr node_ptr);

inline bool is_init() const { return init_; };
void get_joint_position_command(fri_command_t_ref command, const_fri_state_t_ref state);
void get_torque_command(fri_command_t_ref command, const_fri_state_t_ref state);
void get_wrench_command(fri_command_t_ref command, const_fri_state_t_ref state);

void get_command(fri_command_type_ref command, const_fri_state_type_ref state);
inline void set_command(const_ros_command_type_ref command) { command_ = command; };
void init_command(const_fri_state_t_ref state);
inline void set_command_target(const_idl_command_t_ref command) { command_target_ = command; }
inline const_idl_command_t_ref get_command() const { return command_; }
inline const_idl_command_t_ref get_command_target() const { return command_target_; }

protected:
void init_command_(const_fri_state_type_ref state);
// std::unique_ptr<CommandGuard> command_guard_;
// add PID
ros_command_type command_, command_target_;
bool init_;
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr_;
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr_;

std::unique_ptr<CommandGuard> command_guard_;
JointPIDArrayROS joint_position_pid_;
idl_command_t command_, command_target_;
};
} // end of namespace lbr_fri_ros2
} // namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__COMMAND_INTERFACE_HPP_
25 changes: 12 additions & 13 deletions lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,36 +12,35 @@
namespace lbr_fri_ros2 {
class StateInterface {
protected:
// ROS types
using ros_state_type = lbr_fri_msgs::msg::LBRState;
using const_ros_state_type_ref = const ros_state_type &;
using ros_joint_pos_type = ros_state_type::_measured_joint_position_type;
using const_ros_joint_pos_type_ref = const ros_joint_pos_type &;
// ROS IDL types
using idl_state_t = lbr_fri_msgs::msg::LBRState;
using const_idl_state_t_ref = const idl_state_t &;
using idl_joint_pos_t = idl_state_t::_measured_joint_position_type;
using const_idl_joint_pos_t_ref = const idl_joint_pos_t &;

// FRI types
using fri_state_type = KUKA::FRI::LBRState;
using const_fri_state_type_ref = const fri_state_type &;
using fri_session_state_type = KUKA::FRI::ESessionState;
using fri_state_t = KUKA::FRI::LBRState;
using const_fri_state_t_ref = const fri_state_t &;
using fri_session_state_t = KUKA::FRI::ESessionState;

public:
StateInterface() = delete;
StateInterface(
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr,
const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr);

inline const_ros_state_type_ref &get_state() const { return state_; };
inline const_idl_state_t_ref &get_state() const { return state_; };

void set_state(const_fri_state_type_ref state);
void set_state_open_loop(const_fri_state_type_ref state,
const_ros_joint_pos_type_ref joint_position);
void set_state(const_fri_state_t_ref state);
void set_state_open_loop(const_fri_state_t_ref state, const_idl_joint_pos_t_ref joint_position);

protected:
void init_filters_();

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr_;
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr_;

ros_state_type state_;
idl_state_t state_;
JointExponentialFilterArrayROS external_torque_filter_;
JointExponentialFilterArrayROS measured_torque_filter_;
bool filters_init_;
Expand Down
Loading

0 comments on commit 48147cd

Please sign in to comment.