diff --git a/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp index 8f9e4e8a07..8e10789ed2 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp @@ -145,11 +145,12 @@ void createUkfInput(VariablesHandler& stateVariableHandler, UKFInput& input) input.robotBaseAcceleration = baseAcc; } -Eigen::Ref createStateVector(UKFInput& input, - VariablesHandler& stateVariableHandler, - std::shared_ptr kinDyn) +void createStateVector(UKFInput& input, + VariablesHandler& stateVariableHandler, + std::shared_ptr kinDyn, + Eigen::Ref state) { - Eigen::VectorXd state = Eigen::VectorXd::Zero(stateVariableHandler.getNumberOfVariables()); + state.resize(stateVariableHandler.getNumberOfVariables()); Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); @@ -175,8 +176,6 @@ Eigen::Ref createStateVector(UKFInput& input, { state[offset + jointIndex] = jointTorques.jointTorques()[jointIndex]; } - - return state; } void setRandomKinDynState(std::vector& subModelList, @@ -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, diff --git a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp index 24076ec1fd..891b2dceb2 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp @@ -136,12 +136,13 @@ void createUkfInput(VariablesHandler& stateVariableHandler, UKFInput& input) input.robotBaseAcceleration = baseAcc; } -Eigen::Ref createStateVector(UKFInput& input, - VariablesHandler& stateVariableHandler, - std::shared_ptr kinDyn, - IParametersHandler::shared_ptr frictionParamHandler) +void createStateVector(UKFInput& input, + VariablesHandler& stateVariableHandler, + std::shared_ptr kinDyn, + IParametersHandler::shared_ptr frictionParamHandler, + Eigen::Ref state) { - Eigen::VectorXd state = Eigen::VectorXd::Zero(stateVariableHandler.getNumberOfVariables()); + state.resize(stateVariableHandler.getNumberOfVariables()); Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); @@ -152,23 +153,6 @@ Eigen::Ref 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; @@ -182,10 +166,8 @@ Eigen::Ref 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() @@ -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); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp index 129c04515f..37a3cad8e0 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp @@ -145,11 +145,12 @@ void createUkfInput(VariablesHandler& stateVariableHandler, UKFInput& input) input.robotBaseAcceleration = baseAcc; } -Eigen::Ref createStateVector(UKFInput& input, - VariablesHandler& stateVariableHandler, - std::shared_ptr kinDyn) +void createStateVector(UKFInput& input, + VariablesHandler& stateVariableHandler, + std::shared_ptr kinDyn, + Eigen::Ref state) { - Eigen::VectorXd state = Eigen::VectorXd::Zero(stateVariableHandler.getNumberOfVariables()); + state.resize(stateVariableHandler.getNumberOfVariables()); Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); @@ -175,12 +176,6 @@ Eigen::Ref 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& subModelList, @@ -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, diff --git a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp index 05ad718254..e8d4b86481 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp @@ -146,11 +146,12 @@ void createUkfInput(VariablesHandler& stateVariableHandler, UKFInput& input) input.robotBaseAcceleration = baseAcc; } -Eigen::Ref createStateVector(UKFInput& input, - VariablesHandler& stateVariableHandler, - std::shared_ptr kinDyn) +void createStateVector(UKFInput& input, + VariablesHandler& stateVariableHandler, + std::shared_ptr kinDyn, + Eigen::Ref state) { - Eigen::VectorXd state = Eigen::VectorXd::Zero(stateVariableHandler.getNumberOfVariables()); + state.resize(stateVariableHandler.getNumberOfVariables()); Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); @@ -176,8 +177,6 @@ Eigen::Ref createStateVector(UKFInput& input, { state[offset + jointIndex] = jointTorques.jointTorques()[jointIndex]; } - - return state; } TEST_CASE("Joint Velocity State Dynamics") @@ -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);