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

Fix TPE Link velocity not being updated and Model velocity not having any effect. #289

Merged
merged 3 commits into from
Aug 31, 2021
Merged

Fix TPE Link velocity not being updated and Model velocity not having any effect. #289

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
6933c71
Calculate link velocities, fix updates for models
luca-della-vedova Aug 30, 2021
542f56d
Add test for velocity loopback
luca-della-vedova Aug 30, 2021
1f949e4
Avoid pushing duplicated links, change variable name
luca-della-vedova Aug 31, 2021
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
7 changes: 7 additions & 0 deletions tpe/plugin/src/KinematicsFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,13 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
{
auto link = linkIt->second->link;
data.pose = math::eigen3::convert(link->GetWorldPose());
auto modelId = link->GetParent()->GetId();
auto modelPtr = this->models.find(modelId)->second->model;
math::Pose3d parentWorldPose = modelPtr->GetWorldPose();
data.linearVelocity = math::eigen3::convert(parentWorldPose.Rot().Inverse() *
link->GetLinearVelocity() + modelPtr->GetLinearVelocity());
data.angularVelocity = math::eigen3::convert(parentWorldPose.Rot().Inverse() *
link->GetAngularVelocity() + modelPtr->GetAngularVelocity());
}
else
{
Expand Down
34 changes: 34 additions & 0 deletions tpe/plugin/src/SimulationFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,6 +98,40 @@ void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const
}
}

// Iterate over models to make sure link velocities for moving models
// are calculated and sent
for (const auto &[id, info] : this->models)
{
// make sure the model exists
if (info)
{
if (info->model->GetStatic())
continue;
const auto nextPose = info->model->GetPose();
// Note, now prevLinkPoses also containes prevModelPoses
// Data structure has been kept to avoid breaking ABI
luca-della-vedova marked this conversation as resolved.
Show resolved Hide resolved
auto iter = this->prevLinkPoses.find(id);

// If the models's pose is new or has changed, calculate and add all
// the children links' poses to the output poses
if ((iter == this->prevLinkPoses.end()) ||
!iter->second.Pos().Equal(nextPose.Pos(), 1e-6) ||
!iter->second.Rot().Equal(nextPose.Rot(), 1e-6))
{
for (const auto &linkEnt : info->model->GetChildren())
{
WorldPose wp;
wp.pose = linkEnt.second->GetPose();
wp.body = linkEnt.second->GetId();
_changedPoses.entries.push_back(wp);
luca-della-vedova marked this conversation as resolved.
Show resolved Hide resolved
newPoses[id] = linkEnt.second->GetPose();
}
}
else
newPoses[id] = iter->second;
}
}

// Save the new poses so that they can be used to check for updates in the
// next iteration. Re-setting this->prevLinkPoses with the contents of
// newPoses ensures that we aren't caching data for links that were removed
Expand Down
15 changes: 13 additions & 2 deletions tpe/plugin/src/SimulationFeatures_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -450,17 +450,28 @@ TEST_P(SimulationFeatures_TEST, FreeGroup)
auto freeGroupLink = link->FindFreeGroup();
ASSERT_NE(nullptr, freeGroupLink);

StepWorld(world, true);

freeGroup->SetWorldPose(
ignition::math::eigen3::convert(
ignition::math::Pose3d(0, 0, 2, 0, 0, 0)));
freeGroup->SetWorldLinearVelocity(
ignition::math::eigen3::convert(ignition::math::Vector3d(0.5, 0, 0.1)));
ignition::math::eigen3::convert(ignition::math::Vector3d(0.1, 0.2, 0.3)));
freeGroup->SetWorldAngularVelocity(
ignition::math::eigen3::convert(ignition::math::Vector3d(0.1, 0.2, 0)));
ignition::math::eigen3::convert(ignition::math::Vector3d(0.4, 0.5, 0.6)));

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

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

Expand Down