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

Signed-off-by: Louise Poubel <[email protected]>

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>
 

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>

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"))
   {

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:

src/systems/velocity_control/VelocityControl.cc

@@ -88,6 +88,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 commands
   std::vector<std::string> topics;
   if (_sdf->HasElement("topic"))

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,

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();

test/integration/velocity_control_system.cc

@@ -136,6 +136,55 @@ TEST_P(VelocityControlTest, PublishCmd)
       "/model/vehicle_blue/cmd_vel");
 }
 
+/////////////////////////////////////////////////
+TEST_F(VelocityControlTest, InitialVelocity)
+{
+  // Start server
+  ServerConfig serverConfig;
+  serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+      "test", "worlds", "velocity_control.sdf"));
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  // Create a system that records the vehicle poses
+  test::Relay testSystem;
+
+  std::vector<math::Pose3d> poses;
+  testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &,
+    const gazebo::EntityComponentManager &_ecm)
+    {
+      auto id = _ecm.EntityByComponents(
+        components::Model(),
+        components::Name("vehicle_green"));
+      EXPECT_NE(kNullEntity, id);
+
+      auto poseComp = _ecm.Component<components::Pose>(id);
+      ASSERT_NE(nullptr, poseComp);
+
+      poses.push_back(poseComp->Data());
+    });
+  server.AddSystem(testSystem.systemPtr);
+
+  // Run server and check that vehicle moved
+  server.Run(true, 1000, false);
+  EXPECT_EQ(1000u, poses.size());
+
+  // verify that the vehicle is moving in +x and rotating towards -y
+  for (unsigned int i = 1u; i < poses.size(); ++i)
+  {
+    EXPECT_GT(poses[i].Pos().X(), poses[i-1].Pos().X()) << i;
+    EXPECT_LT(poses[i].Pos().Y(), poses[i-1].Pos().Y()) << i;
+    EXPECT_NEAR(poses[i].Pos().Z(), poses[i-1].Pos().Z(), 1e-5);
+    EXPECT_NEAR(poses[i].Rot().Euler().X(),
+        poses[i-1].Rot().Euler().X(), 1e-5) << i;
+    EXPECT_NEAR(poses[i].Rot().Euler().Y(),
+        poses[i-1].Rot().Euler().Y(), 1e-5) << i;
+    EXPECT_LT(poses[i].Rot().Euler().Z(), poses[i-1].Rot().Euler().Z()) << i;
+  }
+}
+
 // Run multiple times
 INSTANTIATE_TEST_SUITE_P(ServerRepeat, VelocityControlTest,
     ::testing::Range(1, 2));

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>
 

test/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>