Skip to content

Commit

Permalink
bullet-featherstone: fix setting angular velocity (#567)
Browse files Browse the repository at this point in the history 
This fixes the SetFreeGroupWorldAngularVelocity
implementation in bullet-featherstone and updates
the FreeGroup test in the simulation_features
common test with the following changes:

* Use FeatureList struct for faster build
* Use sphere.sdf instead of shapes.world to avoid
  initial contact and gravity.
* Verify free group pose and velocities in addition
  to link values.
* Use tighter tolerances for velocity expectations.

Signed-off-by: Steve Peters <[email protected]>
  • Loading branch information
@scpeters
scpeters authored Nov 9, 2023
1 parent cf74036 commit 5e58f88
Show file tree
Hide file tree
Showing 3 changed files with 97 additions and 60 deletions.
24 changes: 1 addition & 23 deletions bullet-featherstone/src/FreeGroupFeatures.cc
View file
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
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
131 changes: 95 additions & 36 deletions test/common_test/simulation_features.cc
View file
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, "sphere.sdf"));

// model free group test
auto model = world->GetModel("sphere");
Expand All @@ -520,31 +538,72 @@ 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_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);
}
}

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 +855,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 +892,7 @@ using FeaturesContactPropertiesCallback = gz::physics::FeatureList<
gz::physics::GetCylinderShapeProperties,
gz::physics::GetCapsuleShapeProperties,
gz::physics::GetEllipsoidShapeProperties
>;
> {};

#ifdef DART_HAS_CONTACT_SURFACE
using ContactSurfaceParams =
Expand Down

0 comments on commit 5e58f88

Please sign in to comment.