diff --git a/examples/worlds/joint_controller.sdf b/examples/worlds/joint_controller.sdf
index b4db4b5cfe..86e54350a7 100644
--- 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">
j1
+ 5.0
diff --git a/examples/worlds/velocity_control.sdf b/examples/worlds/velocity_control.sdf
index 2e104e5f08..71c30486f9 100644
--- a/examples/worlds/velocity_control.sdf
+++ b/examples/worlds/velocity_control.sdf
@@ -485,6 +485,8 @@
+ 0.3 0 0
+ 0 0 -0.1
diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc
index dce17b55c3..64762df643 100644
--- 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("initial_velocity");
+ ignmsg << "Joint velocity initialized to ["
+ << this->dataPtr->jointVelCmd << "]" << std::endl;
+ }
+
if (_sdf->HasElement("use_force_commands") &&
_sdf->Get("use_force_commands"))
{
diff --git a/src/systems/joint_controller/JointController.hh b/src/systems/joint_controller/JointController.hh
index 815a53c772..d3a1fb6587 100644
--- 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 to receive commands in. Defaults to
+ /// `/model//joint//cmd_vel`.
+ ///
+ /// `` 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
index 78b1913d67..4ba40560ec 100644
--- a/src/systems/velocity_control/VelocityControl.cc
+++ b/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("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("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 topics;
if (_sdf->HasElement("topic"))
diff --git a/src/systems/velocity_control/VelocityControl.hh b/src/systems/velocity_control/VelocityControl.hh
index 0b546525a1..08ac7cd569 100644
--- 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 to receive commands in. Defaults to
+ /// `/model//cmd_vel`.
+ ///
+ /// `` Linear velocity to start with.
+ ///
+ /// `` 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
index faf510ec5c..a773c404b2 100644
--- 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/integration/velocity_control_system.cc b/test/integration/velocity_control_system.cc
index df1354b924..32e259fb38 100644
--- a/test/integration/velocity_control_system.cc
+++ b/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 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(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));
diff --git a/test/worlds/joint_controller.sdf b/test/worlds/joint_controller.sdf
index 06b9dd566b..0f3678be24 100644
--- 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">
j1
+ 5.0
diff --git a/test/worlds/velocity_control.sdf b/test/worlds/velocity_control.sdf
index 2e104e5f08..71c30486f9 100644
--- a/test/worlds/velocity_control.sdf
+++ b/test/worlds/velocity_control.sdf
@@ -485,6 +485,8 @@
+ 0.3 0 0
+ 0 0 -0.1