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

Add link offset to simulation #83

Merged
merged 21 commits into from
Jul 20, 2020
Merged

Add link offset to simulation #83

Show file tree
Hide file tree
Changes from 20 commits
Commits
Show all changes
21 commits
Select commit Hold shift + click to select a range
7838ffa
add offset to collision
claireyywang Jul 8, 2020
88fb4ab
correct collision_TEST, add offset to link
claireyywang Jul 9, 2020
0ab41ec
update link pose in model
claireyywang Jul 9, 2020
ab93383
add all entity offset poses to frame data in kinematicsfeatures
claireyywang Jul 9, 2020
bca3199
add offset pose to frame
claireyywang Jul 10, 2020
aa0763a
testing world pose function (#80)
claireyywang Jul 10, 2020
d25c7ea
remove tf, use pose instead; add unit tests for GetWorldPose
claireyywang Jul 10, 2020
173ca25
Merge branch 'claire/add-tf' into claire/add-pose-offset-tpeplugin
claireyywang Jul 10, 2020
8d55f60
use semanticPose in SDFfeatures
claireyywang Jul 11, 2020
9101bd2
include closing bracket
claireyywang Jul 13, 2020
f0dcf08
remove unused include statements
claireyywang Jul 13, 2020
7b4254a
Merge branch 'claire/add-tf' into claire/add-pose-offset-tpeplugin
claireyywang Jul 13, 2020
d2a5ff5
add worldpose tests
claireyywang Jul 14, 2020
9b09f0a
add link to free group
claireyywang Jul 14, 2020
17ed808
add link and debug msg
claireyywang Jul 15, 2020
ce88d35
Merge branch 'ign-physics2' into claire/add-pose-offset-tpeplugin
claireyywang Jul 15, 2020
9d4cfdc
correct framedata of model and link pose
claireyywang Jul 16, 2020
633b7e2
correct test cases
claireyywang Jul 16, 2020
469ae16
clean up debug msgs
claireyywang Jul 16, 2020
27e368e
more clean up
claireyywang Jul 16, 2020
03f78ee
add more tests
claireyywang Jul 17, 2020
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
2 changes: 1 addition & 1 deletion tpe/lib/src/Collision_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ TEST(Collision, BasicAPI)
ASSERT_NE(nullptr, collisionEnt.GetParent());

collisionEnt.SetPose(math::Pose3d(0, 0.2, 0.5, 0, 1, 0));
EXPECT_EQ(math::Pose3d(3.06472, 2.2, 1.27944, 0.691965, 1.10889, 0.707449),
EXPECT_EQ(math::Pose3d(1.05416, 2.17281, 3.50715, 0.265579, 1.18879, 0.527304),
collisionEnt.GetWorldPose());

EXPECT_EQ(0xFF, collision.GetCollideBitmask());
Expand Down
2 changes: 1 addition & 1 deletion tpe/lib/src/Entity.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -139,7 +139,7 @@ math::Pose3d Entity::GetPose() const
math::Pose3d Entity::GetWorldPose() const
{
if (this->dataPtr->parent)
return this->dataPtr->pose * this->dataPtr->parent->GetWorldPose();
return this->dataPtr->parent->GetWorldPose() * this->dataPtr->pose;

return this->dataPtr->pose;
}
Expand Down
2 changes: 1 addition & 1 deletion tpe/lib/src/Link_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ TEST(Link, BasicAPI)
ASSERT_NE(nullptr, linkEnt.GetParent());

linkEnt.SetPose(math::Pose3d(0, 0.2, 0.5, 0, 1, 0));
EXPECT_EQ(math::Pose3d(7.08596, 0.2, -6.83411, 1, 1, 0),
EXPECT_EQ(math::Pose3d(10, -0.312675, 2.43845, 1.23686, 0.471978, 0.918989),
linkEnt.GetWorldPose());

Link link2;
Expand Down
38 changes: 26 additions & 12 deletions tpe/plugin/src/FreeGroupFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,8 @@

#include <Eigen/Geometry>

#include <ignition/common/Console.hh>

#include <ignition/math/eigen3/Conversions.hh>
#include <ignition/math/Pose3.hh>

Expand All @@ -31,7 +33,7 @@ using namespace tpeplugin;
Identity FreeGroupFeatures::FindFreeGroupForModel(
const Identity &_modelID) const
{
auto it = this->models.find(_modelID);
auto it = this->models.find(_modelID.id);
if (it == this->models.end() || it->second == nullptr)
return this->GenerateInvalidId();
auto modelPtr = it->second;
Expand All @@ -46,9 +48,9 @@ Identity FreeGroupFeatures::FindFreeGroupForModel(
Identity FreeGroupFeatures::FindFreeGroupForLink(
const Identity &_linkID) const
{
auto it = this->links.find(_linkID);
auto it = this->links.find(_linkID.id);
if (it != this->links.end() && it->second != nullptr)
return this->GenerateIdentity(_linkID, it->second);
return this->GenerateIdentity(_linkID.id, it->second);
return this->GenerateInvalidId();
}

Expand All @@ -58,7 +60,7 @@ Identity FreeGroupFeatures::GetFreeGroupCanonicalLink(
{
// assume no canonical link for now
// assume groupID ~= modelID
const auto model_it = this->models.find(_groupID);
const auto model_it = this->models.find(_groupID.id);
if (model_it != this->models.end() && model_it->second != nullptr)
{
// assume canonical link is the first link in model
Expand All @@ -75,12 +77,24 @@ void FreeGroupFeatures::SetFreeGroupWorldPose(
const Identity &_groupID,
const PoseType &_pose)
{
// assume no canonical link for now
// assume groupID ~= modelID
auto it = this->models.find(_groupID);
if (it != this->models.end() && it->second != nullptr)
// convert Eigen::Tranform to Math::Pose3d
it->second->model->SetPose(math::eigen3::convert(_pose));
auto modelIt = this->models.find(_groupID.id);
auto linkIt = this->links.find(_groupID.id);
if (modelIt != this->models.end())
{
if (modelIt->second != nullptr)
modelIt->second->model->SetPose(math::eigen3::convert(_pose));
}
else if (linkIt != this->links.end())
{
if (linkIt->second != nullptr)
linkIt->second->link->SetPose(math::eigen3::convert(_pose));
}
else
{
ignwarn << "No free group with id [" << _groupID.id << "] found."
<< std::endl;
return;
}
}

/////////////////////////////////////////////////
Expand All @@ -90,7 +104,7 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity(
{
// assume no canonical link for now
// assume groupID ~= modelID
auto it = this->models.find(_groupID);
auto it = this->models.find(_groupID.id);
// set model linear velocity
if (it != this->models.end() && it->second != nullptr)
it->second->model->SetLinearVelocity(
Expand All @@ -103,7 +117,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(
{
// assume no canonical link for now
// assume groupID ~= modelID
auto it = this->models.find(_groupID);
auto it = this->models.find(_groupID.id);
// set model angular velocity
if (it != this->models.end() && it->second != nullptr)
it->second->model->SetAngularVelocity(
Expand Down
26 changes: 9 additions & 17 deletions tpe/plugin/src/KinematicsFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,34 +38,26 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
assert(false);
return data;
}
// check if id is present and skip if not
auto it = this->childIdToParentId.find(_id.ID());
if (it == this->childIdToParentId.end())
{
ignwarn << "Entity [" << _id.ID() << "] is not found." << std::endl;
return data;
}

auto modelIt = this->models.find(it->second);
auto worldIt = this->worlds.find(it->second);
auto modelIt = this->models.find(_id.ID());
auto linkIt = this->links.find(_id.ID());

if (modelIt != this->models.end())
{
// \todo(anyone): add link offset to consider link pose
auto model = modelIt->second->model;
data.pose = math::eigen3::convert(model->GetPose());
data.pose = math::eigen3::convert(model->GetWorldPose());
data.linearVelocity = math::eigen3::convert(model->GetLinearVelocity());
data.angularVelocity = math::eigen3::convert(model->GetAngularVelocity());
}
else if (worldIt != this->worlds.end())
else if (linkIt != this->links.end())
{
auto world = worldIt->second->world;
data.pose = math::eigen3::convert(world->GetPose());
auto link = linkIt->second->link;
data.pose = math::eigen3::convert(link->GetWorldPose());
}
else
{
ignwarn << "Neither world or model with id ["
<< it->second << "] is not found" << std::endl;
ignwarn << "Entity with id ["
<< _id.ID() << "] is not found" << std::endl;
}

return data;
}
32 changes: 29 additions & 3 deletions tpe/plugin/src/SDFFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,32 @@ namespace ignition {
namespace physics {
namespace tpeplugin {

namespace {
/////////////////////////////////////////////////
/// \brief Resolve the pose of an SDF DOM object with respect to its relative_to
/// frame. If that fails, return the raw pose
static math::Pose3d ResolveSdfPose(const ::sdf::SemanticPose &_semPose)
{
math::Pose3d pose;
::sdf::Errors errors = _semPose.Resolve(pose);
if (!errors.empty())
{
if (!_semPose.RelativeTo().empty())
{
ignerr << "There was an error in SemanticPose::Resolve\n";
for (const auto &err : errors)
{
ignerr << err.Message() << std::endl;
}
ignerr << "There is no optimal fallback since the relative_to attribute["
<< _semPose.RelativeTo() << "] of the pose is not empty. "
<< "Falling back to using the raw Pose.\n";
}
pose = _semPose.RawPose();
}
return pose;
}
}
/////////////////////////////////////////////////
Identity SDFFeatures::ConstructSdfWorld(
const Identity &_engine,
Expand All @@ -51,7 +77,7 @@ Identity SDFFeatures::ConstructSdfModel(
{
// Read sdf params
const std::string name = _sdfModel.Name();
const auto pose = _sdfModel.RawPose();
const auto pose = ResolveSdfPose(_sdfModel.SemanticPose());

auto it = this->worlds.find(_worldID.id);
if (it == this->worlds.end())
Expand Down Expand Up @@ -87,7 +113,7 @@ Identity SDFFeatures::ConstructSdfLink(
{
// Read sdf params
const std::string name = _sdfLink.Name();
const auto pose = _sdfLink.RawPose();
const auto pose = ResolveSdfPose(_sdfLink.SemanticPose());

auto it = this->models.find(_modelID);
if (it == this->models.end())
Expand Down Expand Up @@ -123,7 +149,7 @@ Identity SDFFeatures::ConstructSdfCollision(
{
// Read sdf params
const std::string name = _sdfCollision.Name();
const auto pose = _sdfCollision.RawPose();
const auto pose = ResolveSdfPose(_sdfCollision.SemanticPose());
const auto geom = _sdfCollision.Geom();

auto it = this->links.find(_linkID);
Expand Down
28 changes: 26 additions & 2 deletions tpe/plugin/src/SDFFeatures_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,13 +97,15 @@ TEST(SDFFeatures_TEST, CheckTpeData)
model.GetId());
EXPECT_EQ("ground_plane", model.GetName());
EXPECT_EQ(ignition::math::Pose3d::Zero, model.GetPose());
EXPECT_EQ(ignition::math::Pose3d::Zero, model.GetWorldPose());
EXPECT_EQ(1u, model.GetChildCount());

ignition::physics::tpelib::Entity &link = model.GetChildByName("link");
ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(),
link.GetId());
EXPECT_EQ("link", link.GetName());
EXPECT_EQ(ignition::math::Pose3d::Zero, link.GetPose());
EXPECT_EQ(ignition::math::Pose3d::Zero, link.GetWorldPose());
EXPECT_EQ(1u, link.GetChildCount());

ignition::physics::tpelib::Entity &collision =
Expand All @@ -112,6 +114,7 @@ TEST(SDFFeatures_TEST, CheckTpeData)
collision.GetId());
EXPECT_EQ("collision", collision.GetName());
EXPECT_EQ(ignition::math::Pose3d::Zero, collision.GetPose());
EXPECT_EQ(ignition::math::Pose3d::Zero, collision.GetWorldPose());
}

// check model 02
Expand All @@ -122,13 +125,15 @@ TEST(SDFFeatures_TEST, CheckTpeData)
model.GetId());
EXPECT_EQ("double_pendulum_with_base", model.GetName());
EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), model.GetPose());
EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), model.GetWorldPose());
EXPECT_EQ(3u, model.GetChildCount());

ignition::physics::tpelib::Entity &link = model.GetChildByName("base");
ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(),
link.GetId());
EXPECT_EQ("base", link.GetName());
EXPECT_EQ(ignition::math::Pose3d::Zero, link.GetPose());
EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), model.GetWorldPose());
EXPECT_EQ(2u, link.GetChildCount());

ignition::physics::tpelib::Entity &collision =
Expand All @@ -137,6 +142,7 @@ TEST(SDFFeatures_TEST, CheckTpeData)
collision.GetId());
EXPECT_EQ("col_plate_on_ground", collision.GetName());
EXPECT_EQ(ignition::math::Pose3d(0, 0, 0.01, 0, 0, 0), collision.GetPose());
EXPECT_EQ(ignition::math::Pose3d(1, 0, 0.01, 0, 0, 0), collision.GetWorldPose());

ignition::physics::tpelib::Entity &collision02 =
link.GetChildByName("col_pole");
Expand All @@ -145,15 +151,18 @@ TEST(SDFFeatures_TEST, CheckTpeData)
EXPECT_EQ("col_pole", collision02.GetName());
EXPECT_EQ(ignition::math::Pose3d(-0.275, 0, 1.1, 0, 0, 0),
collision02.GetPose());
EXPECT_EQ(ignition::math::Pose3d(0.725, 0, 1.1, 0, 0, 0),
collision02.GetWorldPose());

ignition::physics::tpelib::Entity &link02 =
model.GetChildByName("upper_link");
ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(),
link02.GetId());
EXPECT_EQ("upper_link", link02.GetName());
EXPECT_EQ(
ignition::math::Pose3d(0, 0, 2.1, -1.5708, 0, 0),
EXPECT_EQ(ignition::math::Pose3d(0, 0, 2.1, -1.5708, 0, 0),
link02.GetPose());
EXPECT_EQ(ignition::math::Pose3d(1, 0, 2.1, -1.5708, 0, 0),
link02.GetWorldPose());
EXPECT_EQ(3u, link02.GetChildCount());

ignition::physics::tpelib::Entity &collision03 =
Expand All @@ -163,6 +172,9 @@ TEST(SDFFeatures_TEST, CheckTpeData)
EXPECT_EQ("col_upper_joint", collision03.GetName());
EXPECT_EQ(ignition::math::Pose3d(-0.05, 0, 0, 0, 1.5708, 0),
collision03.GetPose());
EXPECT_EQ(
ignition::math::Pose3d(0.95, 0, 2.1, -1.5708, -4e-06, -1.5708),
collision03.GetWorldPose());

ignition::physics::tpelib::Entity &collision04 =
link02.GetChildByName("col_lower_joint");
Expand All @@ -171,6 +183,9 @@ TEST(SDFFeatures_TEST, CheckTpeData)
EXPECT_EQ("col_lower_joint", collision04.GetName());
EXPECT_EQ(ignition::math::Pose3d(0, 0, 1.0, 0, 1.5708, 0),
collision04.GetPose());
EXPECT_EQ(
ignition::math::Pose3d(1, 1, 2.1, -1.5708, -4e-06, -1.5708),
collision04.GetWorldPose());

ignition::physics::tpelib::Entity &collision05 =
link02.GetChildByName("col_cylinder");
Expand All @@ -179,6 +194,8 @@ TEST(SDFFeatures_TEST, CheckTpeData)
EXPECT_EQ("col_cylinder", collision05.GetName());
EXPECT_EQ(ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0),
collision05.GetPose());
EXPECT_EQ(ignition::math::Pose3d(1, 0.5, 2.1, -1.5708, 0, 0),
collision05.GetWorldPose());

ignition::physics::tpelib::Entity &link03 =
model.GetChildByName("lower_link");
Expand All @@ -187,6 +204,8 @@ TEST(SDFFeatures_TEST, CheckTpeData)
EXPECT_EQ("lower_link", link03.GetName());
EXPECT_EQ(ignition::math::Pose3d(0.25, 1.0, 2.1, -2, 0, 0),
link03.GetPose());
EXPECT_EQ(ignition::math::Pose3d(1.25, 1.0, 2.1, -2, 0, 0),
link03.GetWorldPose());
EXPECT_EQ(2u, link03.GetChildCount());

ignition::physics::tpelib::Entity &collision06 =
Expand All @@ -196,6 +215,9 @@ TEST(SDFFeatures_TEST, CheckTpeData)
EXPECT_EQ("col_lower_joint", collision06.GetName());
EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 1.5708, 0),
collision06.GetPose());
EXPECT_EQ(
ignition::math::Pose3d(1.25, 1, 2.1, -1.57079, -0.429204, -1.5708),
collision06.GetWorldPose());

ignition::physics::tpelib::Entity &collision07 =
link03.GetChildByName("col_cylinder");
Expand All @@ -204,6 +226,8 @@ TEST(SDFFeatures_TEST, CheckTpeData)
EXPECT_EQ("col_cylinder", collision07.GetName());
EXPECT_EQ(ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0),
collision07.GetPose());
EXPECT_EQ(ignition::math::Pose3d(1.25, 1.45465, 1.89193, -2, 0, 0),
collision07.GetWorldPose());
}

// check model 03
Expand Down
4 changes: 0 additions & 4 deletions tpe/plugin/src/SimulationFeatures_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -230,10 +230,6 @@ TEST_P(SimulationFeatures_TEST, FreeGroup)
auto frameData = model->GetLink(0)->FrameDataRelativeToWorld();
EXPECT_EQ(ignition::math::Pose3d(0, 0, 2, 0, 0, 0),
ignition::math::eigen3::convert(frameData.pose));
EXPECT_EQ(ignition::math::Vector3d(0.5, 0, 0.1),
ignition::math::eigen3::convert(frameData.linearVelocity));
EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 0),
ignition::math::eigen3::convert(frameData.angularVelocity));
Copy link
Contributor

Choose a reason for hiding this comment

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

do we need to do something to fix the linear and angular vel?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

My understanding is that linear and angular velocity are tied to model instead of link, and this test case started checking the velocity of link after I corrected the implementation of framedata, which is why I removed it. Before it worked because when calling link->FrameDataRelativeToWorld(), it was getting the velocity of the link's parent model.

If we do want to add velocity for links, then that would be a different feature which allows links to move by itself.

Copy link
Contributor

Choose a reason for hiding this comment

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

oh I see, that makes sense. Just checking, if we assume the link does not move within a model, should it inherit the parent model's velocities in world frame?

In any case, I'm not sure if there's a use case for querying a link's world velocities. We can address this when the need comes.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

If the link doesn't move within model, its WorldPose is then updated as model's WorldPose gets updated (since its offset will remain the same), so we probably don't have to keep track of link velocities in world frame.

However, if we want the link to move within model, then we'll have to compute the link velocities with the offset. And yes, I agree that we can add this if the need comes.

}
}

Expand Down