Skip to content

Commit 475fdc4

Browse files
srvaldsrvald
authored
Allow setting payload inertia matrix via set_payload service (#1808)
1 parent 7f6ae90 commit 475fdc4

11 files changed

Lines changed: 254 additions & 25 deletions

File tree

‎Universal_Robots_ROS2_Driver.rolling.repos‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@ repositories:
1010
ur_msgs:
1111
type: git
1212
url: https://github.com/ros-industrial/ur_msgs.git
13-
version: humble-devel
13+
version: rolling-devel
1414
ros2_control:
1515
type: git
1616
url: https://github.com/ros-controls/ros2_control.git

‎ur_controllers/include/ur_controllers/gpio_controller.hpp‎

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,13 @@ enum CommandInterfaces
8282
HAND_BACK_CONTROL_CMD = 33,
8383
HAND_BACK_CONTROL_ASYNC_SUCCESS = 34,
8484
ANALOG_OUTPUTS_DOMAIN = 35,
85+
PAYLOAD_INERTIA_IXX = 36,
86+
PAYLOAD_INERTIA_IYY = 37,
87+
PAYLOAD_INERTIA_IZZ = 38,
88+
PAYLOAD_INERTIA_IXY = 39,
89+
PAYLOAD_INERTIA_IXZ = 40,
90+
PAYLOAD_INERTIA_IYZ = 41,
91+
PAYLOAD_TRANSITION_TIME = 42,
8592
};
8693

8794
enum StateInterfaces
@@ -107,6 +114,12 @@ enum StateInterfaces
107114
PAYLOAD_STATE_COG_X = 72,
108115
PAYLOAD_STATE_COG_Y = 73,
109116
PAYLOAD_STATE_COG_Z = 74,
117+
PAYLOAD_STATE_INERTIA_IXX = 75,
118+
PAYLOAD_STATE_INERTIA_IYY = 76,
119+
PAYLOAD_STATE_INERTIA_IZZ = 77,
120+
PAYLOAD_STATE_INERTIA_IXY = 78,
121+
PAYLOAD_STATE_INERTIA_IXZ = 79,
122+
PAYLOAD_STATE_INERTIA_IYZ = 80,
110123
};
111124

112125
class GPIOController : public controller_interface::ControllerInterface
@@ -210,7 +223,8 @@ class GPIOController : public controller_interface::ControllerInterface
210223
*/
211224
bool waitForAsyncCommand(std::function<double(void)> get_value);
212225

213-
bool waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz);
226+
bool waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz, double ixx, double iyy, double izz,
227+
double ixy, double ixz, double iyz, double transition_time);
214228
};
215229
} // namespace ur_controllers
216230

‎ur_controllers/src/gpio_controller.cpp‎

Lines changed: 50 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -135,6 +135,14 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c
135135

136136
config.names.emplace_back(tf_prefix + "gpio/analog_output_domain_cmd");
137137

138+
config.names.emplace_back(tf_prefix + "payload/inertia.ixx");
139+
config.names.emplace_back(tf_prefix + "payload/inertia.iyy");
140+
config.names.emplace_back(tf_prefix + "payload/inertia.izz");
141+
config.names.emplace_back(tf_prefix + "payload/inertia.ixy");
142+
config.names.emplace_back(tf_prefix + "payload/inertia.ixz");
143+
config.names.emplace_back(tf_prefix + "payload/inertia.iyz");
144+
config.names.emplace_back(tf_prefix + "payload/transition_time");
145+
138146
return config;
139147
}
140148

@@ -200,6 +208,12 @@ controller_interface::InterfaceConfiguration ur_controllers::GPIOController::sta
200208
config.names.emplace_back(tf_prefix + "payload/cog.x");
201209
config.names.emplace_back(tf_prefix + "payload/cog.y");
202210
config.names.emplace_back(tf_prefix + "payload/cog.z");
211+
config.names.emplace_back(tf_prefix + "payload/inertia.ixx");
212+
config.names.emplace_back(tf_prefix + "payload/inertia.iyy");
213+
config.names.emplace_back(tf_prefix + "payload/inertia.izz");
214+
config.names.emplace_back(tf_prefix + "payload/inertia.ixy");
215+
config.names.emplace_back(tf_prefix + "payload/inertia.ixz");
216+
config.names.emplace_back(tf_prefix + "payload/inertia.iyz");
203217

204218
return config;
205219
}
@@ -614,10 +628,19 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP
614628
// reset success flag
615629
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
616630

617-
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_MASS].set_value(static_cast<double>(req->mass));
618-
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_X].set_value(req->center_of_gravity.x);
619-
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Y].set_value(req->center_of_gravity.y);
620-
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Z].set_value(req->center_of_gravity.z);
631+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_MASS].set_value(static_cast<double>(req->payload.m));
632+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_X].set_value(req->payload.com.x);
633+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Y].set_value(req->payload.com.y);
634+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Z].set_value(req->payload.com.z);
635+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IXX].set_value(req->payload.ixx);
636+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IYY].set_value(req->payload.iyy);
637+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IZZ].set_value(req->payload.izz);
638+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IXY].set_value(req->payload.ixy);
639+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IXZ].set_value(req->payload.ixz);
640+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_INERTIA_IYZ].set_value(req->payload.iyz);
641+
double transition_time_seconds =
642+
static_cast<double>(req->transition_time.sec) + (static_cast<double>(req->transition_time.nanosec) * 1e-9);
643+
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_TRANSITION_TIME].set_value(transition_time_seconds);
621644

622645
if (!waitForAsyncCommand([&]() {
623646
return command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING);
@@ -635,9 +658,11 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP
635658
}
636659

637660
if (params_.verify_payload_on_set) {
638-
if (!waitForPayloadRtdeMatch(static_cast<double>(req->mass), req->center_of_gravity.x, req->center_of_gravity.y,
639-
req->center_of_gravity.z)) {
640-
RCLCPP_WARN(get_node()->get_logger(), "setPayload reported success but RTDE payload / payload_cog do not match "
661+
if (!waitForPayloadRtdeMatch(static_cast<double>(req->payload.m), req->payload.com.x, req->payload.com.y,
662+
req->payload.com.z, req->payload.ixx, req->payload.iyy, req->payload.izz,
663+
req->payload.ixy, req->payload.ixz, req->payload.iyz, transition_time_seconds)) {
664+
RCLCPP_WARN(get_node()->get_logger(), "setPayload reported success but RTDE payload / payload_cog / "
665+
"payload_inertia do not match "
641666
"the "
642667
"request yet. (This might "
643668
"happen when using the mocked interface.)");
@@ -706,20 +731,35 @@ bool GPIOController::waitForAsyncCommand(std::function<double(void)> get_value)
706731
return true;
707732
}
708733

709-
bool GPIOController::waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz)
734+
bool GPIOController::waitForPayloadRtdeMatch(double mass, double cx, double cy, double cz, double ixx, double iyy,
735+
double izz, double ixy, double ixz, double iyz, double transition_time)
710736
{
711737
constexpr double tol_mass = 1e-3;
712738
constexpr double tol_cog = 1e-4;
739+
constexpr double tol_inertia = 1e-4;
713740
const auto maximum_retries = params_.check_io_successfull_retries;
714741

742+
if (transition_time > 0.0) {
743+
std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(transition_time * 1000)));
744+
}
745+
715746
for (int retries = 0; retries <= maximum_retries; ++retries) {
716747
const auto m = state_interfaces_[StateInterfaces::PAYLOAD_STATE_MASS].get_optional();
717748
const auto sx = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_X].get_optional();
718749
const auto sy = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_Y].get_optional();
719750
const auto sz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_COG_Z].get_optional();
720-
if (m && sx && sy && sz) {
751+
const auto sixx = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IXX].get_optional();
752+
const auto siyy = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IYY].get_optional();
753+
const auto sizz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IZZ].get_optional();
754+
const auto sixy = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IXY].get_optional();
755+
const auto sixz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IXZ].get_optional();
756+
const auto siyz = state_interfaces_[StateInterfaces::PAYLOAD_STATE_INERTIA_IYZ].get_optional();
757+
if (m && sx && sy && sz && sixx && siyy && sizz && sixy && sixz && siyz) {
721758
if (std::abs(*m - mass) <= tol_mass && std::abs(*sx - cx) <= tol_cog && std::abs(*sy - cy) <= tol_cog &&
722-
std::abs(*sz - cz) <= tol_cog) {
759+
std::abs(*sz - cz) <= tol_cog && std::abs(*sixx - ixx) <= tol_inertia &&
760+
std::abs(*siyy - iyy) <= tol_inertia && std::abs(*sizz - izz) <= tol_inertia &&
761+
std::abs(*sixy - ixy) <= tol_inertia && std::abs(*sixz - ixz) <= tol_inertia &&
762+
std::abs(*siyz - iyz) <= tol_inertia) {
723763
return true;
724764
}
725765
}

‎ur_robot_driver/doc/migration/makoa.rst‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
:github_url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/doc/migration/makoa.rst
2+
3+
ur_robot_driver
4+
^^^^^^^^^^^^^^^
5+
6+
Interface change of set_payload service
7+
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
8+
9+
The ``SetPayload`` service in ``ur_msgs`` has changed to use ``geometry_msgs.msg.Inertia`` as the
10+
payload field. Thus, service calls to ``/io_and_status_controller/set_payload`` have to be updated
11+
12+
from
13+
14+
.. code::
15+
16+
ros2 service call /io_and_status_controller/set_payload ur_msgs/srv/SetPayload \
17+
"{payload: {mass: 1.0, center_of_gravity: {x: 0.0, y: 0.0, z: 0.04}}}"
18+
19+
20+
to
21+
22+
.. code::
23+
24+
ros2 service call /io_and_status_controller/set_payload ur_msgs/srv/SetPayload \
25+
"{payload: {m: 1.0, com: {x: 0.0, y: 0.0, z: 0.04}}}"
26+
27+
Payload inertia is now also supported, as well as a transition time:
28+
29+
.. code::
30+
31+
ros2 service call /io_and_status_controller/set_payload ur_msgs/srv/SetPayload "
32+
payload:
33+
m: 1.0
34+
com:
35+
x: 0.0
36+
y: 0.0
37+
z: 0.04
38+
ixx: 0.05
39+
ixy: 0.02
40+
ixz: 0.02
41+
iyy: 0.05
42+
iyz: 0.02
43+
izz: 0.1
44+
transition_time:
45+
sec: 1
46+
nanosec: 0
47+
"

‎ur_robot_driver/doc/migration_notes.rst‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,3 +10,8 @@ Kilted -> Lyrical
1010
^^^^^^^^^^^^^^^^^
1111
.. include:: migration/lyrical.rst
1212
:start-line: 4
13+
14+
Lyrical -> Maoka
15+
^^^^^^^^^^^^^^^^
16+
.. include:: migration/makoa.rst
17+
:start-line: 4

‎ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -284,10 +284,13 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
284284

285285
// Payload stuff
286286
urcl::vector3d_t payload_center_of_gravity_;
287+
urcl::vector6d_t payload_inertia_;
287288
double payload_mass_;
289+
double payload_transition_time_;
288290
double payload_async_success_;
289291
double rtde_payload_mass_ = 0.0;
290292
urcl::vector3d_t rtde_payload_cog_{ 0.0, 0.0, 0.0 };
293+
urcl::vector6d_t rtde_payload_inertia_{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
291294

292295
// Friction model parameters
293296
urcl::vector6d_t friction_model_viscous_;

‎ur_robot_driver/resources/rtde_output_recipe.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,3 +29,4 @@ actual_current
2929
tcp_offset
3030
payload
3131
payload_cog
32+
payload_inertia

‎ur_robot_driver/src/hardware_interface.cpp‎

Lines changed: 37 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -415,7 +415,18 @@ std::vector<hardware_interface::StateInterface> URPositionHardwareInterface::exp
415415
hardware_interface::StateInterface(tf_prefix + "payload", "cog.y", &rtde_payload_cog_[1]));
416416
state_interfaces.emplace_back(
417417
hardware_interface::StateInterface(tf_prefix + "payload", "cog.z", &rtde_payload_cog_[2]));
418-
418+
state_interfaces.emplace_back(
419+
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.ixx", &rtde_payload_inertia_[0]));
420+
state_interfaces.emplace_back(
421+
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.iyy", &rtde_payload_inertia_[1]));
422+
state_interfaces.emplace_back(
423+
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.izz", &rtde_payload_inertia_[2]));
424+
state_interfaces.emplace_back(
425+
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.ixy", &rtde_payload_inertia_[3]));
426+
state_interfaces.emplace_back(
427+
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.ixz", &rtde_payload_inertia_[4]));
428+
state_interfaces.emplace_back(
429+
hardware_interface::StateInterface(tf_prefix + "payload", "inertia.iyz", &rtde_payload_inertia_[5]));
419430
// Motion primitives stuff
420431
state_interfaces.emplace_back(hardware_interface::StateInterface(tf_prefix + HW_IF_MOTION_PRIMITIVES,
421432
"execution_status", &hw_moprim_states_[0]));
@@ -478,6 +489,20 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
478489
hardware_interface::CommandInterface(tf_prefix + "payload", "cog.y", &payload_center_of_gravity_[1]));
479490
command_interfaces.emplace_back(
480491
hardware_interface::CommandInterface(tf_prefix + "payload", "cog.z", &payload_center_of_gravity_[2]));
492+
command_interfaces.emplace_back(
493+
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.ixx", &payload_inertia_[0]));
494+
command_interfaces.emplace_back(
495+
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.iyy", &payload_inertia_[1]));
496+
command_interfaces.emplace_back(
497+
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.izz", &payload_inertia_[2]));
498+
command_interfaces.emplace_back(
499+
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.ixy", &payload_inertia_[3]));
500+
command_interfaces.emplace_back(
501+
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.ixz", &payload_inertia_[4]));
502+
command_interfaces.emplace_back(
503+
hardware_interface::CommandInterface(tf_prefix + "payload", "inertia.iyz", &payload_inertia_[5]));
504+
command_interfaces.emplace_back(
505+
hardware_interface::CommandInterface(tf_prefix + "payload", "transition_time", &payload_transition_time_));
481506
command_interfaces.emplace_back(
482507
hardware_interface::CommandInterface(tf_prefix + "payload", "payload_async_success", &payload_async_success_));
483508

@@ -999,6 +1024,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
9991024
readData(data_package_buffer_, "tcp_offset", tcp_offset_);
10001025
readData(data_package_buffer_, "payload", rtde_payload_mass_);
10011026
readData(data_package_buffer_, "payload_cog", rtde_payload_cog_);
1027+
readData(data_package_buffer_, "payload_inertia", rtde_payload_inertia_);
10021028

10031029
// required transforms
10041030
extractToolPose();
@@ -1138,6 +1164,8 @@ void URPositionHardwareInterface::initAsyncIO()
11381164

11391165
payload_mass_ = NO_NEW_CMD_;
11401166
payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
1167+
payload_inertia_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
1168+
payload_transition_time_ = NO_NEW_CMD_;
11411169

11421170
gravity_vector_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
11431171
friction_model_viscous_.fill(NO_NEW_CMD_);
@@ -1205,10 +1233,16 @@ void URPositionHardwareInterface::checkAsyncIO()
12051233

12061234
if (!std::isnan(payload_mass_) && !std::isnan(payload_center_of_gravity_[0]) &&
12071235
!std::isnan(payload_center_of_gravity_[1]) && !std::isnan(payload_center_of_gravity_[2]) &&
1208-
ur_driver_ != nullptr) {
1209-
payload_async_success_ = ur_driver_->setPayload(payload_mass_, payload_center_of_gravity_);
1236+
!std::isnan(payload_inertia_[0]) && !std::isnan(payload_inertia_[1]) && !std::isnan(payload_inertia_[2]) &&
1237+
!std::isnan(payload_inertia_[3]) && !std::isnan(payload_inertia_[4]) && !std::isnan(payload_inertia_[5]) &&
1238+
!std::isnan(payload_transition_time_) && ur_driver_ != nullptr) {
1239+
payload_async_success_ = ur_driver_->setTargetPayload(payload_mass_, payload_center_of_gravity_, payload_inertia_,
1240+
payload_transition_time_);
1241+
12101242
payload_mass_ = NO_NEW_CMD_;
12111243
payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
1244+
payload_inertia_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
1245+
payload_transition_time_ = NO_NEW_CMD_;
12121246
}
12131247

12141248
if (!std::isnan(gravity_vector_[0]) && !std::isnan(gravity_vector_[1]) && !std::isnan(gravity_vector_[2]) &&

0 commit comments

Comments
 (0)