diff --git a/gazebo/physics/Collision.cc b/gazebo/physics/Collision.cc index 93cc24cf1a..1f955c47dd 100644 --- a/gazebo/physics/Collision.cc +++ b/gazebo/physics/Collision.cc @@ -222,7 +222,7 @@ ignition::math::Vector3d Collision::RelativeLinearVel() const ignition::math::Vector3d Collision::WorldLinearVel() const { if (this->link) - return this->link->WorldLinearVel(); + return this->link->WorldLinearVel(this->RelativePose().Pos()); else return ignition::math::Vector3d::Zero; } diff --git a/gazebo/physics/ode/ODECollision.cc b/gazebo/physics/ode/ODECollision.cc index 18a9ee4bcf..6999d9f0f9 100644 --- a/gazebo/physics/ode/ODECollision.cc +++ b/gazebo/physics/ode/ODECollision.cc @@ -73,6 +73,122 @@ void ODECollision::Load(sdf::ElementPtr _sdf) { this->GetODESurface()->maxVel = 0.0; } + + // Check validity of plowing parameters + const std::string kPlowingWheel = "gz:plowing_wheel"; + sdf::ElementPtr wheelElem = nullptr; + if (_sdf->HasElement(kPlowingWheel)) + { + wheelElem = _sdf->GetElement(kPlowingWheel); + } + + // Try to parse wheel plowing params with verbose == true so errors will + // print once during loading + ODECollisionWheelPlowingParams plowing; + ParseWheelPlowingParams(_sdf, plowing, this->GetScopedName()); +} + +////////////////////////////////////////////////// +bool ODECollision::ParseWheelPlowingParams( + sdf::ElementPtr _sdf, + ODECollisionWheelPlowingParams &_plowing, + const std::string &_scopedNameForErrorMessages) +{ + // Only print error messages of the scoped name is supplied. + bool verbose = !_scopedNameForErrorMessages.empty(); + + // Check for a plowing wheel element + const std::string kPlowingWheel = "gz:plowing_wheel"; + if (!_sdf->HasElement(kPlowingWheel)) + { + return false; + } + sdf::ElementPtr wheelElem = _sdf->GetElement(kPlowingWheel); + + // Check for required elements: max_degrees and saturation velocity + const std::string kPlowingMaxDegrees = "max_degrees"; + if (!wheelElem->HasElement(kPlowingMaxDegrees)) + { + if (verbose) + { + gzerr << "Element <" << kPlowingWheel << "> in collision with name [" + << _scopedNameForErrorMessages << "] is missing required parameter" + << " <" << kPlowingMaxDegrees << ">" << std::endl; + } + return false; + } + + const std::string kPlowingSaturationVelocity = "saturation_velocity"; + if (!wheelElem->HasElement(kPlowingSaturationVelocity)) + { + if (verbose) + { + gzerr << "Element <" << kPlowingWheel << "> in collision with name [" + << _scopedNameForErrorMessages << "] is missing required parameter" + << " <" << kPlowingSaturationVelocity << ">" << std::endl; + } + return false; + } + + // Error unless max_degrees >= 0 + double plowingMaxDegrees = wheelElem->Get(kPlowingMaxDegrees); + if (!(plowingMaxDegrees >= 0)) + { + if (verbose) + { + gzerr << "Element <" << kPlowingWheel << "> in collision with name [" + << _scopedNameForErrorMessages << "] has a parameter " + << "<" << kPlowingMaxDegrees << "> with a value of [" + << plowingMaxDegrees << "] that should be non-negative." + << std::endl; + } + return false; + } + ignition::math::Angle plowingMaxAngle; + plowingMaxAngle.SetDegree(plowingMaxDegrees); + + // Error unless saturation_velocity > 0 + double plowingSaturationVelocity = + wheelElem->Get(kPlowingSaturationVelocity); + if (!(plowingSaturationVelocity > 0)) + { + if (verbose) + { + gzerr << "Element <" << kPlowingWheel << "> in collision with name [" + << _scopedNameForErrorMessages << "] has a parameter " + << "<" << kPlowingSaturationVelocity << "> with a value of [" + << plowingSaturationVelocity << "] that should be positive." + << std::endl; + } + return false; + } + + // Check also for optional element: deadband velocity + const std::string kPlowingDeadbandVelocity = "deadband_velocity"; + // Use deadband velocity = 0 if parameter is not set + double plowingDeadbandVelocity = + wheelElem->Get(kPlowingDeadbandVelocity, 0.0).first; + + // Error unless deadband velocity < saturation velocity + if (!(plowingDeadbandVelocity < plowingSaturationVelocity)) + { + if (verbose) + { + gzerr << "Element <" << kPlowingWheel << "> in collision with name [" + << _scopedNameForErrorMessages << "] has a parameter " + << "<" << kPlowingDeadbandVelocity << "> with a value of [" + << plowingDeadbandVelocity << "] that should be less than the " + << "<" << kPlowingSaturationVelocity << "> whose value is [" + << plowingSaturationVelocity << "]." + << std::endl; + } + return false; + } + + _plowing.maxAngle = plowingMaxAngle; + _plowing.saturationVelocity = plowingSaturationVelocity; + _plowing.deadbandVelocity = plowingDeadbandVelocity; + return true; } ////////////////////////////////////////////////// diff --git a/gazebo/physics/ode/ODECollision.hh b/gazebo/physics/ode/ODECollision.hh index f9a4a21a03..9ef941f393 100644 --- a/gazebo/physics/ode/ODECollision.hh +++ b/gazebo/physics/ode/ODECollision.hh @@ -36,6 +36,21 @@ namespace gazebo /// \addtogroup gazebo_physics_ode /// \{ + /// \brief Data structure for wheel plowing parameters. + class ODECollisionWheelPlowingParams + { + /// \brief Maximum angle by which wheel contact points are rotated. + public: ignition::math::Angle maxAngle; + + /// \brief Plowing saturation velocity: the linear wheel velocity [m/s] + /// at which maximum plowing effect is reached. + public: double saturationVelocity = 0.0; + + /// \brief Plowing deadband velocity: the linear wheel velocity [m/s] + /// below which no plowing effect occurs. + public: double deadbandVelocity = 0.0; + }; + /// \brief Base class for all ODE collisions. class GZ_PHYSICS_VISIBLE ODECollision : public Collision { @@ -90,6 +105,22 @@ namespace gazebo /// \return Dynamically casted pointer to ODESurfaceParams. public: ODESurfaceParamsPtr GetODESurface() const; + /// \brief Parse wheel plowing parameters from a Collision SDF Element. + /// \param[in] _sdf Collision SDF Element to parse from. + /// \param[out] _plowing Wheel plowing parameters object to write to. + /// \param[in] _scopedNameForErrorMessages Scoped name of collision to + /// use in error messages. If empty, no error messages will be printed. + /// parsed. + /// \return True if valid wheel plowing parameters were parsed. + private: static bool ParseWheelPlowingParams( + sdf::ElementPtr _sdf, + ODECollisionWheelPlowingParams &_plowing, + const std::string &_scopedNameForErrorMessages = ""); + + /// \brief Friend with ODEPhysics to allow Collide callback to call + /// ParseWheelPlowingParams. + private: friend class ODEPhysics; + /// \brief Used when this is static to set the posse. private: void OnPoseChangeGlobal(); diff --git a/gazebo/physics/ode/ODEPhysics.cc b/gazebo/physics/ode/ODEPhysics.cc index 5ab43e15c5..7067861457 100644 --- a/gazebo/physics/ode/ODEPhysics.cc +++ b/gazebo/physics/ode/ODEPhysics.cc @@ -77,6 +77,9 @@ #include "gazebo/physics/ode/ODEPhysicsPrivate.hh" +double saturation_deadband(double, double, double, double); + + using namespace gazebo; using namespace physics; @@ -1161,6 +1164,94 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, contact.fdir1[0] = fd.X(); contact.fdir1[1] = fd.Y(); contact.fdir1[2] = fd.Z(); + + // Check for plowing parameters if friction direction is specified + auto collision1Sdf = _collision1->GetSDF(); + auto collision2Sdf = _collision2->GetSDF(); + + const std::string kPlowingTerrain = "gz:plowing_terrain"; + ODECollisionWheelPlowingParams wheelPlowing; + + ODECollision *wheelCollision = nullptr; + sdf::ElementPtr terrainSdf = nullptr; + + if (ODECollision::ParseWheelPlowingParams(collision1Sdf, wheelPlowing) && + collision2Sdf->HasElement(kPlowingTerrain)) + { + wheelCollision = _collision1; + terrainSdf = collision2Sdf; + } + else if (collision1Sdf->HasElement(kPlowingTerrain) && + ODECollision::ParseWheelPlowingParams(collision2Sdf, wheelPlowing)) + { + wheelCollision = _collision2; + terrainSdf = collision1Sdf; + } + + if (wheelCollision && terrainSdf) + { + // compute slope + double slope = wheelPlowing.maxAngle.Radian() / + (wheelPlowing.saturationVelocity - wheelPlowing.deadbandVelocity); + + // Assume origin of collision frame is wheel center + // Compute position and linear velocity of wheel center + // (all vectors in world coordinates unless otherwise specified) + ignition::math::Vector3d wheelPosition = + wheelCollision->WorldPose().Pos(); + ignition::math::Vector3d wheelLinearVelocity = + wheelCollision->WorldLinearVel(); + + ignition::math::Vector3d fdir1 = fd.Normalized(); + ignition::math::Vector3d contactNormalCopy, contactPositionCopy; + // for each pair of contact point and normal + for (unsigned int c = 0; c < numc; ++c) + { + // Copy the contact normal + dReal *contactNormal = + _contactCollisions[this->dataPtr->indices[c]].normal; + contactNormalCopy.Set( + contactNormal[0], contactNormal[1], contactNormal[2]); + + // Compute longitudinal unit vector as normal cross fdir1 + ignition::math::Vector3d unitLongitudinal = + contactNormalCopy.Cross(fdir1); + + // Compute longitudinal speed (dot product) + double wheelSpeedLongitudinal = + wheelLinearVelocity.Dot(unitLongitudinal); + + // Compute plowing angle as function of longitudinal speed + double plowingAngle = + saturation_deadband(wheelPlowing.maxAngle.Radian(), + wheelPlowing.deadbandVelocity, + slope, + wheelSpeedLongitudinal); + + // Construct axis-angle quaternion using fdir1 and plowing angle + ignition::math::Quaterniond plowingRotation(fdir1, plowingAngle); + + // Rotate normal unit vector by plowingRotation and set normal vector + contactNormalCopy = plowingRotation * contactNormalCopy; + contactNormal[0] = contactNormalCopy[0]; + contactNormal[1] = contactNormalCopy[1]; + contactNormal[2] = contactNormalCopy[2]; + + // Construct displacement vector from wheel center to contact point + dReal *contactPosition = + _contactCollisions[this->dataPtr->indices[c]].pos; + contactPositionCopy.Set(contactPosition[0] - wheelPosition[0], + contactPosition[1] - wheelPosition[1], + contactPosition[2] - wheelPosition[2]); + + // Rotate displacement vector by plowingRotation and set contact point + contactPositionCopy = wheelPosition + + plowingRotation * contactPositionCopy; + contactPosition[0] = contactPositionCopy[0]; + contactPosition[1] = contactPositionCopy[1]; + contactPosition[2] = contactPositionCopy[2]; + } + } } // Set the friction coefficients. @@ -1692,3 +1783,41 @@ bool ODEPhysics::GetParam(const std::string &_key, boost::any &_value) const } return true; } + +/// \brief A mathematical function that represents a saturation function +/// that increases linearly until reaching a constant value with the addition +/// of a deadband near zero. The shape of this function for positive input +/// values is illustrated below. +/// \param[in] _maxOutput the maximum output of the function, which is at +/// saturation. +/// \param[in] _deadband the size of the deadband. Any inputs with a smaller +/// absolute value than the deadband produce zero output. +/// \param[in] _slope the slope of the linear portion of the output. +/// \param[in] _input the input to the function. +/// \return the output computed using saturation and deadband. +/** \verbatim + | . + | _________ saturation value . + | / . + | /| . + | /-┘ slope . + | / . + | / . + | / . + --+-------------------------- input + |<----> dead-band + + \endverbatim */ +double saturation_deadband( + double _maxOutput, double _deadband, double _slope, double _input) +{ + if (std::abs(_input) <= std::abs(_deadband)) + { + return 0.0; + } + + double result = _input - std::abs(_deadband) * ignition::math::sgn(_input); + result *= _slope; + + return ignition::math::clamp(result, -_maxOutput, _maxOutput); +} diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 3881c106fe..4efe38d5f7 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -88,6 +88,7 @@ set(tests physics_thread_safe.cc physics_torsional_friction.cc pioneer2dx.cc + plowing_effect.cc plugin.cc plugin_interface.cc rayshape.cc diff --git a/test/integration/plowing_effect.cc b/test/integration/plowing_effect.cc new file mode 100644 index 0000000000..dcc15fd4e5 --- /dev/null +++ b/test/integration/plowing_effect.cc @@ -0,0 +1,385 @@ +/* + * Copyright (C) 2022 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 +#include "gazebo/test/ServerFixture.hh" +#include "gazebo/physics/physics.hh" +#include "gazebo/sensors/sensors.hh" +#include "gazebo/common/common.hh" +#include "scans_cmp.h" + +using namespace gazebo; + +class PlowingEffectTricycle : public ServerFixture +{ + /// \brief unit test for RigidTerrain + public: void RigidTerrain(const std::string &_physicsEngine); + + /// \brief unit test for DeformableTerrain + public: void DeformableTerrain(const std::string &_physicsEngine); + + /// \brief CallbackRigidTerrain && CallbackDeformableTerrain for + /// subscribing to /gazebo/default/physics/contacts topic + private: void CallbackRigidTerrain(const ConstContactsPtr &_msg); + private: void CallbackDeformableTerrain(const ConstContactsPtr &_msg); + + private: physics::CollisionPtr wheelCollisionPtr_ = nullptr; + private: double plowingAngleRigidTerrain_; + + private: const int maxCallbackCountRigidTerrain_ = 200; + private: bool havePrintedNoPlowingEffect_ = false; + private: bool havePrintedLinearPlowingEffect_ = false; + private: bool havePrintedMaxPlowingEffect_ = false; + + /// constant params from SDF + private: double maxRadians_; + private: double deadbandVelocity_; + private: double saturationVelocity_; + private: ignition::math::Vector3 fdir1_; + +}; + +/** plowing effect on four spheres of same size and inertia + * Plowing Effect Sequence: + * modelSphere4 > modelSphere3 > modelSphere2 > modelSphere1 + * where modelSphere1 has zero plowing effect + */ +class PlowingEffectSpheres : public ServerFixture +{ + /// \brief test for max plowing angle at saturation velocity + public: void MaxPlowingAngle(const std::string &_physicsEngine); + private: void CallbackMaxPlowingAngle(const ConstContactsPtr &_msg); + + private: physics::CollisionPtr sphere1CollisionPtr_ = nullptr; + private: physics::CollisionPtr sphere2CollisionPtr_ = nullptr; + private: physics::CollisionPtr sphere3CollisionPtr_ = nullptr; + private: physics::CollisionPtr sphere4CollisionPtr_ = nullptr; + + private: int callbackCount_ = 1; + private: double sphere1PlowingAngle_; + private: double sphere2PlowingAngle_; + private: double sphere3PlowingAngle_; + private: double sphere4PlowingAngle_; +}; + +//////////////////////////////////////////////////////////////////////// +void PlowingEffectTricycle::CallbackRigidTerrain(const ConstContactsPtr &_msg) +{ + std::string wheelCollisionStr = "original_tricycle::wheel_front::collision"; + std::string groundCollisionStr = "plowing_effect_ground_plane::link::collision"; + + // no shift in contact point on rigid surface + for(auto idx = 0; idx < _msg->contact_size(); ++idx) + { + const gazebo::msgs::Contact& contact = _msg->contact(idx); + if(contact.collision1() == wheelCollisionStr && + contact.collision2() == groundCollisionStr) + { + const ignition::math::Vector3& contactPointNormal = + ConvertIgn( contact.normal(0)); + const ignition::math::Vector3& unitNormal = + ignition::math::Vector3::UnitZ; + plowingAngleRigidTerrain_ = acos(contactPointNormal.Dot(unitNormal)/ + contactPointNormal.Length() * unitNormal.Length()); + } + } +} + +void PlowingEffectTricycle::CallbackDeformableTerrain(const ConstContactsPtr &_msg) +{ + std::string wheelCollisionStr = "my_tricycle::wheel_rear_left::collision"; + std::string groundCollisionStr = "plowing_effect_ground_plane::link::collision"; + + bool contactFound = false; + for(auto idx = 0; idx < _msg->contact_size(); ++idx) + { + const gazebo::msgs::Contact& contact = _msg->contact(idx); + if(contact.collision1() == wheelCollisionStr && + contact.collision2() == groundCollisionStr) + { + contactFound = true; + const auto& contactPointNormal = ConvertIgn( contact.normal(0)); + const auto& unitNormal = ignition::math::Vector3::UnitZ; + + // compute longitudinal unit vector as normal cross fdir1_ + const auto& wheelLinearVelocity = wheelCollisionPtr_->WorldLinearVel(); + const auto& unitLongitudinal = fdir1_.Cross(unitNormal); + double wheelSpeedLongitudinal = wheelLinearVelocity.Dot(unitLongitudinal); + + // compute the angle contact point makes with unit normal + double angle = acos(contactPointNormal.Dot(unitNormal) / + contactPointNormal.Length()); + + // check that longitudinal velocity and contact normal point in + // opposite directions + EXPECT_LE(unitLongitudinal.Dot(contactPointNormal), 0); + + // no plowing effect + if(wheelSpeedLongitudinal <= deadbandVelocity_) + { + if (!havePrintedNoPlowingEffect_) + { + havePrintedNoPlowingEffect_ = true; + gzdbg << "No plowing effect" << "\n"; + } + EXPECT_EQ(angle, 0); + } + + // linear plowing effect + else if(wheelSpeedLongitudinal > deadbandVelocity_ && + wheelSpeedLongitudinal < saturationVelocity_) + { + if (!havePrintedLinearPlowingEffect_) + { + havePrintedLinearPlowingEffect_ = true; + gzdbg << "Reached linear plowing effect" << "\n"; + } + ASSERT_LT(angle, maxRadians_); + ASSERT_GE(angle, 0); + } + + // maximum plowing effect. + else if(wheelSpeedLongitudinal >= saturationVelocity_) + { + if (!havePrintedMaxPlowingEffect_) + { + havePrintedMaxPlowingEffect_ = true; + gzdbg << "Reached maximum plowing effect" << "\n"; + } + EXPECT_NEAR(angle, maxRadians_, 1e-6); + } + } + } + EXPECT_TRUE(contactFound); +} + +void PlowingEffectSpheres::CallbackMaxPlowingAngle(const ConstContactsPtr &_msg) +{ + if(callbackCount_ < 2300) + { + callbackCount_ = callbackCount_ + 1; + return; + } + + std::string sphere1CollisionStr = "sphere1::base_link::collision"; + std::string sphere2CollisionStr = "sphere2::base_link::collision"; + std::string sphere3CollisionStr = "sphere3::base_link::collision"; + std::string sphere4CollisionStr = "sphere4::base_link::collision"; + std::string groundCollisionStr = "plowing_effect_ground_plane::link::collision"; + + for(auto idx = 0; idx < _msg->contact_size(); ++idx) + { + const gazebo::msgs::Contact& contact = _msg->contact(idx); + if(contact.collision2() == groundCollisionStr) + { + const auto& contactPointNormal = ConvertIgn( contact.normal(0)); + const auto& unitNormal = ignition::math::Vector3::UnitZ; + + double angle = acos(contactPointNormal.Dot(unitNormal)/ + contactPointNormal.Length() * unitNormal.Length()); + + if(contact.collision1() == sphere1CollisionStr) + { + sphere1PlowingAngle_ = angle; + } + else if(contact.collision1() == sphere2CollisionStr) + { + sphere2PlowingAngle_ = angle; + } + else if(contact.collision1() == sphere3CollisionStr) + { + sphere3PlowingAngle_ = angle; + } + else if(contact.collision1() == sphere4CollisionStr) + { + sphere4PlowingAngle_ = angle; + } + } + } +} + +void PlowingEffectTricycle::RigidTerrain(const std::string &_physicsEngine) +{ + gazebo::common::SystemPaths::Instance()->AddModelPaths( + PROJECT_SOURCE_PATH "/test/models"); + // Load the plowing effect world + Load("worlds/plowing_effect_tricycle_demo.world", true, _physicsEngine); + physics::WorldPtr world = physics::get_world("default"); + ASSERT_TRUE(world != NULL); + + const std::string& contactsTopic = "/gazebo/default/physics/contacts"; + std::list topics = + transport::getAdvertisedTopics("gazebo.msgs.Contacts"); + ASSERT_TRUE(topics.size() == 1); + ASSERT_EQ(*topics.begin(), "/gazebo/default/physics/contacts"); + + transport::SubscriberPtr sub = this->node->Subscribe(contactsTopic, + &PlowingEffectTricycle::CallbackRigidTerrain, this); + world->Step(this->maxCallbackCountRigidTerrain_); + ASSERT_EQ(this->plowingAngleRigidTerrain_, 0); +} + +void PlowingEffectTricycle::DeformableTerrain(const std::string &_physicsEngine) +{ + gazebo::common::SystemPaths::Instance()->AddModelPaths( + PROJECT_SOURCE_PATH "/test/models"); + // Load the plowing effect world + Load("worlds/plowing_effect_tricycle_demo.world", true, _physicsEngine); + physics::WorldPtr world = physics::get_world("default"); + ASSERT_TRUE(world != NULL); + + const std::string& contactsTopic = "/gazebo/default/physics/contacts"; + std::list topics = + transport::getAdvertisedTopics("gazebo.msgs.Contacts"); + ASSERT_TRUE(topics.size() == 1); + ASSERT_EQ(*topics.begin(), "/gazebo/default/physics/contacts"); + + physics::ModelPtr model = world->ModelByName("my_tricycle"); + ASSERT_TRUE(model != nullptr); + + physics::LinkPtr wheelLinkPtr = model->GetLink("wheel_front"); + ASSERT_TRUE(wheelLinkPtr != nullptr); + + this->wheelCollisionPtr_ = wheelLinkPtr->GetCollision("collision"); + ASSERT_TRUE(this->wheelCollisionPtr_ != nullptr); + + auto plowingParams = this->wheelCollisionPtr_->GetSDF()-> + GetElement("gz:plowing_wheel"); + ASSERT_TRUE(plowingParams != nullptr); + + ignition::math::Angle maxAngle; + auto maxDegree = plowingParams->Get("max_degrees"); + maxAngle.SetDegree(maxDegree); + + this->maxRadians_ = maxAngle.Radian(); + this->deadbandVelocity_ = plowingParams->Get("deadband_velocity"); + this->saturationVelocity_ = plowingParams->Get("saturation_velocity"); + + ASSERT_EQ(maxDegree, 5); + ASSERT_EQ(this->deadbandVelocity_, 0.3); + ASSERT_EQ(this->saturationVelocity_, 0.4); + + this->fdir1_ = wheelCollisionPtr_->GetSDF()->GetElement("surface")-> + GetElement("friction")->GetElement("ode")-> + Get>("fdir1"); + ASSERT_EQ(this->fdir1_, ignition::math::Vector3(0, 1, 0)); + + transport::SubscriberPtr sub = this->node->Subscribe(contactsTopic, + &PlowingEffectTricycle::CallbackDeformableTerrain, this); + world->Step(500); +} + +void PlowingEffectSpheres::MaxPlowingAngle(const std::string &_physicsEngine) +{ + // Load the plowing effect world + Load("worlds/plowing_effect_spheres_test.world", true, _physicsEngine); + physics::WorldPtr world = physics::get_world("default"); + ASSERT_TRUE(world != NULL); + + const std::string& contactsTopic = "/gazebo/default/physics/contacts"; + std::list topics = + transport::getAdvertisedTopics("gazebo.msgs.Contacts"); + ASSERT_TRUE(topics.size() == 1); + ASSERT_EQ(*topics.begin(), "/gazebo/default/physics/contacts"); + + physics::ModelPtr modelSphere1 = world->ModelByName("sphere1"); + physics::ModelPtr modelSphere2 = world->ModelByName("sphere2"); + physics::ModelPtr modelSphere3 = world->ModelByName("sphere3"); + physics::ModelPtr modelSphere4 = world->ModelByName("sphere4"); + ASSERT_TRUE(modelSphere1 != nullptr); + ASSERT_TRUE(modelSphere2 != nullptr); + ASSERT_TRUE(modelSphere3 != nullptr); + ASSERT_TRUE(modelSphere4 != nullptr); + + physics::LinkPtr sphere1LinkPtr = modelSphere1->GetLink("base_link"); + physics::LinkPtr sphere2LinkPtr = modelSphere2->GetLink("base_link"); + physics::LinkPtr sphere3LinkPtr = modelSphere3->GetLink("base_link"); + physics::LinkPtr sphere4LinkPtr = modelSphere4->GetLink("base_link"); + ASSERT_TRUE(sphere1LinkPtr != nullptr); + ASSERT_TRUE(sphere2LinkPtr != nullptr); + ASSERT_TRUE(sphere3LinkPtr != nullptr); + ASSERT_TRUE(sphere4LinkPtr != nullptr); + + this->sphere1CollisionPtr_ = sphere1LinkPtr->GetCollision("collision"); + this->sphere2CollisionPtr_ = sphere2LinkPtr->GetCollision("collision"); + this->sphere3CollisionPtr_ = sphere3LinkPtr->GetCollision("collision"); + this->sphere4CollisionPtr_ = sphere4LinkPtr->GetCollision("collision"); + ASSERT_TRUE(sphere1CollisionPtr_ != nullptr); + ASSERT_TRUE(sphere2CollisionPtr_ != nullptr); + ASSERT_TRUE(sphere3CollisionPtr_ != nullptr); + ASSERT_TRUE(sphere4CollisionPtr_ != nullptr); + + auto sphere2PlowingParams = sphere2CollisionPtr_->GetSDF()-> + GetElement("gz:plowing_wheel"); + auto sphere3PlowingParams = sphere3CollisionPtr_->GetSDF()-> + GetElement("gz:plowing_wheel"); + auto sphere4PlowingParams = sphere4CollisionPtr_->GetSDF()-> + GetElement("gz:plowing_wheel"); + ASSERT_TRUE(this->sphere2CollisionPtr_ != nullptr); + ASSERT_TRUE(this->sphere3CollisionPtr_ != nullptr); + ASSERT_TRUE(this->sphere4CollisionPtr_ != nullptr); + + // zero plowing effect for sphere1 + auto sphere2MaxDegree = sphere2PlowingParams->Get("max_degrees"); + auto sphere3MaxDegree = sphere3PlowingParams->Get("max_degrees"); + auto sphere4MaxDegree = sphere4PlowingParams->Get("max_degrees"); + + ignition::math::Angle sphere2MaxAngle; + ignition::math::Angle sphere3MaxAngle; + ignition::math::Angle sphere4MaxAngle; + + sphere2MaxAngle.SetDegree(sphere2MaxDegree); + sphere3MaxAngle.SetDegree(sphere3MaxDegree); + sphere4MaxAngle.SetDegree(sphere4MaxDegree); + + auto sphere2MaxRadians = sphere2MaxAngle.Radian(); + auto sphere3MaxRadians = sphere3MaxAngle.Radian(); + auto sphere4MaxRadians = sphere4MaxAngle.Radian(); + + transport::SubscriberPtr sub = this->node->Subscribe(contactsTopic, + &PlowingEffectSpheres::CallbackMaxPlowingAngle, this); + + world->Step(2500); + + double error = 1e-14; + // verify spheres have reached max plowing angle + EXPECT_EQ(this->sphere1PlowingAngle_, 0); // no plowing effect + EXPECT_NEAR(this->sphere2PlowingAngle_, sphere2MaxRadians, error); + EXPECT_NEAR(this->sphere3PlowingAngle_, sphere3MaxRadians, error); + EXPECT_NEAR(this->sphere4PlowingAngle_, sphere4MaxRadians, error); +} + +TEST_F(PlowingEffectTricycle, RigidTerrain) +{ + RigidTerrain("ode"); +} + +TEST_F(PlowingEffectTricycle, DeformableTerrain) +{ + DeformableTerrain("ode"); +} + +TEST_F(PlowingEffectSpheres, MaxPlowingAngle) +{ + MaxPlowingAngle("ode"); +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/test/models/plowing_effect_ground_plane/model.config b/test/models/plowing_effect_ground_plane/model.config new file mode 100644 index 0000000000..f08fabc87a --- /dev/null +++ b/test/models/plowing_effect_ground_plane/model.config @@ -0,0 +1,21 @@ + + + + Ground Plane with plowing effect + 1.0 + model.sdf + + + Nate Koenig + nate@osrfoundation.org + + + + Aditya Pande + aditya.pande@openrobotics.org + + + + A simple ground plane to demonstrate the plowing effect. + + diff --git a/test/models/plowing_effect_ground_plane/model.sdf b/test/models/plowing_effect_ground_plane/model.sdf new file mode 100644 index 0000000000..b9a37b2b12 --- /dev/null +++ b/test/models/plowing_effect_ground_plane/model.sdf @@ -0,0 +1,43 @@ + + + + true + + + + + + 0 0 1 + 100 100 + + + + + 0xffff + + + + 100 + 50 + + + + + + false + + + 0 0 1 + 100 100 + + + + + + + + + diff --git a/test/models/plowing_effect_trisphere_cycle/model.config b/test/models/plowing_effect_trisphere_cycle/model.config new file mode 100644 index 0000000000..80c3e60a81 --- /dev/null +++ b/test/models/plowing_effect_trisphere_cycle/model.config @@ -0,0 +1,26 @@ + + + + Tricycle with spherical wheels and plowing effect + 1.0 + model.sdf + + + Steve Peters + scpeters@osrfoundation.org + + + + Aditya Pande + aditya.pande@openrobotics.org + + + + A tricycle with spherical wheels and front-wheel steering. + The first friction direction for each wheel is parallel to the joint axis + for each wheel spin joint, which corresponds to the wheel's lateral + direction. + The plowing parameters are specified in the //collision/gz:plowing_wheel + element for each wheel's sperical collision element. + + diff --git a/test/models/plowing_effect_trisphere_cycle/model.sdf b/test/models/plowing_effect_trisphere_cycle/model.sdf new file mode 100644 index 0000000000..6978851be5 --- /dev/null +++ b/test/models/plowing_effect_trisphere_cycle/model.sdf @@ -0,0 +1,429 @@ + + + + + -0.40855911616047164 0 0.38502293110800634 0 -0.522020852957719 0 + + 0.0 0 0 0 0 0 + 10 + + 0.22799999999999998 + 0.7435210984814149 + 0.9655210984814149 + 0 + 0 + 0 + + + + -0.4713346258704366 0 0 1.5707963267948966 0 0 + + + 1.0392304845413263 + 0.03 + + + + + -0.4713346258704366 0 0 1.5707963267948966 0 0 + + + 1.0392304845413263 + 0.03 + + + + + + + + 0 0.17155177419583564 0 0 1.5707963267948966 -0.3490658503988659 + + + 1.0031676644991372 + 0.03 + + + + + 0 0.17155177419583564 0 0 1.5707963267948966 -0.3490658503988659 + + + 1.0031676644991372 + 0.03 + + + + + + + + 0 -0.17155177419583564 0 0 1.5707963267948966 0.3490658503988659 + + + 1.0031676644991372 + 0.03 + + + + + 0 -0.17155177419583564 0 0 1.5707963267948966 0.3490658503988659 + + + 1.0031676644991372 + 0.03 + + + + + + + + + 0.04144088383952833 0 0.38502293110800634 0 -0.17453292519943295 0 + + 3 + + 0.15820312499999997 + 0.058359374999999984 + 0.10265624999999999 + 0 + 0 + 0 + + + + 0 0 0.397747564417433 1.5707963267948966 0 0 + + + 0.6363961030678927 + 0.0375 + + + + + 0 0 0.397747564417433 1.5707963267948966 0 0 + + + 0.6363961030678927 + 0.0375 + + + + + + + + 0 0 0.2386485386504598 0 0 0 + + + 0.31819805153394637 + 0.0375 + + + + + 0 0 0.2386485386504598 0 0 0 + + + 0.31819805153394637 + 0.0375 + + + + + + + + 0 0 -0.23864853865045976 1.5707963267948966 0 0 + + + 0.6363961030678927 + 0.015 + + + + + 0 0 -0.23864853865045976 1.5707963267948966 0 0 + + + 0.6363961030678927 + 0.015 + + + + + + + + 0 0.15909902576697318 -0.07954951288348658 0.7853981633974483 0 0 + + + 0.45 + 0.0375 + + + + + 0 0.15909902576697318 -0.07954951288348658 0.7853981633974483 0 0 + + + 0.45 + 0.0375 + + + + + + + + 0 -0.15909902576697318 -0.07954951288348658 -0.7853981633974483 0 0 + + + 0.45 + 0.0375 + + + + + 0 -0.15909902576697318 -0.07954951288348658 -0.7853981633974483 0 0 + + + 0.45 + 0.0375 + + + + + + + + + 0.08288176767905665 0 0.15 0 0 0 + + 0.5 + + 0.0045 + 0.0045 + 0.0045 + 0 + 0 + 0 + + + + + 5 + 0.3 + 0.4 + + + + 0.15 + + + + + + 4000 + 16 + 10 + 0.0005 + + + + + 0.7 + 0.7 + 0 1 0 + + + + + + + + 0.15 + + + + + + + + + frame + fork + + 0 0 1 + + 1.0 + + + -0.9599310885968813 + 0.9599310885968813 + + + + + fork + wheel_front + + 0 1 0 + + + + -0.8171182323209433 0.5196152422706631 0.15 0 0 0 + + 0.5 + + 0.0045 + 0.0045 + 0.0045 + 0 + 0 + 0 + + + + + 5 + 0.3 + 0.4 + + + + 0.15 + + + + + + 4000 + 16 + 10 + 0.0005 + + + + + 0.7 + 0.7 + 0 1 0 + + + + + + + + 0.15 + + + + + + + + + frame + wheel_rear_left + + 0 1 0 + + + + -0.8171182323209433 -0.5196152422706631 0.15 0 0 0 + + 0.5 + + 0.0045 + 0.0045 + 0.0045 + 0 + 0 + 0 + + + + + 5 + 0.3 + 0.4 + + + + 0.15 + + + + + + 4000 + 16 + 10 + 0.0005 + + + + + 0.7 + 0.7 + 0 1 0 + + + + + + + + 0.15 + + + + + + + + + frame + wheel_rear_right + + 0 1 0 + + + + diff --git a/test/worlds/plowing_effect_spheres_test.world b/test/worlds/plowing_effect_spheres_test.world new file mode 100644 index 0000000000..a6e16f2231 --- /dev/null +++ b/test/worlds/plowing_effect_spheres_test.world @@ -0,0 +1,239 @@ + + + + + + 1.5 0 -9.75 + + + model://plowing_effect_ground_plane + + + model://sun + + + + 0 0 0.4 0 0 0 + + + 5.0 + + 0.5333 + 0.5333 + 0.5333 + 0 + 0 + 0 + + + + + + 0.4 + + + + + + + 0.4 + + + + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 1 0.4 0 0 0 + + + 5.0 + + 0.5333 + 0.5333 + 0.5333 + 0 + 0 + 0 + + + + + 3.0 + 0.2 + 0.6 + + + + 0.4 + + + + + + 40000 + 16 + 10 + 0.005 + + + + + 0 1 0 + + + + + + + + 0.4 + + + + + + + + + + + 0 2 0.4 0 0 0 + + + 5.0 + + 0.5333 + 0.5333 + 0.5333 + 0 + 0 + 0 + + + + + 6.0 + 0.2 + 0.6 + + + + 0.4 + + + + + + 40000 + 16 + 10 + 0.005 + + + + + 0 1 0 + + + + + + + + 0.4 + + + + + + + + + + + 0 3 0.4 0 0 0 + + + 5.0 + + 0.5333 + 0.5333 + 0.5333 + 0 + 0 + 0 + + + + + 8.0 + 0.2 + 0.6 + + + + 0.4 + + + + + + 40000 + 16 + 10 + 0.005 + + + + + 0 1 0 + + + + + + + + 0.4 + + + + + + + + + + \ No newline at end of file diff --git a/test/worlds/plowing_effect_tricycle_demo.world b/test/worlds/plowing_effect_tricycle_demo.world new file mode 100644 index 0000000000..a62f5780cf --- /dev/null +++ b/test/worlds/plowing_effect_tricycle_demo.world @@ -0,0 +1,166 @@ + + + + + 0 0 -9.75 + + model://plowing_effect_ground_plane + + + model://sun + + + + model://plowing_effect_trisphere_cycle + my_tricycle + 0 0 0 0 0 0 + + + + 3.0 + 3.0 + 32 + + + 3.0 + 3.0 + 32 + + + 3.0 + 3.0 + 32 + + + + + + wheel_front_steer + 0 + 9 0 0.1 + + + wheel_rear_left_spin + wheel_rear_right_spin + wheel_front_spin + 6.0 + 9 0 0 + + + wheel_rear_left_spin + wheel_rear_right_spin + 2.15 + + + + + + + + + model://trisphere_cycle + original_tricycle + 0 2 0 0 0 0 + + + + 3.0 + 3.0 + 32 + + + 3.0 + 3.0 + 32 + + + 3.0 + 3.0 + 32 + + + + + + wheel_front_steer + 0 + 9 0 0.1 + + + wheel_rear_left_spin + wheel_rear_right_spin + wheel_front_spin + 6.0 + 9 0 0 + + + wheel_rear_left_spin + wheel_rear_right_spin + 2.15 + + + + + + + + + + 1.5 -4 2.5 0 0.5 1.6 + orbit + + + + +