Skip to content

Commit

Permalink
Fixed Odometry Publisher Angular Velocity Singularity in 3D (#2348)
Browse files Browse the repository at this point in the history 
The angular velocity was calculated using euler angles which led to singularities

---------

Signed-off-by: Tom Creutz <[email protected]>
Signed-off-by: Shameek Ganguly <[email protected]>
Signed-off-by: Addisu Z. Taddese <[email protected]>
Co-authored-by: Addisu Z. Taddese <[email protected]>
Co-authored-by: Aditya Pande <[email protected]>
Co-authored-by: Ian Chen <[email protected]>
Co-authored-by: Shameek Ganguly <[email protected]>
  • Loading branch information
@tomcreutz @azeey
@adityapande-1995 @iche033 @shameekganguly
5 people authored Aug 26, 2024
1 parent d662000 commit bf05593
Show file tree
Hide file tree
Showing 3 changed files with 222 additions and 25 deletions.
59 changes: 34 additions & 25 deletions src/systems/odometry_publisher/OdometryPublisher.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,15 @@ class gz::sim::systems::OdometryPublisherPrivate
public: void UpdateOdometry(const gz::sim::UpdateInfo &_info,
const gz::sim::EntityComponentManager &_ecm);

/// \brief Calculates angular velocity in body frame from world frame poses.
/// \param[in] _lastPose Pose at last timestep in world frame.
/// \param[in] _currentPose Pose at current timestep in world frame.
/// \param[in] _dt Time elapsed from last to current timestep.
/// \return Angular velocity computed in body frame at current timestep.
public: static math::Vector3d CalculateAngularVelocity(
const math::Pose3d &_lastPose, const math::Pose3d &_currentPose,
std::chrono::duration<double> _dt);

/// \brief Gazebo communication node.
public: transport::Node node;

Expand Down Expand Up @@ -119,18 +128,12 @@ OdometryPublisher::OdometryPublisher()
std::get<0>(this->dataPtr->linearMean).SetWindowSize(10);
std::get<1>(this->dataPtr->linearMean).SetWindowSize(10);
std::get<2>(this->dataPtr->angularMean).SetWindowSize(10);
std::get<0>(this->dataPtr->linearMean).Clear();
std::get<1>(this->dataPtr->linearMean).Clear();
std::get<2>(this->dataPtr->angularMean).Clear();

if (this->dataPtr->dimensions == 3)
{
std::get<2>(this->dataPtr->linearMean).SetWindowSize(10);
std::get<0>(this->dataPtr->angularMean).SetWindowSize(10);
std::get<1>(this->dataPtr->angularMean).SetWindowSize(10);
std::get<2>(this->dataPtr->linearMean).Clear();
std::get<0>(this->dataPtr->angularMean).Clear();
std::get<1>(this->dataPtr->angularMean).Clear();
}
}

Expand Down Expand Up @@ -329,6 +332,26 @@ void OdometryPublisher::PostUpdate(const UpdateInfo &_info,
this->dataPtr->UpdateOdometry(_info, _ecm);
}

//////////////////////////////////////////////////
math::Vector3d OdometryPublisherPrivate::CalculateAngularVelocity(
const math::Pose3d &_lastPose, const math::Pose3d &_currentPose,
std::chrono::duration<double> _dt)
{
// Compute the first order finite difference between current and previous
// rotation as quaternion.
const math::Quaterniond rotationDiff =
_currentPose.Rot() * _lastPose.Rot().Inverse();

math::Vector3d rotationAxis;
double rotationAngle;
rotationDiff.AxisAngle(rotationAxis, rotationAngle);

const math::Vector3d angularVelocity =
(rotationAngle / _dt.count()) * rotationAxis;

return _currentPose.Rot().RotateVectorReverse(angularVelocity);
}

//////////////////////////////////////////////////
void OdometryPublisherPrivate::UpdateOdometry(
const gz::sim::UpdateInfo &_info,
Expand Down Expand Up @@ -375,10 +398,8 @@ void OdometryPublisherPrivate::UpdateOdometry(
double linearDisplacementY = pose.Pos().Y() - this->lastUpdatePose.Pos().Y();

double currentYaw = pose.Rot().Yaw();
const double lastYaw = this->lastUpdatePose.Rot().Yaw();
while (currentYaw < lastYaw - GZ_PI) currentYaw += 2 * GZ_PI;
while (currentYaw > lastYaw + GZ_PI) currentYaw -= 2 * GZ_PI;
const float yawDiff = currentYaw - lastYaw;
const math::Vector3d angularVelocityBody = CalculateAngularVelocity(
this->lastUpdatePose, pose, dt);

// Get velocities assuming 2D
if (this->dimensions == 2)
Expand Down Expand Up @@ -406,18 +427,6 @@ void OdometryPublisherPrivate::UpdateOdometry(
// Get velocities and roll/pitch rates assuming 3D
else if (this->dimensions == 3)
{
double currentRoll = pose.Rot().Roll();
const double lastRoll = this->lastUpdatePose.Rot().Roll();
while (currentRoll < lastRoll - GZ_PI) currentRoll += 2 * GZ_PI;
while (currentRoll > lastRoll + GZ_PI) currentRoll -= 2 * GZ_PI;
const float rollDiff = currentRoll - lastRoll;

double currentPitch = pose.Rot().Pitch();
const double lastPitch = this->lastUpdatePose.Rot().Pitch();
while (currentPitch < lastPitch - GZ_PI) currentPitch += 2 * GZ_PI;
while (currentPitch > lastPitch + GZ_PI) currentPitch -= 2 * GZ_PI;
const float pitchDiff = currentPitch - lastPitch;

double linearDisplacementZ =
pose.Pos().Z() - this->lastUpdatePose.Pos().Z();
math::Vector3 linearDisplacement(linearDisplacementX, linearDisplacementY,
Expand All @@ -427,8 +436,8 @@ void OdometryPublisherPrivate::UpdateOdometry(
std::get<0>(this->linearMean).Push(linearVelocity.X());
std::get<1>(this->linearMean).Push(linearVelocity.Y());
std::get<2>(this->linearMean).Push(linearVelocity.Z());
std::get<0>(this->angularMean).Push(rollDiff / dt.count());
std::get<1>(this->angularMean).Push(pitchDiff / dt.count());
std::get<0>(this->angularMean).Push(angularVelocityBody.X());
std::get<1>(this->angularMean).Push(angularVelocityBody.Y());
msg.mutable_twist()->mutable_linear()->set_x(
std::get<0>(this->linearMean).Mean() +
gz::math::Rand::DblNormal(0, this->gaussianNoise));
Expand All @@ -447,7 +456,7 @@ void OdometryPublisherPrivate::UpdateOdometry(
}

// Set yaw rate
std::get<2>(this->angularMean).Push(yawDiff / dt.count());
std::get<2>(this->angularMean).Push(angularVelocityBody.Z());
msg.mutable_twist()->mutable_angular()->set_z(
std::get<2>(this->angularMean).Mean() +
gz::math::Rand::DblNormal(0, this->gaussianNoise));
Expand Down
91 changes: 91 additions & 0 deletions test/integration/odometry_publisher.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@

#include <gz/common/Console.hh>
#include <gz/common/Util.hh>
#include <gz/math/Helpers.hh>
#include <gz/math/Pose3.hh>
#include <gz/transport/Node.hh>
#include <gz/utils/ExtraTestMacros.hh>
Expand Down Expand Up @@ -380,6 +381,86 @@ class OdometryPublisherTest
EXPECT_NEAR(poses.back().Rot().Z(), tfPoses.back().Rot().Z(), 1e-2);
}

/// \param[in] _sdfFile SDF file to load.
/// \param[in] _odomTopic Odometry topic.
protected: void TestMovement3dAtSingularity(const std::string &_sdfFile,
const std::string &_odomTopic)
{
// Start server
ServerConfig serverConfig;
serverConfig.SetSdfFile(_sdfFile);

Server server(serverConfig);
EXPECT_FALSE(server.Running());
EXPECT_FALSE(*server.Running(0));

// Create a system that records the body poses
test::Relay testSystem;

std::vector<math::Pose3d> poses;
testSystem.OnPostUpdate([&poses](const sim::UpdateInfo &,
const sim::EntityComponentManager &_ecm)
{
auto id = _ecm.EntityByComponents(
components::Model(),
components::Name("test_body"));
EXPECT_NE(kNullEntity, id);

auto poseComp = _ecm.Component<components::Pose>(id);
ASSERT_NE(nullptr, poseComp);
poses.push_back(poseComp->Data());
});
server.AddSystem(testSystem.systemPtr);

std::vector<math::Vector3d> odomAngVels;
// Create function to store data from odometry messages
std::function<void(const msgs::Odometry &)> odomCb =
[&](const msgs::Odometry &_msg)
{
odomAngVels.push_back(msgs::Convert(_msg.twist().angular()));
};

// Create node for publishing twist messages
transport::Node node;
auto cmdVel = node.Advertise<msgs::Twist>("/model/test_body/cmd_vel");
node.Subscribe(_odomTopic, odomCb);

// Set an angular velocity command that would cause pitch to update from 0
// to PI in 1 second, crossing the singularity when pitch is PI/2.
const math::Vector3d angVelCmd(0.0, GZ_PI, 0);
msgs::Twist msg;
msgs::Set(msg.mutable_linear(), math::Vector3d::Zero);
msgs::Set(msg.mutable_angular(), angVelCmd);
cmdVel.Publish(msg);

// Run server while the model moves with the velocities set earlier
server.Run(true, 1000, false);

// Poses for 1s
ASSERT_EQ(1000u, poses.size());

int sleep = 0;
int maxSleep = 30;
// Default publishing frequency for odometryPublisher is 50Hz.
for (; (odomAngVels.size() < 50) &&
sleep < maxSleep; ++sleep)
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
EXPECT_NE(maxSleep, sleep);

// Odom for 1s
ASSERT_FALSE(odomAngVels.empty());
EXPECT_EQ(50u, odomAngVels.size());

// Check accuracy of velocities published in the odometry message
for (size_t i = 1; i < odomAngVels.size(); ++i) {
EXPECT_NEAR(odomAngVels[i].X(), angVelCmd[0], 1e-1);
EXPECT_NEAR(odomAngVels[i].Y(), angVelCmd[1], 1e-1);
EXPECT_NEAR(odomAngVels[i].Z(), angVelCmd[2], 1e-1);
}
}

/// \param[in] _sdfFile SDF file to load.
/// \param[in] _odomTopic Odometry topic.
/// \param[in] _frameId Name of the world-fixed coordinate frame
Expand Down Expand Up @@ -624,6 +705,16 @@ TEST_P(OdometryPublisherTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Movement3d))
"/model/X3/odometry", "/model/X3/pose", "X3/odom", "X3/base_footprint");
}

/////////////////////////////////////////////////
TEST_P(OdometryPublisherTest,
GZ_UTILS_TEST_DISABLED_ON_WIN32(Movement3dAtSingularity))
{
TestMovement3dAtSingularity(
gz::common::joinPaths(PROJECT_SOURCE_PATH,
"test", "worlds", "odometry_publisher_3d_singularity.sdf"),
"/model/test_body/odometry");
}

/////////////////////////////////////////////////
TEST_P(OdometryPublisherTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(OdomFrameId))
{
Expand Down
97 changes: 97 additions & 0 deletions test/worlds/odometry_publisher_3d_singularity.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,97 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="default">

<physics name="1ms" type="ode">
<max_step_size>0.001</max_step_size>
<real_time_factor>0</real_time_factor>
</physics>
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>1 1 1 1</diffuse>
<specular>0.5 0.5 0.5 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

<model name="test_body">
<pose>0 0 1 0 0 0</pose>
<link name="base_link">
<inertial>
<mass>1.5</mass>
<inertia>
<ixx>0.0347563</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.07</iyy>
<iyz>0</iyz>
<izz>0.0977</izz>
</inertia>
</inertial>
<collision name="base_link_inertia_collision">
<geometry>
<box>
<size>0.30 0.42 0.11</size>
</box>
</geometry>
</collision>
<visual name="base_link_inertia_visual">
<geometry>
<box>
<size>0.30 0.42 0.11</size>
</box>
</geometry>
</visual>
</link>
<plugin
filename="gz-sim-velocity-control-system"
name="gz::sim::systems::VelocityControl">
<initial_linear>0 0 0</initial_linear>
<initial_angular>0 3.1415 0</initial_angular>
</plugin>
<plugin
filename="gz-sim-odometry-publisher-system"
name="gz::sim::systems::OdometryPublisher">
<dimensions>3</dimensions>
</plugin>
</model>

</world>
</sdf>

0 comments on commit bf05593

Please sign in to comment.