Setting the intiial velocity for a model or joint (#693)

Signed-off-by: Louise Poubel <[email protected]>
gazebosim · Jul 30, 2021 · 57ce159 · 57ce159
1 parent 12a60f6
commit 57ce159
Show file tree

Hide file tree

Showing 9 changed files with 58 additions and 3 deletions.
diff --git a/examples/worlds/joint_controller.sdf b/examples/worlds/joint_controller.sdf
@@ -158,6 +158,7 @@
         filename="ignition-gazebo-joint-controller-system"
         name="ignition::gazebo::systems::JointController">
         <joint_name>j1</joint_name>
+        <initial_velocity>5.0</initial_velocity>
       </plugin>
     </model>
 

diff --git a/examples/worlds/velocity_control.sdf b/examples/worlds/velocity_control.sdf
@@ -485,6 +485,8 @@
       <plugin
         filename="ignition-gazebo-velocity-control-system"
         name="ignition::gazebo::systems::VelocityControl">
+        <initial_linear>0.3 0 0</initial_linear>
+        <initial_angular>0 0 -0.1</initial_angular>
       </plugin>
 
     </model>

diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc
@@ -98,6 +98,13 @@ void JointController::Configure(const Entity &_entity,
     return;
   }
 
+  if (_sdf->HasElement("initial_velocity"))
+  {
+    this->dataPtr->jointVelCmd = _sdf->Get<double>("initial_velocity");
+    ignmsg << "Joint velocity initialized to ["
+           << this->dataPtr->jointVelCmd << "]" << std::endl;
+  }
+
   if (_sdf->HasElement("use_force_commands") &&
       _sdf->Get<bool>("use_force_commands"))
   {

diff --git a/src/systems/joint_controller/JointController.hh b/src/systems/joint_controller/JointController.hh
@@ -42,6 +42,11 @@ namespace systems
   /// using force commands. If this parameter is not set or is false, the
   /// controller will use velocity commands internally.
   ///
+  /// `<topic>` Topic to receive commands in. Defaults to
+  ///     `/model/<model_name>/joint/<joint_name>/cmd_vel`.
+  ///
+  /// `<initial_velocity>` Velocity to start with.
+  ///
   /// ### Velocity mode: No additional parameters are required.
   ///
   /// ### Force mode: The controller accepts the next optional parameters:

diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc
@@ -116,6 +116,25 @@ void VelocityControl::Configure(const Entity &_entity,
     return;
   }
 
+  if (_sdf->HasElement("initial_linear"))
+  {
+    this->dataPtr->linearVelocity = _sdf->Get<math::Vector3d>("initial_linear");
+    msgs::Set(this->dataPtr->targetVel.mutable_linear(),
+        this->dataPtr->linearVelocity);
+    ignmsg << "Linear velocity initialized to ["
+           << this->dataPtr->linearVelocity << "]" << std::endl;
+  }
+
+  if (_sdf->HasElement("initial_angular"))
+  {
+    this->dataPtr->angularVelocity =
+        _sdf->Get<math::Vector3d>("initial_angular");
+    msgs::Set(this->dataPtr->targetVel.mutable_angular(),
+        this->dataPtr->angularVelocity);
+    ignmsg << "Angular velocity initialized to ["
+           << this->dataPtr->angularVelocity << "]" << std::endl;
+  }
+
   // Subscribe to model commands
   std::string modelTopic{
       "/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"};

diff --git a/src/systems/velocity_control/VelocityControl.hh b/src/systems/velocity_control/VelocityControl.hh
@@ -35,6 +35,15 @@ namespace systems
 
   /// \brief Linear and angular velocity controller
   /// which is directly set on a model.
+  ///
+  /// ## System Parameters
+  ///
+  /// `<topic>` Topic to receive commands in. Defaults to
+  ///     `/model/<model_name>/cmd_vel`.
+  ///
+  /// `<initial_linear>` Linear velocity to start with.
+  ///
+  /// `<initial_angular>` Linear velocity to start with.
   class VelocityControl
       : public System,
         public ISystemConfigure,

diff --git a/test/integration/joint_controller_system.cc b/test/integration/joint_controller_system.cc
@@ -103,12 +103,21 @@ TEST_F(JointControllerTestFixture, JointVelocityCommand)
 
   server.AddSystem(testSystem.systemPtr);
 
-  const std::size_t initIters = 10;
+  const std::size_t initIters = 1000;
   server.Run(true, initIters, false);
   EXPECT_EQ(initIters, angularVelocities.size());
-  for (const auto &angVel : angularVelocities)
+  for (auto i = 0u; i < angularVelocities.size(); ++i)
   {
-    EXPECT_NEAR(0, angVel.Length(), TOL);
+    if (i == 0)
+    {
+      EXPECT_NEAR(0.0, angularVelocities[i].Length(), TOL)
+          << "Iteration [" << i << "]";
+    }
+    else
+    {
+      EXPECT_NEAR(5.0, angularVelocities[i].Length(), TOL)
+          << "Iteration [" << i << "]";
+    }
   }
 
   angularVelocities.clear();

diff --git a/test/worlds/joint_controller.sdf b/test/worlds/joint_controller.sdf
@@ -151,6 +151,7 @@
         filename="ignition-gazebo-joint-controller-system"
         name="ignition::gazebo::systems::JointController">
         <joint_name>j1</joint_name>
+        <initial_velocity>5.0</initial_velocity>
       </plugin>
     </model>
 

diff --git a/test/worlds/velocity_control.sdf b/test/worlds/velocity_control.sdf
@@ -486,6 +486,8 @@
       <plugin
         filename="ignition-gazebo-velocity-control-system"
         name="ignition::gazebo::systems::VelocityControl">
+        <initial_linear>0.3 0 0</initial_linear>
+        <initial_angular>0 0 -0.1</initial_angular>
       </plugin>
 
     </model>