Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
The table of contents is too big for display.
Diff view
Diff view
  •  
  •  
  •  
16 changes: 7 additions & 9 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ This is the accompanying code of the paper

## Getting Started

Let's start by creating and populating the colcon workspace with the `unisa_acg_ros2` package suite and its dependencies.
Let's start by creating and populating the colcon workspace with the repo and its dependencies.
In this case we will use `wstool` for convenience, which can be installed via aptitude:

```bash
Expand Down Expand Up @@ -65,34 +65,34 @@ ros2 launch fer_gravity_compensation_pd_controller_demo gravity_compensation_pd_

In a new terminal, send the joint space trajectory:
```bash
ros2 launch follow_joint_trajectory_action_client follow_joint_trajectory_action_client.launch.py input_trajectory_filename:=fer_exciting_acg_trajectory_42_real_0_no_vel action_name:=joint_space_reference_generator fraction_feedback_messages_to_save:=1
ros2 launch follow_joint_trajectory_action_client follow_joint_trajectory_action_client.launch.py input_trajectory_filename:=fer_exciting_xxx_trajectory_42_real_0_no_vel action_name:=joint_space_reference_generator fraction_feedback_messages_to_save:=1
```

### JRG + PID + simulated FER

Launch the simulation, which starts FER with PID controller:
```bash
ros2 launch acg_resources_fer_moveit_config gazebo_ros2_control_demo.launch.py hand:=false controller:=pid_controller enable_effort_interfaces:=true controllers_file:=fer_reference_generator.yaml
ros2 launch xxx_resources_fer_moveit_config gazebo_ros2_control_demo.launch.py hand:=false controller:=pid_controller enable_effort_interfaces:=true controllers_file:=fer_reference_generator.yaml
```

Then, load and activate the `joint_space_command_controller`, for example using the GUI:
Then, load and activate the `joint_space_reference_generator`, for example using the GUI:
```bash
ros2 run rqt_controller_manager rqt_controller_manager
```

In a new terminal, send the joint space trajectory:
```bash
ros2 launch follow_joint_trajectory_action_client follow_joint_trajectory_action_client.launch.py input_trajectory_filename:=fer_exciting_acg_trajectory_42_real action_name:=joint_space_reference_generator fraction_feedback_messages_to_save:=1
ros2 launch follow_joint_trajectory_action_client follow_joint_trajectory_action_client.launch.py input_trajectory_filename:=fer_exciting_xxx_trajectory_42_real action_name:=joint_space_reference_generator fraction_feedback_messages_to_save:=1
```

### JRG + PID + simulated UR10

Launch the simulation, which starts UR10 with PID controller:
```bash
ros2 launch acg_resources_ur10_moveit_config gazebo_ros2_control_demo.launch.py controllers_file:=ur10_test_reference_controllers.yaml rviz_config_file:=config/joint_space_reference_generator_view_ee_pose.rviz initial_joint_controller:=pid_controller
ros2 launch xxx_resources_ur10_moveit_config gazebo_ros2_control_demo.launch.py controllers_file:=ur10_test_reference_controllers.yaml rviz_config_file:=config/joint_space_reference_generator_view_ee_pose.rviz initial_joint_controller:=pid_controller
```

Then, load and activate `joint_space_command_controller`, for example using the GUI:
Then, load and activate `joint_space_reference_generator`, for example using the GUI:
```bash
ros2 run rqt_controller_manager rqt_controller_manager
```
Expand Down Expand Up @@ -124,5 +124,3 @@ In a new terminal, send task-space trajectory:
```bash
ros2 launch follow_task_trajectory_action_client follow_task_trajectory_action_client.launch.py input_trajectory_filename:=wall_sliding_trajectory action_name:=task_space_reference_generator fraction_feedback_messages_to_save:=1
```

© *2025 Automatic Control Group (DIEM, University of Salerno)*
6 changes: 3 additions & 3 deletions cartesian_pose_controller/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -14,14 +14,14 @@ if(CMAKE_COMPILER_IS_GNUCXX
endif()

set(CARTESIAN_POSE_CONTROLLER_INCLUDE_DEPENDS
acg_control_msgs
xxx_control_msgs
controller_interface
generate_parameter_library
geometry_msgs
pluginlib
kinematics_interface
acg_hardware_interface_facade
acg_common_libraries
xxx_hardware_interface_facade
xxx_common_libraries
rclcpp
)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,12 +19,12 @@
#include <kinematics_interface/kinematics_interface.hpp>
#include <pluginlib/class_loader.hpp> // for kinematics_loader_

#include <acg_hardware_interface_facade/state_reader.hpp>
#include <acg_hardware_interface_facade/reference_reader.hpp>
#include <acg_hardware_interface_facade/command_writer.hpp>
#include <acg_control_msgs/msg/task_space_point.hpp>
#include <acg_control_msgs/msg/joint_wrench_point.hpp>
#include <acg_common_libraries/kinematics.hpp>
#include <xxx_hardware_interface_facade/state_reader.hpp>
#include <xxx_hardware_interface_facade/reference_reader.hpp>
#include <xxx_hardware_interface_facade/command_writer.hpp>
#include <xxx_control_msgs/msg/task_space_point.hpp>
#include <xxx_control_msgs/msg/joint_wrench_point.hpp>
#include <xxx_common_libraries/kinematics.hpp>

// auto-generated by generate_parameter_library
#include "cartesian_pose_controller/cartesian_pose_controller_parameters.hpp"
Expand Down Expand Up @@ -119,7 +119,7 @@ class CartesianPoseController : public controller_interface::ChainableController
/**
* @brief Convenience typedef for the Eigen library.
*/
typedef Eigen::Matrix<double, acg_kinematics::NUM_CARTESIAN_DOF, 1> Vector6d;
typedef Eigen::Matrix<double, xxx_kinematics::NUM_CARTESIAN_DOF, 1> Vector6d;

/**
* @brief Constant to store the duration of the throttle interval as an integral value in milliseconds.
Expand All @@ -134,7 +134,7 @@ class CartesianPoseController : public controller_interface::ChainableController
/**
* @brief Internal variable to store the current joint state of the robot.
*/
acg_hardware_interface_facade::RobotJointState robot_joint_state_;
xxx_hardware_interface_facade::RobotJointState robot_joint_state_;

/**
* @brief Number of joints to control.
Expand All @@ -154,12 +154,12 @@ class CartesianPoseController : public controller_interface::ChainableController
/**
* @brief Class for reading the state interfaces.
*/
acg_hardware_interface_facade::StateReader state_reader_;
xxx_hardware_interface_facade::StateReader state_reader_;

/**
* @brief Class for writing commands to the robot.
*/
acg_hardware_interface_facade::CommandWriter command_writer_;
xxx_hardware_interface_facade::CommandWriter command_writer_;

/**
* @brief The task space reference frame with respect to which the task space reference is defined.
Expand All @@ -179,22 +179,22 @@ class CartesianPoseController : public controller_interface::ChainableController
/**
* @brief Current task space reference received from the previous controller in the chain.
*/
acg_control_msgs::msg::TaskSpacePoint task_space_reference_;
xxx_control_msgs::msg::TaskSpacePoint task_space_reference_;

/**
* @brief Last task space reference used in the previous iteration.
*/
acg_control_msgs::msg::TaskSpacePoint last_reference_;
xxx_control_msgs::msg::TaskSpacePoint last_reference_;

/**
* @brief Current joint space command computed by the controller.
*/
acg_control_msgs::msg::JointWrenchPoint joint_space_command_;
xxx_control_msgs::msg::JointWrenchPoint joint_space_command_;

/**
* @brief Last joint space command used in the previous iteration.
*/
acg_control_msgs::msg::JointWrenchPoint last_command_;
xxx_control_msgs::msg::JointWrenchPoint last_command_;

/**
* @brief Eigen vector to store joint velocities computed during the control law calculation.
Expand All @@ -204,12 +204,12 @@ class CartesianPoseController : public controller_interface::ChainableController
/**
* @brief Gain matrix used for computing the control law.
*/
Eigen::Matrix<double, acg_kinematics::NUM_CARTESIAN_DOF, acg_kinematics::NUM_CARTESIAN_DOF> K_matrix_;
Eigen::Matrix<double, xxx_kinematics::NUM_CARTESIAN_DOF, xxx_kinematics::NUM_CARTESIAN_DOF> K_matrix_;

/**
* @brief Kinematics solver used for robot kinematics calculations.
*/
acg_kinematics::RTKinematicsSolver robot_kinematics_;
xxx_kinematics::RTKinematicsSolver robot_kinematics_;

/**
* @brief Upper physical limits of the robot joints for positions.
Expand All @@ -229,7 +229,7 @@ class CartesianPoseController : public controller_interface::ChainableController
/**
* @brief Class for reading the task space reference received as input.
*/
acg_hardware_interface_facade::ReferenceReader reference_reader_;
xxx_hardware_interface_facade::ReferenceReader reference_reader_;
}; // class CartesianPoseController

} // namespace cartesian_pose_controller
6 changes: 3 additions & 3 deletions cartesian_pose_controller/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,9 @@

<buildtool_depend>ament_cmake</buildtool_depend>

<depend>acg_common_libraries</depend>
<depend>acg_control_msgs</depend>
<depend>acg_hardware_interface_facade</depend>
<depend>xxx_common_libraries</depend>
<depend>xxx_control_msgs</depend>
<depend>xxx_hardware_interface_facade</depend>
<depend>controller_interface</depend>
<depend>generate_parameter_library</depend>
<depend>geometry_msgs</depend>
Expand Down
18 changes: 9 additions & 9 deletions cartesian_pose_controller/src/cartesian_pose_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,8 @@
*/

#include <pluginlib/class_list_macros.hpp>
#include <acg_common_libraries/message_utilities.hpp>
#include <acg_common_libraries/urdf_utilities.hpp>
#include <xxx_common_libraries/message_utilities.hpp>
#include <xxx_common_libraries/urdf_utilities.hpp>

#include "cartesian_pose_controller/cartesian_pose_controller.hpp"

Expand Down Expand Up @@ -117,7 +117,7 @@ controller_interface::CallbackReturn CartesianPoseController::on_init()

// Initialize the K matrix with the gains from the parameter handler
K_matrix_.setIdentity();
for (std::size_t i = 0; i < acg_kinematics::NUM_CARTESIAN_DOF; i++)
for (std::size_t i = 0; i < xxx_kinematics::NUM_CARTESIAN_DOF; i++)
{
K_matrix_(i, i) = parameter_handler_->get_params().k_matrix_gains[i];
}
Expand Down Expand Up @@ -148,7 +148,7 @@ controller_interface::CallbackReturn CartesianPoseController::on_configure(const
std::vector<std::string> state_interfaces_names;
state_interfaces_names.emplace_back("position");

acg_hardware_interface_facade::StateInterfaceNamesOverrideConfig state_interfaces_names_override;
xxx_hardware_interface_facade::StateInterfaceNamesOverrideConfig state_interfaces_names_override;
state_interfaces_names_override.position_state_interfaces = parameter_handler_->get_params().state_interfaces_names_override.position;

state_reader_.configure_state_interfaces(state_interfaces_names, parameter_handler_->get_params().joints,
Expand All @@ -158,7 +158,7 @@ controller_interface::CallbackReturn CartesianPoseController::on_configure(const
std::vector<std::string> command_interface_names = parameter_handler_->get_params().command_interfaces;

// Setting the command interface names override struct based on the configuration file for the command interfaces
acg_hardware_interface_facade::CommandInterfaceNamesOverrideConfig command_interface_names_override;
xxx_hardware_interface_facade::CommandInterfaceNamesOverrideConfig command_interface_names_override;
command_interface_names_override.joint_position_interface_names = parameter_handler_->get_params().command_interfaces_names_override.joint_position;
command_interface_names_override.joint_velocity_interface_names = parameter_handler_->get_params().command_interfaces_names_override.joint_velocity;

Expand All @@ -170,7 +170,7 @@ controller_interface::CallbackReturn CartesianPoseController::on_configure(const
reference_interfaces.emplace_back("pose");

// Setting the command interface names override struct based on the configuration file for the reference interfaces
acg_hardware_interface_facade::CommandInterfaceNamesOverrideConfig reference_interface_names_override;
xxx_hardware_interface_facade::CommandInterfaceNamesOverrideConfig reference_interface_names_override;
reference_interface_names_override.task_space_pose_interface_names =
parameter_handler_->get_params().reference_interfaces_names_override.task_space_pose;
if (parameter_handler_->get_params().use_twist_reference)
Expand Down Expand Up @@ -296,7 +296,7 @@ controller_interface::return_type CartesianPoseController::update_and_write_comm
{
// Check if the task space reference is valid.
// Note that the task space reference is not valid (contains NaN values) when the previous controller in the chain is not activated.
if (acg_message_utilities::is_nan(task_space_reference_.pose) || acg_message_utilities::is_nan(task_space_reference_.twist))
if (xxx_message_utilities::is_nan(task_space_reference_.pose) || xxx_message_utilities::is_nan(task_space_reference_.twist))
{
RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), DURATION_MS_,
"Task space reference is NaN. Using the last valid reference.");
Expand Down Expand Up @@ -340,7 +340,7 @@ bool CartesianPoseController::compute_control_law_(const rclcpp::Duration& perio
robot_kinematics_.compute_forward_kinematics(robot_joint_state_.positions, tip_link_, task_space_reference_frame_, task_space_robot_state);

Vector6d pose_error;
acg_kinematics::compute_pose_error(task_space_reference_.pose, task_space_robot_state, pose_error);
xxx_kinematics::compute_pose_error(task_space_reference_.pose, task_space_robot_state, pose_error);

Vector6d desired_twist;
if (reference_reader_.has_task_space_twist_interface())
Expand All @@ -352,7 +352,7 @@ bool CartesianPoseController::compute_control_law_(const rclcpp::Duration& perio
{
// Computing the desired twist from the last reference and the current reference
Vector6d reference_diff;
acg_kinematics::compute_pose_error(task_space_reference_.pose, last_reference_.pose, reference_diff);
xxx_kinematics::compute_pose_error(task_space_reference_.pose, last_reference_.pose, reference_diff);
desired_twist = reference_diff / period.seconds();
}

Expand Down
10 changes: 5 additions & 5 deletions end_effectors/end_effector_builder/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -89,15 +89,15 @@ An example end-effector configuration file is the following:

```yaml
ft_sensor_mounting:
description_package: "acg_resources_generic_mounting_description"
description_package: "xxx_resources_generic_mounting_description"
description_file: "urdf/generic_mounting.urdf.xacro"
description_params:
height: 0.01
radius: 0.05
mass: 0.05

force_torque_sensor:
description_package: "acg_resources_generic_force_torque_sensor_description"
description_package: "xxx_resources_generic_force_torque_sensor_description"
description_file: "urdf/generic_force_torque_sensor.urdf.xacro"
description_params:
height: 0.04
Expand All @@ -114,15 +114,15 @@ force_torque_sensor:
update_rate: 100

tool_mounting:
description_package: "acg_resources_generic_mounting_description"
description_package: "xxx_resources_generic_mounting_description"
description_file: "urdf/generic_mounting.urdf.xacro"
description_params:
height: 0.01
radius: 0.025
mass: 0.0

tool:
description_package: "acg_resources_long_handle_description"
description_package: "xxx_resources_long_handle_description"
description_file: "urdf/long_handle.urdf.xacro"
```

Expand Down Expand Up @@ -180,4 +180,4 @@ Each top-level key in the `yaml` file represents a component of the end-effector
The `yaml` file can also contain additional top-level keys that are not directly related to the end-effector building process.
These keys can be used to store additional information about the end-effector, such as metadata or configuration parameters that are not directly used by the macros in this package.
Please refer to the specific package documentation for more information on how to use these additional keys.
For an example on how to extend the end-effector configuration file with additional keys, see the [acg_resources_ur10_moveit_config package documentation](../../acg_resources_ur10_moveit_config/README.md#end-effector-configuration).
For an example on how to extend the end-effector configuration file with additional keys, see the [xxx_resources_ur10_moveit_config package documentation](../../xxx_resources_ur10_moveit_config/README.md#end-effector-configuration).
2 changes: 1 addition & 1 deletion end_effectors/end_effector_builder/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@

<buildtool_depend>ament_cmake</buildtool_depend>

<exec_depend>acg_resources_ft_sensor_gazebo_description</exec_depend>
<exec_depend>xxx_resources_ft_sensor_gazebo_description</exec_depend>
<exec_depend>gazebo_force_torque_sensor</exec_depend>

<export>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
<xacro:property name="is_tool_mounting_present" value="${'tool_mounting' in ee_config}"/>
<xacro:property name="is_tool_present" value="${'tool' in ee_config}"/>

<xacro:include filename="$(find acg_common_libraries)/xacro/define_property_from_dict.xacro"/>
<xacro:include filename="$(find xxx_common_libraries)/xacro/define_property_from_dict.xacro"/>
<xacro:define_property_from_dict input_dict="${ee_config.get('force_torque_sensor', None)}" name="sensed_joint_name" default_value="ft_sensor_measure_joint"/>
<xacro:define_property_from_dict input_dict="${ee_config.get('force_torque_sensor', None)}" name="sensing_frame_name" default_value="ft_sensing_frame"/>

Expand Down Expand Up @@ -59,9 +59,9 @@
</xacro:unless>

<xacro:if value="${sim_ignition}">
<xacro:include filename="$(find acg_resources_ft_sensor_gazebo_description)/urdf/gazebo_ft_sensor_description.xacro"/>
<xacro:include filename="$(find xxx_resources_ft_sensor_gazebo_description)/urdf/gazebo_ft_sensor_description.xacro"/>
<xacro:gazebo_ft_sensor_description config="${ee_config.force_torque_sensor.get('gazebo_params', None)}" ft_sensed_joint="${sensed_joint_name}"/>
<xacro:include filename="$(find acg_resources_ft_sensor_gazebo_description)/urdf/gazebo_force_torque_sensor.ros2_control.xacro"/>
<xacro:include filename="$(find xxx_resources_ft_sensor_gazebo_description)/urdf/gazebo_force_torque_sensor.ros2_control.xacro"/>
<xacro:build_ros2_control_description config="${ee_config.force_torque_sensor.get('ros2_control_params', None)}"/>
</xacro:if>

Expand Down
4 changes: 2 additions & 2 deletions end_effectors/force_torque_sensors/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,8 @@ The `.urdf.xacro` file must define a xacro macro named `build_force_torque_senso
* `config`: a dictionary containing the configuration parameters of the sensor provided by the user.
The provided configuration parameters should be used to generate the URDF description of the sensor.
A configuration parameter can be either mandatory or optional (i.e. it has a default value).
To read the value of a configuration parameter, the [`define_property_from_dict`](../../acg_common_libraries/xacro/define_property_from_dict.xacro) macro should be used.
For more information on how to use this macro, please refer to the [`acg_common_libraries` package README](../../acg_common_libraries/README.md#using-the-define_property_from_dict-macro) or any of the existing mountings in this library.
To read the value of a configuration parameter, the [`define_property_from_dict`](../../xxx_common_libraries/xacro/define_property_from_dict.xacro) macro should be used.
For more information on how to use this macro, please refer to the [`xxx_common_libraries` package README](../../xxx_common_libraries/README.md#using-the-define_property_from_dict-macro) or any of the existing mountings in this library.
* `enable_ft_sensing`: a boolean that indicates whether the force/torque sensor should sense forces and torques.
If `true`, the `sensed_joint_name` and `sensing_frame_name` parameters should be defined.
If `false`, the sensor will not sense forces and torques, but it will still be mounted to the robot.
Expand Down

This file was deleted.

Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.8)
project(acg_resources_ft_sensor_gazebo_description)
project(xxx_resources_ft_sensor_gazebo_description)

find_package(ament_cmake REQUIRED)

Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
# acg_resources_ft_sensor_gazebo_description
# xxx_resources_ft_sensor_gazebo_description

This package collects resources needed for simulating a force/torque sensor in the Gazebo simulator.

Expand Down
Loading