Skip to content

Commit

Permalink
Fixed issues from review.
Browse files Browse the repository at this point in the history 
Signed-off-by: Martin Pecka <[email protected]>
  • Loading branch information
@peci1 @azeey
peci1 authored and azeey committed Oct 28, 2021
1 parent 1136e8c commit ead9f03
Show file tree
Hide file tree
Showing 4 changed files with 160 additions and 61 deletions.
2 changes: 1 addition & 1 deletion include/ignition/gazebo/components/Serialization.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -196,7 +196,7 @@ namespace serializers
{
_out << _data.size();
for (const auto& datum : _data)
_out << datum;
_out << " " << datum;
return _out;
}

Expand Down
24 changes: 12 additions & 12 deletions src/systems/physics/Physics.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -345,21 +345,21 @@ class ignition::gazebo::systems::PhysicsPrivate
// Joint position limits command
/// \brief Feature list for setting joint position limits.
public: struct JointPositionLimitsCommandFeatureList : physics::FeatureList<
physics::SetJointPositionLimitsCommandFeature>{};
physics::SetJointPositionLimitsFeature>{};


//////////////////////////////////////////////////
// Joint velocity limits command
/// \brief Feature list for setting joint velocity limits.
public: struct JointVelocityLimitsCommandFeatureList : physics::FeatureList<
physics::SetJointVelocityLimitsCommandFeature>{};
physics::SetJointVelocityLimitsFeature>{};


//////////////////////////////////////////////////
// Joint effort limits command
/// \brief Feature list for setting joint effort limits.
public: struct JointEffortLimitsCommandFeatureList : physics::FeatureList<
physics::SetJointEffortLimitsCommandFeature>{};
physics::SetJointEffortLimitsFeature>{};


//////////////////////////////////////////////////
Expand Down Expand Up @@ -1282,7 +1282,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
_entity);
if (posLimits && !posLimits->Data().empty())
{
auto limits = posLimits->Data();
const auto& limits = posLimits->Data();

if (limits.size() != jointPhys->GetDegreesOfFreedom())
{
Expand All @@ -1303,8 +1303,8 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)

for (std::size_t i = 0; i < nDofs; ++i)
{
jointPosLimitsFeature->SetMinPositionCommand(i, limits[i].X());
jointPosLimitsFeature->SetMaxPositionCommand(i, limits[i].Y());
jointPosLimitsFeature->SetMinPosition(i, limits[i].X());
jointPosLimitsFeature->SetMaxPosition(i, limits[i].Y());
}
}
}
Expand All @@ -1313,7 +1313,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
_entity);
if (velLimits && !velLimits->Data().empty())
{
auto limits = velLimits->Data();
const auto& limits = velLimits->Data();

if (limits.size() != jointPhys->GetDegreesOfFreedom())
{
Expand All @@ -1334,8 +1334,8 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)

for (std::size_t i = 0; i < nDofs; ++i)
{
jointVelLimitsFeature->SetMinVelocityCommand(i, limits[i].X());
jointVelLimitsFeature->SetMaxVelocityCommand(i, limits[i].Y());
jointVelLimitsFeature->SetMinVelocity(i, limits[i].X());
jointVelLimitsFeature->SetMaxVelocity(i, limits[i].Y());
}
}
}
Expand All @@ -1344,7 +1344,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
_entity);
if (effLimits && !effLimits->Data().empty())
{
auto limits = effLimits->Data();
const auto& limits = effLimits->Data();

if (limits.size() != jointPhys->GetDegreesOfFreedom())
{
Expand All @@ -1365,8 +1365,8 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)

for (std::size_t i = 0; i < nDofs; ++i)
{
jointEffLimitsFeature->SetMinEffortCommand(i, limits[i].X());
jointEffLimitsFeature->SetMaxEffortCommand(i, limits[i].Y());
jointEffLimitsFeature->SetMinEffort(i, limits[i].X());
jointEffLimitsFeature->SetMaxEffort(i, limits[i].Y());
}
}
}
Expand Down
105 changes: 105 additions & 0 deletions test/integration/components.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,9 @@
#include "ignition/gazebo/components/Inertial.hh"
#include "ignition/gazebo/components/Joint.hh"
#include "ignition/gazebo/components/JointAxis.hh"
#include "ignition/gazebo/components/JointEffortLimitsCmd.hh"
#include "ignition/gazebo/components/JointPositionLimitsCmd.hh"
#include "ignition/gazebo/components/JointVelocityLimitsCmd.hh"
#include "ignition/gazebo/components/JointType.hh"
#include "ignition/gazebo/components/JointVelocity.hh"
#include "ignition/gazebo/components/JointVelocityCmd.hh"
Expand Down Expand Up @@ -530,6 +533,108 @@ TEST_F(ComponentsTest, JointAxis)
EXPECT_EQ(comp3.Data().XyzExpressedIn(), "__model__");
}

/////////////////////////////////////////////////
TEST_F(ComponentsTest, JointEffortLimitsCmd)
{
// Create components
auto comp1 = components::JointEffortLimitsCmd();
auto comp2 = components::JointEffortLimitsCmd();

// Equality operators
EXPECT_EQ(comp1, comp2);
EXPECT_TRUE(comp1 == comp2);
EXPECT_FALSE(comp1 != comp2);

// Stream operators
std::ostringstream ostr;
comp1.Serialize(ostr);
EXPECT_EQ("0", ostr.str());

comp2.Data().push_back(math::Vector2d(-1.0, 1.0));

std::ostringstream ostr2;
comp2.Serialize(ostr2);
EXPECT_EQ("1 -1 1", ostr2.str());

comp2.Data().push_back(math::Vector2d(-2.5, 2.5));

std::ostringstream ostr3;
comp2.Serialize(ostr3);
EXPECT_EQ("2 -1 1 -2.5 2.5", ostr3.str());

std::istringstream istr("ignored");
components::JointEffortLimitsCmd comp3;
comp3.Deserialize(istr);
}

/////////////////////////////////////////////////
TEST_F(ComponentsTest, JointPositionLimitsCmd)
{
// Create components
auto comp1 = components::JointPositionLimitsCmd();
auto comp2 = components::JointPositionLimitsCmd();

// Equality operators
EXPECT_EQ(comp1, comp2);
EXPECT_TRUE(comp1 == comp2);
EXPECT_FALSE(comp1 != comp2);

// Stream operators
std::ostringstream ostr;
comp1.Serialize(ostr);
EXPECT_EQ("0", ostr.str());

comp2.Data().push_back(math::Vector2d(-1.0, 1.0));

std::ostringstream ostr2;
comp2.Serialize(ostr2);
EXPECT_EQ("1 -1 1", ostr2.str());

comp2.Data().push_back(math::Vector2d(-2.5, 2.5));

std::ostringstream ostr3;
comp2.Serialize(ostr3);
EXPECT_EQ("2 -1 1 -2.5 2.5", ostr3.str());

std::istringstream istr("ignored");
components::JointPositionLimitsCmd comp3;
comp3.Deserialize(istr);
}

/////////////////////////////////////////////////
TEST_F(ComponentsTest, JointVelocityLimitsCmd)
{
// Create components
auto comp1 = components::JointVelocityLimitsCmd();
auto comp2 = components::JointVelocityLimitsCmd();

// Equality operators
EXPECT_EQ(comp1, comp2);
EXPECT_TRUE(comp1 == comp2);
EXPECT_FALSE(comp1 != comp2);

// Stream operators
std::ostringstream ostr;
comp1.Serialize(ostr);
EXPECT_EQ("0", ostr.str());

comp2.Data().push_back(math::Vector2d(-1.0, 1.0));

std::ostringstream ostr2;
comp2.Serialize(ostr2);
EXPECT_EQ("1 -1 1", ostr2.str());

comp2.Data().push_back(math::Vector2d(-2.5, 2.5));

std::ostringstream ostr3;
comp2.Serialize(ostr3);
EXPECT_EQ("2 -1 1 -2.5 2.5", ostr3.str());

std::istringstream istr("ignored");
components::JointVelocityLimitsCmd comp3;
comp3.Deserialize(istr);
}

/////////////////////////////////////////////////
TEST_F(ComponentsTest, JointType)
{
Expand Down
90 changes: 42 additions & 48 deletions test/integration/physics_system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -856,45 +856,43 @@ TEST_F(PhysicsSystemFixture, JointPositionLimitsCommandComponent)
{
if (_name->Data() == rotatingJointName)
{
auto limitComp =
_ecm.Component<components::JointPositionLimitsCmd>(_entity);

if (iteration == 0u)
{
auto limitComp =
_ecm.Component<components::JointPositionLimitsCmd>(_entity);
EXPECT_EQ(nullptr, limitComp);
_ecm.CreateComponent(_entity,
components::JointPositionLimitsCmd ({{-1e-6, 1e-6}}));
_ecm.CreateComponent(_entity, components::JointPosition());
}
else if (iteration == 50u)
{
auto limitComp =
_ecm.Component<components::JointPositionLimitsCmd>(_entity);
EXPECT_NE(nullptr, limitComp);
limitComp->Data() = {{-1e6, 1e6}};
if (limitComp)
{
limitComp->Data() = {{-1e6, 1e6}};
}
}
else
{
auto limitComp =
_ecm.Component<components::JointPositionLimitsCmd>(_entity);
EXPECT_NE(nullptr, limitComp);
if (limitComp)
{
EXPECT_EQ(0u, limitComp->Data().size());
}
if (iteration >= 30u && iteration < 40u)
{
if (iteration == 30u)
_ecm.CreateComponent(_entity, components::JointForceCmd(
std::vector<double>({100.0})));
else
_ecm.Component<components::JointForceCmd>(_entity)->Data() =
{100.0};
_ecm.SetComponentData<components::JointForceCmd>(
_entity, {100.0});
}
else if (iteration >= 40u && iteration < 50u)
{
if (iteration == 40u)
_ecm.CreateComponent(_entity, components::JointVelocityCmd(
std::vector<double>({1.0})));
else
_ecm.Component<components::JointVelocityCmd>(
_entity)->Data() = {1.0};
_ecm.SetComponentData<components::JointVelocityCmd>(
_entity, {1.0});
}
}
++iteration;
Expand Down Expand Up @@ -981,45 +979,43 @@ TEST_F(PhysicsSystemFixture, JointVelocityLimitsCommandComponent)
{
if (_name->Data() == rotatingJointName)
{
auto limitComp =
_ecm.Component<components::JointVelocityLimitsCmd>(_entity);

if (iteration == 0u)
{
auto limitComp =
_ecm.Component<components::JointVelocityLimitsCmd>(_entity);
EXPECT_EQ(nullptr, limitComp);
_ecm.CreateComponent(_entity,
components::JointVelocityLimitsCmd ({{-0.1, 0.1}}));
_ecm.CreateComponent(_entity, components::JointVelocity());
}
else if (iteration == 50u)
{
auto limitComp =
_ecm.Component<components::JointVelocityLimitsCmd>(_entity);
EXPECT_NE(nullptr, limitComp);
limitComp->Data() = {{-1e6, 1e6}};
if (limitComp)
{
limitComp->Data() = {{-1e6, 1e6}};
}
}
else
{
auto limitComp =
_ecm.Component<components::JointVelocityLimitsCmd>(_entity);
EXPECT_NE(nullptr, limitComp);
if (limitComp)
{
EXPECT_EQ(0u, limitComp->Data().size());
}
if (iteration >= 30u && iteration < 40u)
{
if (iteration == 30u)
_ecm.CreateComponent(_entity, components::JointForceCmd(
std::vector<double>({100.0})));
else
_ecm.Component<components::JointForceCmd>(_entity)->Data() =
{100.0};
_ecm.SetComponentData<components::JointForceCmd>(
_entity, {100.0});
}
else if (iteration >= 40u && iteration < 50u)
{
if (iteration == 40u)
_ecm.CreateComponent(_entity, components::JointVelocityCmd(
std::vector<double>({1.0})));
else
_ecm.Component<components::JointVelocityCmd>(
_entity)->Data() = {1.0};
_ecm.SetComponentData<components::JointVelocityCmd>(
_entity, {1.0});
}
}
++iteration;
Expand Down Expand Up @@ -1107,45 +1103,43 @@ TEST_F(PhysicsSystemFixture, JointEffortLimitsCommandComponent)
{
if (_name->Data() == rotatingJointName)
{
auto limitComp =
_ecm.Component<components::JointEffortLimitsCmd>(_entity);

if (iteration == 0u)
{
auto limitComp =
_ecm.Component<components::JointEffortLimitsCmd>(_entity);
EXPECT_EQ(nullptr, limitComp);
_ecm.CreateComponent(_entity,
components::JointEffortLimitsCmd ({{-1e-6, 1e-6}}));
_ecm.CreateComponent(_entity, components::JointPosition());
}
else if (iteration == 50u)
{
auto limitComp =
_ecm.Component<components::JointEffortLimitsCmd>(_entity);
EXPECT_NE(nullptr, limitComp);
limitComp->Data() = {{-1e9, 1e9}};
if (limitComp)
{
limitComp->Data() = {{-1e9, 1e9}};
}
}
else
{
auto limitComp =
_ecm.Component<components::JointEffortLimitsCmd>(_entity);
EXPECT_NE(nullptr, limitComp);
if (limitComp)
{
EXPECT_EQ(0u, limitComp->Data().size());
}
if (iteration >= 30u && iteration < 40u)
{
if (iteration == 30u)
_ecm.CreateComponent(_entity, components::JointForceCmd(
std::vector<double>({100.0})));
else
_ecm.Component<components::JointForceCmd>(_entity)->Data() =
{100.0};
_ecm.SetComponentData<components::JointForceCmd>(
_entity, {100.0});
}
else if (iteration >= 40u && iteration < 50u)
{
if (iteration == 40u)
_ecm.CreateComponent(_entity, components::JointVelocityCmd(
std::vector<double>({1.0})));
else
_ecm.Component<components::JointVelocityCmd>(
_entity)->Data() = {1.0};
_ecm.SetComponentData<components::JointVelocityCmd>(
_entity, {1.0});
}
else if (iteration >= 50u)
{
Expand Down

0 comments on commit ead9f03

Please sign in to comment.