Skip to content

Commit

Permalink
1.10/joint.sdf: better default limit values
Browse files Browse the repository at this point in the history
* Use `±inf` instead of `±1e16` for position limits
* Use `inf` instead of `-1` for effort, velocity limits

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
  • Loading branch information
scpeters committed Aug 12, 2022
1 parent 12e9c1f commit d057b07
Show file tree
Hide file tree
Showing 5 changed files with 24 additions and 16 deletions.
6 changes: 6 additions & 0 deletions Migration.md
Original file line number Diff line number Diff line change
Expand Up @@ -538,6 +538,12 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1.

### Modifications

1. **joint.sdf**: axis limits default values have changed
+ `//limit/lower`: `-inf` (formerly `-1e16`)
+ `//limit/upper`: `inf` (formerly `1e16`)
+ `//limit/velocity`: `inf` (formerly `-1`)
+ `//limit/effort`: `inf` (formerly `-1`)

1. **plugin.sdf**: name attribute is now optional with empty default value.

## SDFormat specification 1.8 to 1.9
Expand Down
16 changes: 8 additions & 8 deletions sdf/1.10/joint.sdf
Original file line number Diff line number Diff line change
Expand Up @@ -74,16 +74,16 @@
</element> <!-- End Dynamics -->
<element name="limit" required="1">
<description>specifies the limits of this joint</description>
<element name="lower" type="double" default="-1e16" required="1">
<element name="lower" type="double" default="-inf" required="1">
<description>Specifies the lower joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous.</description>
</element>
<element name="upper" type="double" default="1e16" required="1">
<element name="upper" type="double" default="inf" required="1">
<description>Specifies the upper joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous.</description>
</element>
<element name="effort" type="double" default="-1" required="0">
<element name="effort" type="double" default="inf" required="0">
<description>A value for enforcing the maximum joint effort applied. Limit is not enforced if value is negative.</description>
</element>
<element name="velocity" type="double" default="-1" required="0">
<element name="velocity" type="double" default="inf" required="0">
<description>A value for enforcing the maximum joint velocity.</description>
</element>

Expand Down Expand Up @@ -132,16 +132,16 @@

<element name="limit" required="1">
<description></description>
<element name="lower" type="double" default="-1e16" required="0">
<element name="lower" type="double" default="-inf" required="0">
<description>An attribute specifying the lower joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous.</description>
</element>
<element name="upper" type="double" default="1e16" required="0">
<element name="upper" type="double" default="inf" required="0">
<description>An attribute specifying the upper joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous.</description>
</element>
<element name="effort" type="double" default="-1" required="0">
<element name="effort" type="double" default="inf" required="0">
<description>An attribute for enforcing the maximum joint effort applied by Joint::SetForce. Limit is not enforced if value is negative.</description>
</element>
<element name="velocity" type="double" default="-1" required="0">
<element name="velocity" type="double" default="inf" required="0">
<description>(not implemented) An attribute for enforcing the maximum joint velocity.</description>
</element>

Expand Down
9 changes: 5 additions & 4 deletions src/JointAxis.cc
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
*/
#include <algorithm>
#include <iterator>
#include <limits>

#include <gz/math/Pose3.hh>
#include <gz/math/Vector3.hh>
Expand Down Expand Up @@ -60,18 +61,18 @@ class sdf::JointAxis::Implementation

/// \brief Specifies the lower joint limit (radians for revolute joints,
/// meters for prismatic joints). Omit if joint is continuous.
public: double lower = -1e16;
public: double lower = -std::numeric_limits<double>::infinity();

/// \brief Specifies the upper joint limit (radians for revolute joints,
/// meters for prismatic joints). Omit if joint is continuous.
public: double upper = 1e16;
public: double upper = std::numeric_limits<double>::infinity();

/// \brief A value for enforcing the maximum joint effort applied.
/// Limit is not enforced if value is negative.
public: double effort = -1;
public: double effort = std::numeric_limits<double>::infinity();

/// \brief A value for enforcing the maximum joint velocity.
public: double maxVelocity = -1;
public: double maxVelocity = std::numeric_limits<double>::infinity();

/// \brief Joint stop stiffness.
public: double stiffness = 1e8;
Expand Down
5 changes: 3 additions & 2 deletions src/JointAxis_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
*/

#include <gtest/gtest.h>
#include <limits>
#include "sdf/JointAxis.hh"

/////////////////////////////////////////////////
Expand All @@ -29,8 +30,8 @@ TEST(DOMJointAxis, Construction)
EXPECT_DOUBLE_EQ(0.0, axis.Friction());
EXPECT_DOUBLE_EQ(0.0, axis.SpringReference());
EXPECT_DOUBLE_EQ(0.0, axis.SpringStiffness());
EXPECT_DOUBLE_EQ(-1e16, axis.Lower());
EXPECT_DOUBLE_EQ(1e16, axis.Upper());
EXPECT_DOUBLE_EQ(-std::numeric_limits<double>::infinity(), axis.Lower());
EXPECT_DOUBLE_EQ(std::numeric_limits<double>::infinity(), axis.Upper());
EXPECT_DOUBLE_EQ(std::numeric_limits<double>::infinity(), axis.Effort());
EXPECT_DOUBLE_EQ(std::numeric_limits<double>::infinity(), axis.MaxVelocity());
EXPECT_DOUBLE_EQ(1e8, axis.Stiffness());
Expand Down
4 changes: 2 additions & 2 deletions test/integration/joint_axis_dom.cc
Original file line number Diff line number Diff line change
Expand Up @@ -244,8 +244,8 @@ TEST(DOMJointAxis, InfiniteLimits)
ASSERT_NE(nullptr, joint);
auto axis = joint->Axis(0);
ASSERT_NE(nullptr, axis);
EXPECT_DOUBLE_EQ(-1e16, axis->Lower());
EXPECT_DOUBLE_EQ(1e16, axis->Upper());
EXPECT_DOUBLE_EQ(-kInf, axis->Lower());
EXPECT_DOUBLE_EQ(kInf, axis->Upper());
EXPECT_DOUBLE_EQ(kInf, axis->Effort());
EXPECT_DOUBLE_EQ(kInf, axis->MaxVelocity());
}
Expand Down

0 comments on commit d057b07

Please sign in to comment.