Skip to content

Commit ad292a2

Browse files
kamiradikamiradi
committed
adds controller boilerplate and a simple gui to test
1 parent f73ddf8 commit ad292a2

File tree

6 files changed

+190
-1
lines changed

6 files changed

+190
-1
lines changed

‎CMakeLists.txt

+12
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,9 @@ find_package(ament_cmake REQUIRED)
99
# Common cmake code applied to all moveit packages
1010
find_package(moveit_common REQUIRED)
1111
moveit_package()
12+
set(CMAKE_AUTOMOC ON)
13+
set(CMAKE_AUTOUIC ON)
14+
set(CMAKE_AUTORCC ON)
1215

1316
find_package(ament_cmake REQUIRED)
1417
find_package(Eigen3 REQUIRED)
@@ -25,6 +28,10 @@ find_package(rviz_visual_tools REQUIRED)
2528
find_package(warehouse_ros REQUIRED)
2629
find_package(shape_msgs REQUIRED)
2730
find_package(ros2_control REQUIRED)
31+
find_package(controller_interface REQUIRED)
32+
find_package(hardware_interface REQUIRED)
33+
find_package(std_msgs REQUIRED)
34+
find_package(Qt5 REQUIRED COMPONENTS Widgets)
2835

2936
generate_parameter_library(ktopt_moveit_parameters
3037
parameters/ktopt_moveit_parameters.yaml)
@@ -50,6 +57,7 @@ add_library(
5057
src/ktopt_planning_context.cpp
5158
# TOPPRA
5259
src/add_toppra_time_parameterization.cpp
60+
src/joint_stiffness_controller.cpp
5361
# Conversions
5462
src/conversions.cpp)
5563

@@ -61,6 +69,10 @@ ament_target_dependencies(
6169
pluginlib
6270
rclcpp
6371
shape_msgs
72+
rclcpp
73+
controller_interface
74+
hardware_interface
75+
std_msgs
6476
ros2_control)
6577
target_link_libraries(moveit_drake drake::drake ktopt_moveit_parameters)
6678

‎demo/CMakeLists.txt

+8
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,14 @@ ament_target_dependencies(pipeline_testbench_example
55
add_executable(constrained_planning_demo src/constrained_planning_demo.cpp)
66
ament_target_dependencies(constrained_planning_demo
77
${THIS_PACKAGE_INCLUDE_DEPENDS})
8+
# GUI
9+
add_executable(stiffness_gui src/control_gui.cpp)
10+
target_link_libraries(stiffness_gui Qt5::Widgets)
11+
ament_target_dependencies(stiffness_gui rclcpp std_msgs)
12+
13+
install(TARGETS stiffness_gui
14+
DESTINATION lib/${PROJECT_NAME}
15+
)
816

917
install(TARGETS pipeline_testbench_example constrained_planning_demo
1018
DESTINATION lib/${PROJECT_NAME})

‎demo/src/control_gui.cpp

+73
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
#include <QApplication>
2+
#include <QPushButton>
3+
#include <QVBoxLayout>
4+
#include <QDoubleSpinBox>
5+
#include <QWidget>
6+
#include <QLabel>
7+
#include <rclcpp/rclcpp.hpp>
8+
#include <std_msgs/msg/float64_multi_array.hpp>
9+
10+
class StiffnessGUI : public QWidget {
11+
Q_OBJECT
12+
13+
public:
14+
StiffnessGUI(std::shared_ptr<rclcpp::Node> node) : node_(node) {
15+
publisher_ = node_->create_publisher<std_msgs::msg::Float64MultiArray>("/joint_stiffness_controller/command", 10);
16+
17+
auto *layout = new QVBoxLayout(this);
18+
19+
pos_spin1 = new QDoubleSpinBox();
20+
pos_spin2 = new QDoubleSpinBox();
21+
stiff_spin1 = new QDoubleSpinBox();
22+
stiff_spin2 = new QDoubleSpinBox();
23+
24+
layout->addWidget(new QLabel("Position Joint 1"));
25+
layout->addWidget(pos_spin1);
26+
layout->addWidget(new QLabel("Stiffness Joint 1"));
27+
layout->addWidget(stiff_spin1);
28+
layout->addWidget(new QLabel("Position Joint 2"));
29+
layout->addWidget(pos_spin2);
30+
layout->addWidget(new QLabel("Stiffness Joint 2"));
31+
layout->addWidget(stiff_spin2);
32+
33+
auto *send_btn = new QPushButton("Send Command");
34+
layout->addWidget(send_btn);
35+
connect(send_btn, &QPushButton::clicked, this, &StiffnessGUI::send_command);
36+
}
37+
38+
private slots:
39+
void send_command() {
40+
auto msg = std_msgs::msg::Float64MultiArray();
41+
msg.data = {
42+
pos_spin1->value(), pos_spin2->value(),
43+
stiff_spin1->value(), stiff_spin2->value()
44+
};
45+
publisher_->publish(msg);
46+
}
47+
48+
private:
49+
std::shared_ptr<rclcpp::Node> node_;
50+
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_;
51+
52+
QDoubleSpinBox *pos_spin1;
53+
QDoubleSpinBox *pos_spin2;
54+
QDoubleSpinBox *stiff_spin1;
55+
QDoubleSpinBox *stiff_spin2;
56+
};
57+
58+
int main(int argc, char **argv) {
59+
rclcpp::init(argc, argv);
60+
QApplication app(argc, argv);
61+
62+
auto node = rclcpp::Node::make_shared("stiffness_gui");
63+
StiffnessGUI gui(node);
64+
gui.show();
65+
66+
std::thread spin_thread([&]() { rclcpp::spin(node); });
67+
68+
int ret = app.exec();
69+
rclcpp::shutdown();
70+
spin_thread.join();
71+
return ret;
72+
}
73+
#include "control_gui.moc"

‎include/stiffness_control/joint_stiffness_controller.hpp

+32
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
#pragma once
2+
3+
#include <rclcpp/rclcpp.hpp>
4+
#include <controller_interface/controller_interface.hpp>
5+
#include <hardware_interface/loaned_command_interface.hpp>
6+
#include <hardware_interface/loaned_state_interface.hpp>
7+
#include <std_msgs/msg/float64_multi_array.hpp>
8+
9+
namespace joint_stiffness_controller {
10+
11+
class JointStiffnessController : public controller_interface::ControllerInterface {
12+
public:
13+
JointStiffnessController();
14+
15+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
16+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
17+
controller_interface::return_type update(const rclcpp::Time&, const rclcpp::Duration&) override;
18+
controller_interface::CallbackReturn on_init() override;
19+
controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& state) override;
20+
21+
private:
22+
std::vector<hardware_interface::LoanedCommandInterface> position_cmd_handles_;
23+
std::vector<hardware_interface::LoanedStateInterface> position_state_handles_;
24+
25+
rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr command_sub_;
26+
std::vector<double> desired_positions_;
27+
std::vector<double> stiffness_gains_;
28+
29+
void command_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
30+
};
31+
32+
} // namespace joint_stiffness_controller

‎plugin_descriptions.xml

+7-1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
<library path="moveit_drake">
1+
<library path="moveit_drake">
22
<!-- KTOPT adapter -->
33
<class name="ktopt_interface/KTOptPlanner"
44
type="ktopt_interface::KTOptPlannerManager"
@@ -13,4 +13,10 @@
1313
Given a path, add time parameterization with TOPRRA.
1414
</description>
1515
</class>
16+
<!-- Joint Stiffness Controller -->
17+
<class name="moveit_drake/JointStiffnessController" type="moveit_drake::JointStiffnessController" base_class_type="controller_interface::ControllerInterface">
18+
<description>
19+
A controller for joint stiffness control.
20+
</description>
21+
</class>
1622
</library>

‎src/joint_stiffness_controller.cpp

+58
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
#include "stiffness_control/joint_stiffness_controller.hpp"
2+
3+
namespace joint_stiffness_controller {
4+
5+
JointStiffnessController::JointStiffnessController() {}
6+
7+
controller_interface::CallbackReturn JointStiffnessController::on_init() {
8+
return controller_interface::CallbackReturn::SUCCESS;
9+
}
10+
11+
controller_interface::CallbackReturn JointStiffnessController::on_configure(const rclcpp_lifecycle::State&) {
12+
auto node = get_node();
13+
command_sub_ = node->create_subscription<std_msgs::msg::Float64MultiArray>(
14+
"~/command", 10, std::bind(&JointStiffnessController::command_callback, this, std::placeholders::_1));
15+
return controller_interface::CallbackReturn::SUCCESS;
16+
}
17+
18+
controller_interface::InterfaceConfiguration JointStiffnessController::command_interface_configuration() const {
19+
return {
20+
controller_interface::interface_configuration_type::INDIVIDUAL,
21+
{"joint1/position", "joint2/position"} // replace with your actual joints
22+
};
23+
}
24+
25+
controller_interface::InterfaceConfiguration JointStiffnessController::state_interface_configuration() const {
26+
return {
27+
controller_interface::interface_configuration_type::INDIVIDUAL,
28+
{"joint1/position", "joint2/position"} // same joints
29+
};
30+
}
31+
32+
void JointStiffnessController::command_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) {
33+
size_t n = msg->data.size() / 2;
34+
desired_positions_.resize(n);
35+
stiffness_gains_.resize(n);
36+
for (size_t i = 0; i < n; ++i) {
37+
desired_positions_[i] = msg->data[i];
38+
stiffness_gains_[i] = msg->data[i + n];
39+
}
40+
}
41+
42+
controller_interface::return_type JointStiffnessController::update(
43+
const rclcpp::Time&, const rclcpp::Duration&) {
44+
for (size_t i = 0; i < position_cmd_handles_.size(); ++i) {
45+
double error = desired_positions_[i] - position_state_handles_[i].get_optional().value_or(0.0);
46+
double command = stiffness_gains_[i] * error;
47+
bool success = position_cmd_handles_[i].set_value(position_state_handles_[i].get_optional().value_or(0.0) + command);
48+
if (!success) {
49+
RCLCPP_WARN(get_node()->get_logger(), "Failed to set command for joint %zu", i);
50+
}
51+
}
52+
return controller_interface::return_type::OK;
53+
}
54+
55+
} // namespace joint_stiffness_controller
56+
57+
#include "pluginlib/class_list_macros.hpp"
58+
PLUGINLIB_EXPORT_CLASS(joint_stiffness_controller::JointStiffnessController, controller_interface::ControllerInterface)

0 commit comments

Comments
 (0)