Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding HaltMotion (Or new suggested name) to physics plugin #728

Merged
merged 9 commits into from
May 18, 2021
Merged
Changes from 5 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
40 changes: 40 additions & 0 deletions include/ignition/gazebo/components/HaltMotion.hh
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
/*
* Copyright (C) 2019 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.
*
*/
#ifndef IGNITION_GAZEBO_COMPONENTS_HALT_MOTION_HH_
#define IGNITION_GAZEBO_COMPONENTS_HALT_MOTION_HH_

#include <ignition/gazebo/components/Factory.hh>
#include <ignition/gazebo/components/Component.hh>
#include <ignition/gazebo/config.hh>

namespace ignition
{
namespace gazebo
{
// Inline bracket to help doxygen filtering.
inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
namespace components
{
/// \brief A component used to turn off a model's joint's movement.
using HaltMotion = Component<bool, class HaltMotionTag>;
IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.HaltMotion",
HaltMotion)
}
}
}
}
#endif
16 changes: 11 additions & 5 deletions src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc
Original file line number Diff line number Diff line change
@@ -31,6 +31,7 @@
#include <ignition/gazebo/Link.hh>
#include <ignition/gazebo/Model.hh>
#include <ignition/gazebo/Util.hh>
#include <ignition/common/Profiler.hh>

#include <ignition/plugin/Register.hh>

@@ -164,9 +165,14 @@ void KineticEnergyMonitor::Configure(const Entity &_entity,
}

//////////////////////////////////////////////////
void KineticEnergyMonitor::PostUpdate(const UpdateInfo &/*_info*/,
void KineticEnergyMonitor::PostUpdate(const UpdateInfo &_info,
const EntityComponentManager &_ecm)
{
IGN_PROFILE("KineticEnergyMonitor::PostUpdate");
// Nothing left to do if paused or the publisher wasn't created.
if (_info.paused || !this->dataPtr->pub)
return;

if (this->dataPtr->linkEntity != kNullEntity)
{
Link link(this->dataPtr->linkEntity);
@@ -191,10 +197,10 @@ void KineticEnergyMonitor::PostUpdate(const UpdateInfo &/*_info*/,
}
}

IGNITION_ADD_PLUGIN(KineticEnergyMonitor, System,
KineticEnergyMonitor::ISystemConfigure,
KineticEnergyMonitor::ISystemPostUpdate
)
IGNITION_ADD_PLUGIN(KineticEnergyMonitor,
ignition::gazebo::System,
KineticEnergyMonitor::ISystemConfigure,
KineticEnergyMonitor::ISystemPostUpdate)

IGNITION_ADD_PLUGIN_ALIAS(KineticEnergyMonitor,
"ignition::gazebo::systems::KineticEnergyMonitor")
4 changes: 2 additions & 2 deletions src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh
Original file line number Diff line number Diff line change
@@ -97,8 +97,8 @@ namespace systems
</plugin>
</model>
\endverbatim */
class IGNITION_GAZEBO_VISIBLE KineticEnergyMonitor:
public System,
class IGNITION_GAZEBO_VISIBLE KineticEnergyMonitor
: public System,
nkoenig marked this conversation as resolved.
Show resolved Hide resolved
public ISystemConfigure,
public ISystemPostUpdate
{
12 changes: 11 additions & 1 deletion src/systems/physics/Physics.cc
Original file line number Diff line number Diff line change
@@ -118,6 +118,7 @@
#include "ignition/gazebo/components/Static.hh"
#include "ignition/gazebo/components/ThreadPitch.hh"
#include "ignition/gazebo/components/World.hh"
#include "ignition/gazebo/components/HaltMotion.hh"

#include "EntityFeatureMap.hh"

@@ -1232,8 +1233,17 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
if (nullptr == jointPhys)
return true;

auto haltMotionComp = _ecm.Component<components::HaltMotion>(
_ecm.ParentEntity(_entity));
bool haltMotion = false;
if (haltMotionComp)
{
haltMotion = haltMotionComp->Data();
}


// Model is out of battery
if (this->entityOffMap[_ecm.ParentEntity(_entity)])
if (this->entityOffMap[_ecm.ParentEntity(_entity)] || haltMotion)
{
std::size_t nDofs = jointPhys->GetDegreesOfFreedom();
for (std::size_t i = 0; i < nDofs; ++i)