Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix Dynamics constructor not setting control range #10

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Fix Dynamics constructor not setting control range #10

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 10 additions & 0 deletions include/mppi/dynamics/dynamics.cu
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,16 @@ void Dynamics<CLASS_T, PARAMS_T>::paramsToDevice(bool synchronize)
}
}

template <class CLASS_T, class PARAMS_T>
void Dynamics<CLASS_T, PARAMS_T>::setDefaultControlRanges()
{
for (int i = 0; i < CONTROL_DIM; i++)
{
control_rngs_[i].x = -FLT_MAX;
control_rngs_[i].y = FLT_MAX;
}
}

template <class CLASS_T, class PARAMS_T>
void Dynamics<CLASS_T, PARAMS_T>::setControlRanges(std::array<float2, CONTROL_DIM>& control_rngs, bool synchronize)
{
Expand Down
11 changes: 5 additions & 6 deletions include/mppi/dynamics/dynamics.cuh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}

/**
Expand All @@ -124,6 +119,7 @@ protected:
Dynamics(PARAMS_T& params, cudaStream_t stream = 0) : Managed(stream)
{
setParams(params);
setDefaultControlRanges();
}

public:
Expand Down Expand Up @@ -519,6 +515,9 @@ protected:
PARAMS_T params_;

bool requires_buffer_ = false;

private:
void setDefaultControlRanges();
};

#ifdef __CUDACC__
Expand Down
49 changes: 47 additions & 2 deletions tests/dynamics/dynamics_generic_tests.cu
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,11 @@ public:
{
}

DynamicsTester(DynamicsTesterParams<STATE_DIM, CONTROL_DIM>& params, cudaStream_t stream = 0)
: PARENT_CLASS(params, stream)
{
}

DynamicsTester(std::array<float2, CONTROL_DIM> control_rngs, cudaStream_t stream = 0)
: PARENT_CLASS(control_rngs, stream)
{
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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<DynamicsTester<>, 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)
Expand Down Expand Up @@ -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);
}
}

Expand Down