Skip to content

Commit

Permalink
Make added mass gravity tests more focused
Browse files Browse the repository at this point in the history 
Signed-off-by: Michael Carroll <[email protected]>
  • Loading branch information
@mjcarroll
mjcarroll committed Jan 10, 2023
1 parent 82e0cb4 commit 4dc22d6
Show file tree
Hide file tree
Showing 3 changed files with 104 additions and 89 deletions.
1 change: 1 addition & 0 deletions dartsim/src/SimulationFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,7 @@ void SimulationFeatures::WorldForwardStep(

auto mass = info->inertial->MassMatrix().Mass();
auto g = world->getGravity();

info->link->addExtForce(mass * g, com, false, true);
}
}
Expand Down
86 changes: 39 additions & 47 deletions test/common_test/world_features.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,7 @@ using GravityFeatures = gz::physics::FeatureList<
using GravityFeaturesTestTypes =
::testing::Types<GravityFeatures>;
TYPED_TEST_SUITE(WorldFeaturesTest,
GravityFeatures);
GravityFeatures,);

/////////////////////////////////////////////////
TYPED_TEST(WorldFeaturesTest, GravityFeatures)
Expand Down Expand Up @@ -225,60 +225,31 @@ TYPED_TEST(WorldFeaturesTest, GravityAddedMassFeatures)
auto graphErrors = sdfWorld->ValidateGraphs();
EXPECT_EQ(0u, graphErrors.size()) << graphErrors;

Eigen::Vector3d gravity = {0, 0, -9.8};

AssertVectorApprox vectorPredicate(1e-6);
EXPECT_PRED_FORMAT2(vectorPredicate, gravity,
world->GetGravity());

world->SetGravity({8, 4, 3});
EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d(8, 4, 3),
world->GetGravity());
AssertVectorApprox vectorPredicate6(1e-6);

world->SetGravity(gravity);
// Set gravity to a nice round number
world->SetGravity({0, 0, -10});

auto model = world->GetModel("sphere");
ASSERT_NE(nullptr, model);
// Link poses
const Eigen::Vector3d initialLinkPosition(0, 0, 2);
const Eigen::Vector3d finalLinkPosition(0, 0, -3.005);
const Eigen::Vector3d finalLinkPositionAddedMass(0, 0, -0.5025);

auto link = model->GetLink(0);
ASSERT_NE(nullptr, link);
// This tests that the physics plugin correctly considers added mass.
for (auto modelName: {"sphere", "sphere_zero_added_mass", "sphere_added_mass", "heavy_sphere"})
{
auto model = world->GetModel(modelName);
ASSERT_NE(nullptr, model);

AssertVectorApprox vectorPredicate6(1e-6);
auto link = model->GetLink(0);
ASSERT_NE(nullptr, link);

// initial link pose
const Eigen::Vector3d initialLinkPosition(0, 0, 2);
{
Eigen::Vector3d pos = link->FrameDataRelativeToWorld().pose.translation();
EXPECT_PRED_FORMAT2(vectorPredicate6,
initialLinkPosition,
pos);
}

auto linkFrameID = link->GetFrameID();

// Get default gravity in link frame, which is pitched by pi/4
EXPECT_PRED_FORMAT2(vectorPredicate6,
Eigen::Vector3d(6.92964645563, 0, -6.92964645563),
world->GetGravity(linkFrameID));

// set gravity along X axis of linked frame, which is pitched by pi/4
world->SetGravity(Eigen::Vector3d(1.4142135624, 0, 0), linkFrameID);

EXPECT_PRED_FORMAT2(vectorPredicate6,
Eigen::Vector3d(1, 0, -1),
world->GetGravity());

// test other SetGravity API
// set gravity along Z axis of linked frame, which is pitched by pi/4
gz::physics::RelativeForce3d relativeGravity(
linkFrameID, Eigen::Vector3d(0, 0, 1.4142135624));
world->SetGravity(relativeGravity);

EXPECT_PRED_FORMAT2(vectorPredicate6,
Eigen::Vector3d(1, 0, 1),
world->GetGravity());

// Confirm that changed gravity direction affects pose of link
gz::physics::ForwardStep::Input input;
gz::physics::ForwardStep::State state;
gz::physics::ForwardStep::Output output;
Expand All @@ -289,11 +260,32 @@ TYPED_TEST(WorldFeaturesTest, GravityAddedMassFeatures)
world->Step(output, state, input);
}

AssertVectorApprox vectorPredicate2(1e-2);
// Confirm that the models with zero added mass behave consistently
for (auto modelName: {"sphere", "sphere_zero_added_mass", "heavy_sphere"})
{
auto model = world->GetModel(modelName);
ASSERT_NE(nullptr, model);

auto link = model->GetLink(0);
ASSERT_NE(nullptr, link);

Eigen::Vector3d pos = link->FrameDataRelativeToWorld().pose.translation();
EXPECT_PRED_FORMAT2(vectorPredicate2,
Eigen::Vector3d(0.5, 0, 2.5),
EXPECT_PRED_FORMAT2(vectorPredicate6,
finalLinkPosition,
pos);
}

for (auto modelName: {"sphere_added_mass"})
{
auto model = world->GetModel(modelName);
ASSERT_NE(nullptr, model);

auto link = model->GetLink(0);
ASSERT_NE(nullptr, link);

Eigen::Vector3d pos = link->FrameDataRelativeToWorld().pose.translation();
EXPECT_PRED_FORMAT2(vectorPredicate6,
finalLinkPositionAddedMass,
pos);
}
}
Expand Down
106 changes: 64 additions & 42 deletions test/common_test/worlds/falling_added_mass.world
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,43 @@
filename="gz-sim-physics-system"
name="gz::sim::systems::v0::Physics">
</plugin>

<model name="sphere">
<pose>0 0 2 0 0.78539816339 0</pose>
<pose>0 0 2 0 0 0</pose>
<link name="sphere_link">
<pose>0.0 0.0 0.0 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.4</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.4</iyy>
<iyz>0</iyz>
<izz>0.4</izz>
</inertia>
<mass>1.0</mass>
</inertial>
<visual name="sphere_visual">
<pose>0.0 0.0 0.0 0 0 0</pose>
<geometry>
<sphere>
<radius>1</radius>
</sphere>
</geometry>
</visual>
<collision name="sphere_collision">
<pose>0.0 0.0 0.0 0 0 0</pose>
<geometry>
<sphere>
<radius>1</radius>
</sphere>
</geometry>
</collision>
</link>
</model>

<model name="sphere_zero_added_mass">
<pose>0 0 2 0 0 0</pose>
<link name="sphere_link">
<pose>0.0 0.0 0.0 0 0 0</pose>
<inertial>
Expand Down Expand Up @@ -43,50 +78,35 @@
</fluid_added_mass>
<mass>1.0</mass>
</inertial>
<visual name="sphere_visual">
<pose>0.0 0.0 0.0 0 0 0</pose>
<geometry>
<sphere>
<radius>1</radius>
</sphere>
</geometry>
</visual>
<collision name="sphere_collision">
<pose>0.0 0.0 0.0 0 0 0</pose>
<geometry>
<sphere>
<radius>1</radius>
</sphere>
</geometry>
</collision>
</link>
</model>
<model name="box">
<static>true</static>
<pose>0 0 -0.5 0 0 0.78539816339</pose>
<link name="box_link">

<model name="sphere_added_mass">
<pose>0 0 2 0 0 0</pose>
<link name="sphere_link">
<pose>0.0 0.0 0.0 0 0 0</pose>
<inertial>
<inertia>
<ixx>1</ixx>
<ixx>0.4</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyy>0.4</iyy>
<iyz>0</iyz>
<izz>1</izz>
<izz>0.4</izz>
</inertia>
<fluid_added_mass>
<xx>0</xx>
<xx>1</xx>
<xy>0</xy>
<xz>0</xz>
<xp>0</xp>
<xq>0</xq>
<xr>0</xr>
<yy>0</yy>
<yy>1</yy>
<yz>0</yz>
<yp>0</yp>
<yq>0</yq>
<yr>0</yr>
<zz>0</zz>
<zz>1</zz>
<zp>0</zp>
<zq>0</zq>
<zr>0</zr>
Expand All @@ -99,22 +119,24 @@
</fluid_added_mass>
<mass>1.0</mass>
</inertial>
<pose>0.0 0.0 0.0 0 0 0</pose>
<collision name="box_collision">
<geometry>
<box>
<size>100 100 1</size>
</box>
</geometry>
</collision>
</link>
</model>

<visual name="box_visual">
<geometry>
<box>
<size>100 100 1</size>
</box>
</geometry>
</visual>
<model name="heavy_sphere">
<pose>0 0 2 0 0 0</pose>
<link name="sphere_link">
<pose>0.0 0.0 0.0 0 0 0</pose>
<inertial>
<inertia>
<ixx>0.4</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.4</iyy>
<iyz>0</iyz>
<izz>0.4</izz>
</inertia>
<mass>2.0</mass>
</inertial>
</link>
</model>
</world>
Expand Down

0 comments on commit 4dc22d6

Please sign in to comment.