diff --git a/mujoco_ros/test/mujoco_ros_plugin_test.cpp b/mujoco_ros/test/mujoco_ros_plugin_test.cpp
index fbd05274..0f16b6b8 100644
--- a/mujoco_ros/test/mujoco_ros_plugin_test.cpp
+++ b/mujoco_ros/test/mujoco_ros_plugin_test.cpp
@@ -49,7 +49,17 @@ int main(int argc, char **argv)
 {
 	testing::InitGoogleTest(&argc, argv);
 	ros::init(argc, argv, "mujoco_ros_plugin_test");
-	return RUN_ALL_TESTS();
+
+	// Create spinner to communicate with ROS
+	ros::AsyncSpinner spinner(1);
+	spinner.start();
+	ros::NodeHandle nh;
+	int ret = RUN_ALL_TESTS();
+
+	// Stop spinner and shutdown ROS before returning
+	spinner.stop();
+	ros::shutdown();
+	return ret;
 }
 
 class LoadedPluginFixture : public ::testing::Test
@@ -145,7 +155,6 @@ TEST_F(BaseEnvFixture, LoadPlugin)
 	EXPECT_EQ(env.getNumCBReadyPlugins(), 1) << "Env should have 1 plugin loaded!";
 
 	env.shutdown();
-	nh->setParam("unpause", true);
 }
 
 TEST_F(LoadedPluginFixture, ResetPlugin)
@@ -221,7 +230,6 @@ TEST_F(BaseEnvFixture, FailedLoad)
 
 	env.shutdown();
 	nh->setParam("should_fail", false);
-	nh->setParam("unpause", true);
 }
 
 TEST_F(BaseEnvFixture, FailedLoadRecoverReload)
@@ -315,5 +323,69 @@ TEST_F(BaseEnvFixture, FailedLoadReset)
 
 	env.shutdown();
 	nh->setParam("should_fail", false);
-	nh->setParam("unpause", true);
+}
+
+TEST_F(LoadedPluginFixture, PluginStats_InitialPaused)
+{
+	EXPECT_EQ(env_ptr->settings_.run, 0) << "Env should be paused!";
+
+	mujoco_ros_msgs::GetPluginStats srv;
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/get_plugin_stats", true))
+	    << "Plugin stats service should exist!";
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/get_plugin_stats", srv))
+	    << "Get plugin stats service call failed!";
+	EXPECT_EQ(srv.response.stats.size(), 1) << "Should have 1 plugin stats!";
+	EXPECT_EQ(srv.response.stats[0].plugin_type, "mujoco_ros/TestPlugin") << "Should be TestPlugin!";
+	EXPECT_GT(srv.response.stats[0].load_time, -1) << "Load time should be set!";
+	EXPECT_EQ(srv.response.stats[0].reset_time, -1) << "Reset time should be unset!";
+	EXPECT_NEAR(srv.response.stats[0].ema_steptime_control, 0, 1e-8) << "Control time should be unset!";
+	EXPECT_NEAR(srv.response.stats[0].ema_steptime_passive, 0, 1e-8) << "Passive time should be unset!";
+	EXPECT_NEAR(srv.response.stats[0].ema_steptime_render, 0, 1e-8) << "Render time should be unset!";
+	EXPECT_NEAR(srv.response.stats[0].ema_steptime_last_stage, 0, 1e-8) << "Last stage time should be unset!";
+}
+
+TEST_F(LoadedPluginFixture, PluginStats_SetTimesOnStep)
+{
+	EXPECT_EQ(env_ptr->settings_.run, 0) << "Env should be paused!";
+
+	env_ptr->step(1);
+	// sleep for a bit to ensure the plugin callbacks have been called
+	std::this_thread::sleep_for(std::chrono::milliseconds(1));
+
+	mujoco_ros_msgs::GetPluginStats srv;
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/get_plugin_stats", true))
+	    << "Plugin stats service should exist!";
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/get_plugin_stats", srv))
+	    << "Get plugin stats service call failed!";
+
+	EXPECT_EQ(srv.response.stats.size(), 1) << "Should have 1 plugin stats!";
+	EXPECT_EQ(srv.response.stats[0].plugin_type, "mujoco_ros/TestPlugin") << "Should be TestPlugin!";
+	EXPECT_GT(srv.response.stats[0].load_time, -1) << "Load time should be set!";
+	EXPECT_EQ(srv.response.stats[0].reset_time, -1) << "Reset time should be unset!";
+	EXPECT_GT(srv.response.stats[0].ema_steptime_control, -1) << "Control time should be unset!";
+	EXPECT_GT(srv.response.stats[0].ema_steptime_passive, -1) << "Passive time should be unset!";
+	// EXPECT_GT(srv.response.stats[0].ema_steptime_render, -1) << "Render time should be unset!"; // TODO: add when
+	// rendering is enabled in tests
+	EXPECT_GT(srv.response.stats[0].ema_steptime_last_stage, -1) << "Last stage time should be unset!";
+}
+
+TEST_F(LoadedPluginFixture, PluginStats_ResetTimeOnReset)
+{
+	env_ptr->settings_.reset_request = 1;
+	float seconds                    = 0;
+	while (env_ptr->settings_.reset_request != 0 && seconds < 2) {
+		std::this_thread::sleep_for(std::chrono::milliseconds(1));
+		seconds += 0.001;
+	}
+	EXPECT_LT(seconds, 2) << "Env reset ran into 2 seconds timeout!";
+
+	mujoco_ros_msgs::GetPluginStats srv;
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/get_plugin_stats", true))
+	    << "Plugin stats service should exist!";
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/get_plugin_stats", srv))
+	    << "Get plugin stats service call failed!";
+
+	EXPECT_EQ(srv.response.stats.size(), 1) << "Should have 1 plugin stats!";
+	EXPECT_EQ(srv.response.stats[0].plugin_type, "mujoco_ros/TestPlugin") << "Should be TestPlugin!";
+	EXPECT_GT(srv.response.stats[0].reset_time, -1) << "Reset time should be unset!";
 }
diff --git a/mujoco_ros/test/ros_interface_test.cpp b/mujoco_ros/test/ros_interface_test.cpp
index 036df143..74ee2b3d 100644
--- a/mujoco_ros/test/ros_interface_test.cpp
+++ b/mujoco_ros/test/ros_interface_test.cpp
@@ -1845,3 +1845,389 @@ TEST_F(PendulumEnvFixture, GetStateUintLoadInProgress) {
    EXPECT_EQ(srv.response.state.value, 1) << "State should be load issued (1)!";
 }
 */
+
+TEST_F(PendulumEnvFixture, LoadInitialJointPositions_Valid)
+{
+	mjModel *m = env_ptr->getModelPtr();
+	mjData *d  = env_ptr->getDataPtr();
+
+	EXPECT_FALSE(env_ptr->settings_.run) << "Simulation should be paused!";
+	EXPECT_NEAR(d->time, 0, 1e-6) << "Simulation time should be 0.0!";
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/load_initial_joint_states", true))
+	    << "Load initial joint states service should be available!";
+
+	std::map<std::string, std::string> joint_states;
+	joint_states.insert({ "balljoint", "0. 1. 0. 0." });
+	joint_states.insert({ "joint1", "0.3" });
+	joint_states.insert({ "joint2", "0.5" });
+	joint_states.insert({ "ball_freejoint", "1. 0.9 0.8 0.0 0.0 0.0 1.0" });
+
+	// verify initial joint states are different
+	int ids[4];
+	ids[0] = mujoco_ros::util::jointName2id(m, "balljoint");
+	ids[1] = mujoco_ros::util::jointName2id(m, "joint1");
+	ids[2] = mujoco_ros::util::jointName2id(m, "joint2");
+	ids[3] = mujoco_ros::util::jointName2id(m, "ball_freejoint");
+
+	EXPECT_NE(ids[0], -1) << "'balljoint' should be found as joint in model!";
+	EXPECT_NE(ids[1], -1) << "'joint1' should be found as joint in model!";
+	EXPECT_NE(ids[2], -1) << "'joint2' should be found as joint in model!";
+	EXPECT_NE(ids[3], -1) << "'ball_freejoint' should be found as joint in model!";
+
+	compare_qpos(d, m->jnt_qposadr[ids[0]], "balljoint", { 1.0, 0.0, 0.0, 0.0 });
+	compare_qpos(d, m->jnt_qposadr[ids[1]], "joint1", { 0.0 });
+	compare_qpos(d, m->jnt_qposadr[ids[2]], "joint2", { 0.0 });
+	compare_qpos(d, m->jnt_qposadr[ids[3]], "ball_freejoint", { 1.0, 0.0, 0.06, 1.0, 0.0, 0.0, 0.0 });
+
+	nh->setParam(env_ptr->getHandleNamespace() + "/initial_joint_positions/joint_map", joint_states);
+
+	std_srvs::Empty srv;
+
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/load_initial_joint_states", srv))
+	    << "Load initial joint states service call failed!";
+
+	nh->deleteParam(env_ptr->getHandleNamespace() + "/initial_joint_positions/joint_map");
+
+	compare_qpos(d, m->jnt_qposadr[ids[0]], "balljoint", { 0.0, 1.0, 0.0, 0.0 });
+	compare_qpos(d, m->jnt_qposadr[ids[1]], "joint1", { 0.3 });
+	compare_qpos(d, m->jnt_qposadr[ids[2]], "joint2", { 0.5 });
+	compare_qpos(d, m->jnt_qposadr[ids[3]], "ball_freejoint", { 1.0, 0.9, 0.8, 0.0, 0.0, 0.0, 1.0 });
+}
+
+TEST_F(PendulumEnvFixture, LoadInitialJointPositions_NoParams)
+{
+	mjModel *m = env_ptr->getModelPtr();
+	mjData *d  = env_ptr->getDataPtr();
+
+	EXPECT_FALSE(env_ptr->settings_.run) << "Simulation should be paused!";
+	EXPECT_NEAR(d->time, 0, 1e-6) << "Simulation time should be 0.0!";
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/load_initial_joint_states", true))
+	    << "Load initial joint states service should be available!";
+
+	std_srvs::Empty srv;
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/load_initial_joint_states", srv))
+	    << "Load initial joint states service call failed!";
+
+	int ids[4];
+	ids[0] = mujoco_ros::util::jointName2id(m, "balljoint");
+	ids[1] = mujoco_ros::util::jointName2id(m, "joint1");
+	ids[2] = mujoco_ros::util::jointName2id(m, "joint2");
+	ids[3] = mujoco_ros::util::jointName2id(m, "ball_freejoint");
+
+	EXPECT_NE(ids[0], -1) << "'balljoint' should be found as joint in model!";
+	EXPECT_NE(ids[1], -1) << "'joint1' should be found as joint in model!";
+	EXPECT_NE(ids[2], -1) << "'joint2' should be found as joint in model!";
+	EXPECT_NE(ids[3], -1) << "'ball_freejoint' should be found as joint in model!";
+
+	compare_qpos(d, m->jnt_qposadr[ids[0]], "balljoint", { 1.0, 0.0, 0.0, 0.0 });
+	compare_qpos(d, m->jnt_qposadr[ids[1]], "joint1", { 0.0 });
+	compare_qpos(d, m->jnt_qposadr[ids[2]], "joint2", { 0.0 });
+	compare_qpos(d, m->jnt_qposadr[ids[3]], "ball_freejoint", { 1.0, 0.0, 0.06, 1.0, 0.0, 0.0, 0.0 });
+}
+
+TEST_F(PendulumEnvFixture, LoadInitialJointPositions_InvalidJointName)
+{
+	mjModel *m = env_ptr->getModelPtr();
+	mjData *d  = env_ptr->getDataPtr();
+
+	EXPECT_FALSE(env_ptr->settings_.run) << "Simulation should be paused!";
+	EXPECT_NEAR(d->time, 0, 1e-6) << "Simulation time should be 0.0!";
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/load_initial_joint_states", true))
+	    << "Load initial joint states service should be available!";
+
+	int ids[4];
+	ids[0] = mujoco_ros::util::jointName2id(m, "balljoint");
+	ids[1] = mujoco_ros::util::jointName2id(m, "joint1");
+	ids[2] = mujoco_ros::util::jointName2id(m, "joint2");
+	ids[3] = mujoco_ros::util::jointName2id(m, "ball_freejoint");
+
+	EXPECT_NE(ids[0], -1) << "'balljoint' should be found as joint in model!";
+	EXPECT_NE(ids[1], -1) << "'joint1' should be found as joint in model!";
+	EXPECT_NE(ids[2], -1) << "'joint2' should be found as joint in model!";
+	EXPECT_NE(ids[3], -1) << "'ball_freejoint' should be found as joint in model!";
+
+	std::map<std::string, std::string> joint_states;
+	joint_states.insert({ "invalid_joint", "0.3" });
+
+	nh->setParam(env_ptr->getHandleNamespace() + "/initial_joint_positions/joint_map", joint_states);
+
+	std_srvs::Empty srv;
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/load_initial_joint_states", srv))
+	    << "Load initial joint states service call failed!";
+	nh->deleteParam(env_ptr->getHandleNamespace() + "/initial_joint_positions/joint_map");
+
+	compare_qpos(d, m->jnt_qposadr[ids[0]], "balljoint", { 1.0, 0.0, 0.0, 0.0 });
+	compare_qpos(d, m->jnt_qposadr[ids[1]], "joint1", { 0.0 });
+	compare_qpos(d, m->jnt_qposadr[ids[2]], "joint2", { 0.0 });
+	compare_qpos(d, m->jnt_qposadr[ids[3]], "ball_freejoint", { 1.0, 0.0, 0.06, 1.0, 0.0, 0.0, 0.0 });
+}
+
+TEST_F(PendulumEnvFixture, LoadInitialJointPositions_InvalidDOFs)
+{
+	mjModel *m = env_ptr->getModelPtr();
+	mjData *d  = env_ptr->getDataPtr();
+
+	EXPECT_FALSE(env_ptr->settings_.run) << "Simulation should be paused!";
+	EXPECT_NEAR(d->time, 0, 1e-6) << "Simulation time should be 0.0!";
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/load_initial_joint_states", true))
+	    << "Load initial joint states service should be available!";
+
+	int ids[4];
+	ids[0] = mujoco_ros::util::jointName2id(m, "balljoint");
+	ids[1] = mujoco_ros::util::jointName2id(m, "joint1");
+	ids[2] = mujoco_ros::util::jointName2id(m, "joint2");
+	ids[3] = mujoco_ros::util::jointName2id(m, "ball_freejoint");
+
+	EXPECT_NE(ids[0], -1) << "'balljoint' should be found as joint in model!";
+	EXPECT_NE(ids[1], -1) << "'joint1' should be found as joint in model!";
+	EXPECT_NE(ids[2], -1) << "'joint2' should be found as joint in model!";
+	EXPECT_NE(ids[3], -1) << "'ball_freejoint' should be found as joint in model!";
+
+	std::map<std::string, std::string> joint_states;
+	joint_states.insert({ "balljoint", "0. 1. 0." }); // Invalid DOFs for balljoint
+	joint_states.insert({ "joint1", "0.3 0.4" }); // Invalid DOFs for hinge joint
+	joint_states.insert({ "ball_freejoint", "0.6" }); // Invalid DOFs for freejoint
+
+	nh->setParam(env_ptr->getHandleNamespace() + "/initial_joint_positions/joint_map", joint_states);
+
+	std_srvs::Empty srv;
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/load_initial_joint_states", srv))
+	    << "Load initial joint states service call failed!";
+	nh->deleteParam(env_ptr->getHandleNamespace() + "/initial_joint_positions/joint_map");
+
+	compare_qpos(d, m->jnt_qposadr[ids[0]], "balljoint", { 1.0, 0.0, 0.0, 0.0 });
+	compare_qpos(d, m->jnt_qposadr[ids[1]], "joint1", { 0.0 });
+	compare_qpos(d, m->jnt_qposadr[ids[2]], "joint2", { 0.0 });
+	compare_qpos(d, m->jnt_qposadr[ids[3]], "ball_freejoint", { 1.0, 0.0, 0.06, 1.0, 0.0, 0.0, 0.0 });
+}
+
+TEST_F(PendulumEnvFixture, LoadInitialJointVels_Valid)
+{
+	mjModel *m = env_ptr->getModelPtr();
+	mjData *d  = env_ptr->getDataPtr();
+
+	EXPECT_FALSE(env_ptr->settings_.run) << "Simulation should be paused!";
+	EXPECT_NEAR(d->time, 0, 1e-6) << "Simulation time should be 0.0!";
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/load_initial_joint_states", true))
+	    << "Load initial joint states service should be available!";
+
+	std::map<std::string, std::string> joint_states;
+	joint_states.insert({ "balljoint", "0.2 1. 0.1" });
+	joint_states.insert({ "joint1", "0.3" });
+	joint_states.insert({ "joint2", "0.5" });
+	joint_states.insert({ "ball_freejoint", "1. 0.9 0.8 0.2 0.3 1.0" });
+
+	int ids[4];
+	ids[0] = mujoco_ros::util::jointName2id(m, "balljoint");
+	ids[1] = mujoco_ros::util::jointName2id(m, "joint1");
+	ids[2] = mujoco_ros::util::jointName2id(m, "joint2");
+	ids[3] = mujoco_ros::util::jointName2id(m, "ball_freejoint");
+
+	EXPECT_NE(ids[0], -1) << "'balljoint' should be found as joint in model!";
+	EXPECT_NE(ids[1], -1) << "'joint1' should be found as joint in model!";
+	EXPECT_NE(ids[2], -1) << "'joint2' should be found as joint in model!";
+	EXPECT_NE(ids[3], -1) << "'ball_freejoint' should be found as joint in model!";
+
+	compare_qvel(d, m->jnt_dofadr[ids[0]], "balljoint", { 0.0, 0.0, 0.0 });
+	compare_qvel(d, m->jnt_dofadr[ids[1]], "joint1", { 0.0 });
+	compare_qvel(d, m->jnt_dofadr[ids[2]], "joint2", { 0.0 });
+	compare_qvel(d, m->jnt_dofadr[ids[3]], "ball_freejoint", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 });
+
+	nh->setParam(env_ptr->getHandleNamespace() + "/initial_joint_velocities/joint_map", joint_states);
+
+	std_srvs::Empty srv;
+
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/load_initial_joint_states", srv))
+	    << "Load initial joint states service call failed!";
+	nh->deleteParam(env_ptr->getHandleNamespace() + "/initial_joint_velocities/joint_map");
+
+	compare_qvel(d, m->jnt_dofadr[ids[0]], "balljoint", { 0.2, 1.0, 0.1 });
+	compare_qvel(d, m->jnt_dofadr[ids[1]], "joint1", { 0.3 });
+	compare_qvel(d, m->jnt_dofadr[ids[2]], "joint2", { 0.5 });
+	compare_qvel(d, m->jnt_dofadr[ids[3]], "ball_freejoint", { 1.0, 0.9, 0.8, 0.2, 0.3, 1.0 });
+}
+
+TEST_F(PendulumEnvFixture, LoadInitialJointVels_NoParams)
+{
+	mjModel *m = env_ptr->getModelPtr();
+	mjData *d  = env_ptr->getDataPtr();
+
+	EXPECT_FALSE(env_ptr->settings_.run) << "Simulation should be paused!";
+	EXPECT_NEAR(d->time, 0, 1e-6) << "Simulation time should be 0.0!";
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/load_initial_joint_states", true))
+	    << "Load initial joint states service should be available!";
+
+	std_srvs::Empty srv;
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/load_initial_joint_states", srv))
+	    << "Load initial joint states service call failed!";
+
+	int ids[4];
+	ids[0] = mujoco_ros::util::jointName2id(m, "balljoint");
+	ids[1] = mujoco_ros::util::jointName2id(m, "joint1");
+	ids[2] = mujoco_ros::util::jointName2id(m, "joint2");
+	ids[3] = mujoco_ros::util::jointName2id(m, "ball_freejoint");
+
+	EXPECT_NE(ids[0], -1) << "'balljoint' should be found as joint in model!";
+	EXPECT_NE(ids[1], -1) << "'joint1' should be found as joint in model!";
+	EXPECT_NE(ids[2], -1) << "'joint2' should be found as joint in model!";
+	EXPECT_NE(ids[3], -1) << "'ball_freejoint' should be found as joint in model!";
+
+	compare_qvel(d, m->jnt_dofadr[ids[0]], "balljoint", { 0.0, 0.0, 0.0 });
+	compare_qvel(d, m->jnt_dofadr[ids[1]], "joint1", { 0.0 });
+	compare_qvel(d, m->jnt_dofadr[ids[2]], "joint2", { 0.0 });
+	compare_qvel(d, m->jnt_dofadr[ids[3]], "ball_freejoint", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 });
+}
+
+TEST_F(PendulumEnvFixture, LoadInitialJointVels_InvalidJointName)
+{
+	mjModel *m = env_ptr->getModelPtr();
+	mjData *d  = env_ptr->getDataPtr();
+
+	EXPECT_FALSE(env_ptr->settings_.run) << "Simulation should be paused!";
+	EXPECT_NEAR(d->time, 0, 1e-6) << "Simulation time should be 0.0!";
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/load_initial_joint_states", true))
+	    << "Load initial joint states service should be available!";
+
+	int ids[4];
+	ids[0] = mujoco_ros::util::jointName2id(m, "balljoint");
+	ids[1] = mujoco_ros::util::jointName2id(m, "joint1");
+	ids[2] = mujoco_ros::util::jointName2id(m, "joint2");
+	ids[3] = mujoco_ros::util::jointName2id(m, "ball_freejoint");
+
+	EXPECT_NE(ids[0], -1) << "'balljoint' should be found as joint in model!";
+	EXPECT_NE(ids[1], -1) << "'joint1' should be found as joint in model!";
+	EXPECT_NE(ids[2], -1) << "'joint2' should be found as joint in model!";
+	EXPECT_NE(ids[3], -1) << "'ball_freejoint' should be found as joint in model!";
+
+	std::map<std::string, std::string> joint_states;
+	joint_states.insert({ "invalid_joint", "0.3" });
+
+	nh->setParam(env_ptr->getHandleNamespace() + "/initial_joint_velocities/joint_map", joint_states);
+
+	std_srvs::Empty srv;
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/load_initial_joint_states", srv))
+	    << "Load initial joint states service call failed!";
+	nh->deleteParam(env_ptr->getHandleNamespace() + "/initial_joint_velocities/joint_map");
+
+	compare_qvel(d, m->jnt_dofadr[ids[0]], "balljoint", { 0.0, 0.0, 0.0 });
+	compare_qvel(d, m->jnt_dofadr[ids[1]], "joint1", { 0.0 });
+	compare_qvel(d, m->jnt_dofadr[ids[2]], "joint2", { 0.0 });
+	compare_qvel(d, m->jnt_dofadr[ids[3]], "ball_freejoint", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 });
+}
+
+TEST_F(PendulumEnvFixture, LoadInitialJointVels_InvalidDOFs)
+{
+	mjModel *m = env_ptr->getModelPtr();
+	mjData *d  = env_ptr->getDataPtr();
+
+	EXPECT_FALSE(env_ptr->settings_.run) << "Simulation should be paused!";
+	EXPECT_NEAR(d->time, 0, 1e-6) << "Simulation time should be 0.0!";
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/load_initial_joint_states", true))
+	    << "Load initial joint states service should be available!";
+
+	int ids[4];
+	ids[0] = mujoco_ros::util::jointName2id(m, "balljoint");
+	ids[1] = mujoco_ros::util::jointName2id(m, "joint1");
+	ids[2] = mujoco_ros::util::jointName2id(m, "joint2");
+	ids[3] = mujoco_ros::util::jointName2id(m, "ball_freejoint");
+
+	EXPECT_NE(ids[0], -1) << "'balljoint' should be found as joint in model!";
+	EXPECT_NE(ids[1], -1) << "'joint1' should be found as joint in model!";
+	EXPECT_NE(ids[2], -1) << "'joint2' should be found as joint in model!";
+	EXPECT_NE(ids[3], -1) << "'ball_freejoint' should be found as joint in model!";
+
+	std::map<std::string, std::string> joint_states;
+	joint_states.insert({ "balljoint", "0. 1." }); // Invalid DOFs for balljoint
+	joint_states.insert({ "joint1", "0.3 0.4" }); // Invalid DOFs for hinge joint
+	joint_states.insert({ "ball_freejoint", "0.6" }); // Invalid DOFs for freejoint
+
+	nh->setParam(env_ptr->getHandleNamespace() + "/initial_joint_velocities/joint_map", joint_states);
+
+	std_srvs::Empty srv;
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/load_initial_joint_states", srv))
+	    << "Load initial joint states service call failed!";
+	nh->deleteParam(env_ptr->getHandleNamespace() + "/initial_joint_velocities/joint_map");
+
+	compare_qvel(d, m->jnt_dofadr[ids[0]], "balljoint", { 0.0, 0.0, 0.0 });
+	compare_qvel(d, m->jnt_dofadr[ids[1]], "joint1", { 0.0 });
+	compare_qvel(d, m->jnt_dofadr[ids[2]], "joint2", { 0.0 });
+	compare_qvel(d, m->jnt_dofadr[ids[3]], "ball_freejoint", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 });
+}
+
+TEST_F(PendulumEnvFixture, SetRTFactor_Increase)
+{
+	mujoco_ros_msgs::SetFloat srv;
+	srv.request.value = 1.5; // Increase real-time factor
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/set_rt_factor", true))
+	    << "Set RT factor service call failed!";
+
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/set_rt_factor", srv))
+	    << "Set RT factor service call failed!";
+	EXPECT_TRUE(srv.response.success) << "Service call was not successful!";
+
+	EXPECT_FLOAT_EQ(env_ptr->percentRealTime[env_ptr->settings_.real_time_index], 150.f)
+	    << "Real-time factor should be set to 1.5!";
+}
+
+TEST_F(PendulumEnvFixture, SetRTFactor_Decrease)
+{
+	mujoco_ros_msgs::SetFloat srv;
+	srv.request.value = 0.5; // Decrease real-time factor
+
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/set_rt_factor", true))
+	    << "Set RT factor service call failed!";
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/set_rt_factor", srv))
+	    << "Set RT factor service call failed!";
+	EXPECT_TRUE(srv.response.success) << "Service call was not successful!";
+	EXPECT_FLOAT_EQ(env_ptr->percentRealTime[env_ptr->settings_.real_time_index], 50.f)
+	    << "Real-time factor should be set to 0.5!";
+}
+
+TEST_F(PendulumEnvFixture, SetRTFactor_UnboundMode)
+{
+	mujoco_ros_msgs::SetFloat srv;
+	srv.request.value = -1; // Set to unbound mode
+
+	EXPECT_TRUE(ros::service::exists(env_ptr->getHandleNamespace() + "/set_rt_factor", true))
+	    << "Set RT factor service call failed!";
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/set_rt_factor", srv))
+	    << "Set RT factor service call failed!";
+	EXPECT_TRUE(srv.response.success) << "Service call was not successful!";
+	EXPECT_EQ(env_ptr->settings_.real_time_index, 0) << "Real-time factor should be set to unbound mode!";
+}
+
+TEST_F(PendulumEnvFixture, SetRTFactor_OutOfBounds)
+{
+	mujoco_ros_msgs::SetFloat srv;
+	srv.request.value = 1000; // Set to a value outside the boundaries
+
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/set_rt_factor", srv))
+	    << "Set RT factor service call failed!";
+	EXPECT_TRUE(srv.response.success) << "Service call was not successful!";
+	EXPECT_FLOAT_EQ(env_ptr->percentRealTime[env_ptr->settings_.real_time_index], 2000.0f)
+	    << "Real-time factor should be clipped to the maximum boundary value!";
+}
+
+TEST_F(PendulumEnvFixture, SetRTFactor_RoundUpClosest)
+{
+	mujoco_ros_msgs::SetFloat srv;
+	srv.request.value = 0.45; // Set to a value outside the boundaries
+
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/set_rt_factor", srv))
+	    << "Set RT factor service call failed!";
+	EXPECT_TRUE(srv.response.success) << "Service call was not successful!";
+	EXPECT_FLOAT_EQ(env_ptr->percentRealTime[env_ptr->settings_.real_time_index], 50.0f)
+	    << "Real-time factor should be clipped to the maximum boundary value!";
+}
+
+TEST_F(PendulumEnvFixture, SetRTFactor_RoundDownClosest)
+{
+	mujoco_ros_msgs::SetFloat srv;
+	srv.request.value = 0.44; // Set to a value outside the boundaries
+
+	EXPECT_TRUE(ros::service::call(env_ptr->getHandleNamespace() + "/set_rt_factor", srv))
+	    << "Set RT factor service call failed!";
+	EXPECT_TRUE(srv.response.success) << "Service call was not successful!";
+	EXPECT_FLOAT_EQ(env_ptr->percentRealTime[env_ptr->settings_.real_time_index], 40.0f)
+	    << "Real-time factor should be clipped to the maximum boundary value!";
+}