Skip to content

Commit

Permalink
Allow to reconfigure cartesian compliance controller at runtime
Browse files Browse the repository at this point in the history
  • Loading branch information
Lennart Nachtigall committed Oct 13, 2023
1 parent 03057bd commit 94d6279
Show file tree
Hide file tree
Showing 5 changed files with 118 additions and 96 deletions.
26 changes: 14 additions & 12 deletions cartesian_compliance_controller/src/cartesian_compliance_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,19 +99,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
return TYPE::ERROR;
}

// Make sure compliance link is part of the robot chain
m_compliance_ref_link = get_node()->get_parameter("compliance_ref_link").as_string();
if(!Base::robotChainContains(m_compliance_ref_link))
{
RCLCPP_ERROR_STREAM(get_node()->get_logger(),
m_compliance_ref_link << " is not part of the kinematic chain from "
<< Base::m_robot_base_link << " to "
<< Base::m_end_effector_link);
return TYPE::ERROR;
}

// Make sure sensor wrenches are interpreted correctly
ForceBase::setFtSensorReferenceFrame(m_compliance_ref_link);

return TYPE::SUCCESS;
}
Expand All @@ -126,6 +114,20 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
{
return TYPE::ERROR;
}
// Make sure compliance link is part of the robot chain
m_compliance_ref_link = get_node()->get_parameter("compliance_ref_link").as_string();
if(!Base::robotChainContains(m_compliance_ref_link))
{
RCLCPP_ERROR_STREAM(get_node()->get_logger(), "[CartesianComplianceController]: " <<
m_compliance_ref_link << " is not part of the kinematic chain from "
<< Base::m_robot_base_link << " to "
<< Base::m_end_effector_link);
return TYPE::ERROR;
}

// Make sure sensor wrenches are interpreted correctly
ForceBase::setFtSensorReferenceFrame(m_compliance_ref_link);

return TYPE::SUCCESS;
}

Expand Down
5 changes: 4 additions & 1 deletion cartesian_controller_base/src/ForwardDynamicsSolver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,10 @@ namespace cartesian_controller_base{
m_jnt_space_inertia.resize(m_number_joints);

// Set the initial value if provided at runtime, else use default value.
m_min = nh->declare_parameter<double>(m_params + "/link_mass", 0.1);
if(!nh->has_parameter(m_params + "/link_mass"))
m_min = nh->declare_parameter<double>(m_params + "/link_mass", 0.1);
else
m_min = nh->get_parameter(m_params + "/link_mass").get_value<double>();

RCLCPP_INFO(nh->get_logger(), "Forward dynamics solver initialized");
RCLCPP_INFO(nh->get_logger(), "Forward dynamics solver has control over %i joints", m_number_joints);
Expand Down
141 changes: 75 additions & 66 deletions cartesian_controller_base/src/cartesian_controller_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,7 +154,79 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
}

// Get kinematics specific configuration

// Get names of actuated joints
m_joint_names = get_node()->get_parameter("joints").as_string_array();
if (m_joint_names.empty())
{
RCLCPP_ERROR(get_node()->get_logger(), "joints array is empty");
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
}
// Check command interfaces.
// We support position, velocity, or both.
m_cmd_interface_types = get_node()->get_parameter("command_interfaces").as_string_array();
if (m_cmd_interface_types.empty())
{
RCLCPP_ERROR(get_node()->get_logger(), "No command_interfaces specified");
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
}
for (const auto& type : m_cmd_interface_types)
{
RCLCPP_INFO_STREAM(get_node()->get_logger(), type);
if (type != hardware_interface::HW_IF_POSITION && type != hardware_interface::HW_IF_VELOCITY)
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Unsupported command interface: %s. Choose position or velocity",
type.c_str());
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
}
}



// Controller-internal state publishing
m_feedback_pose_publisher =
std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped> >(
get_node()->create_publisher<geometry_msgs::msg::PoseStamped>(
std::string(get_node()->get_name()) + "/current_pose", 3));

m_feedback_twist_publisher =
std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::TwistStamped> >(
get_node()->create_publisher<geometry_msgs::msg::TwistStamped>(
std::string(get_node()->get_name()) + "/current_twist", 3));

m_configured = true;
RCLCPP_INFO_STREAM(get_node()->get_logger(), "Configured controller");
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_deactivate(
const rclcpp_lifecycle::State & previous_state)
{
stopCurrentMotion();

if (m_active)
{
m_joint_cmd_pos_handles.clear();
m_joint_cmd_vel_handles.clear();
m_joint_state_pos_handles.clear();
this->release_interfaces();
m_active = false;
}
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_activate(
const rclcpp_lifecycle::State & previous_state)
{
if (m_active)
{
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}


// Get kinematics specific configuration
urdf::Model robot_model;
KDL::Tree robot_tree;

Expand Down Expand Up @@ -197,13 +269,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
}

// Get names of actuated joints
m_joint_names = get_node()->get_parameter("joints").as_string_array();
if (m_joint_names.empty())
{
RCLCPP_ERROR(get_node()->get_logger(), "joints array is empty");
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
}


// Parse joint limits
KDL::JntArray upper_pos_limits(m_joint_names.size());
Expand Down Expand Up @@ -239,65 +305,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
// Initialize Cartesian pd controllers
m_spatial_controller.init(get_node());

// Check command interfaces.
// We support position, velocity, or both.
m_cmd_interface_types = get_node()->get_parameter("command_interfaces").as_string_array();
if (m_cmd_interface_types.empty())
{
RCLCPP_ERROR(get_node()->get_logger(), "No command_interfaces specified");
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
}
for (const auto& type : m_cmd_interface_types)
{
if (type != hardware_interface::HW_IF_POSITION && type != hardware_interface::HW_IF_VELOCITY)
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Unsupported command interface: %s. Choose position or velocity",
type.c_str());
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
}
}

// Controller-internal state publishing
m_feedback_pose_publisher =
std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped> >(
get_node()->create_publisher<geometry_msgs::msg::PoseStamped>(
std::string(get_node()->get_name()) + "/current_pose", 3));

m_feedback_twist_publisher =
std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::TwistStamped> >(
get_node()->create_publisher<geometry_msgs::msg::TwistStamped>(
std::string(get_node()->get_name()) + "/current_twist", 3));

m_configured = true;

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_deactivate(
const rclcpp_lifecycle::State & previous_state)
{
stopCurrentMotion();

if (m_active)
{
m_joint_cmd_pos_handles.clear();
m_joint_cmd_vel_handles.clear();
m_joint_state_pos_handles.clear();
this->release_interfaces();
m_active = false;
}
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_activate(
const rclcpp_lifecycle::State & previous_state)
{
if (m_active)
{
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

// Get command handles.
for (const auto& type : m_cmd_interface_types)
Expand All @@ -319,6 +327,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
return CallbackReturn::ERROR;
}
}


// Get state handles.
if (!controller_interface::get_ordered_interfaces(state_interfaces_,
Expand Down
41 changes: 24 additions & 17 deletions cartesian_force_controller/src/cartesian_force_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,11 +89,29 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
return ret;
}

// Make sure sensor link is part of the robot chain
m_target_wrench_subscriber = get_node()->create_subscription<geometry_msgs::msg::WrenchStamped>(
get_node()->get_name() + std::string("/target_wrench"),
10,
std::bind(&CartesianForceController::targetWrenchCallback, this, std::placeholders::_1));




return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianForceController::on_activate(
const rclcpp_lifecycle::State & previous_state)
{
RCLCPP_INFO_STREAM(get_node()->get_logger(), "[CartesianForceController] activating");
Base::on_activate(previous_state);


// Make sure sensor link is part of the robot chain
m_ft_sensor_ref_link = get_node()->get_parameter("ft_sensor_ref_link").as_string();
if(!Base::robotChainContains(m_ft_sensor_ref_link))
{
RCLCPP_ERROR_STREAM(get_node()->get_logger(),
RCLCPP_ERROR_STREAM(get_node()->get_logger(), "[CartesianForceController]: " <<
m_ft_sensor_ref_link << " is not part of the kinematic chain from "
<< Base::m_robot_base_link << " to "
<< Base::m_end_effector_link);
Expand All @@ -103,27 +121,16 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
// Make sure sensor wrenches are interpreted correctly
setFtSensorReferenceFrame(Base::m_end_effector_link);

m_target_wrench_subscriber = get_node()->create_subscription<geometry_msgs::msg::WrenchStamped>(
get_node()->get_name() + std::string("/target_wrench"),
10,
std::bind(&CartesianForceController::targetWrenchCallback, this, std::placeholders::_1));

m_target_wrench.setZero();
m_ft_sensor_wrench.setZero();


m_ft_sensor_wrench_subscriber =
get_node()->create_subscription<geometry_msgs::msg::WrenchStamped>(
get_node()->get_name() + std::string("/ft_sensor_wrench"),
10,
std::bind(&CartesianForceController::ftSensorWrenchCallback, this, std::placeholders::_1));

m_target_wrench.setZero();
m_ft_sensor_wrench.setZero();

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianForceController::on_activate(
const rclcpp_lifecycle::State & previous_state)
{
Base::on_activate(previous_state);
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

Expand Down
1 change: 1 addition & 0 deletions cartesian_motion_controller/src/cartesian_motion_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianMotionController::on_activate(
const rclcpp_lifecycle::State & previous_state)
{
RCLCPP_INFO_STREAM(get_node()->get_logger(), "[CartesianMotionController] activating");
Base::on_activate(previous_state);

// Reset simulation with real joint state
Expand Down

0 comments on commit 94d6279

Please sign in to comment.