Skip to content

Commit

Permalink
Trying resolving mem alloc issues
Browse files Browse the repository at this point in the history
  • Loading branch information
@isorrentino
isorrentino committed Nov 2, 2023
1 parent 808836c commit ca3c604
Show file tree
Hide file tree
Showing 4 changed files with 30 additions and 52 deletions.
14 changes: 7 additions & 7 deletions src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,11 +145,12 @@ void createUkfInput(VariablesHandler& stateVariableHandler, UKFInput& input)
input.robotBaseAcceleration = baseAcc;
}

Eigen::Ref<Eigen::VectorXd> createStateVector(UKFInput& input,
VariablesHandler& stateVariableHandler,
std::shared_ptr<iDynTree::KinDynComputations> kinDyn)
void createStateVector(UKFInput& input,
VariablesHandler& stateVariableHandler,
std::shared_ptr<iDynTree::KinDynComputations> kinDyn,
Eigen::Ref<Eigen::VectorXd> state)
{
Eigen::VectorXd state = Eigen::VectorXd::Zero(stateVariableHandler.getNumberOfVariables());
state.resize(stateVariableHandler.getNumberOfVariables());

Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size);

Expand All @@ -175,8 +176,6 @@ Eigen::Ref<Eigen::VectorXd> createStateVector(UKFInput& input,
{
state[offset + jointIndex] = jointTorques.jointTorques()[jointIndex];
}

return state;
}

void setRandomKinDynState(std::vector<SubModel>& subModelList,
Expand Down Expand Up @@ -318,7 +317,8 @@ TEST_CASE("Accelerometer Measurement Dynamics")
createUkfInput(stateVariableHandler, input);

// Create the state vector
Eigen::VectorXd state = createStateVector(input, stateVariableHandler, kinDyn);
Eigen::VectorXd state;
createStateVector(input, stateVariableHandler, kinDyn, state);

// Set the kindyn submodel state
setRandomKinDynState(subModelList,
Expand Down
36 changes: 9 additions & 27 deletions src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,12 +136,13 @@ void createUkfInput(VariablesHandler& stateVariableHandler, UKFInput& input)
input.robotBaseAcceleration = baseAcc;
}

Eigen::Ref<Eigen::VectorXd> createStateVector(UKFInput& input,
VariablesHandler& stateVariableHandler,
std::shared_ptr<iDynTree::KinDynComputations> kinDyn,
IParametersHandler::shared_ptr frictionParamHandler)
void createStateVector(UKFInput& input,
VariablesHandler& stateVariableHandler,
std::shared_ptr<iDynTree::KinDynComputations> kinDyn,
IParametersHandler::shared_ptr frictionParamHandler,
Eigen::Ref<Eigen::VectorXd> state)
{
Eigen::VectorXd state = Eigen::VectorXd::Zero(stateVariableHandler.getNumberOfVariables());
state.resize(stateVariableHandler.getNumberOfVariables());

Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size);

Expand All @@ -152,23 +153,6 @@ Eigen::Ref<Eigen::VectorXd> createStateVector(UKFInput& input,
state[offset + jointIndex] = jointVel(jointIndex);
}

// Compute friction torques from coefficients and state
offset = stateVariableHandler.getVariable("tau_F").offset;
size = stateVariableHandler.getVariable("tau_F").size;
Eigen::VectorXd k0;
REQUIRE(frictionParamHandler->getParameter("friction_k0", k0));
Eigen::VectorXd k1;
REQUIRE(frictionParamHandler->getParameter("friction_k1", k1));
Eigen::VectorXd k2;
REQUIRE(frictionParamHandler->getParameter("friction_k2", k2));
Eigen::VectorXd friction
= k0.array() * (k1.array() * jointVel.array()).tanh() + k2.array() * jointVel.array();

for (int jointIndex = 0; jointIndex < size; jointIndex++)
{
state[offset + jointIndex] = friction(jointIndex);
}

// Compute joint torques from inverse dynamics on the full model
offset = stateVariableHandler.getVariable("tau_m").offset;
size = stateVariableHandler.getVariable("tau_m").size;
Expand All @@ -182,10 +166,8 @@ Eigen::Ref<Eigen::VectorXd> createStateVector(UKFInput& input,
jointTorques);
for (int jointIndex = 0; jointIndex < size; jointIndex++)
{
state[offset + jointIndex] = jointTorques.jointTorques()[jointIndex] + friction(jointIndex);
state[offset + jointIndex] = jointTorques.jointTorques()[jointIndex];
}

return state;
}

IParametersHandler::shared_ptr createFrictionParameterHandler()
Expand Down Expand Up @@ -303,8 +285,8 @@ TEST_CASE("Friction Torque Dynamics")
createUkfInput(stateVariableHandler, input);

// Create the state vector
Eigen::VectorXd state
= createStateVector(input, stateVariableHandler, kinDyn, frictionParameterHandler);
Eigen::VectorXd state;
createStateVector(input, stateVariableHandler, kinDyn, frictionParameterHandler, state);

// Set input and state to the dynamics
tauFDynamics.setInput(input);
Expand Down
18 changes: 7 additions & 11 deletions src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,11 +145,12 @@ void createUkfInput(VariablesHandler& stateVariableHandler, UKFInput& input)
input.robotBaseAcceleration = baseAcc;
}

Eigen::Ref<Eigen::VectorXd> createStateVector(UKFInput& input,
VariablesHandler& stateVariableHandler,
std::shared_ptr<iDynTree::KinDynComputations> kinDyn)
void createStateVector(UKFInput& input,
VariablesHandler& stateVariableHandler,
std::shared_ptr<iDynTree::KinDynComputations> kinDyn,
Eigen::Ref<Eigen::VectorXd> state)
{
Eigen::VectorXd state = Eigen::VectorXd::Zero(stateVariableHandler.getNumberOfVariables());
state.resize(stateVariableHandler.getNumberOfVariables());

Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size);

Expand All @@ -175,12 +176,6 @@ Eigen::Ref<Eigen::VectorXd> createStateVector(UKFInput& input,
{
state[offset + jointIndex] = jointTorques.jointTorques()[jointIndex];
}

state.segment(stateVariableHandler.getVariable("r_leg_ft_gyro_bias").offset,
stateVariableHandler.getVariable("r_leg_ft_gyro_bias").size)
= Eigen::VectorXd::Random(stateVariableHandler.getVariable("r_leg_ft_gyro_bias").size);

return state;
}

void setRandomKinDynState(std::vector<SubModel>& subModelList,
Expand Down Expand Up @@ -305,7 +300,8 @@ TEST_CASE("Gyroscope Measurement Dynamics")
createUkfInput(stateVariableHandler, input);

// Create the state vector
Eigen::VectorXd state = createStateVector(input, stateVariableHandler, kinDyn);
Eigen::VectorXd state;
createStateVector(input, stateVariableHandler, kinDyn, state);

// Set the kindyn submodel state
setRandomKinDynState(subModelList,
Expand Down
14 changes: 7 additions & 7 deletions src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,11 +146,12 @@ void createUkfInput(VariablesHandler& stateVariableHandler, UKFInput& input)
input.robotBaseAcceleration = baseAcc;
}

Eigen::Ref<Eigen::VectorXd> createStateVector(UKFInput& input,
VariablesHandler& stateVariableHandler,
std::shared_ptr<iDynTree::KinDynComputations> kinDyn)
void createStateVector(UKFInput& input,
VariablesHandler& stateVariableHandler,
std::shared_ptr<iDynTree::KinDynComputations> kinDyn,
Eigen::Ref<Eigen::VectorXd> state)
{
Eigen::VectorXd state = Eigen::VectorXd::Zero(stateVariableHandler.getNumberOfVariables());
state.resize(stateVariableHandler.getNumberOfVariables());

Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size);

Expand All @@ -176,8 +177,6 @@ Eigen::Ref<Eigen::VectorXd> createStateVector(UKFInput& input,
{
state[offset + jointIndex] = jointTorques.jointTorques()[jointIndex];
}

return state;
}

TEST_CASE("Joint Velocity State Dynamics")
Expand Down Expand Up @@ -241,7 +240,8 @@ TEST_CASE("Joint Velocity State Dynamics")
createUkfInput(stateVariableHandler, input);

// Create the state vector
Eigen::VectorXd state = createStateVector(input, stateVariableHandler, kinDyn);
Eigen::VectorXd state;
createStateVector(input, stateVariableHandler, kinDyn, state);

// Set input and state to the dynamics
dsDynamics.setInput(input);
Expand Down

0 comments on commit ca3c604

Please sign in to comment.