Skip to content

Commit

Permalink
Extend joint limits structure with deceleration limits. (#977)
Browse files Browse the repository at this point in the history
  • Loading branch information
@destogl
destogl authored Mar 25, 2023
1 parent 6496fee commit c8cfa05
Show file tree
Hide file tree
Showing 4 changed files with 60 additions and 0 deletions.
6 changes: 6 additions & 0 deletions joint_limits/include/joint_limits/joint_limits.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,11 +38,13 @@ struct JointLimits
max_position(std::numeric_limits<double>::quiet_NaN()),
max_velocity(std::numeric_limits<double>::quiet_NaN()),
max_acceleration(std::numeric_limits<double>::quiet_NaN()),
max_deceleration(std::numeric_limits<double>::quiet_NaN()),
max_jerk(std::numeric_limits<double>::quiet_NaN()),
max_effort(std::numeric_limits<double>::quiet_NaN()),
has_position_limits(false),
has_velocity_limits(false),
has_acceleration_limits(false),
has_deceleration_limits(false),
has_jerk_limits(false),
has_effort_limits(false),
angle_wraparound(false)
Expand All @@ -53,12 +55,14 @@ struct JointLimits
double max_position;
double max_velocity;
double max_acceleration;
double max_deceleration;
double max_jerk;
double max_effort;

bool has_position_limits;
bool has_velocity_limits;
bool has_acceleration_limits;
bool has_deceleration_limits;
bool has_jerk_limits;
bool has_effort_limits;
bool angle_wraparound;
Expand All @@ -73,6 +77,8 @@ struct JointLimits
<< max_velocity << "]\n";
ss_output << " has acceleration limits: " << (has_acceleration_limits ? "true" : "false")
<< " [" << max_acceleration << "]\n";
ss_output << " has deceleration limits: " << (has_deceleration_limits ? "true" : "false")
<< " [" << max_deceleration << "]\n";
ss_output << " has jerk limits: " << (has_jerk_limits ? "true" : "false") << " [" << max_jerk
<< "]\n";
ss_output << " has effort limits: " << (has_effort_limits ? "true" : "false") << " ["
Expand Down
29 changes: 29 additions & 0 deletions joint_limits/include/joint_limits/joint_limits_rosparam.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,8 @@ namespace joint_limits
* max_velocity: double
* has_acceleration_limits: bool
* max_acceleration: double
* has_deceleration_limits: bool
* max_deceleration: double
* has_jerk_limits: bool
* max_jerk: double
* has_effort_limits: bool
Expand Down Expand Up @@ -101,6 +103,9 @@ inline bool declare_parameters(
auto_declare<bool>(param_itf, param_base_name + ".has_acceleration_limits", false);
auto_declare<double>(
param_itf, param_base_name + ".max_acceleration", std::numeric_limits<double>::quiet_NaN());
auto_declare<bool>(param_itf, param_base_name + ".has_deceleration_limits", false);
auto_declare<double>(
param_itf, param_base_name + ".max_deceleration", std::numeric_limits<double>::quiet_NaN());
auto_declare<bool>(param_itf, param_base_name + ".has_jerk_limits", false);
auto_declare<double>(
param_itf, param_base_name + ".max_jerk", std::numeric_limits<double>::quiet_NaN());
Expand Down Expand Up @@ -173,6 +178,8 @@ inline bool declare_parameters(
* max_velocity: double
* has_acceleration_limits: bool
* max_acceleration: double
* has_deceleration_limits: bool
* max_deceleration: double
* has_jerk_limits: bool
* max_jerk: double
* has_effort_limits: bool
Expand All @@ -194,6 +201,8 @@ inline bool declare_parameters(
* max_velocity: 2.0
* has_acceleration_limits: true
* max_acceleration: 5.0
* has_deceleration_limits: true
* max_deceleration: 7.5
* has_jerk_limits: true
* max_jerk: 100.0
* has_effort_limits: true
Expand Down Expand Up @@ -231,6 +240,8 @@ inline bool get_joint_limits(
!param_itf->has_parameter(param_base_name + ".max_velocity") &&
!param_itf->has_parameter(param_base_name + ".has_acceleration_limits") &&
!param_itf->has_parameter(param_base_name + ".max_acceleration") &&
!param_itf->has_parameter(param_base_name + ".has_deceleration_limits") &&
!param_itf->has_parameter(param_base_name + ".max_deceleration") &&
!param_itf->has_parameter(param_base_name + ".has_jerk_limits") &&
!param_itf->has_parameter(param_base_name + ".max_jerk") &&
!param_itf->has_parameter(param_base_name + ".has_effort_limits") &&
Expand Down Expand Up @@ -315,6 +326,24 @@ inline bool get_joint_limits(
}
}

// Deceleration limits
if (param_itf->has_parameter(param_base_name + ".has_deceleration_limits"))
{
limits.has_deceleration_limits =
param_itf->get_parameter(param_base_name + ".has_deceleration_limits").as_bool();
if (
limits.has_deceleration_limits &&
param_itf->has_parameter(param_base_name + ".max_deceleration"))
{
limits.max_deceleration =
param_itf->get_parameter(param_base_name + ".max_deceleration").as_double();
}
else
{
limits.has_deceleration_limits = false;
}
}

// Jerk limits
if (param_itf->has_parameter(param_base_name + ".has_jerk_limits"))
{
Expand Down
5 changes: 5 additions & 0 deletions joint_limits/test/joint_limits_rosparam.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,8 @@ JointLimitsRosparamTestNode:
max_velocity: 2.0
has_acceleration_limits: true
max_acceleration: 5.0
has_deceleration_limits: true
max_deceleration: 7.5
has_jerk_limits: true
max_jerk: 100.0
has_effort_limits: true
Expand All @@ -26,6 +28,7 @@ JointLimitsRosparamTestNode:
has_position_limits: true
has_velocity_limits: true
has_acceleration_limits: true
has_deceleration_limits: true
has_jerk_limits: true
has_effort_limits: true

Expand All @@ -35,6 +38,7 @@ JointLimitsRosparamTestNode:
max_position: 1.0
max_velocity: 2.0
max_acceleration: 5.0
max_deceleration: 7.5
max_jerk: 100.0
max_effort: 20.0

Expand All @@ -43,6 +47,7 @@ JointLimitsRosparamTestNode:
has_position_limits: false
has_velocity_limits: false
has_acceleration_limits: false
has_deceleration_limits: false
has_jerk_limits: false
has_effort_limits: false
angle_wraparound: true # should be accepted, has no position limits
Expand Down
20 changes: 20 additions & 0 deletions joint_limits/test/joint_limits_rosparam_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
EXPECT_TRUE(std::isnan(limits.max_velocity));
EXPECT_FALSE(limits.has_acceleration_limits);
EXPECT_TRUE(std::isnan(limits.max_acceleration));
EXPECT_FALSE(limits.has_deceleration_limits);
EXPECT_TRUE(std::isnan(limits.max_deceleration));
EXPECT_FALSE(limits.has_jerk_limits);
EXPECT_TRUE(std::isnan(limits.max_jerk));
EXPECT_FALSE(limits.has_effort_limits);
Expand All @@ -76,6 +78,8 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
EXPECT_TRUE(std::isnan(limits.max_velocity));
EXPECT_FALSE(limits.has_acceleration_limits);
EXPECT_TRUE(std::isnan(limits.max_acceleration));
EXPECT_FALSE(limits.has_deceleration_limits);
EXPECT_TRUE(std::isnan(limits.max_deceleration));
EXPECT_FALSE(limits.has_jerk_limits);
EXPECT_TRUE(std::isnan(limits.max_jerk));
EXPECT_FALSE(limits.has_effort_limits);
Expand All @@ -100,6 +104,9 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
EXPECT_TRUE(limits.has_acceleration_limits);
EXPECT_EQ(5.0, limits.max_acceleration);

EXPECT_TRUE(limits.has_deceleration_limits);
EXPECT_EQ(7.5, limits.max_deceleration);

EXPECT_TRUE(limits.has_jerk_limits);
EXPECT_EQ(100.0, limits.max_jerk);

Expand All @@ -120,6 +127,7 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
EXPECT_FALSE(limits.has_position_limits);
EXPECT_FALSE(limits.has_velocity_limits);
EXPECT_FALSE(limits.has_acceleration_limits);
EXPECT_FALSE(limits.has_deceleration_limits);
EXPECT_FALSE(limits.has_jerk_limits);
EXPECT_FALSE(limits.has_effort_limits);
}
Expand All @@ -134,6 +142,7 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
EXPECT_FALSE(limits.has_position_limits);
EXPECT_FALSE(limits.has_velocity_limits);
EXPECT_FALSE(limits.has_acceleration_limits);
EXPECT_FALSE(limits.has_deceleration_limits);
EXPECT_FALSE(limits.has_jerk_limits);
EXPECT_FALSE(limits.has_effort_limits);
}
Expand All @@ -147,6 +156,7 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
EXPECT_TRUE(limits.has_position_limits);
EXPECT_TRUE(limits.has_velocity_limits);
EXPECT_TRUE(limits.has_acceleration_limits);
EXPECT_TRUE(limits.has_deceleration_limits);
EXPECT_TRUE(limits.has_jerk_limits);
EXPECT_TRUE(limits.has_effort_limits);

Expand All @@ -156,6 +166,7 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
EXPECT_FALSE(limits.has_position_limits);
EXPECT_FALSE(limits.has_velocity_limits);
EXPECT_FALSE(limits.has_acceleration_limits);
EXPECT_FALSE(limits.has_deceleration_limits);
EXPECT_FALSE(limits.has_jerk_limits);
EXPECT_FALSE(limits.has_effort_limits);
EXPECT_TRUE(limits.angle_wraparound);
Expand Down Expand Up @@ -191,6 +202,9 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits)
EXPECT_FALSE(limits.has_acceleration_limits);
EXPECT_TRUE(std::isnan(limits.max_acceleration));

EXPECT_FALSE(limits.has_deceleration_limits);
EXPECT_TRUE(std::isnan(limits.max_deceleration));

EXPECT_FALSE(limits.has_jerk_limits);
EXPECT_TRUE(std::isnan(limits.max_jerk));

Expand Down Expand Up @@ -317,6 +331,9 @@ TEST_F(JointLimitsUndeclaredRosParamTest, parse_declared_joint_limits_node)
EXPECT_TRUE(limits.has_acceleration_limits);
EXPECT_EQ(5.0, limits.max_acceleration);

EXPECT_TRUE(limits.has_deceleration_limits);
EXPECT_EQ(7.5, limits.max_deceleration);

EXPECT_TRUE(limits.has_jerk_limits);
EXPECT_EQ(100.0, limits.max_jerk);

Expand Down Expand Up @@ -356,6 +373,9 @@ TEST_F(JointLimitsLifecycleNodeUndeclaredRosParamTest, parse_declared_joint_limi
EXPECT_TRUE(limits.has_acceleration_limits);
EXPECT_EQ(5.0, limits.max_acceleration);

EXPECT_TRUE(limits.has_deceleration_limits);
EXPECT_EQ(7.5, limits.max_deceleration);

EXPECT_TRUE(limits.has_jerk_limits);
EXPECT_EQ(100.0, limits.max_jerk);

Expand Down

0 comments on commit c8cfa05

Please sign in to comment.