Skip to content

Commit c60df1e

Browse files
Hydran00Hydran00
committed
Refactor controller configurations to drop compliance_ref_link and added gravity compensation controller
1 parent 100b8d1 commit c60df1e

File tree

10 files changed

+353
-24
lines changed

10 files changed

+353
-24
lines changed

‎cartesian_impedance_controller/README.md‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ cartesian_impedance_controller:
2020
end_effector_link: "tool0"
2121
robot_base_link: "base_link"
2222
ft_sensor_ref_link: "sensor_link"
23-
compliance_ref_link: "compliance_link"
23+
command_current_configuration: false # for KUKA set this to true, for other robots set to false
2424
max_impedance_force: 10.0 # (N) maximum task force
2525
delta_tau_max: 1.0 # (Nm) maximum torque increment in one control cycle
2626
joints:

‎effort_controller_base/include/effort_controller_base/effort_controller_base.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -192,10 +192,11 @@ class EffortControllerBase : public controller_interface::ControllerInterface {
192192

193193
// Dynamic parameters
194194
std::string m_end_effector_link;
195-
std::string m_compliance_ref_link;
195+
// std::string m_compliance_ref_link;
196196
std::string m_robot_base_link;
197197
bool m_compensate_gravity;
198198
bool m_compensate_coriolis;
199+
bool m_command_current_configuration_; // KUKA specific, for KUKA set this to true
199200
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
200201
m_joint_state_pos_handles;
201202
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
@@ -235,7 +236,6 @@ class EffortControllerBase : public controller_interface::ControllerInterface {
235236
KDL::JntArray m_joint_effort_limits;
236237
double m_delta_tau_max;
237238

238-
bool m_kuka_hw;
239239
};
240240

241241
} // namespace effort_controller_base

‎effort_controller_base/src/effort_controller_base.cpp‎

Lines changed: 20 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ EffortControllerBase::on_init() {
6060
auto_declare<std::string>("robot_base_link", "");
6161
auto_declare<std::string>("compliance_ref_link", "");
6262
auto_declare<std::string>("end_effector_link", "");
63-
auto_declare<bool>("kuka_hw", false);
63+
auto_declare<bool>("command_current_configuration", false);
6464
auto_declare<bool>("compensate_gravity", false);
6565
auto_declare<bool>("compensate_coriolis", false);
6666
auto_declare<double>("delta_tau_max", 1.0);
@@ -153,13 +153,13 @@ EffortControllerBase::on_configure(
153153
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
154154
CallbackReturn::ERROR;
155155
}
156-
m_compliance_ref_link =
157-
get_node()->get_parameter("compliance_ref_link").as_string();
158-
if (m_compliance_ref_link.empty()) {
159-
RCLCPP_ERROR(get_node()->get_logger(), "compliance_ref_link is empty");
160-
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
161-
CallbackReturn::ERROR;
162-
}
156+
// m_compliance_ref_link =
157+
// get_node()->get_parameter("compliance_ref_link").as_string();
158+
// if (m_compliance_ref_link.empty()) {
159+
// RCLCPP_ERROR(get_node()->get_logger(), "compliance_ref_link is empty");
160+
// return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
161+
// CallbackReturn::ERROR;
162+
// }
163163

164164
// Build a kinematic chain of the robot
165165
if (!robot_model.initString(m_robot_description)) {
@@ -183,14 +183,14 @@ EffortControllerBase::on_configure(
183183
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
184184
CallbackReturn::ERROR;
185185
}
186-
if (!robotChainContains(m_compliance_ref_link)) {
187-
RCLCPP_ERROR_STREAM(get_node()->get_logger(),
188-
m_compliance_ref_link
189-
<< " is not part of the kinematic chain from "
190-
<< m_robot_base_link << " to "
191-
<< m_end_effector_link);
192-
return CallbackReturn::ERROR;
193-
}
186+
// if (!robotChainContains(m_compliance_ref_link)) {
187+
// RCLCPP_ERROR_STREAM(get_node()->get_logger(),
188+
// m_compliance_ref_link
189+
// << " is not part of the kinematic chain from "
190+
// << m_robot_base_link << " to "
191+
// << m_end_effector_link);
192+
// return CallbackReturn::ERROR;
193+
// }
194194

195195
// Get names of actuated joints
196196
m_joint_names = get_node()->get_parameter("joints").as_string_array();
@@ -287,8 +287,8 @@ EffortControllerBase::on_configure(
287287
CallbackReturn::ERROR;
288288
}
289289
// Check if kuka is been used
290-
m_kuka_hw = get_node()->get_parameter("kuka_hw").as_bool();
291-
if (m_kuka_hw == true) {
290+
m_command_current_configuration_ = get_node()->get_parameter("command_current_configuration").as_bool();
291+
if (m_command_current_configuration_ == true) {
292292
RCLCPP_WARN(
293293
get_node()->get_logger(),
294294
"Using Kuka, the position will be overwritten at each control cycle to "
@@ -340,7 +340,7 @@ EffortControllerBase::on_activate(
340340

341341
// Get command handles.
342342
// Position
343-
if (m_kuka_hw == true) {
343+
if (m_command_current_configuration_ == true) {
344344
if (!controller_interface::get_ordered_interfaces(
345345
command_interfaces_, m_joint_names,
346346
hardware_interface::HW_IF_POSITION, m_joint_cmd_pos_handles)) {
@@ -420,7 +420,7 @@ void EffortControllerBase::writeJointEffortCmds() {
420420
}
421421
}
422422
}
423-
if (m_kuka_hw == true) {
423+
if (m_command_current_configuration_ == true) {
424424
for (size_t i = 0; i < m_joint_number; ++i) {
425425
m_joint_cmd_pos_handles[i].get().set_value(m_joint_positions(i));
426426
}

‎gravity_compensation/CMakeLists.txt‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(gravity_compensation)
3+
4+
set(CMAKE_CXX_STANDARD 17)
5+
set(ADDITIONAL_COMPILE_OPTIONS -Wall -Wextra -Wpedantic -Wno-unused-parameter)
6+
add_compile_options(${ADDITIONAL_COMPILE_OPTIONS})
7+
# build in release
8+
if(NOT CMAKE_BUILD_TYPE)
9+
set(CMAKE_BUILD_TYPE Release)
10+
endif()
11+
find_package(Eigen3 3.3 REQUIRED NO_MODULE)
12+
add_definitions(-DEIGEN_MPL2_ONLY)
13+
find_package(ament_cmake REQUIRED)
14+
find_package(rclcpp REQUIRED)
15+
find_package(effort_controller_base REQUIRED)
16+
find_package(debug_msg REQUIRED)
17+
18+
# Convenience variable for dependencies
19+
set(THIS_PACKAGE_INCLUDE_DEPENDS
20+
rclcpp
21+
effort_controller_base
22+
Eigen3
23+
debug_msg
24+
)
25+
26+
ament_export_dependencies(
27+
${THIS_PACKAGE_INCLUDE_DEPENDS}
28+
)
29+
30+
#--------------------------------------------------------------------------------
31+
# Libraries
32+
#--------------------------------------------------------------------------------
33+
add_library(${PROJECT_NAME} SHARED
34+
src/gravity_compensation.cpp
35+
)
36+
37+
target_include_directories(${PROJECT_NAME}
38+
PUBLIC
39+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
40+
$<INSTALL_INTERFACE:include>
41+
PRIVATE
42+
${EIGEN3_INCLUDE_DIR}
43+
)
44+
45+
ament_target_dependencies(${PROJECT_NAME}
46+
${${PROJECT_NAME}_EXPORTED_TARGETS}
47+
${THIS_PACKAGE_INCLUDE_DEPENDS}
48+
)
49+
50+
# Prevent pluginlib from using boost
51+
target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
52+
53+
#--------------------------------------------------------------------------------
54+
# Install and export
55+
#--------------------------------------------------------------------------------
56+
57+
pluginlib_export_plugin_description_file(controller_interface gravity_compensation_plugin.xml)
58+
59+
# Note: The workflow as described here https://docs.ros.org/en/foxy/How-To-Guides/Ament-CMake-Documentation.html#building-a-library
60+
# does not work for me.
61+
62+
#ament_export_targets(my_targets_from_this_package HAS_LIBRARY_TARGET)
63+
64+
install(
65+
DIRECTORY include/
66+
DESTINATION include
67+
)
68+
69+
install(
70+
TARGETS ${PROJECT_NAME}
71+
#EXPORT my_targets_from_this_package
72+
LIBRARY DESTINATION lib
73+
ARCHIVE DESTINATION lib
74+
RUNTIME DESTINATION bin
75+
#INCLUDES DESTINATION include
76+
)
77+
78+
# Note: For the target based workflow, they seem to be superfluous.
79+
# But since that doesn't work yet, I'll add them just in case.
80+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
81+
ament_export_include_directories(
82+
include
83+
)
84+
ament_export_libraries(
85+
${PROJECT_NAME}
86+
)
87+
88+
ament_package()

‎gravity_compensation/README.md‎

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
# Gravity Compensation Controller
2+
3+
This controller implements a simple gravity compensation controller for a robot arm. It computes the required torque commands to counteract the gravitational forces acting on the robot's joints. For KUKA robots, it just send zero torque command, since the KUKA robots are already gravity compensated.
4+
5+
## Example Configuration
6+
Below is an example `controller_manager.yaml` for a controller specific configuration.
7+
```yaml
8+
controller_manager:
9+
ros__parameters:
10+
update_rate: 1000 # Hz
11+
12+
gravity_compensation:
13+
type: gravity_compensation/GravityCompensation
14+
15+
# More controller instances here
16+
# ...
17+
18+
/**/gravity_compensation:
19+
ros__parameters:
20+
end_effector_link: "lbr_link_ee"
21+
ft_sensor_ref_link: "lbr_link_ee" # Reference link for the force-torque sensor, if applicable
22+
robot_base_link: "lbr_link_0"
23+
compensate_gravity: false # for robots different than KUKA set to true
24+
compensate_coriolis: false # for robots different than KUKA set to true
25+
command_current_configuration: true # for KUKA set this to true, for other robots set to false
26+
command_interfaces:
27+
- effort
28+
state_interfaces:
29+
- position
30+
- velocity
31+
joints:
32+
- lbr_A1
33+
- lbr_A2
34+
- lbr_A3
35+
- lbr_A4
36+
- lbr_A5
37+
- lbr_A6
38+
- lbr_A7
39+
40+
41+
# More controller specifications here
42+
# ...
43+
44+
```
45+

‎gravity_compensation/gravity_compensation_plugin.xml‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
<library path="gravity_compensation">
2+
<class name="gravity_compensation/GravityCompensation"
3+
type="gravity_compensation::GravityCompensation"
4+
base_class_type="controller_interface::ControllerInterface">
5+
<description>
6+
The GravityCompensation which computes the torque commands for a robot joint based on the
7+
impedance control law.
8+
</description>
9+
</class>
10+
</library>

‎gravity_compensation/include/gravity_compensation/gravity_compensation.h‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
#ifndef GRAVITY_COMPENSATION_H_INCLUDED
2+
#define GRAVITY_COMPENSATION_H_INCLUDED
3+
4+
#include <effort_controller_base/effort_controller_base.h>
5+
6+
#include <controller_interface/controller_interface.hpp>
7+
8+
#include "controller_interface/controller_interface.hpp"
9+
#include "debug_msg/msg/debug.hpp"
10+
#include "effort_controller_base/Utility.h"
11+
#include "geometry_msgs/msg/pose_stamped.hpp"
12+
#include "geometry_msgs/msg/wrench_stamped.hpp"
13+
#define DEBUG 0
14+
namespace gravity_compensation {
15+
16+
class GravityCompensation
17+
: public virtual effort_controller_base::EffortControllerBase {
18+
public:
19+
GravityCompensation();
20+
21+
virtual LifecycleNodeInterface::CallbackReturn on_init() override;
22+
23+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
24+
on_configure(const rclcpp_lifecycle::State &previous_state) override;
25+
26+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
27+
on_activate(const rclcpp_lifecycle::State &previous_state) override;
28+
29+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
30+
on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
31+
32+
controller_interface::return_type update(
33+
const rclcpp::Time &time, const rclcpp::Duration &period) override;
34+
35+
ctrl::VectorND computeTorque();
36+
37+
using Base = effort_controller_base::EffortControllerBase;
38+
39+
40+
private:
41+
ctrl::Vector6D compensateGravity();
42+
43+
ctrl::Vector6D m_ft_sensor_wrench;
44+
std::string m_ft_sensor_ref_link;
45+
KDL::Frame m_ft_sensor_transform;
46+
47+
ctrl::MatrixND m_identity;
48+
};
49+
50+
} // namespace gravity_compensation {
51+
52+
#endif

‎gravity_compensation/package.xml‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>gravity_compensation</name>
5+
<version>0.0.0</version>
6+
<description>The gravity_compensation package</description>
7+
<maintainer email="[email protected]">Davide nardi</maintainer>
8+
<license>BSD</license>
9+
<url type="repository">https://github.com/lucabeber/effort_controllers</url>
10+
<author email="[email protected]">Davide Nardi</author>
11+
12+
<buildtool_depend>ament_cmake</buildtool_depend>
13+
14+
<depend>rclcpp</depend>
15+
<depend>effort_controller_base</depend>
16+
<depend>controller_interface</depend>
17+
<depend>debug_msg</depend>
18+
19+
<export>
20+
<build_type>ament_cmake</build_type>
21+
<controller_interface plugin="${prefix}/gravity_compensation_plugin.xml" />
22+
</export>
23+
</package>

0 commit comments

Comments
 (0)