diff --git a/include/mppi/dynamics/dynamics.cu b/include/mppi/dynamics/dynamics.cu index 8bde793..6e42cfa 100644 --- a/include/mppi/dynamics/dynamics.cu +++ b/include/mppi/dynamics/dynamics.cu @@ -16,6 +16,16 @@ void Dynamics::paramsToDevice(bool synchronize) } } +template +void Dynamics::setDefaultControlRanges() +{ + for (int i = 0; i < CONTROL_DIM; i++) + { + control_rngs_[i].x = -FLT_MAX; + control_rngs_[i].y = FLT_MAX; + } +} + template void Dynamics::setControlRanges(std::array& control_rngs, bool synchronize) { diff --git a/include/mppi/dynamics/dynamics.cuh b/include/mppi/dynamics/dynamics.cuh index b74e18e..466d9ab 100644 --- a/include/mppi/dynamics/dynamics.cuh +++ b/include/mppi/dynamics/dynamics.cuh @@ -97,12 +97,7 @@ protected: */ Dynamics(cudaStream_t stream = 0) : Managed(stream) { - // TODO handle at Managed - for (int i = 0; i < CONTROL_DIM; i++) - { - control_rngs_[i].x = -FLT_MAX; - control_rngs_[i].y = FLT_MAX; - } + setDefaultControlRanges(); } /** @@ -124,6 +119,7 @@ protected: Dynamics(PARAMS_T& params, cudaStream_t stream = 0) : Managed(stream) { setParams(params); + setDefaultControlRanges(); } public: @@ -519,6 +515,9 @@ protected: PARAMS_T params_; bool requires_buffer_ = false; + +private: + void setDefaultControlRanges(); }; #ifdef __CUDACC__ diff --git a/tests/dynamics/dynamics_generic_tests.cu b/tests/dynamics/dynamics_generic_tests.cu index 87fe490..77ba370 100644 --- a/tests/dynamics/dynamics_generic_tests.cu +++ b/tests/dynamics/dynamics_generic_tests.cu @@ -40,6 +40,11 @@ public: { } + DynamicsTester(DynamicsTesterParams& params, cudaStream_t stream = 0) + : PARENT_CLASS(params, stream) + { + } + DynamicsTester(std::array control_rngs, cudaStream_t stream = 0) : PARENT_CLASS(control_rngs, stream) { @@ -135,6 +140,18 @@ TEST(Dynamics, setParamsCPU) EXPECT_EQ(params_result.var_4.y, params.var_4.y); EXPECT_EQ(params_result.var_4.z, params.var_4.z); EXPECT_EQ(params_result.var_4.w, params.var_4.w); + + // Test params constructor + params.var_4.z = 1.5; + DynamicsTester<> tester_2 = DynamicsTester<>(params); + params_result = tester_2.getParams(); + + EXPECT_EQ(params_result.var_1, params.var_1); + EXPECT_EQ(params_result.var_2, params.var_2); + EXPECT_EQ(params_result.var_4.x, params.var_4.x); + EXPECT_EQ(params_result.var_4.y, params.var_4.y); + EXPECT_EQ(params_result.var_4.z, params.var_4.z); + EXPECT_EQ(params_result.var_4.w, params.var_4.w); } TEST(Dynamics, setParamsGPU) @@ -162,6 +179,19 @@ TEST(Dynamics, setParamsGPU) EXPECT_EQ(params_result.var_4.y, params.var_4.y); EXPECT_EQ(params_result.var_4.z, params.var_4.z); EXPECT_EQ(params_result.var_4.w, params.var_4.w); + + // Test params constructor + params.var_4.z = 1.5; + DynamicsTester<> tester_2 = DynamicsTester<>(params); + tester_2.GPUSetup(); + launchParameterTestKernel, DynamicsTesterParams<>>(tester_2, params_result); + + EXPECT_EQ(params_result.var_1, params.var_1); + EXPECT_EQ(params_result.var_2, params.var_2); + EXPECT_EQ(params_result.var_4.x, params.var_4.x); + EXPECT_EQ(params_result.var_4.y, params.var_4.y); + EXPECT_EQ(params_result.var_4.z, params.var_4.z); + EXPECT_EQ(params_result.var_4.w, params.var_4.w); } TEST(Dynamics, ClassConstants) @@ -192,8 +222,23 @@ TEST(Dynamics, SetControlRangesDefault) auto ranges_2 = tester.getControlRanges(); for (int i = 0; i < ranges_2.size(); i++) { - EXPECT_FLOAT_EQ(ranges[i].x, -FLT_MAX); - EXPECT_FLOAT_EQ(ranges[i].y, FLT_MAX); + EXPECT_FLOAT_EQ(ranges_2[i].x, -FLT_MAX); + EXPECT_FLOAT_EQ(ranges_2[i].y, FLT_MAX); + } + + auto params3 = tester.getParams(); + DynamicsTester<> tester_3 = DynamicsTester<>(params3); + auto ranges_3 = tester_3.getControlRanges(); + EXPECT_FLOAT_EQ(ranges_3[0].x, -FLT_MAX); + EXPECT_FLOAT_EQ(ranges_3[0].y, FLT_MAX); + + auto params4 = tester_2.getParams(); + DynamicsTester<4, 2> tester_4 = DynamicsTester<4, 2>(params4); + auto ranges_4 = tester_4.getControlRanges(); + for (int i = 0; i < ranges_4.size(); i++) + { + EXPECT_FLOAT_EQ(ranges_4[i].x, -FLT_MAX); + EXPECT_FLOAT_EQ(ranges_4[i].y, FLT_MAX); } }