[Citadel] [TPE] Add velocity cmd

include/ignition/gazebo/components/AngularVelocityCmd.hh

@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2020 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_ANGULARVELOCITYCMD_HH_
+#define IGNITION_GAZEBO_COMPONENTS_ANGULARVELOCITYCMD_HH_
+
+#include <ignition/math/Vector3.hh>
+
+#include <ignition/gazebo/config.hh>
+
+#include <ignition/gazebo/components/Factory.hh>
+#include "ignition/gazebo/components/Component.hh"
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace components
+{
+ /// \brief A component type that contains angular velocity cmd of an entity
+ /// represented by ignition::math::Vector3d.
+ using AngularVelocityCmd =
+ Component<math::Vector3d, class AngularVelocityCmdTag>;
+ IGN_GAZEBO_REGISTER_COMPONENT(
+ "ign_gazebo_components.AngularVelocityCmd", AngularVelocityCmd)
+
+ /// \brief A component type that contains angular velocity cmd
+ /// of an entity in the world frame represented by ignition::math::Vector3d.
+ using WorldAngularVelocityCmd =
+ Component<math::Vector3d, class WorldAngularVelocityCmdTag>;
+ IGN_GAZEBO_REGISTER_COMPONENT(
+ "ign_gazebo_components.WorldAngularVelocityCmd", WorldAngularVelocityCmd)
+}
+}
+}
+}
+
+#endif

include/ignition/gazebo/components/LinearVelocityCmd.hh

@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2020 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_LINEARVELOCITYCMD_HH_
+#define IGNITION_GAZEBO_COMPONENTS_LINEARVELOCITYCMD_HH_
+
+#include <ignition/math/Vector3.hh>
+
+#include <ignition/gazebo/config.hh>
+
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Component.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace components
+{
+ /// \brief A component type that contains linear velocity of an entity
+ /// represented by ignition::math::Vector3d.
+ using LinearVelocityCmd = Component<
+ math::Vector3d, class LinearVelocityCmdTag>;
+ IGN_GAZEBO_REGISTER_COMPONENT(
+ "ign_gazebo_components.LinearVelocityCmd", LinearVelocityCmd)
+
+ /// \brief A component type that contains linear velocity of an entity in the
+ /// world frame represented by ignition::math::Vector3d.
+ using WorldLinearVelocityCmd =
+ Component<math::Vector3d, class WorldLinearVelocityCmdTag>;
+ IGN_GAZEBO_REGISTER_COMPONENT(
+ "ign_gazebo_components.WorldLinearVelocityCmd", WorldLinearVelocityCmd)
+}
+}
+}
+}
+
+#endif

src/systems/CMakeLists.txt

@@ -97,4 +97,5 @@ add_subdirectory(thermal)
 add_subdirectory(touch_plugin)
 add_subdirectory(triggered_publisher)
 add_subdirectory(user_commands)
+add_subdirectory(velocity_control)
 add_subdirectory(wind_effects)

src/systems/physics/Physics.cc

@@ -29,6 +29,7 @@
 #include <ignition/common/SystemPaths.hh>
 #include <ignition/math/AxisAlignedBox.hh>
 #include <ignition/math/eigen3/Conversions.hh>
+#include <ignition/math/Vector3.hh>
 #include <ignition/physics/config.hh>
 #include <ignition/physics/FeatureList.hh>
 #include <ignition/physics/FeaturePolicy.hh>
@@ -75,6 +76,7 @@
 // Components
 #include "ignition/gazebo/components/AngularAcceleration.hh"
 #include "ignition/gazebo/components/AngularVelocity.hh"
+#include "ignition/gazebo/components/AngularVelocityCmd.hh"
 #include "ignition/gazebo/components/AxisAlignedBox.hh"
 #include "ignition/gazebo/components/BatterySoC.hh"
 #include "ignition/gazebo/components/CanonicalLink.hh"
@@ -95,6 +97,7 @@
 #include "ignition/gazebo/components/JointVelocityReset.hh"
 #include "ignition/gazebo/components/LinearAcceleration.hh"
 #include "ignition/gazebo/components/LinearVelocity.hh"
+#include "ignition/gazebo/components/LinearVelocityCmd.hh"
 #include "ignition/gazebo/components/Link.hh"
 #include "ignition/gazebo/components/Model.hh"
 #include "ignition/gazebo/components/Name.hh"
@@ -421,6 +424,25 @@ class ignition::gazebo::systems::PhysicsPrivate
  public: std::unordered_map<Entity, JointVelocityCommandPtrType>
  entityJointVelocityCommandMap;
 
+ //////////////////////////////////////////////////
+ // World velocity command
+ public: using WorldVelocityCommandFeatureList =
+ ignition::physics::FeatureList<
+ ignition::physics::SetFreeGroupWorldVelocity>;
+
+ /// \brief Free group type with world velocity command.
+ public: using FreeGroupVelocityCmdPtrType =
+ ignition::physics::FreeGroupPtr<
+ ignition::physics::FeaturePolicy3d,
+ WorldVelocityCommandFeatureList>;
+
+ /// \brief A map between free group entity ids in the ECM
+ /// to FreeGroup Entities in ign-physics, with velocity command feature.
+ /// All FreeGroup on this map are casted for
+ /// `WorldVelocityCommandFeatureList`.
+ public: std::unordered_map<Entity, FreeGroupVelocityCmdPtrType>
+ entityWorldVelocityCommandMap;
+
  //////////////////////////////////////////////////
  // Meshes
 
@@ -1316,6 +1338,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
  return true;
  });
 
+ // Update model pose
  _ecm.Each<components::Model, components::WorldPoseCmd>(
  [&](const Entity &_entity, const components::Model *,
  const components::WorldPoseCmd *_poseCmd)
@@ -1364,6 +1387,68 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
  return true;
  });
 
+ // Update model angular velocity
+ _ecm.Each<components::Model, components::AngularVelocityCmd>(
+ [&](const Entity &_entity, const components::Model *,
+ const components::AngularVelocityCmd *_angularVelocityCmd)
+ {
+ auto modelIt = this->entityModelMap.find(_entity);
+ if (modelIt == this->entityModelMap.end())
+ return true;
+
+ auto freeGroup = modelIt->second->FindFreeGroup();
+ if (!freeGroup)
+ return true;
+
+ const components::Pose *poseComp =
+ _ecm.Component<components::Pose>(_entity);
+ math::Vector3d worldAngularVel = poseComp->Data().Rot() *
+ _angularVelocityCmd->Data();
+
+ auto worldAngularVelFeature = entityCast(_entity, freeGroup,
+ this->entityWorldVelocityCommandMap);
+ if (!worldAngularVelFeature)
+ {
+ return true;
+ }
+
+ worldAngularVelFeature->SetWorldAngularVelocity(
+ math::eigen3::convert(worldAngularVel));
+
+ return true;
+ });
+
+ // Update model linear velocity
+ _ecm.Each<components::Model, components::LinearVelocityCmd>(
+ [&](const Entity &_entity, const components::Model *,
+ const components::LinearVelocityCmd *_linearVelocityCmd)
+ {
+ auto modelIt = this->entityModelMap.find(_entity);
+ if (modelIt == this->entityModelMap.end())
+ return true;
+
+ auto freeGroup = modelIt->second->FindFreeGroup();
+ if (!freeGroup)
+ return true;
+
+ const components::Pose *poseComp =
+ _ecm.Component<components::Pose>(_entity);
+ math::Vector3d worldLinearVel = poseComp->Data().Rot() *
+ _linearVelocityCmd->Data();
+
+ auto worldLinearVelFeature = entityCast(_entity, freeGroup,
+ this->entityWorldVelocityCommandMap);
+ if (!worldLinearVelFeature)
+ {
+ return true;
+ }
+
+ worldLinearVelFeature->SetWorldLinearVelocity(
+ math::eigen3::convert(worldLinearVel));
+
+ return true;
+ });
+
  // Clear pending commands
  // Note: Removing components from inside an Each call can be dangerous.
  // Instead, we collect all the entities that have the desired components and

src/systems/velocity_control/CMakeLists.txt

@@ -0,0 +1,6 @@
+gz_add_system(velocity-control
+ SOURCES
+ VelocityControl.cc
+ PUBLIC_LINK_LIBS
+ ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+)