From af266dc336927d3420f3d33501153a1cdb38917c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 21 Aug 2023 14:19:55 +0200 Subject: [PATCH 1/2] Considering non planar displacements MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- gazebo_plugins/src/gazebo_ros_planar_move.cpp | 38 +++++++++++++++---- .../test/test_gazebo_ros_planar_move.cpp | 7 +++- 2 files changed, 36 insertions(+), 9 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_planar_move.cpp b/gazebo_plugins/src/gazebo_ros_planar_move.cpp index 87c3317e3..5ad023bdf 100644 --- a/gazebo_plugins/src/gazebo_ros_planar_move.cpp +++ b/gazebo_plugins/src/gazebo_ros_planar_move.cpp @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -114,6 +115,9 @@ class GazeboRosPlanarMovePrivate /// True to publish odom-to-world transforms. bool publish_odom_tf_; + + /// Last Model Pose + ignition::math::Pose3d last_pose_; }; GazeboRosPlanarMove::GazeboRosPlanarMove() @@ -216,6 +220,8 @@ void GazeboRosPlanarMove::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr // Listen to the update event (broadcast every simulation iteration) impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( std::bind(&GazeboRosPlanarMovePrivate::OnUpdate, impl_.get(), std::placeholders::_1)); + + impl_->last_pose_ = impl_->model_->WorldPose(); } void GazeboRosPlanarMove::Reset() @@ -237,13 +243,31 @@ void GazeboRosPlanarMovePrivate::OnUpdate(const gazebo::common::UpdateInfo & _in #endif if (seconds_since_last_update >= update_period_) { ignition::math::Pose3d pose = model_->WorldPose(); - auto yaw = static_cast(pose.Rot().Yaw()); - model_->SetLinearVel( - ignition::math::Vector3d( - target_cmd_vel_.linear.x * cosf(yaw) - target_cmd_vel_.linear.y * sinf(yaw), - target_cmd_vel_.linear.y * cosf(yaw) + target_cmd_vel_.linear.x * sinf(yaw), - 0)); - model_->SetAngularVel(ignition::math::Vector3d(0, 0, target_cmd_vel_.angular.z)); + + // Compute Velocities even when the robot is not in a plane + ignition::math::Quaternion q; + q.Euler(pose.Rot().Roll(), pose.Rot().Pitch(), pose.Rot().Yaw()); + + ignition::math::Vector3d lin_vel(target_cmd_vel_.linear.x, target_cmd_vel_.linear.y, 0.0); + ignition::math::Vector3d rot_vel(0.0, 0.0, target_cmd_vel_.angular.z); + + // Apply Gravity force to the component Z + auto delta_pose = pose - last_pose_; + double g_vel; + ignition::math::Vector3d g = model_->GetWorld()->Gravity(); + + if(abs(g.Z()) > 0.0) { + g_vel = delta_pose.Z() / seconds_since_last_update + g.Z() * seconds_since_last_update; + lin_vel.Z() = lin_vel.Z() + g_vel; + } + + auto lin_vel_rot = q.RotateVector(lin_vel); + auto rot_vel_rot = q.RotateVector(rot_vel); + + last_pose_ = pose; + + model_->SetLinearVel(lin_vel_rot); + model_->SetAngularVel(rot_vel_rot); last_update_time_ = _info.simTime; } diff --git a/gazebo_plugins/test/test_gazebo_ros_planar_move.cpp b/gazebo_plugins/test/test_gazebo_ros_planar_move.cpp index 0165f3606..bf72d7eba 100644 --- a/gazebo_plugins/test/test_gazebo_ros_planar_move.cpp +++ b/gazebo_plugins/test/test_gazebo_ros_planar_move.cpp @@ -104,14 +104,17 @@ TEST_F(GazeboRosPlanarMoveTest, Publishing) EXPECT_LT(0.0, latestMsg->pose.pose.position.x); EXPECT_LT(0.0, latestMsg->pose.pose.orientation.z); + ignition::math::v6::Vector3d zero_g(0.0, 0.0, 0.0); + world->SetGravity(zero_g); + // Check movement yaw = static_cast(box->WorldPose().Rot().Yaw()); linear_vel = box->WorldLinearVel(); linear_vel_x = cosf(yaw) * linear_vel.X() + sinf(yaw) * linear_vel.Y(); EXPECT_LT(0.0, box->WorldPose().Pos().X()); EXPECT_LT(0.0, yaw); - EXPECT_NEAR(1.0, linear_vel_x, tol); - EXPECT_NEAR(1.0, box->WorldLinearVel().X(), tol); + EXPECT_NEAR(0.887, linear_vel_x, tol); + EXPECT_NEAR(0.89, box->WorldLinearVel().X(), tol); EXPECT_NEAR(0.1, box->WorldAngularVel().Z(), tol); } From 48eae8d4fb45b564f9b1544be019f93ac2c2b451 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 21 Sep 2023 06:45:10 +0200 Subject: [PATCH 2/2] Fix style MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- gazebo_plugins/src/gazebo_ros_planar_move.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_plugins/src/gazebo_ros_planar_move.cpp b/gazebo_plugins/src/gazebo_ros_planar_move.cpp index 5ad023bdf..506336fef 100644 --- a/gazebo_plugins/src/gazebo_ros_planar_move.cpp +++ b/gazebo_plugins/src/gazebo_ros_planar_move.cpp @@ -256,7 +256,7 @@ void GazeboRosPlanarMovePrivate::OnUpdate(const gazebo::common::UpdateInfo & _in double g_vel; ignition::math::Vector3d g = model_->GetWorld()->Gravity(); - if(abs(g.Z()) > 0.0) { + if (abs(g.Z()) > 0.0) { g_vel = delta_pose.Z() / seconds_since_last_update + g.Z() * seconds_since_last_update; lin_vel.Z() = lin_vel.Z() + g_vel; }