diff --git a/lbr_bringup/config/config.rviz b/lbr_bringup/config/config.rviz index b3a3df75..c2e77ab4 100644 --- a/lbr_bringup/config/config.rviz +++ b/lbr_bringup/config/config.rviz @@ -9,6 +9,7 @@ Panels: - /RobotModel1 - /RobotModel1/Description Topic1 - /Wrench1 + - /InteractiveMarkers1 Splitter Ratio: 0.40760868787765503 Tree Height: 724 - Class: rviz_common/Selection @@ -143,6 +144,15 @@ Visualization Manager: Torque Arrow Scale: 0.30000001192092896 Torque Color: 170; 85; 255 Value: true + - Class: rviz_default_plugins/InteractiveMarkers + Enable Transparency: true + Enabled: true + Interactive Markers Namespace: /lbr/motion_control_handle + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Value: true Enabled: true Global Options: Background Color: 21; 21; 26 @@ -225,5 +235,5 @@ Window Geometry: Views: collapsed: false Width: 1850 - X: 1080 - Y: 450 + X: 710 + Y: 382 diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index c498fe63..957e59bc 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -43,9 +43,9 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( controller="lbr_state_broadcaster" ) - # lbr_motion_control_handle = LBRROS2ControlMixin.node_controller_spawner( - # controller="motion_control_handle" - # ) + lbr_motion_control_handle = LBRROS2ControlMixin.node_controller_spawner( + controller="motion_control_handle" + ) controller = LBRROS2ControlMixin.node_controller_spawner( controller=LaunchConfiguration("ctrl") ) @@ -57,8 +57,8 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: joint_state_broadcaster, force_torque_broadcaster, lbr_state_broadcaster, - controller, # lbr_motion_control_handle, + controller, ], ) ) diff --git a/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp b/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp index 9e700ffe..a640affd 100644 --- a/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp +++ b/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp @@ -14,7 +14,7 @@ #include "lbr_fri_ros2/utils.hpp" class TorqueSineOverlay { - static constexpr double amplitude_ = 15.; // Nm + static constexpr double amplitude_ = 0.5; // Nm static constexpr double frequency_ = 0.25; // Hz public: diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.xacro index 7e63d77f..075111d9 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.xacro @@ -292,5 +292,14 @@ + + + + + + + + + - \ No newline at end of file + diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index 5228a443..86dc6a72 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -31,15 +31,17 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, } // PID - if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(pid_parameters_, state.sample_time); - } - joint_position_pid_.compute( - command_target_.joint_position, state.measured_joint_position, - std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), - command_.joint_position); + // if (!joint_position_pid_.is_initialized()) { + // joint_position_pid_.initialize(pid_parameters_, state.sample_time); + // } + // joint_position_pid_.compute( + // command_target_.joint_position, state.measured_joint_position, + // std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), + // command_.joint_position); + command_.torque = command_target_.torque; - + command_.joint_position = command_target_.joint_position; + // RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), "Command: " << command_.torque.data()[0]<< " " << command_.torque.data()[1]<< " " << command_.torque.data()[2]<< " " << command_.torque.data()[3]<< " " << command_.torque.data()[4]<< " " << command_.torque.data()[5]<< " " << command_.torque.data()[6]); // validate if (!command_guard_->is_valid_command(command_, state)) { std::string err = "Invalid command."; diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 0d7a0b62..8ce52b57 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -2,7 +2,7 @@ # managers regardless of their namespace. Usefull in multi-robot setups. /**/controller_manager: ros__parameters: - update_rate: 500 + update_rate: 1000 # ROS 2 control broadcasters joint_state_broadcaster: @@ -84,7 +84,7 @@ # tool. When you specify a target_wrench, i.e. some additional forces that # your robot should apply to its environment, that target_wrench gets # applied in this frame. - end_effector_link: "link_ee" + end_effector_link: "link_tool" # This is usually the link directly before the first actuated joint. All # controllers will build a kinematic chain from this link up to @@ -100,12 +100,12 @@ # This is the link that the robot feels compliant about. It does not need # to coincide with the end_effector_link, but for many use cases, this - # configuration is handy. When working with a screwdriver, for instance, + # configuration is handy. When working with a screwdriver, for instance, # setting compliance_ref_link == end_effector_link makes it easy to specify # downward pushing forces without generating unwanted offset moments. # On the other hand, an application could benefit from yielding a little in # the robot's wrist while drawing a line on a surface with a pen. - compliance_ref_link: "link_ee" + compliance_ref_link: "link_tool" command_interfaces: - effort @@ -122,16 +122,16 @@ - A7 delta_tau_max: 1.0 # Nm stiffness: # w.r.t. compliance_ref_link coordinates - trans_x: 200.0 - trans_y: 200.0 - trans_z: 200.0 - rot_x: 20.0 - rot_y: 20.0 - rot_z: 20.0 + trans_x: 2800.0 # 1900.0 + trans_y: 2800.0 # 1900.0 + trans_z: 1500.0 # 700.0 + rot_x: 180.0 + rot_y: 180.0 + rot_z: 180.0 kuka: true /**/motion_control_handle: ros__parameters: - end_effector_link: "link_ee" + end_effector_link: "link_tool" robot_base_link: "link_0" ft_sensor_ref_link: "link_ee" joints: @@ -145,7 +145,7 @@ /**/gravity_compensation: ros__parameters: - end_effector_link: "link_ee" + end_effector_link: "link_tool" robot_base_link: "link_0" ft_sensor_ref_link: "link_ee" command_interfaces: @@ -161,4 +161,4 @@ - A5 - A6 - A7 - kuka: true \ No newline at end of file + kuka: true diff --git a/lbr_ros2_control/src/lbr_torque_command_controller.cpp b/lbr_ros2_control/src/lbr_torque_command_controller.cpp index 2892f8eb..82eef2a3 100644 --- a/lbr_ros2_control/src/lbr_torque_command_controller.cpp +++ b/lbr_ros2_control/src/lbr_torque_command_controller.cpp @@ -80,6 +80,14 @@ bool LBRTorqueCommandController::reference_command_interfaces_() { torque_command_interfaces_.emplace_back(std::ref(command_interface)); } } + // Print command interfaces + RCLCPP_INFO(this->get_node()->get_logger(), "Command interfaces:"); + for (const auto &joint_position_command_interface : joint_position_command_interfaces_) { + RCLCPP_INFO(this->get_node()->get_logger(), " %s", joint_position_command_interface.get().get_name()); + } + for (const auto &torque_command_interface : torque_command_interfaces_) { + RCLCPP_INFO(this->get_node()->get_logger(), " %s", torque_command_interface.get().get_name()); + } if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { RCLCPP_ERROR( this->get_node()->get_logger(),