819 lines
30 KiB
C++
819 lines
30 KiB
C++
/*
|
|
* Copyright (C) 2015 Open Source Robotics Foundation
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*
|
|
*/
|
|
|
|
#include "gazebo/gui/GuiIface.hh"
|
|
#include "gazebo/gui/MainWindow.hh"
|
|
#include "gazebo/gui/model/ModelData.hh"
|
|
#include "gazebo/gui/model/ModelData_TEST.hh"
|
|
|
|
using namespace gazebo;
|
|
|
|
/////////////////////////////////////////////////
|
|
void ModelData_TEST::Clone()
|
|
{
|
|
this->resMaxPercentChange = 5.0;
|
|
this->shareMaxPercentChange = 2.0;
|
|
|
|
this->Load("worlds/empty.world");
|
|
|
|
// Create the main window.
|
|
gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
|
|
QVERIFY(mainWindow != nullptr);
|
|
mainWindow->Load();
|
|
mainWindow->Init();
|
|
mainWindow->show();
|
|
|
|
// Get the user camera and scene
|
|
gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
|
|
QVERIFY(cam != nullptr);
|
|
gazebo::rendering::ScenePtr scene = cam->GetScene();
|
|
QVERIFY(scene != nullptr);
|
|
|
|
this->ProcessEventsAndDraw(mainWindow);
|
|
|
|
gui::LinkData *link = new gui::LinkData();
|
|
|
|
double mass = 1.0;
|
|
ignition::math::Vector3d size = ignition::math::Vector3d::One;
|
|
|
|
// create a link
|
|
msgs::Model model;
|
|
msgs::AddBoxLink(model, mass, size);
|
|
link->Load(msgs::LinkToSDF(model.link(0)));
|
|
rendering::VisualPtr linkVis(new rendering::Visual("model::box_link",
|
|
scene->WorldVisual()));
|
|
link->SetLinkVisual(linkVis);
|
|
|
|
// add a visual
|
|
rendering::VisualPtr vis(
|
|
new rendering::Visual("model::box_link::visual", linkVis));
|
|
vis->Load(msgs::VisualToSDF(model.link(0).visual(0)));
|
|
link->AddVisual(vis);
|
|
|
|
// add a collision visual
|
|
rendering::VisualPtr collisionVis(
|
|
new rendering::Visual("model::box_link::collision", linkVis));
|
|
collisionVis->Load(msgs::VisualToSDF(model.link(0).visual(0)));
|
|
link->AddCollision(collisionVis);
|
|
|
|
// verify clone link
|
|
std::string cloneLinkName = "box_link_clone";
|
|
gui::LinkData *cloneLink = link->Clone(cloneLinkName);
|
|
QCOMPARE(cloneLink->GetName(), cloneLinkName);
|
|
QCOMPARE(cloneLink->Scale(), ignition::math::Vector3d::One);
|
|
QCOMPARE(cloneLink->Pose(), ignition::math::Pose3d::Zero);
|
|
QVERIFY(cloneLink->LinkVisual() != nullptr);
|
|
QCOMPARE(cloneLink->LinkVisual()->GetName(), "model::" + cloneLinkName);
|
|
QVERIFY(cloneLink->Scale() == ignition::math::Vector3d::One);
|
|
|
|
// verify clone link visual
|
|
QCOMPARE(cloneLink->visuals.size(), link->visuals.size());
|
|
QVERIFY(cloneLink->visuals.size() == 1u);
|
|
rendering::VisualPtr cloneVis = cloneLink->visuals.begin()->first;
|
|
QVERIFY(cloneVis != nullptr);
|
|
QCOMPARE(cloneVis->GetName(), "model::" + cloneLinkName + "::visual");
|
|
QCOMPARE(cloneVis->GetGeometryType(), std::string("box"));
|
|
QCOMPARE(cloneVis->GetGeometrySize(), size);
|
|
|
|
// verify clone link collision
|
|
QCOMPARE(cloneLink->collisions.size(), link->collisions.size());
|
|
QVERIFY(cloneLink->collisions.size() == 1u);
|
|
rendering::VisualPtr cloneCol = cloneLink->collisions.begin()->first;
|
|
QVERIFY(cloneCol != nullptr);
|
|
QCOMPARE(cloneCol->GetName(), "model::" + cloneLinkName + "::collision");
|
|
QCOMPARE(cloneCol->GetGeometryType(), std::string("box"));
|
|
QCOMPARE(cloneCol->GetGeometrySize(), size);
|
|
|
|
// verify link sdf
|
|
sdf::ElementPtr linkSDF = cloneLink->linkSDF;
|
|
QVERIFY(linkSDF->HasElement("inertial"));
|
|
sdf::ElementPtr inertialElem = linkSDF->GetElement("inertial");
|
|
QVERIFY(inertialElem->HasElement("inertia"));
|
|
sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia");
|
|
QVERIFY(inertialElem->HasElement("mass"));
|
|
sdf::ElementPtr massElem = inertialElem->GetElement("mass");
|
|
QVERIFY(ignition::math::equal(massElem->Get<double>(), mass));
|
|
|
|
msgs::Inertial inertialMsg = model.link(0).inertial();
|
|
double ixx = inertialMsg.ixx();
|
|
double iyy = inertialMsg.iyy();
|
|
double izz = inertialMsg.izz();
|
|
double ixy = inertialMsg.ixy();
|
|
double ixz = inertialMsg.ixz();
|
|
double iyz = inertialMsg.iyz();
|
|
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixx"), ixx));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("iyy"), iyy));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("izz"), izz));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixy"), ixy));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixz"), ixz));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("iyz"), iyz));
|
|
|
|
// verify visual sdf
|
|
sdf::ElementPtr visualElem = cloneVis->GetSDF();
|
|
QCOMPARE(visualElem->Get<std::string>("name"), std::string("visual"));
|
|
sdf::ElementPtr visualGeomElem = visualElem->GetElement("geometry");
|
|
QVERIFY(visualGeomElem->HasElement("box"));
|
|
sdf::ElementPtr visualGeomBoxElem = visualGeomElem->GetElement("box");
|
|
QCOMPARE(visualGeomBoxElem->Get<ignition::math::Vector3d>("size"),
|
|
size);
|
|
|
|
// verify collision sdf
|
|
sdf::ElementPtr colElem = cloneCol->GetSDF();
|
|
QCOMPARE(colElem->Get<std::string>("name"), std::string("collision"));
|
|
sdf::ElementPtr colGeomElem = colElem->GetElement("geometry");
|
|
QVERIFY(colGeomElem ->HasElement("box"));
|
|
sdf::ElementPtr colGeomBoxElem = colGeomElem->GetElement("box");
|
|
QCOMPARE(colGeomBoxElem->Get<ignition::math::Vector3d>("size"),
|
|
size);
|
|
|
|
delete link;
|
|
|
|
mainWindow->close();
|
|
delete mainWindow;
|
|
mainWindow = nullptr;
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void ModelData_TEST::LinkScale()
|
|
{
|
|
this->resMaxPercentChange = 5.0;
|
|
this->shareMaxPercentChange = 2.0;
|
|
|
|
this->Load("worlds/empty.world");
|
|
|
|
// Create the main window.
|
|
gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
|
|
QVERIFY(mainWindow != nullptr);
|
|
mainWindow->Load();
|
|
mainWindow->Init();
|
|
mainWindow->show();
|
|
|
|
// Get the user camera and scene
|
|
gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
|
|
QVERIFY(cam != nullptr);
|
|
gazebo::rendering::ScenePtr scene = cam->GetScene();
|
|
QVERIFY(scene != nullptr);
|
|
|
|
this->ProcessEventsAndDraw(mainWindow);
|
|
|
|
// box
|
|
{
|
|
gui::LinkData *link = new gui::LinkData();
|
|
|
|
double mass = 1.0;
|
|
ignition::math::Vector3d size = ignition::math::Vector3d::One;
|
|
|
|
// create a link
|
|
msgs::Model model;
|
|
msgs::AddBoxLink(model, mass, size);
|
|
link->Load(msgs::LinkToSDF(model.link(0)));
|
|
rendering::VisualPtr linkVis(new rendering::Visual("box_link", scene));
|
|
link->SetLinkVisual(linkVis);
|
|
|
|
// add a collision visual
|
|
rendering::VisualPtr collisionVis(
|
|
new rendering::Visual("box_link::collision", linkVis));
|
|
collisionVis->Load(msgs::VisualToSDF(model.link(0).visual(0)));
|
|
link->AddCollision(collisionVis);
|
|
|
|
// verify scale
|
|
ignition::math::Vector3d scale = ignition::math::Vector3d::One;
|
|
QVERIFY(link->Scale() == scale);
|
|
|
|
sdf::ElementPtr linkSDF = link->linkSDF;
|
|
QVERIFY(linkSDF->HasElement("inertial"));
|
|
sdf::ElementPtr inertialElem = linkSDF->GetElement("inertial");
|
|
QVERIFY(inertialElem->HasElement("inertia"));
|
|
sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia");
|
|
QVERIFY(inertialElem->HasElement("mass"));
|
|
sdf::ElementPtr massElem = inertialElem->GetElement("mass");
|
|
|
|
// verify mass
|
|
QVERIFY(ignition::math::equal(massElem->Get<double>(), mass));
|
|
|
|
// verify inertia values
|
|
msgs::Inertial inertialMsg = model.link(0).inertial();
|
|
double ixx = inertialMsg.ixx();
|
|
double iyy = inertialMsg.iyy();
|
|
double izz = inertialMsg.izz();
|
|
double ixy = inertialMsg.ixy();
|
|
double ixz = inertialMsg.ixz();
|
|
double iyz = inertialMsg.iyz();
|
|
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixx"), ixx));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("iyy"), iyy));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("izz"), izz));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixy"), ixy));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixz"), ixz));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("iyz"), iyz));
|
|
|
|
// set new scale and verify inertial values
|
|
{
|
|
// set scale
|
|
ignition::math::Vector3d newScale =
|
|
ignition::math::Vector3d(3.0, 2.0, 1.0);
|
|
collisionVis->SetScale(newScale);
|
|
link->SetScale(newScale);
|
|
|
|
// verify new scale
|
|
QVERIFY(link->Scale() == newScale);
|
|
|
|
// change in scale
|
|
ignition::math::Vector3d dScale = newScale / scale;
|
|
|
|
// verify new mass
|
|
double density = mass / (size.X() * size.Y() * size.Z());
|
|
ignition::math::Vector3d newSize = dScale * size;
|
|
double newMass = density * (newSize.X() * newSize.Y() * newSize.Z());
|
|
QVERIFY(ignition::math::equal(massElem->Get<double>(), newMass));
|
|
|
|
// verify new inertia values
|
|
// use msgs::AddBoxLink to help us compute the expected inertia values.
|
|
msgs::AddBoxLink(model, newMass, newScale);
|
|
msgs::Inertial newInertialMsg = model.link(1).inertial();
|
|
double newIxx = newInertialMsg.ixx();
|
|
double newIyy = newInertialMsg.iyy();
|
|
double newIzz = newInertialMsg.izz();
|
|
double newIxy = newInertialMsg.ixy();
|
|
double newIxz = newInertialMsg.ixz();
|
|
double newIyz = newInertialMsg.iyz();
|
|
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixx"), newIxx, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("iyy"), newIyy, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("izz"), newIzz, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixy"), newIxy, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixz"), newIxz, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("iyz"), newIyz, 1e-3));
|
|
|
|
scale = newScale;
|
|
mass = newMass;
|
|
size = newSize;
|
|
}
|
|
|
|
// set another scale and verify inertial values
|
|
{
|
|
// set scale
|
|
ignition::math::Vector3d newScale =
|
|
ignition::math::Vector3d(1.2, 3.8, 2.5);
|
|
collisionVis->SetScale(newScale);
|
|
link->SetScale(newScale);
|
|
|
|
// verify new scale
|
|
QVERIFY(link->Scale() == newScale);
|
|
|
|
// change in scale
|
|
ignition::math::Vector3d dScale = newScale / scale;
|
|
|
|
// verify new mass
|
|
double density = mass / (size.X() * size.Y() * size.Z());
|
|
ignition::math::Vector3d newSize = dScale * size;
|
|
double newMass = density * (newSize.X() * newSize.Y() * newSize.Z());
|
|
|
|
QVERIFY(ignition::math::equal(massElem->Get<double>(), newMass));
|
|
|
|
// verify new inertia values
|
|
// use msgs::AddBoxLink to help us compute the expected inertia values.
|
|
msgs::AddBoxLink(model, newMass, newScale);
|
|
msgs::Inertial newInertialMsg = model.link(2).inertial();
|
|
double newIxx = newInertialMsg.ixx();
|
|
double newIyy = newInertialMsg.iyy();
|
|
double newIzz = newInertialMsg.izz();
|
|
double newIxy = newInertialMsg.ixy();
|
|
double newIxz = newInertialMsg.ixz();
|
|
double newIyz = newInertialMsg.iyz();
|
|
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixx"), newIxx, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("iyy"), newIyy, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("izz"), newIzz, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixy"), newIxy, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixz"), newIxz, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("iyz"), newIyz, 1e-3));
|
|
}
|
|
delete link;
|
|
}
|
|
|
|
// cylinder
|
|
{
|
|
gui::LinkData *link = new gui::LinkData();
|
|
|
|
double mass = 1.0;
|
|
double radius = 0.5;
|
|
double length = 1.0;
|
|
|
|
msgs::Model model;
|
|
// set reasonable inertial values based on geometry
|
|
msgs::AddCylinderLink(model, mass, radius, length);
|
|
link->Load(msgs::LinkToSDF(model.link(0)));
|
|
rendering::VisualPtr linkVis(new rendering::Visual("cylinder_link", scene));
|
|
link->SetLinkVisual(linkVis);
|
|
|
|
// add a collision visual
|
|
rendering::VisualPtr collisionVis(
|
|
new rendering::Visual("cylinder_link::collision", linkVis));
|
|
collisionVis->Load(msgs::VisualToSDF(model.link(0).visual(0)));
|
|
link->AddCollision(collisionVis);
|
|
|
|
// verify scale
|
|
ignition::math::Vector3d scale = ignition::math::Vector3d::One;
|
|
QVERIFY(link->Scale() == scale);
|
|
|
|
sdf::ElementPtr linkSDF = link->linkSDF;
|
|
QVERIFY(linkSDF->HasElement("inertial"));
|
|
sdf::ElementPtr inertialElem = linkSDF->GetElement("inertial");
|
|
QVERIFY(inertialElem->HasElement("inertia"));
|
|
sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia");
|
|
QVERIFY(inertialElem->HasElement("mass"));
|
|
sdf::ElementPtr massElem = inertialElem->GetElement("mass");
|
|
|
|
// verify mass
|
|
QVERIFY(ignition::math::equal(massElem->Get<double>(), mass));
|
|
|
|
// verify inertia values
|
|
msgs::Inertial inertialMsg = model.link(0).inertial();
|
|
double ixx = inertialMsg.ixx();
|
|
double iyy = inertialMsg.iyy();
|
|
double izz = inertialMsg.izz();
|
|
double ixy = inertialMsg.ixy();
|
|
double ixz = inertialMsg.ixz();
|
|
double iyz = inertialMsg.iyz();
|
|
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixx"), ixx));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("iyy"), iyy));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("izz"), izz));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixy"), ixy));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixz"), ixz));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("iyz"), iyz));
|
|
|
|
// set new scale and verify inertial values
|
|
{
|
|
// set scale
|
|
ignition::math::Vector3d newScale =
|
|
ignition::math::Vector3d(8.5, 8.5, 1.5);
|
|
collisionVis->SetScale(newScale);
|
|
link->SetScale(newScale);
|
|
|
|
// verify new scale
|
|
QVERIFY(link->Scale() == newScale);
|
|
|
|
// change in scale
|
|
ignition::math::Vector3d dScale = newScale / scale;
|
|
|
|
// verify new mass
|
|
double density = mass / (M_PI*radius*radius*length);
|
|
double newRadius = radius * dScale.X();
|
|
double newLength = length * dScale.Z();
|
|
double newMass = density * (M_PI*newRadius*newRadius*newLength);
|
|
|
|
QVERIFY(ignition::math::equal(massElem->Get<double>(), newMass));
|
|
|
|
// verify new inertia values
|
|
// use msgs::AddCylinderLink to help us compute the
|
|
// expected inertia values.
|
|
msgs::AddCylinderLink(model, newMass, newRadius, newLength);
|
|
msgs::Inertial newInertialMsg = model.link(1).inertial();
|
|
double newIxx = newInertialMsg.ixx();
|
|
double newIyy = newInertialMsg.iyy();
|
|
double newIzz = newInertialMsg.izz();
|
|
double newIxy = newInertialMsg.ixy();
|
|
double newIxz = newInertialMsg.ixz();
|
|
double newIyz = newInertialMsg.iyz();
|
|
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixx"), newIxx, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("iyy"), newIyy, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("izz"), newIzz, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixy"), newIxy, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixz"), newIxz, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("iyz"), newIyz, 1e-3));
|
|
|
|
// update variables for next scale operation
|
|
scale = newScale;
|
|
mass = newMass;
|
|
radius = newRadius;
|
|
length = newLength;
|
|
}
|
|
|
|
// set another scale and verify inertial values
|
|
{
|
|
// set scale
|
|
ignition::math::Vector3d newScale =
|
|
ignition::math::Vector3d(1.2, 1.2, 3.4);
|
|
collisionVis->SetScale(newScale);
|
|
link->SetScale(newScale);
|
|
|
|
// verify new scale
|
|
QVERIFY(link->Scale() == newScale);
|
|
|
|
// change in scale
|
|
ignition::math::Vector3d dScale = newScale / scale;
|
|
|
|
// verify new mass
|
|
double density = mass / (M_PI*radius*radius*length);
|
|
double newRadius = radius * dScale.X();
|
|
double newLength = length * dScale.Z();
|
|
double newMass = density * (M_PI*newRadius*newRadius*newLength);
|
|
QVERIFY(ignition::math::equal(massElem->Get<double>(), newMass));
|
|
|
|
// verify new inertia values
|
|
// use msgs::AddCylinderLink to help us compute
|
|
// the expected inertia values.
|
|
msgs::AddCylinderLink(model, newMass, newRadius, newLength);
|
|
msgs::Inertial newInertialMsg = model.link(2).inertial();
|
|
double newIxx = newInertialMsg.ixx();
|
|
double newIyy = newInertialMsg.iyy();
|
|
double newIzz = newInertialMsg.izz();
|
|
double newIxy = newInertialMsg.ixy();
|
|
double newIxz = newInertialMsg.ixz();
|
|
double newIyz = newInertialMsg.iyz();
|
|
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixx"), newIxx, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("iyy"), newIyy, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("izz"), newIzz, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixy"), newIxy, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixz"), newIxz, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("iyz"), newIyz, 1e-3));
|
|
}
|
|
delete link;
|
|
}
|
|
|
|
// sphere
|
|
{
|
|
gui::LinkData *link = new gui::LinkData();
|
|
|
|
double mass = 1.0;
|
|
double radius = 0.5;
|
|
|
|
msgs::Model model;
|
|
// set reasonable inertial values based on geometry
|
|
msgs::AddSphereLink(model, mass, radius);
|
|
link->Load(msgs::LinkToSDF(model.link(0)));
|
|
rendering::VisualPtr linkVis(new rendering::Visual("sphere_link", scene));
|
|
link->SetLinkVisual(linkVis);
|
|
|
|
// add a collision visual
|
|
rendering::VisualPtr collisionVis(
|
|
new rendering::Visual("sphere_link::collision", linkVis));
|
|
collisionVis->Load(msgs::VisualToSDF(model.link(0).visual(0)));
|
|
link->AddCollision(collisionVis);
|
|
|
|
// verify scale
|
|
ignition::math::Vector3d scale = ignition::math::Vector3d::One;
|
|
QVERIFY(link->Scale() == scale);
|
|
|
|
sdf::ElementPtr linkSDF = link->linkSDF;
|
|
QVERIFY(linkSDF->HasElement("inertial"));
|
|
sdf::ElementPtr inertialElem = linkSDF->GetElement("inertial");
|
|
QVERIFY(inertialElem->HasElement("inertia"));
|
|
sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia");
|
|
QVERIFY(inertialElem->HasElement("mass"));
|
|
sdf::ElementPtr massElem = inertialElem->GetElement("mass");
|
|
|
|
// verify mass
|
|
QVERIFY(ignition::math::equal(massElem->Get<double>(), mass));
|
|
|
|
// verify inertia values
|
|
msgs::Inertial inertialMsg = model.link(0).inertial();
|
|
double ixx = inertialMsg.ixx();
|
|
double iyy = inertialMsg.iyy();
|
|
double izz = inertialMsg.izz();
|
|
double ixy = inertialMsg.ixy();
|
|
double ixz = inertialMsg.ixz();
|
|
double iyz = inertialMsg.iyz();
|
|
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixx"), ixx));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("iyy"), iyy));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("izz"), izz));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixy"), ixy));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixz"), ixz));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("iyz"), iyz));
|
|
|
|
// set new scale and verify inertial values
|
|
{
|
|
// set scale
|
|
ignition::math::Vector3d newScale =
|
|
ignition::math::Vector3d(2.5, 2.5, 2.5);
|
|
collisionVis->SetScale(newScale);
|
|
link->SetScale(newScale);
|
|
|
|
// verify new scale
|
|
QVERIFY(link->Scale() == newScale);
|
|
|
|
// change in scale
|
|
ignition::math::Vector3d dScale = newScale / scale;
|
|
|
|
// verify new mass
|
|
double density = mass / (4/3*M_PI*radius*radius*radius);
|
|
double newRadius = radius * dScale.X();
|
|
double newMass = density * (4/3*M_PI*newRadius*newRadius*newRadius);
|
|
QVERIFY(ignition::math::equal(massElem->Get<double>(), newMass));
|
|
|
|
// verify new inertia values
|
|
// use msgs::AddSphereLink to help us compute the expected inertia values.
|
|
msgs::AddSphereLink(model, newMass, newRadius);
|
|
msgs::Inertial newInertialMsg = model.link(1).inertial();
|
|
double newIxx = newInertialMsg.ixx();
|
|
double newIyy = newInertialMsg.iyy();
|
|
double newIzz = newInertialMsg.izz();
|
|
double newIxy = newInertialMsg.ixy();
|
|
double newIxz = newInertialMsg.ixz();
|
|
double newIyz = newInertialMsg.iyz();
|
|
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixx"), newIxx, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("iyy"), newIyy, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("izz"), newIzz, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixy"), newIxy, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixz"), newIxz, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("iyz"), newIyz, 1e-3));
|
|
|
|
// update variables for next scale operation
|
|
scale = newScale;
|
|
mass = newMass;
|
|
radius = newRadius;
|
|
}
|
|
|
|
// set another scale and verify inertial values
|
|
{
|
|
// set scale
|
|
ignition::math::Vector3d newScale =
|
|
ignition::math::Vector3d(3.1, 3.1, 3.1);
|
|
collisionVis->SetScale(newScale);
|
|
link->SetScale(newScale);
|
|
|
|
// verify new scale
|
|
QVERIFY(link->Scale() == newScale);
|
|
|
|
// change in scale
|
|
ignition::math::Vector3d dScale = newScale / scale;
|
|
|
|
// verify new mass
|
|
double density = mass / (4/3*M_PI*radius*radius*radius);
|
|
double newRadius = radius * dScale.X();
|
|
double newMass = density * (4/3*M_PI*newRadius*newRadius*newRadius);
|
|
QVERIFY(ignition::math::equal(massElem->Get<double>(), newMass));
|
|
|
|
// verify new inertia values
|
|
// use msgs::AddSphereLink to help us compute the expected inertia values.
|
|
msgs::AddSphereLink(model, newMass, newRadius);
|
|
msgs::Inertial newInertialMsg = model.link(2).inertial();
|
|
double newIxx = newInertialMsg.ixx();
|
|
double newIyy = newInertialMsg.iyy();
|
|
double newIzz = newInertialMsg.izz();
|
|
double newIxy = newInertialMsg.ixy();
|
|
double newIxz = newInertialMsg.ixz();
|
|
double newIyz = newInertialMsg.iyz();
|
|
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixx"), newIxx, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("iyy"), newIyy, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("izz"), newIzz, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixy"), newIxy, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixz"), newIxz, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("iyz"), newIyz, 1e-3));
|
|
}
|
|
delete link;
|
|
}
|
|
|
|
// non-unit sphere
|
|
{
|
|
gui::LinkData *link = new gui::LinkData();
|
|
|
|
double mass = 2.8;
|
|
double radius = 1.2;
|
|
|
|
msgs::Model model;
|
|
// set reasonable inertial values based on geometry
|
|
msgs::AddSphereLink(model, mass, radius);
|
|
link->Load(msgs::LinkToSDF(model.link(0)));
|
|
rendering::VisualPtr linkVis(new rendering::Visual("sphere_link2", scene));
|
|
link->SetLinkVisual(linkVis);
|
|
|
|
// add a collision visual
|
|
rendering::VisualPtr collisionVis(
|
|
new rendering::Visual("sphere_link2::collision", linkVis));
|
|
collisionVis->Load(msgs::VisualToSDF(model.link(0).visual(0)));
|
|
link->AddCollision(collisionVis);
|
|
|
|
// verify scale
|
|
ignition::math::Vector3d scale = ignition::math::Vector3d::One;
|
|
QVERIFY(link->Scale() == scale);
|
|
|
|
sdf::ElementPtr linkSDF = link->linkSDF;
|
|
QVERIFY(linkSDF->HasElement("inertial"));
|
|
sdf::ElementPtr inertialElem = linkSDF->GetElement("inertial");
|
|
QVERIFY(inertialElem->HasElement("inertia"));
|
|
sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia");
|
|
QVERIFY(inertialElem->HasElement("mass"));
|
|
sdf::ElementPtr massElem = inertialElem->GetElement("mass");
|
|
|
|
// verify mass
|
|
QVERIFY(ignition::math::equal(massElem->Get<double>(), mass));
|
|
|
|
// verify inertia values
|
|
msgs::Inertial inertialMsg = model.link(0).inertial();
|
|
double ixx = inertialMsg.ixx();
|
|
double iyy = inertialMsg.iyy();
|
|
double izz = inertialMsg.izz();
|
|
double ixy = inertialMsg.ixy();
|
|
double ixz = inertialMsg.ixz();
|
|
double iyz = inertialMsg.iyz();
|
|
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixx"), ixx));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("iyy"), iyy));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("izz"), izz));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixy"), ixy));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixz"), ixz));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("iyz"), iyz));
|
|
|
|
// set new scale and verify inertial values
|
|
{
|
|
// set scale
|
|
ignition::math::Vector3d newScale =
|
|
ignition::math::Vector3d(2.0, 2.0, 2.0);
|
|
collisionVis->SetScale(newScale *
|
|
ignition::math::Vector3d(radius*2, radius*2, radius*2));
|
|
link->SetScale(newScale);
|
|
|
|
// verify new scale
|
|
QVERIFY(link->Scale() == newScale);
|
|
|
|
// change in scale
|
|
ignition::math::Vector3d dScale = newScale / scale;
|
|
|
|
// verify new mass
|
|
double density = mass / (4/3*M_PI*radius*radius*radius);
|
|
double newRadius = radius * dScale.X();
|
|
double newMass = density * (4/3*M_PI*newRadius*newRadius*newRadius);
|
|
|
|
QVERIFY(ignition::math::equal(massElem->Get<double>(), newMass));
|
|
|
|
// verify new inertia values
|
|
// use msgs::AddSphereLink to help us compute the expected inertia values.
|
|
msgs::AddSphereLink(model, newMass, newRadius);
|
|
msgs::Inertial newInertialMsg = model.link(1).inertial();
|
|
double newIxx = newInertialMsg.ixx();
|
|
double newIyy = newInertialMsg.iyy();
|
|
double newIzz = newInertialMsg.izz();
|
|
double newIxy = newInertialMsg.ixy();
|
|
double newIxz = newInertialMsg.ixz();
|
|
double newIyz = newInertialMsg.iyz();
|
|
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixx"), newIxx, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("iyy"), newIyy, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("izz"), newIzz, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixy"), newIxy, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("ixz"), newIxz, 1e-3));
|
|
QVERIFY(ignition::math::equal(
|
|
inertiaElem->Get<double>("iyz"), newIyz, 1e-3));
|
|
}
|
|
delete link;
|
|
}
|
|
|
|
// precision test: scale down and back up
|
|
{
|
|
gui::LinkData *link = new gui::LinkData();
|
|
|
|
double mass = 1.0;
|
|
double radius = 0.5;
|
|
|
|
msgs::Model model;
|
|
// set reasonable inertial values based on geometry
|
|
msgs::AddSphereLink(model, mass, radius);
|
|
link->Load(msgs::LinkToSDF(model.link(0)));
|
|
rendering::VisualPtr linkVis(new rendering::Visual("sphere_link3", scene));
|
|
link->SetLinkVisual(linkVis);
|
|
|
|
// add a collision visual
|
|
rendering::VisualPtr collisionVis(
|
|
new rendering::Visual("sphere_link3::collision", linkVis));
|
|
collisionVis->Load(msgs::VisualToSDF(model.link(0).visual(0)));
|
|
link->AddCollision(collisionVis);
|
|
|
|
// verify scale
|
|
ignition::math::Vector3d scale = ignition::math::Vector3d::One;
|
|
QVERIFY(link->Scale() == scale);
|
|
|
|
sdf::ElementPtr linkSDF = link->linkSDF;
|
|
QVERIFY(linkSDF->HasElement("inertial"));
|
|
sdf::ElementPtr inertialElem = linkSDF->GetElement("inertial");
|
|
QVERIFY(inertialElem->HasElement("inertia"));
|
|
sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia");
|
|
QVERIFY(inertialElem->HasElement("mass"));
|
|
sdf::ElementPtr massElem = inertialElem->GetElement("mass");
|
|
|
|
// verify mass
|
|
QVERIFY(ignition::math::equal(massElem->Get<double>(), mass));
|
|
|
|
// verify inertia values
|
|
msgs::Inertial inertialMsg = model.link(0).inertial();
|
|
double ixx = inertialMsg.ixx();
|
|
double iyy = inertialMsg.iyy();
|
|
double izz = inertialMsg.izz();
|
|
double ixy = inertialMsg.ixy();
|
|
double ixz = inertialMsg.ixz();
|
|
double iyz = inertialMsg.iyz();
|
|
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixx"), ixx));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("iyy"), iyy));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("izz"), izz));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixy"), ixy));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixz"), ixz));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("iyz"), iyz));
|
|
|
|
// scale down
|
|
double scaleFactor = 1;
|
|
for (unsigned int i = 10; i <= 1e6; i = i*10)
|
|
{
|
|
// set scale
|
|
scaleFactor = 1.0/static_cast<double>(i);
|
|
ignition::math::Vector3d newScale =
|
|
ignition::math::Vector3d(scaleFactor, scaleFactor, scaleFactor);
|
|
collisionVis->SetScale(newScale);
|
|
link->SetScale(newScale);
|
|
// verify new scale
|
|
QVERIFY(link->Scale() == newScale);
|
|
}
|
|
// scale up
|
|
for (unsigned int i = 1e5; i >= 1; i = i/10)
|
|
{
|
|
// set scale
|
|
scaleFactor = 1.0/static_cast<double>(i);
|
|
ignition::math::Vector3d newScale =
|
|
ignition::math::Vector3d(scaleFactor, scaleFactor, scaleFactor);
|
|
collisionVis->SetScale(newScale);
|
|
link->SetScale(newScale);
|
|
// verify new scale
|
|
QVERIFY(link->Scale() == newScale);
|
|
}
|
|
// verify against original mass and inertia values
|
|
QVERIFY(ignition::math::equal(massElem->Get<double>(), mass));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixx"), ixx));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("iyy"), iyy));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("izz"), izz));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixy"), ixy));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("ixz"), ixz));
|
|
QVERIFY(ignition::math::equal(inertiaElem->Get<double>("iyz"), iyz));
|
|
delete link;
|
|
}
|
|
|
|
mainWindow->close();
|
|
delete mainWindow;
|
|
mainWindow = nullptr;
|
|
}
|
|
|
|
// Generate a main function for the test
|
|
QTEST_MAIN(ModelData_TEST)
|