Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

bullet-featherstone: fix setting angular velocity #567

Merged
merged 7 commits into from
Nov 9, 2023
Merged

bullet-featherstone: fix setting angular velocity #567

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
3bce17d
simulation_features test: struct for faster build
scpeters Nov 9, 2023
33e12fa
Use separate FeatureList for FreeGroup test
scpeters Nov 9, 2023
a4359ef
Refactor FreeGroup test
scpeters Nov 9, 2023
28a0d15
bullet-featherstone: fix setting angular velocity
scpeters Nov 9, 2023
d604c92
Use test world without gravity to fix tpe
scpeters Nov 9, 2023
1afbd32
Merge branch 'gz-physics6' into scpeters/bullet_freegroup_test
scpeters Nov 9, 2023
018e8bd
remove outdated comment
scpeters Nov 9, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 1 addition & 23 deletions bullet-featherstone/src/FreeGroupFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,29 +68,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(

if (model != nullptr)
{
// Set angular velocity the each one of the joints of the model
for (const auto& jointID : model->jointEntityIds)
{
auto jointInfo = this->joints[jointID];
if (!jointInfo->motor)
{
auto *modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
jointInfo->motor = std::make_shared<btMultiBodyJointMotor>(
modelInfo->body.get(),
std::get<InternalJoint>(jointInfo->identifier).indexInBtModel,
0,
static_cast<btScalar>(0),
static_cast<btScalar>(jointInfo->effort));
auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
world->world->addMultiBodyConstraint(jointInfo->motor.get());
}

if (jointInfo->motor)
{
jointInfo->motor->setVelocityTarget(
static_cast<btScalar>(_angularVelocity[2]));
}
}
model->body->setBaseOmega(convertVec(_angularVelocity));
}
}

Expand Down
2 changes: 1 addition & 1 deletion bullet-featherstone/src/KinematicsFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
}
}

if (model->body == nullptr)
if (!model || model->body == nullptr)
model = this->FrameInterface<ModelInfo>(_id);
}

Expand Down
136 changes: 100 additions & 36 deletions test/common_test/simulation_features.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@
#include <sdf/Root.hh>

// The features that an engine must have to be loaded by this loader.
using Features = gz::physics::FeatureList<
struct Features : gz::physics::FeatureList<
gz::physics::ConstructEmptyWorldFeature,

gz::physics::FindFreeGroupFeature,
Expand Down Expand Up @@ -84,7 +84,7 @@ using Features = gz::physics::FeatureList<
gz::physics::GetCylinderShapeProperties,
gz::physics::GetCapsuleShapeProperties,
gz::physics::GetEllipsoidShapeProperties
>;
> {};

template <class T>
class SimulationFeaturesTest:
Expand Down Expand Up @@ -139,6 +139,8 @@ gz::physics::World3dPtr<T> LoadPluginAndWorld(
return world;
}

using AssertVectorApprox = gz::physics::test::AssertVectorApprox;

/// \brief Step forward in a world
/// \param[in] _world The world to step in
/// \param[in] _firstTime Whether this is the very first time this world is
Expand Down Expand Up @@ -184,11 +186,11 @@ std::pair<bool, gz::physics::ForwardStep::Output> StepWorld(
}

// The features that an engine must have to be loaded by this loader.
using FeaturesContacts = gz::physics::FeatureList<
struct FeaturesContacts : gz::physics::FeatureList<
gz::physics::sdf::ConstructSdfWorld,
gz::physics::GetContactsFromLastStepFeature,
gz::physics::ForwardStep
>;
> {};

template <class T>
class SimulationFeaturesContactsTest :
Expand Down Expand Up @@ -218,10 +220,10 @@ TYPED_TEST(SimulationFeaturesContactsTest, Contacts)


// The features that an engine must have to be loaded by this loader.
using FeaturesStep = gz::physics::FeatureList<
struct FeaturesStep : gz::physics::FeatureList<
gz::physics::sdf::ConstructSdfWorld,
gz::physics::ForwardStep
>;
> {};

template <class T>
class SimulationFeaturesStepTest :
Expand Down Expand Up @@ -303,7 +305,7 @@ TYPED_TEST(SimulationFeaturesFallingTest, Falling)


// The features that an engine must have to be loaded by this loader.
using FeaturesShapeFeatures = gz::physics::FeatureList<
struct FeaturesShapeFeatures : gz::physics::FeatureList<
gz::physics::sdf::ConstructSdfWorld,
gz::physics::GetModelFromWorld,
gz::physics::GetLinkFromModel,
Expand All @@ -321,7 +323,7 @@ using FeaturesShapeFeatures = gz::physics::FeatureList<
gz::physics::GetCylinderShapeProperties,
gz::physics::GetCapsuleShapeProperties,
gz::physics::GetEllipsoidShapeProperties
>;
> {};

template <class T>
class SimulationFeaturesShapeFeaturesTest :
Expand Down Expand Up @@ -490,22 +492,38 @@ TYPED_TEST(SimulationFeaturesShapeFeaturesTest, ShapeFeatures)
}
}

struct FreeGroupFeatures : gz::physics::FeatureList<
gz::physics::FindFreeGroupFeature,
gz::physics::SetFreeGroupWorldPose,
gz::physics::SetFreeGroupWorldVelocity,

gz::physics::GetModelFromWorld,
gz::physics::GetLinkFromModel,

gz::physics::FreeGroupFrameSemantics,
gz::physics::LinkFrameSemantics,

gz::physics::sdf::ConstructSdfWorld,

gz::physics::ForwardStep
> {};

template <class T>
class SimulationFeaturesTestBasic :
class SimulationFeaturesTestFreeGroup :
public SimulationFeaturesTest<T>{};
using SimulationFeaturesTestBasicTypes =
::testing::Types<Features>;
TYPED_TEST_SUITE(SimulationFeaturesTestBasic,
SimulationFeaturesTestBasicTypes);
using SimulationFeaturesTestFreeGroupTypes =
::testing::Types<FreeGroupFeatures>;
TYPED_TEST_SUITE(SimulationFeaturesTestFreeGroup,
SimulationFeaturesTestFreeGroupTypes);

TYPED_TEST(SimulationFeaturesTestBasic, FreeGroup)
TYPED_TEST(SimulationFeaturesTestFreeGroup, FreeGroup)
{
for (const std::string &name : this->pluginNames)
{
auto world = LoadPluginAndWorld<Features>(
auto world = LoadPluginAndWorld<FreeGroupFeatures>(
this->loader,
name,
gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world"));
gz::common::joinPaths(TEST_WORLD_DIR, "falling.world"));

// model free group test
auto model = world->GetModel("sphere");
Expand All @@ -520,31 +538,77 @@ TYPED_TEST(SimulationFeaturesTestBasic, FreeGroup)
auto freeGroupLink = link->FindFreeGroup();
ASSERT_NE(nullptr, freeGroupLink);

StepWorld<Features>(world, true);
StepWorld<FreeGroupFeatures>(world, true);

// Set initial pose.
const gz::math::Pose3d initialPose{0, 0, 2, 0, 0, 0};
freeGroup->SetWorldPose(
gz::math::eigen3::convert(
gz::math::Pose3d(0, 0, 2, 0, 0, 0)));
freeGroup->SetWorldLinearVelocity(
gz::math::eigen3::convert(gz::math::Vector3d(0.1, 0.2, 0.3)));
freeGroup->SetWorldAngularVelocity(
gz::math::eigen3::convert(gz::math::Vector3d(0.4, 0.5, 0.6)));

auto frameData = model->GetLink(0)->FrameDataRelativeToWorld();
EXPECT_EQ(gz::math::Pose3d(0, 0, 2, 0, 0, 0),
gz::math::eigen3::convert(frameData.pose));
gz::math::eigen3::convert(initialPose));

// Set initial velocities.
const Eigen::Vector3d initialLinearVelocity{0.1, 0.2, 0.3};
const Eigen::Vector3d initialAngularVelocity{0.4, 0.5, 0.6};
freeGroup->SetWorldLinearVelocity(initialLinearVelocity);
freeGroup->SetWorldAngularVelocity(initialAngularVelocity);

auto freeGroupFrameData = freeGroup->FrameDataRelativeToWorld();
auto linkFrameData = model->GetLink(0)->FrameDataRelativeToWorld();

// Before stepping, check that pose matches what was set.
EXPECT_EQ(initialPose,
gz::math::eigen3::convert(freeGroupFrameData.pose));
EXPECT_EQ(initialPose,
gz::math::eigen3::convert(linkFrameData.pose));

// Before stepping, check that velocities match what was set.
AssertVectorApprox vectorPredicateVelocity(1e-7);
EXPECT_PRED_FORMAT2(vectorPredicateVelocity,
initialLinearVelocity,
freeGroupFrameData.linearVelocity);
EXPECT_PRED_FORMAT2(vectorPredicateVelocity,
initialLinearVelocity,
linkFrameData.linearVelocity);
EXPECT_PRED_FORMAT2(vectorPredicateVelocity,
initialAngularVelocity,
freeGroupFrameData.angularVelocity);
EXPECT_PRED_FORMAT2(vectorPredicateVelocity,
initialAngularVelocity,
linkFrameData.angularVelocity);

// Step the world
StepWorld<Features>(world, false);
// Check that the first link's velocities are updated
frameData = model->GetLink(0)->FrameDataRelativeToWorld();
EXPECT_TRUE(gz::math::Vector3d(0.1, 0.2, 0.3).Equal(
gz::math::eigen3::convert(frameData.linearVelocity), 0.1));
EXPECT_EQ(gz::math::Vector3d(0.4, 0.5, 0.6),
gz::math::eigen3::convert(frameData.angularVelocity));
StepWorld<FreeGroupFeatures>(world, false);

// Check the velocities again.
freeGroupFrameData = freeGroup->FrameDataRelativeToWorld();
linkFrameData = model->GetLink(0)->FrameDataRelativeToWorld();

// Expect linear velocity to be affected by gravity.
const Eigen::Vector3d linearVelocityAfterStep{0.1, 0.2, 0.3 - 9.8 * 0.001};
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

TPE is failing these expectations

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

should be fixed by d604c92

EXPECT_PRED_FORMAT2(vectorPredicateVelocity,
linearVelocityAfterStep,
freeGroupFrameData.linearVelocity);
EXPECT_PRED_FORMAT2(vectorPredicateVelocity,
linearVelocityAfterStep,
linkFrameData.linearVelocity);

// Expect angular velocity is unchanged.
EXPECT_PRED_FORMAT2(vectorPredicateVelocity,
initialAngularVelocity,
freeGroupFrameData.angularVelocity);
EXPECT_PRED_FORMAT2(vectorPredicateVelocity,
initialAngularVelocity,
linkFrameData.angularVelocity);
}
}

template <class T>
class SimulationFeaturesTestBasic :
public SimulationFeaturesTest<T>{};
using SimulationFeaturesTestBasicTypes =
::testing::Types<Features>;
TYPED_TEST_SUITE(SimulationFeaturesTestBasic,
SimulationFeaturesTestBasicTypes);

TYPED_TEST(SimulationFeaturesTestBasic, ShapeBoundingBox)
{
for (const std::string &name : this->pluginNames)
Expand Down Expand Up @@ -796,7 +860,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts)
}
}

using FeaturesContactPropertiesCallback = gz::physics::FeatureList<
struct FeaturesContactPropertiesCallback : gz::physics::FeatureList<
gz::physics::ConstructEmptyWorldFeature,

gz::physics::FindFreeGroupFeature,
Expand Down Expand Up @@ -833,7 +897,7 @@ using FeaturesContactPropertiesCallback = gz::physics::FeatureList<
gz::physics::GetCylinderShapeProperties,
gz::physics::GetCapsuleShapeProperties,
gz::physics::GetEllipsoidShapeProperties
>;
> {};

#ifdef DART_HAS_CONTACT_SURFACE
using ContactSurfaceParams =
Expand Down
Loading