diff --git a/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp b/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp index 1069908a..f4edd04a 100644 --- a/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp +++ b/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp @@ -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; } @@ -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; } diff --git a/cartesian_controller_base/src/ForwardDynamicsSolver.cpp b/cartesian_controller_base/src/ForwardDynamicsSolver.cpp index 21bc2cea..5bc9da17 100644 --- a/cartesian_controller_base/src/ForwardDynamicsSolver.cpp +++ b/cartesian_controller_base/src/ForwardDynamicsSolver.cpp @@ -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(m_params + "/link_mass", 0.1); + if(!nh->has_parameter(m_params + "/link_mass")) + m_min = nh->declare_parameter(m_params + "/link_mass", 0.1); + else + m_min = nh->get_parameter(m_params + "/link_mass").get_value(); 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); diff --git a/cartesian_controller_base/src/cartesian_controller_base.cpp b/cartesian_controller_base/src/cartesian_controller_base.cpp index aad4749e..0325b089 100644 --- a/cartesian_controller_base/src/cartesian_controller_base.cpp +++ b/cartesian_controller_base/src/cartesian_controller_base.cpp @@ -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 >( + get_node()->create_publisher( + std::string(get_node()->get_name()) + "/current_pose", 3)); + + m_feedback_twist_publisher = + std::make_shared >( + get_node()->create_publisher( + 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; @@ -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()); @@ -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 >( - get_node()->create_publisher( - std::string(get_node()->get_name()) + "/current_pose", 3)); - m_feedback_twist_publisher = - std::make_shared >( - get_node()->create_publisher( - 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) @@ -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_, diff --git a/cartesian_force_controller/src/cartesian_force_controller.cpp b/cartesian_force_controller/src/cartesian_force_controller.cpp index 954ce82c..5b59389b 100644 --- a/cartesian_force_controller/src/cartesian_force_controller.cpp +++ b/cartesian_force_controller/src/cartesian_force_controller.cpp @@ -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( + 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); @@ -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( - 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( 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; } diff --git a/cartesian_motion_controller/src/cartesian_motion_controller.cpp b/cartesian_motion_controller/src/cartesian_motion_controller.cpp index 359152e4..9b29fe63 100644 --- a/cartesian_motion_controller/src/cartesian_motion_controller.cpp +++ b/cartesian_motion_controller/src/cartesian_motion_controller.cpp @@ -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