Skip to content

Commit

Permalink
Fix bugs.
Browse files Browse the repository at this point in the history
  • Loading branch information
@isorrentino
isorrentino committed Apr 26, 2023
1 parent 20559de commit d183d30
Show file tree
Hide file tree
Showing 12 changed files with 253 additions and 111 deletions.
4 changes: 2 additions & 2 deletions ...ators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,8 @@ class AccelerometerMeasurementDynamics : public Dynamics
Eigen::VectorXd m_Jsdotdot;
Eigen::Vector3d m_accRg;
Eigen::Vector3d m_vCrossW;
Eigen::Vector3d linVel;
Eigen::Vector3d angVel;
Eigen::Vector3d m_linVel;
Eigen::Vector3d m_angVel;

public:
/*
Expand Down
2 changes: 1 addition & 1 deletion src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ class SubModelKinDynWrapper
* updateDynamicsVariableState updates the value of all the member variables containing
* information about the robot kinematics and dynamics
*/
bool updateDynamicsVariableState(bool isCorrectStep);
bool updateDynamicsVariableState(bool updateRobotDynamicsOnly);

/**
* @brief Compute the contribution of external contacts on the joint torques.
Expand Down
30 changes: 27 additions & 3 deletions src/Estimators/src/AccelerometerMeasurementDynamics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,6 +150,9 @@ bool RDE::AccelerometerMeasurementDynamics::finalize(const System::VariablesHand

m_accFrameVel.setZero();

m_linVel.setZero();
m_angVel.setZero();

return true;
}

Expand Down Expand Up @@ -248,17 +251,38 @@ bool RDE::AccelerometerMeasurementDynamics::update()
// J_dot nu + base_J v_dot_base + joint_J s_dotdot - acc_Rot_world gravity + bias
for (int index = 0; index < m_subModelsWithAccelerometer.size(); index++)
{
// std::cout << "------------------------------------------------------- Submodel index " << m_subModelsWithAccelerometer[index] << ", acc name " << m_name << std::endl;

m_JdotNu = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerBiasAcceleration(m_name);

// std::cout << "m_JdotNu" << std::endl;
// std::cout << m_JdotNu << std::endl;

m_JvdotBase = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerJacobian(m_name).block(0, 0, 6, 6) *
m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getBaseAcceleration().coeffs();

// std::cout << "m_JvdotBase" << std::endl;
// std::cout << m_JvdotBase << std::endl;

m_accRg = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerRotation(m_name).rotation() * m_gravity;

linVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(0, 3);
angVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(3, 6);
// std::cout << "m_accRg" << std::endl;
// std::cout << m_accRg << std::endl;

// std::cout << "accelerometer velocity\n" << m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs() << std::endl;

m_linVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(0, 3);
m_angVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(3, 3);

// std::cout << "Lin vel" << std::endl;
// std::cout << m_linVel << std::endl;
// std::cout << "Ang vel" << std::endl;
// std::cout << m_angVel << std::endl;

m_vCrossW = m_linVel.cross(m_angVel);

m_vCrossW = linVel.cross(angVel);
// std::cout << "m_vCrossW" << std::endl;
// std::cout << m_vCrossW << std::endl;

m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) = m_JdotNu.segment(0, 3) + m_JvdotBase.segment(0, 3) - m_vCrossW - m_accRg + m_bias;

Expand Down
8 changes: 4 additions & 4 deletions src/Estimators/src/SubModelKinDynWrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,7 +124,7 @@ bool RDE::SubModelKinDynWrapper::initialize(const RDE::SubModel& subModel)

bool RDE::SubModelKinDynWrapper::updateState(manif::SE3d::Tangent& robotBaseAcceleration,
Eigen::Ref<const Eigen::VectorXd> robotJointAcceleration,
bool isCorrectStep)
bool updateRobotDynamicsOnly)
{
constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::"
"updateKinDynState]";
Expand Down Expand Up @@ -169,10 +169,10 @@ bool RDE::SubModelKinDynWrapper::updateState(manif::SE3d::Tangent& robotBaseAcce
return false;
}

return updateDynamicsVariableState(isCorrectStep);
return updateDynamicsVariableState(updateRobotDynamicsOnly);
}

bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState(bool isCorrectStep)
bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState(bool updateRobotDynamicsOnly)
{
constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::"
"updateDynamicsVariableState]";
Expand Down Expand Up @@ -206,7 +206,7 @@ bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState(bool isCorrectStep)
}
}

if (isCorrectStep)
if (!updateRobotDynamicsOnly)
{
// Update accelerometer jacobians, dJnu, rotMatrix
for (int idx = 0; idx < m_subModel.getAccelerometerList().size(); idx++)
Expand Down
135 changes: 96 additions & 39 deletions src/Estimators/src/UkfMeasurement.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,10 +32,11 @@ struct RDE::UkfMeasurement::Impl
Eigen::Vector3d gravity{0, 0, -Math::StandardAccelerationOfGravitation}; /**< Gravity vector. */

Eigen::MatrixXd covarianceR; /**< Covariance matrix. */
std::size_t measurementSize; /**< Length of the measurement vector. */
int measurementSize{0}; /**< Length of the measurement vector. */
double dT; /**< Sampling time */

std::map<std::string, std::shared_ptr<Dynamics>> dynamicsList; /**< List of the dynamics composing the process model. */
std::vector<std::pair<std::string, std::shared_ptr<Dynamics>>> dynamicsList;
// std::map<std::string, std::shared_ptr<Dynamics>> dynamicsList; /**< List of the dynamics composing the process model. */

System::VariablesHandler measurementVariableHandler; /**< Variable handler describing the measurement vector. */
System::VariablesHandler stateVariableHandler; /**< Variable handler describing the state vector. */
Expand Down Expand Up @@ -74,6 +75,8 @@ struct RDE::UkfMeasurement::Impl

int offsetMeasurement; /**< Offset used to fill the measurement vector. */

bool updateRobotDynamicsOnly{false};

void unpackState()
{
jointVelocityState = currentState.segment(stateVariableHandler.getVariable("ds").offset,
Expand Down Expand Up @@ -124,13 +127,13 @@ struct RDE::UkfMeasurement::Impl
// compute joint acceleration per each sub-model containing the accelerometer
for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++)
{
// Update the kindyn wrapper object of the submodel
kinDynWrapperList[subModelIdx]->updateState(ukfInput.robotBaseAcceleration,
jointAccelerationState,
updateRobotDynamicsOnly);

if (subModelList[subModelIdx].getModel().getNrOfDOFs() > 0)
{
// Update the kindyn wrapper object of the submodel
kinDynWrapperList[subModelIdx]->updateState(ukfInput.robotBaseAcceleration,
jointAccelerationState,
false);

totalTorqueFromContacts[subModelIdx].setZero();

// Contribution of FT measurements
Expand Down Expand Up @@ -222,26 +225,44 @@ bool RDE::UkfMeasurement::finalize(const System::VariablesHandler& handler)
m_pimpl->measurementSize = 0;

// finalize all the dynamics
for (auto& [name, dynamics] : m_pimpl->dynamicsList)
// for (auto& [name, dynamics] : m_pimpl->dynamicsList)
// {
// if(!dynamics->finalize(handler))
// {
// BipedalLocomotion::log()->error("{} Error while finalizing the dynamics named {}", logPrefix, name);
// return false;
// }

// m_pimpl->measurementSize += dynamics->size();
// }
for (int indexDyn1 = 0; indexDyn1 < m_pimpl->dynamicsList.size(); indexDyn1++)
{
if(!dynamics->finalize(handler))
if(!m_pimpl->dynamicsList[indexDyn1].second->finalize(handler))
{
BipedalLocomotion::log()->error("{} Error while finalizing the dynamics named {}", logPrefix, name);
BipedalLocomotion::log()->error("{} Error while finalizing the dynamics named {}", logPrefix, m_pimpl->dynamicsList[indexDyn1].first);
return false;
}

m_pimpl->measurementSize += dynamics->size();
m_pimpl->measurementSize += m_pimpl->dynamicsList[indexDyn1].second->size();
}

// Set value of measurement covariance matrix
m_pimpl->covarianceR.resize(m_pimpl->measurementSize, m_pimpl->measurementSize);
m_pimpl->covarianceR.setZero();

// for (auto& [name, dynamics] : m_pimpl->dynamicsList)
// {
// m_pimpl->measurementVariableHandler.addVariable(name, dynamics->getCovariance().size());

for (auto& [name, dynamics] : m_pimpl->dynamicsList)
// m_pimpl->covarianceR.block(m_pimpl->measurementVariableHandler.getVariable(name).offset, m_pimpl->measurementVariableHandler.getVariable(name).offset,
// m_pimpl->measurementVariableHandler.getVariable(name).size, m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getCovariance().asDiagonal();
// }
for (int indexDyn2 = 0; indexDyn2 < m_pimpl->dynamicsList.size(); indexDyn2++)
{
m_pimpl->measurementVariableHandler.addVariable(name, dynamics->getCovariance().size());
m_pimpl->measurementVariableHandler.addVariable(m_pimpl->dynamicsList[indexDyn2].first, m_pimpl->dynamicsList[indexDyn2].second->getCovariance().size());

m_pimpl->covarianceR.block(m_pimpl->measurementVariableHandler.getVariable(name).offset, m_pimpl->measurementVariableHandler.getVariable(name).offset,
m_pimpl->measurementVariableHandler.getVariable(name).size, m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getCovariance().asDiagonal();
m_pimpl->covarianceR.block(m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset,
m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size, m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size) = m_pimpl->dynamicsList[indexDyn2].second->getCovariance().asDiagonal();
}

m_pimpl->jointVelocityState.resize(m_pimpl->kinDynFullModel->model().getNrOfDOFs());
Expand All @@ -267,6 +288,8 @@ bool RDE::UkfMeasurement::finalize(const System::VariablesHandler& handler)

m_pimpl->measurementDescription = bfl::VectorDescription(m_pimpl->measurementSize);

m_pimpl->predictedMeasurement.resize(m_pimpl->measurementSize, 2*m_pimpl->stateVariableHandler.getNumberOfVariables()+1);

m_pimpl->isFinalized = true;

return true;
Expand Down Expand Up @@ -375,7 +398,8 @@ std::unique_ptr<RDE::UkfMeasurement> RDE::UkfMeasurement::build(std::weak_ptr<co
dynamicsInstance->initialize(dynamicsGroup);

// add dynamics to the list
measurement->m_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance});
// measurement->m_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance});
measurement->m_pimpl->dynamicsList.emplace_back(dynamicsName, dynamicsInstance);
}

measurement->m_pimpl->inputDescription = bfl::VectorDescription(stateVariableHandler.getNumberOfVariables());
Expand Down Expand Up @@ -417,8 +441,6 @@ std::pair<bool, bfl::Data> RDE::UkfMeasurement::predictedMeasure(const Eigen::Re
{
constexpr auto logPrefix = "[UkfMeasurement::propagate]";

m_pimpl->predictedMeasurement.resize(m_pimpl->measurementSize, cur_states.cols());

// Check that everything is initialized and set
if (!m_pimpl->isFinalized)
{
Expand All @@ -435,9 +457,9 @@ std::pair<bool, bfl::Data> RDE::UkfMeasurement::predictedMeasure(const Eigen::Re
// Get input of ukf from provider
m_pimpl->ukfInput = m_pimpl->ukfInputProvider->getOutput();

for (int index = 0; index < cur_states.cols(); index++)
for (int sample = 0; sample < cur_states.cols(); sample++)
{
m_pimpl->currentState = cur_states.block(0, index, cur_states.rows(), 1);;
m_pimpl->currentState = cur_states.block(0, sample, cur_states.rows(), 1);;

m_pimpl->unpackState();

Expand All @@ -448,8 +470,6 @@ std::pair<bool, bfl::Data> RDE::UkfMeasurement::predictedMeasure(const Eigen::Re
m_pimpl->jointVelocityState,
m_pimpl->gravity);

m_pimpl->unpackState();

if (!m_pimpl->updateState())
{
BipedalLocomotion::log()->error("{} The joint accelerations are not updated.", logPrefix);
Expand All @@ -462,23 +482,39 @@ std::pair<bool, bfl::Data> RDE::UkfMeasurement::predictedMeasure(const Eigen::Re
// This could be parallelized

// Update all the dynamics
for (auto& [name, dynamics] : m_pimpl->dynamicsList)
// for (auto& [name, dynamics] : m_pimpl->dynamicsList)
// {
// std::cout << "variable name = " << name << " , variable offset = " << m_pimpl->measurementVariableHandler.getVariable(name).offset << std::endl;

// dynamics->setState(m_pimpl->currentState);

// dynamics->setInput(m_pimpl->ukfInput);

// if (!dynamics->update())
// {
// BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name);
// throw std::runtime_error("Error");
// }

// m_pimpl->tempPredictedMeas.segment(m_pimpl->measurementVariableHandler.getVariable(name).offset,
// m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable();
// }
for (int indexDyn = 0; indexDyn < m_pimpl->dynamicsList.size(); indexDyn++)
{
dynamics->setState(m_pimpl->currentState);
m_pimpl->dynamicsList[indexDyn].second->setState(m_pimpl->currentState);

dynamics->setInput(m_pimpl->ukfInput);
m_pimpl->dynamicsList[indexDyn].second->setInput(m_pimpl->ukfInput);

if (!dynamics->update())
if (!m_pimpl->dynamicsList[indexDyn].second->update())
{
BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name);
BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, m_pimpl->dynamicsList[indexDyn].first);
throw std::runtime_error("Error");
}

m_pimpl->tempPredictedMeas.segment(m_pimpl->measurementVariableHandler.getVariable(name).offset,
m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable();
m_pimpl->tempPredictedMeas.segment(m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn].first).offset,
m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn].first).size) = m_pimpl->dynamicsList[indexDyn].second->getUpdatedVariable();
}
m_pimpl->predictedMeasurement.block(0, index, m_pimpl->measurementSize, 1) = m_pimpl->tempPredictedMeas;

m_pimpl->predictedMeasurement.block(0, sample, m_pimpl->measurementSize, 1) = m_pimpl->tempPredictedMeas;
}

return std::make_pair(true, m_pimpl->predictedMeasurement);
Expand Down Expand Up @@ -517,25 +553,46 @@ bool RDE::UkfMeasurement::freeze(const bfl::Data& data)

m_pimpl->measurementMap = bfl::any::any_cast<std::map<std::string, Eigen::VectorXd>>(data);

for (auto& [name, dynamics] : m_pimpl->dynamicsList)
// for (auto& [name, dynamics] : m_pimpl->dynamicsList)
// {
// m_pimpl->offsetMeasurement = m_pimpl->measurementVariableHandler.getVariable(name).offset;

// if(m_pimpl->measurementMap.count(name) == 0)
// {
// BipedalLocomotion::log()->error("{} Measurement with name `{}` not found.", logPrefix, name);
// return false;
// }

// // If more sub-models share the same accelerometer or gyroscope sensor, the measurement vector is concatenated
// // a number of times equal to the number of sub-models using the sensor.
// while(m_pimpl->offsetMeasurement <
// (m_pimpl->measurementVariableHandler.getVariable(name).offset + m_pimpl->measurementVariableHandler.getVariable(name).size))
// {
// m_pimpl->measurement.segment(m_pimpl->offsetMeasurement, m_pimpl->measurementMap[name].size())
// = m_pimpl->measurementMap[name];

// m_pimpl->offsetMeasurement += m_pimpl->measurementMap[name].size();
// }
// }
for (int index = 0; index < m_pimpl->dynamicsList.size(); index++)
{
m_pimpl->offsetMeasurement = m_pimpl->measurementVariableHandler.getVariable(name).offset;
m_pimpl->offsetMeasurement = m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[index].first).offset;

if(m_pimpl->measurementMap.count(name) == 0)
if(m_pimpl->measurementMap.count(m_pimpl->dynamicsList[index].first) == 0)
{
BipedalLocomotion::log()->error("{} Measurement with name `{}` not found.", logPrefix, name);
BipedalLocomotion::log()->error("{} Measurement with name `{}` not found.", logPrefix, m_pimpl->dynamicsList[index].first);
return false;
}

// If more sub-models share the same accelerometer or gyroscope sensor, the measurement vector is concatenated
// a number of times equal to the number of sub-models using the sensor.
while(m_pimpl->offsetMeasurement <
(m_pimpl->measurementVariableHandler.getVariable(name).offset + m_pimpl->measurementVariableHandler.getVariable(name).size))
(m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[index].first).offset + m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[index].first).size))
{
m_pimpl->measurement.segment(m_pimpl->offsetMeasurement, m_pimpl->measurementMap[name].size())
= m_pimpl->measurementMap[name];
m_pimpl->measurement.segment(m_pimpl->offsetMeasurement, m_pimpl->measurementMap[m_pimpl->dynamicsList[index].first].size())
= m_pimpl->measurementMap[m_pimpl->dynamicsList[index].first];

m_pimpl->offsetMeasurement += m_pimpl->measurementMap[name].size();
m_pimpl->offsetMeasurement += m_pimpl->measurementMap[m_pimpl->dynamicsList[index].first].size();
}
}

Expand Down
Loading

0 comments on commit d183d30

Please sign in to comment.