Modulo 5.0.0
Loading...
Searching...
No Matches
RobotControllerInterface.cpp
1#include "modulo_controllers/RobotControllerInterface.hpp"
2
3#include "ament_index_cpp/get_package_share_directory.hpp"
4#include <hardware_interface/types/hardware_interface_type_values.hpp>
5
6#include <state_representation/exceptions/JointNotFoundException.hpp>
7
8using namespace state_representation;
9
10namespace modulo_controllers {
11
12static const std::map<std::string, JointStateVariable> interface_map =// NOLINT(cert-err58-cpp)
13 {{hardware_interface::HW_IF_POSITION, JointStateVariable::POSITIONS},
14 {hardware_interface::HW_IF_VELOCITY, JointStateVariable::VELOCITIES},
15 {hardware_interface::HW_IF_ACCELERATION, JointStateVariable::ACCELERATIONS},
16 {hardware_interface::HW_IF_EFFORT, JointStateVariable::TORQUES}};
17
19
21 bool robot_model_required, const std::string& control_type, bool load_geometries)
22 : ControllerInterface(true),
23 control_type_(control_type),
24 robot_model_required_(robot_model_required),
25 load_geometries_(load_geometries),
26 new_joint_command_ready_(false),
27 command_decay_factor_(0.0),
28 command_rate_limit_(std::numeric_limits<double>::infinity()) {
29 if (!control_type.empty() && interface_map.find(control_type) == interface_map.cend()) {
30 RCLCPP_ERROR(get_node()->get_logger(), "Invalid control type: %s", control_type.c_str());
31 throw std::invalid_argument("Invalid control type");
32 }
33}
34
35rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotControllerInterface::add_interfaces() {
37 std::make_shared<Parameter<std::string>>("task_space_frame"),
38 "The frame name in the robot model to use for kinematics calculations (defaults to the last frame in the "
39 "model)");
41 "sort_joints", true,
42 "If true, re-arrange the 'joints' parameter into a physically correct order according to the robot model");
44 std::make_shared<Parameter<std::string>>("ft_sensor_name"),
45 "Optionally, the name of a force-torque sensor in the hardware interface");
47 std::make_shared<Parameter<std::string>>("ft_sensor_reference_frame"),
48 "The reference frame of the force-torque sensor in the robot model");
50 "command_half_life", 0.1,
51 "A time constant for the exponential decay of the commanded velocity, acceleration or torque if no new command "
52 "is set");
54 "command_rate_limit", command_rate_limit_,
55 "The maximum allowable change in command on any interface expressed in command units / second");
56 return CallbackReturn::SUCCESS;
57}
58
59rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotControllerInterface::on_configure() {
60 if (*get_parameter("robot_description")) {
61 std::stringstream timestamp;
62 timestamp << std::time(nullptr);
63 auto urdf_path = "/tmp/" + hardware_name_ + "_" + timestamp.str();
64 try {
65 robot_model::Model::create_urdf_from_string(get_parameter_value<std::string>("robot_description"), urdf_path);
66 if (load_geometries_) {
67 robot_ = std::make_shared<robot_model::Model>(hardware_name_, urdf_path, [](const std::string& package_name) {
68 return ament_index_cpp::get_package_share_directory(package_name) + "/";
69 });
70 RCLCPP_DEBUG(get_node()->get_logger(), "Generated robot model with collision features");
71 } else {
72 robot_ = std::make_shared<robot_model::Model>(hardware_name_, urdf_path);
73 RCLCPP_DEBUG(get_node()->get_logger(), "Generated robot model");
74 }
75 } catch (const std::exception& ex) {
76 RCLCPP_WARN(
77 get_node()->get_logger(),
78 "Could not generate robot model with temporary urdf from string content at path %s: %s", urdf_path.c_str(),
79 ex.what());
80 }
81 }
82 if (robot_model_required_ && robot_ == nullptr) {
83 RCLCPP_ERROR(get_node()->get_logger(), "Robot model is not available even though it's required by the controller.");
84 return CallbackReturn::ERROR;
85 }
86
87 if (robot_) {
88 if (get_parameter("task_space_frame")->is_empty()) {
89 task_space_frame_ = robot_->get_frames().back();
90 } else {
92 if (!robot_->get_pinocchio_model().existFrame(task_space_frame_)) {
93 RCLCPP_ERROR(
94 get_node()->get_logger(), "Provided task space frame %s does not exist in the robot model!",
95 task_space_frame_.c_str());
96 return CallbackReturn::ERROR;
97 }
98 }
99 }
100
101 auto joints = get_parameter("joints");
102 if (joints->is_empty() && !control_type_.empty()) {
103 RCLCPP_ERROR(
104 get_node()->get_logger(), "The 'joints' parameter is required for a non-zero control type %s!",
105 control_type_.c_str());
106 return CallbackReturn::ERROR;
107 }
108 joints_ = joints->get_parameter_value<std::vector<std::string>>();
109
110 // sort the interface joint names using the robot model if necessary
111 if (get_parameter_value<bool>("sort_joints") && robot_) {
112 if (joints_.size() != robot_->get_number_of_joints()) {
113 RCLCPP_WARN(
114 get_node()->get_logger(),
115 "The number of interface joints (%zu) does not match the number of joints in the robot model (%u)",
116 joints_.size(), robot_->get_number_of_joints());
117 }
118 std::vector<std::string> ordered_joints;
119 for (const auto& ordered_name : robot_->get_joint_frames()) {
120 for (const auto& joint_name : joints_) {
121 if (joint_name == ordered_name) {
122 ordered_joints.push_back(joint_name);
123 break;
124 }
125 }
126 }
127 if (ordered_joints.size() < joints_.size()) {
128 RCLCPP_ERROR(
129 get_node()->get_logger(), "%zu interface joints were not found in the robot model",
130 joints_.size() - ordered_joints.size());
131 return CallbackReturn::ERROR;
132 }
133 joints_ = ordered_joints;
134 }
135 joint_state_ = JointState(hardware_name_, joints_);
136
137 // set command interfaces from joints
138 if (!control_type_.empty()) {
139 if (control_type_ == hardware_interface::HW_IF_POSITION) {
140 previous_joint_command_values_ = std::vector<double>(joints_.size(), std::numeric_limits<double>::quiet_NaN());
141 } else {
142 previous_joint_command_values_ = std::vector<double>(joints_.size(), 0.0);
143 }
144 for (const auto& joint : joints_) {
145 add_command_interface(joint, control_type_);
146 }
147 }
148
149 auto ft_sensor_name = get_parameter("ft_sensor_name");
150 if (!ft_sensor_name->is_empty()) {
151 ft_sensor_name_ = ft_sensor_name->get_parameter_value<std::string>();
152 auto ft_sensor_reference_frame = get_parameter("ft_sensor_reference_frame");
153 if (ft_sensor_reference_frame->is_empty()) {
154 RCLCPP_ERROR(
155 get_node()->get_logger(),
156 "The 'ft_sensor_reference_frame' parameter cannot be empty if a force-torque sensor is specified!");
157 return CallbackReturn::ERROR;
158 }
159 ft_sensor_reference_frame_ = ft_sensor_reference_frame->get_parameter_value<std::string>();
160 // TODO check that there is no joint in between
161 if (robot_ != nullptr && !robot_->get_pinocchio_model().existFrame(ft_sensor_reference_frame_)) {
162 RCLCPP_ERROR(
163 get_node()->get_logger(), "The FT sensor reference frame '%s' does not exist on the robot model!",
165 return CallbackReturn::ERROR;
166 }
167 }
168
169 command_decay_factor_ = -log(0.5) / get_parameter_value<double>("command_half_life");
170 command_rate_limit_ = get_parameter_value<double>("command_rate_limit");
171
172 RCLCPP_DEBUG(get_node()->get_logger(), "Configuration of RobotControllerInterface successful");
173 return CallbackReturn::SUCCESS;
174}
175
176rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotControllerInterface::on_activate() {
177 // initialize a force torque sensor state if applicable
178 if (!ft_sensor_name_.empty()) {
179 try {
186 } catch (const std::out_of_range&) {
187 RCLCPP_ERROR(
188 get_node()->get_logger(), "The force torque sensor '%s' does not have all the expected state interfaces!",
189 ft_sensor_name_.c_str());
190 return CallbackReturn::FAILURE;
191 }
192 ft_sensor_ = CartesianWrench(ft_sensor_name_, ft_sensor_reference_frame_);
193 }
194
195 RCLCPP_DEBUG(get_node()->get_logger(), "Activation of RobotControllerInterface successful");
196 return CallbackReturn::SUCCESS;
197}
198
199rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotControllerInterface::on_deactivate() {
200 if (!control_type_.empty() && control_type_ != hardware_interface::HW_IF_POSITION) {
201 std::fill(previous_joint_command_values_.begin(), previous_joint_command_values_.end(), 0.0);
202 }
203 RCLCPP_DEBUG(get_node()->get_logger(), "Deactivation of RobotControllerInterface successful");
204 return CallbackReturn::SUCCESS;
205}
206
207controller_interface::return_type RobotControllerInterface::read_state_interfaces() {
209 if (status != controller_interface::return_type::OK) {
210 return status;
211 }
212
213 if (joint_state_.get_size() > 0 && joint_state_.is_empty()) {
214 RCLCPP_DEBUG(get_node()->get_logger(), "Reading first joint state");
215 joint_state_.set_zero();
216 }
217
218 for (const auto& joint : joints_) {
219 auto data = get_state_interfaces(joint);
220 unsigned int joint_index;
221 try {
222 joint_index = joint_state_.get_joint_index(joint);
223 } catch (exceptions::JointNotFoundException& ex) {
224 RCLCPP_ERROR_STREAM_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, ex.what());
225 return controller_interface::return_type::ERROR;
226 }
227 for (const auto& [interface, value] : data) {
228 if (!std::isfinite(value)) {
229 RCLCPP_WARN_THROTTLE(
230 get_node()->get_logger(), *get_node()->get_clock(), 1000, "Value of state interface '%s/%s' is not finite",
231 joint.c_str(), interface.c_str());
232 return controller_interface::return_type::ERROR;
233 }
234 switch (interface_map.at(interface)) {
235 case JointStateVariable::POSITIONS:
236 joint_state_.set_position(value, joint_index);
237 break;
238 case JointStateVariable::VELOCITIES:
239 joint_state_.set_velocity(value, joint_index);
240 break;
241 case JointStateVariable::ACCELERATIONS:
242 joint_state_.set_acceleration(value, joint_index);
243 break;
244 case JointStateVariable::TORQUES:
245 joint_state_.set_torque(value, joint_index);
246 break;
247 default:
248 break;
249 }
250 }
251 }
252
253 if (!ft_sensor_name_.empty()) {
254 const auto& ft_sensor = get_state_interfaces(ft_sensor_name_);
255 std::vector<double> wrench = {ft_sensor.at("force.x"), ft_sensor.at("force.y"), ft_sensor.at("force.z"),
256 ft_sensor.at("torque.x"), ft_sensor.at("torque.y"), ft_sensor.at("torque.z")};
257 ft_sensor_.set_wrench(wrench);
258 }
259
260 return controller_interface::return_type::OK;
261}
262
263controller_interface::return_type RobotControllerInterface::write_command_interfaces(const rclcpp::Duration& period) {
264 if (!control_type_.empty()) {
265 double decay = 1.0 - command_decay_factor_ * period.seconds();
266 decay = std::max(0.0, decay);
267 double rate_limit = command_rate_limit_ * period.seconds();
268
269 auto joint_command_values = joint_command_values_.readFromRT();
270 if (joint_command_values == nullptr) {
271 new_joint_command_ready_ = false;
272 }
273
274 for (std::size_t index = 0; index < joints_.size(); ++index) {
275 double previous_command = previous_joint_command_values_.at(index);
276 if (new_joint_command_ready_) {
277 auto new_command = joint_command_values->at(index);
278 if (std::isfinite(previous_command) && std::abs(new_command - previous_command) > rate_limit) {
279 double command_offset = new_command - previous_command > 0.0 ? rate_limit : -rate_limit;
280 new_command = previous_command + command_offset;
281 }
282 set_command_interface(joints_.at(index), control_type_, new_command);
283 previous_joint_command_values_.at(index) = new_command;
284 } else if (control_type_ != hardware_interface::HW_IF_POSITION) {
285 auto new_command = previous_command * decay;
286 set_command_interface(joints_.at(index), control_type_, new_command);
287 previous_joint_command_values_.at(index) = new_command;
288 }
289 }
290 new_joint_command_ready_ = false;
291 }
292
294}
295
297 return joint_state_;
298}
299
301 // TODO check if recompute is necessary?
303 return cartesian_state_;
304}
305
307 return ft_sensor_;
308}
309
311 if (robot_ != nullptr) {
312 cartesian_state_ = robot_->forward_kinematics(joint_state_, task_space_frame_);
313 cartesian_state_.set_twist(robot_->forward_velocity(joint_state_, task_space_frame_).get_twist());
314
315 if (!ft_sensor_.is_empty() && (ft_sensor_.get_reference_frame() == cartesian_state_.get_name())) {
316 auto ft_sensor_in_robot_frame = cartesian_state_ * ft_sensor_;
317 cartesian_state_.set_wrench(ft_sensor_in_robot_frame.get_wrench());
318 }
319 }
320}
321
322void RobotControllerInterface::set_joint_command(const JointState& joint_command) {
323 if (!joint_command) {
324 RCLCPP_DEBUG(get_node()->get_logger(), "set_joint_command called with an empty JointState");
325 return;
326 }
327
328 std::vector<double> joint_command_values;
329 joint_command_values.reserve(joints_.size());
330
331 for (const auto& joint : joints_) {
332 try {
333 switch (interface_map.at(control_type_)) {
334 case JointStateVariable::POSITIONS:
335 joint_command_values.push_back(joint_command.get_position(joint));
336 break;
337 case JointStateVariable::VELOCITIES:
338 joint_command_values.push_back(joint_command.get_velocity(joint));
339 break;
340 case JointStateVariable::ACCELERATIONS:
341 joint_command_values.push_back(joint_command.get_acceleration(joint));
342 break;
343 case JointStateVariable::TORQUES:
344 joint_command_values.push_back(joint_command.get_torque(joint));
345 break;
346 default:
347 break;
348 }
349 } catch (const state_representation::exceptions::JointNotFoundException& ex) {
350 RCLCPP_WARN_STREAM_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, ex.what());
351 }
352 }
353
354 if (joint_command_values.size() == joints_.size()) {
355 joint_command_values_.writeFromNonRT(joint_command_values);
356 new_joint_command_ready_ = true;
357 } else {
358 RCLCPP_WARN_THROTTLE(
359 get_node()->get_logger(), *get_node()->get_clock(), 1000,
360 "Unable to set command values from provided joint command");
361 }
362}
363
364bool RobotControllerInterface::on_validate_parameter_callback(const std::shared_ptr<ParameterInterface>& parameter) {
365 if ((parameter->get_name() == "command_half_life" || parameter->get_name() == "command_rate_limit")
366 && parameter->get_parameter_value<double>() < 0.0) {
367 RCLCPP_ERROR(
368 get_node()->get_logger(), "Parameter value of '%s' should be greater than 0", parameter->get_name().c_str());
369 return false;
370 }
371 return true;
372}
373
374}// namespace modulo_controllers
std::shared_ptr< state_representation::ParameterInterface > get_parameter(const std::string &name) const
Get a parameter by name.
T get_parameter_value(const std::string &name) const
Get a parameter value by name.
void add_parameter(const std::shared_ptr< state_representation::ParameterInterface > &parameter, const std::string &description, bool read_only=false)
Add a parameter.
virtual controller_interface::return_type write_command_interfaces(const rclcpp::Duration &period)
Write the command interfaces.
virtual controller_interface::return_type read_state_interfaces()
Read the state interfaces.
std::unordered_map< std::string, double > get_state_interfaces(const std::string &name) const
Get a map containing the state interfaces by name of the parent tag.
double get_state_interface(const std::string &name, const std::string &interface) const
Get the value of a state interface by name.
void set_command_interface(const std::string &name, const std::string &interface, double value)
Set the value of a command interface by name.
std::string hardware_name_
The hardware name provided by a parameter.
void add_command_interface(const std::string &name, const std::string &interface)
Add a command interface to the controller by name.
Base controller class that automatically associates joints with a JointState object.
std::string ft_sensor_name_
The name of a force torque sensor in the hardware description.
const state_representation::CartesianState & get_cartesian_state()
Access the Cartesian state object.
CallbackReturn on_activate() override
Activate the controller.
bool on_validate_parameter_callback(const std::shared_ptr< state_representation::ParameterInterface > &parameter) override
Parameter validation function to be redefined by derived controller classes.
const state_representation::JointState & get_joint_state()
Access the joint state object.
std::string ft_sensor_reference_frame_
The sensing reference frame.
CallbackReturn on_deactivate() override
Deactivate the controller.
const state_representation::CartesianWrench & get_ft_sensor()
Access the Cartesian wrench object.
std::shared_ptr< robot_model::Model > robot_
Robot model object generated from URDF.
void compute_cartesian_state()
Compute the Cartesian state from forward kinematics of the current joint state.
void set_joint_command(const state_representation::JointState &joint_command)
Set the joint command object.
CallbackReturn add_interfaces() override
Add interfaces like parameters, signals, services, and predicates to the controller.
std::string task_space_frame_
The frame in task space for forward kinematics calculations, if applicable.
CallbackReturn on_configure() override
Configure the controller.