Skip to content

Commit

Permalink
Add a test that replicates #1596
Browse files Browse the repository at this point in the history
  • Loading branch information
@jslee02
jslee02 committed Aug 22, 2021
1 parent 0ae9fe3 commit f8db10d
Show file tree
Hide file tree
Showing 3 changed files with 279 additions and 0 deletions.
182 changes: 182 additions & 0 deletions data/sdf/test/test_issue1596.model
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,182 @@
<sdf version="1.4">
<model name="model_1">
<link name="link_00">
<gravity>true</gravity>
<pose>0 0 2 0 0 0</pose>
<inertial>
<mass>0.1</mass>
<pose>0 0 -0.5 0 0 0</pose>
<inertia>
<ixx>0.01</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.01</iyy>
<iyz>0</iyz>
<izz>0.01</izz>
</inertia>
</inertial>
<collision name="col">
<pose>0 0 -0.5 0 0 0</pose>
<geometry>
<box>
<size>0.10 0.20 1.0</size>
</box>
</geometry>
</collision>
<visual name="vis">
<pose>0 0 -0.5 0 0 0</pose>
<geometry>
<box>
<size>0.10 0.20 1.0</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Green</name>
</script>
</material>
</visual>
<visual name="vis2">
<pose>0 0.125 -0.5 0 0 0</pose>
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Green</name>
</script>
</material>
</visual>
</link>
<link name="link_01">
<gravity>true</gravity>
<pose>0 0 1.0 0 0 0</pose>
<inertial>
<mass>0.1</mass>
<pose>0 0 -0.5 0 0 0</pose>
<inertia>
<ixx>0.01</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.01</iyy>
<iyz>0</iyz>
<izz>0.01</izz>
</inertia>
</inertial>
<collision name="col">
<pose>0 0 -0.5 0 0 0</pose>
<geometry>
<box>
<size>0.10 0.20 1.0</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.005</min_depth>
</ode>
</contact>
<friction>
<ode>
<mu>1.0</mu>
<mu2>1.0</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="vis">
<pose>0 0 -0.5 0 0 0</pose>
<geometry>
<box>
<size>0.10 0.20 1.0</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Red</name>
</script>
</material>
</visual>
<visual name="vis2">
<pose>0 0.125 -0.5 0 0 0</pose>
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Green</name>
</script>
</material>
</visual>
</link>

<joint name="joint_00" type="universal">
<parent>world</parent>
<child>link_00</child>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.1</lower>
<upper>0.1</upper>
<stiffness>1e6</stiffness>
</limit>
<dynamics>
<damping>0.0001</damping>
</dynamics>
</axis>
<axis2>
<xyz>0 1 0</xyz>
<limit>
<lower>-0.1</lower>
<upper>0.1</upper>
<stiffness>1e6</stiffness>
</limit>
<dynamics>
<damping>0.0001</damping>
</dynamics>
</axis2>
<physics>
<ode>
<cfm_damping>1</cfm_damping>
</ode>
</physics>
</joint>
<joint name="joint_01" type="universal">
<child>link_01</child>
<parent>link_00</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.1</lower>
<upper>0.1</upper>
</limit>
<dynamics>
<damping>0.1</damping>
</dynamics>
</axis>
<axis2>
<xyz>0 1 0</xyz>
<limit>
<lower>-0.1</lower>
<upper>0.1</upper>
</limit>
<dynamics>
<damping>0.1</damping>
</dynamics>
</axis2>
<physics>
<ode>
<cfm_damping>1</cfm_damping>
</ode>
</physics>
</joint>
</model>
</sdf>
3 changes: 3 additions & 0 deletions unittests/regression/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,9 @@ dart_add_test("regression" test_Issue1243 test_Issue1243.cpp)
if(TARGET dart-utils)
dart_add_test("regression" test_Issue1583)
target_link_libraries(test_Issue1583 dart-utils)

dart_add_test("regression" test_Issue1596)
target_link_libraries(test_Issue1596 dart-utils)
endif()

if(TARGET dart-utils-urdf)
Expand Down
94 changes: 94 additions & 0 deletions unittests/regression/test_Issue1596.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,94 @@
/*
* Copyright (c) 2011-2020, The DART development contributors
* All rights reserved.
*
* The list of contributors can be found at:
* https://github.com/dartsim/dart/blob/master/LICENSE
*
* This file is provided under the following "BSD-style" License:
* Redistribution and use in source and binary forms, with or
* without modification, are permitted provided that the following
* conditions are met:
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
* CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
* USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/

#include <TestHelpers.hpp>
#include <dart/dart.hpp>
#include <dart/gui/osg/osg.hpp>
#include <dart/utils/sdf/sdf.hpp>
#include <gtest/gtest.h>

//========================================================================================
TEST(Issue1596, ServoJointWithPositionLimits)
{
#if NDEBUG // release
const auto num_steps = 50000;
#else
const auto num_steps = 1000;
#endif
const double pos_lb = -0.1;
const double pos_ub = +0.1;

auto skel = dart::utils::SdfParser::readSkeleton(
"dart://sample/sdf/test/test_issue1596.model");
ASSERT_NE(skel, nullptr);

auto world = dart::simulation::World::create();
world->setGravity(Eigen::Vector3d(1, 1, 0));
world->addSkeleton(skel);
ASSERT_EQ(world->getNumSkeletons(), 1);

auto* joint0 = skel->getJoint("joint_00");
auto* joint1 = skel->getJoint("joint_01");
std::vector<dart::dynamics::Joint*> joints = {joint0, joint1};

for (auto joint : joints)
{
ASSERT_NE(joint, nullptr);
ASSERT_EQ(
joint->getType(), dart::dynamics::UniversalJoint::getStaticType());
EXPECT_EQ(joint->getNumDofs(), 2);
joint->setLimitEnforcement(true);

EXPECT_DOUBLE_EQ(0, joint->getPosition(0));
EXPECT_DOUBLE_EQ(0, joint->getPosition(1));
EXPECT_DOUBLE_EQ(0, joint->getVelocity(0));
EXPECT_DOUBLE_EQ(0, joint->getVelocity(1));
EXPECT_DOUBLE_EQ(0, joint->getAcceleration(0));
EXPECT_DOUBLE_EQ(0, joint->getAcceleration(1));
}

for (std::size_t i = 0; i < num_steps; ++i)
{
world->step();

for (const auto joint : joints)
{
const double pos0 = joint->getPosition(0);
const double pos1 = joint->getPosition(1);

EXPECT_LE(pos0, pos_ub + 1e-6);
EXPECT_LE(pos1, pos_ub + 1e-6);
EXPECT_GE(pos0, pos_lb - 1e-6);
EXPECT_GE(pos1, pos_lb - 1e-6);
}
}
}

0 comments on commit f8db10d

Please sign in to comment.