diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 2480ad0b1..9677eea31 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -3016,10 +3016,6 @@ void CreateJoint(TiXmlElement *_root, Values2str(1, &_link->parent_joint->limits->lower)); AddKeyValue(jointAxisLimit, "upper", Values2str(1, &_link->parent_joint->limits->upper)); - AddKeyValue(jointAxisLimit, "effort", - Values2str(1, &_link->parent_joint->limits->effort)); - AddKeyValue(jointAxisLimit, "velocity", - Values2str(1, &_link->parent_joint->limits->velocity)); } else if (_link->parent_joint->type != urdf::Joint::CONTINUOUS) { @@ -3041,11 +3037,11 @@ void CreateJoint(TiXmlElement *_root, Values2str(1, &_link->parent_joint->limits->lower)); AddKeyValue(jointAxisLimit, "upper", Values2str(1, &_link->parent_joint->limits->upper)); - AddKeyValue(jointAxisLimit, "effort", - Values2str(1, &_link->parent_joint->limits->effort)); - AddKeyValue(jointAxisLimit, "velocity", - Values2str(1, &_link->parent_joint->limits->velocity)); } + AddKeyValue(jointAxisLimit, "effort", + Values2str(1, &_link->parent_joint->limits->effort)); + AddKeyValue(jointAxisLimit, "velocity", + Values2str(1, &_link->parent_joint->limits->velocity)); } } diff --git a/test/integration/urdf_joint_parameters.cc b/test/integration/urdf_joint_parameters.cc index 7b178cc54..7a92d8a6e 100644 --- a/test/integration/urdf_joint_parameters.cc +++ b/test/integration/urdf_joint_parameters.cc @@ -65,6 +65,13 @@ TEST(SDFParser, JointAxisParameters) ASSERT_TRUE(dynamics->HasElement("friction")); EXPECT_DOUBLE_EQ(value, dynamics->Get("damping")); EXPECT_DOUBLE_EQ(value, dynamics->Get("friction")); + + EXPECT_TRUE(axis->HasElement("limit")); + sdf::ElementPtr limit = axis->GetElement("limit"); + EXPECT_TRUE(limit->HasElement("effort")); + EXPECT_TRUE(limit->HasElement("velocity")); + EXPECT_DOUBLE_EQ(value, limit->Get("effort")); + EXPECT_DOUBLE_EQ(value, limit->Get("velocity")); } EXPECT_EQ(bitmask, 0x3u); diff --git a/test/integration/urdf_joint_parameters.urdf b/test/integration/urdf_joint_parameters.urdf index e33fb28c1..cd1e466c3 100644 --- a/test/integration/urdf_joint_parameters.urdf +++ b/test/integration/urdf_joint_parameters.urdf @@ -9,6 +9,7 @@ + @@ -31,6 +32,7 @@ +