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

Repair the code for the Tilt Observer #7

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Repair the code for the Tilt Observer #7

Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d16edae
constructor of TiltEstimator changed
ArnaudDmt Apr 15, 2024
fc5f9ba
adapt to changes on TiltEstimator
ArnaudDmt Aug 21, 2024
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
212 changes: 87 additions & 125 deletions src/TiltEstimator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,14 +7,14 @@
* $Id$
*/

#include <hrpUtil/Eigen3d.h>
#include <Eigen/Geometry>
#include <hrpModel/ModelLoaderUtil.h>
#include <hrpModel/Link.h>
#include <hrpModel/ModelLoaderUtil.h>
#include <hrpModel/Sensor.h>
#include <hrpUtil/Eigen3d.h>

#include <state-observation/tools/rigid-body-kinematics.hpp>
#include <hrpsys-state-observation/TiltEstimator.h>
#include <state-observation/tools/rigid-body-kinematics.hpp>

Comment on lines 10 to 18
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why did you need to change this part? Is it an automatic formatting feature?

namespace so = stateObservation;

Expand All @@ -25,72 +25,45 @@ const double gamma_const = 15;
const double sampling_time_const = 0.002;

static const std::string alpha_char = so::tools::toString(alpha_const);
static const std::string beta_char = so::tools::toString(beta_const);
static const std::string beta_char = so::tools::toString(beta_const);
static const std::string gamma_char = so::tools::toString(gamma_const);

// Module specification
// <rtc-template block="module_spec">
static const char* TiltEstimator_spec[] =
{
"implementation_id", "TiltEstimator",
"type_name", "TiltEstimator",
"description", "Tilt Estimator component",
"version", "1.0",
"vendor", "AIST",
"category", "example",
"activity_type", "DataFlowComponent",
"max_instance", "10",
"language", "C++",
"lang_type", "compile",
// Configuration variables
"conf.default.alpha", alpha_char.c_str(),
"conf.default.beta", beta_char.c_str(),
"conf.default.gamma", gamma_char.c_str(),
""
};
static const char *TiltEstimator_spec[] = {
"implementation_id", "TiltEstimator", "type_name", "TiltEstimator",
"description", "Tilt Estimator component", "version", "1.0", "vendor",
"AIST", "category", "example", "activity_type", "DataFlowComponent",
"max_instance", "10", "language", "C++", "lang_type", "compile",
// Configuration variables
"conf.default.alpha", alpha_char.c_str(), "conf.default.beta",
beta_char.c_str(), "conf.default.gamma", gamma_char.c_str(), ""};
// </rtc-template>
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please revert this modification as the new layout is less readable (maybe include guards to avoid reformatting).




TiltEstimator::TiltEstimator(RTC::Manager* manager)
: RTC::DataFlowComponentBase(manager),
// <rtc-template block="initializer">
m_accIn("acc", m_acc),
m_rateIn("rate", m_rate),
m_qIn("q", m_q),
m_rpyBEstIn("rpyBEst", m_rpyBEst),
m_pBEstIn("pBEst", m_pBEst),
m_rpyFEstIn("rpyFEst", m_rpyFEst),
m_pFEstIn("pFEst", m_pFEst),
m_rpySOut("rpyS", m_rpyS),

// </rtc-template>
m_alpha(alpha_const),
m_beta(beta_const),
m_gamma(gamma_const),
estimator_(m_alpha, m_beta, m_gamma, dt_),
m_pF_prev(so::Vector3::Zero()),
dt_(sampling_time_const),
firstSample_(true)
{
TiltEstimator::TiltEstimator(RTC::Manager *manager)
: RTC::DataFlowComponentBase(manager),
// <rtc-template block="initializer">
m_accIn("acc", m_acc), m_rateIn("rate", m_rate), m_qIn("q", m_q),
m_rpyBEstIn("rpyBEst", m_rpyBEst), m_pBEstIn("pBEst", m_pBEst),
m_rpyFEstIn("rpyFEst", m_rpyFEst), m_pFEstIn("pFEst", m_pFEst),
m_rpySOut("rpyS", m_rpyS),

// </rtc-template>
m_alpha(alpha_const), m_beta(beta_const), m_gamma(gamma_const),
estimator_(m_alpha, m_beta, m_gamma, dt_), m_pF_prev(so::Vector3::Zero()),
dt_(sampling_time_const), firstSample_(true) {
estimator_.setSamplingTime(dt_);

xk_.resize(9);
xk_ << so::Vector3::Zero(), so::Vector3::Zero(), so::Vector3(0, 0, 1); // so::Vector3(0.49198, 0.66976, 0.55622);
xk_ << so::Vector3::Zero(), so::Vector3::Zero(),
so::Vector3(0, 0, 1); // so::Vector3(0.49198, 0.66976, 0.55622);
estimator_.setState(xk_, 0);
std::cout << "Tilt Estimator constructor" << std::endl;
}

TiltEstimator::~TiltEstimator() {}


TiltEstimator::~TiltEstimator()
{
}



RTC::ReturnCode_t TiltEstimator::onInitialize()
{
RTC::ReturnCode_t TiltEstimator::onInitialize() {
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">

Expand All @@ -107,7 +80,7 @@ RTC::ReturnCode_t TiltEstimator::onInitialize()

// Set OutPort buffers
addOutPort("rpyS", m_rpySOut);

// Set service provider to Ports

// Set service consumers to Ports
Expand All @@ -119,10 +92,10 @@ RTC::ReturnCode_t TiltEstimator::onInitialize()
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("alpha", m_alpha, alpha_char.c_str());
bindParameter("beta", m_beta, beta_char.c_str());
bindParameter("beta", m_beta, beta_char.c_str());
bindParameter("gamma", m_gamma, gamma_char.c_str());

RTC::Properties& prop = getProperties();
RTC::Properties &prop = getProperties();
coil::stringTo(dt_, prop["dt"].c_str());

estimator_.setSamplingTime(dt_);
Expand All @@ -131,23 +104,24 @@ RTC::ReturnCode_t TiltEstimator::onInitialize()

// Parameters for CORBA

RTC::Manager& rtcManager = RTC::Manager::instance();
RTC::Manager &rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];

int comPos = nameServer.find(",");
if (comPos < 0) {
comPos = nameServer.length();
}

nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());

// Parameters for the internal robot model

m_robot = hrp::BodyPtr(new hrp::Body());

if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext()))) {

if (!loadBodyFromModelLoader(
m_robot, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext()))) {
std::cerr << "Failed to load model [" << prop["model"] << "] in "
<< m_profile.instance_name << std::endl;
return RTC::RTC_ERROR;
Expand All @@ -156,7 +130,6 @@ RTC::ReturnCode_t TiltEstimator::onInitialize()
return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t TiltEstimator::onFinalize()
{
Expand All @@ -177,67 +150,65 @@ RTC::ReturnCode_t TiltEstimator::onShutdown(RTC::UniqueId ec_id)
}
*/

RTC::ReturnCode_t TiltEstimator::onActivated(RTC::UniqueId ec_id) {
std::cout << m_profile.instance_name << ": onActivated(" << ec_id << ")"
<< std::endl;

RTC::ReturnCode_t TiltEstimator::onActivated(RTC::UniqueId ec_id)
{
std::cout << m_profile.instance_name << ": onActivated(" << ec_id << ")" << std::endl;

RTC::Properties& prop = getProperties();
RTC::Properties &prop = getProperties();
coil::stringTo(dt_, prop["dt"].c_str());

return RTC::RTC_OK;
}



RTC::ReturnCode_t TiltEstimator::onDeactivated(RTC::UniqueId ec_id)
{
std::cout << m_profile.instance_name << ": onDeactivated(" << ec_id << ")" << std::endl;
RTC::ReturnCode_t TiltEstimator::onDeactivated(RTC::UniqueId ec_id) {
std::cout << m_profile.instance_name << ": onDeactivated(" << ec_id << ")"
<< std::endl;

return RTC::RTC_OK;
}



RTC::ReturnCode_t TiltEstimator::onExecute(RTC::UniqueId ec_id)
{
RTC::ReturnCode_t TiltEstimator::onExecute(RTC::UniqueId ec_id) {
estimator_.setAlpha(m_alpha);
estimator_.setBeta(m_beta);
estimator_.setGamma(m_gamma);
so::Vector3 ya = so::Vector3::Zero();
so::Vector3 yg = so::Vector3::Zero();

so::Vector3 ya = so::Vector3::Zero();
so::Vector3 yg = so::Vector3::Zero();
so::Vector3 v_C = so::Vector3::Zero();

if (m_accIn.isNew()) {
m_accIn.read();
ya << m_acc.data.ax, m_acc.data.ay, m_acc.data.az;
}

if (m_rateIn.isNew()) {
m_rateIn.read();
yg << m_rate.data.avx, m_rate.data.avy, m_rate.data.avz;
}

so::Matrix3 RF = so::Matrix3::Identity();

if (m_rpyBEstIn.isNew() || m_rpyFEstIn.isNew()) {

m_rpyBEstIn.read();
m_rpyFEstIn.read();

so::Matrix3 RB = so::kine::rollPitchYawToRotationMatrix(m_rpyBEst.data.r, m_rpyBEst.data.p, m_rpyBEst.data.y);
RF = so::kine::rollPitchYawToRotationMatrix(m_rpyFEst.data.r, m_rpyFEst.data.p, m_rpyFEst.data.y);

so::Matrix3 RB = so::kine::rollPitchYawToRotationMatrix(
m_rpyBEst.data.r, m_rpyBEst.data.p, m_rpyBEst.data.y);
RF = so::kine::rollPitchYawToRotationMatrix(
m_rpyFEst.data.r, m_rpyFEst.data.p, m_rpyFEst.data.y);

so::Matrix3 R = RF.transpose() * RB;

so::Vector3 w;
if (firstSample_)
w.setZero();
else {
w = so::kine::rotationMatrixToRotationVector(R * m_robot->rootLink()->R.transpose()) / dt_;
w = so::kine::rotationMatrixToRotationVector(
R * m_robot->rootLink()->R.transpose()) /
dt_;
}

m_robot->rootLink()->R = R;
m_robot->rootLink()->w = w;
}
Expand All @@ -252,7 +223,7 @@ RTC::ReturnCode_t TiltEstimator::onExecute(RTC::UniqueId ec_id)
so::Vector3 pF;
pF << m_pFEst.data.x, m_pFEst.data.y, m_pFEst.data.z;

so::Vector3 p = RF.transpose()*(pB - pF);
so::Vector3 p = RF.transpose() * (pB - pF);

so::Vector3 v;
if (firstSample_)
Expand All @@ -267,12 +238,12 @@ RTC::ReturnCode_t TiltEstimator::onExecute(RTC::UniqueId ec_id)
v_C.setZero();
else
v_C = RF.transpose() * (pF - m_pF_prev) / dt_;

m_pF_prev = pF;
}

if (m_qIn.isNew()) {

m_qIn.read();

so::Vector dq;
Expand All @@ -284,45 +255,42 @@ RTC::ReturnCode_t TiltEstimator::onExecute(RTC::UniqueId ec_id)
dq[i] = 0;
else
dq[i] = (m_q.data[i] - m_robot->joint(i)->q) / dt_;

m_robot->joint(i)->q = m_q.data[i];
m_robot->joint(i)->dq = dq[i];
}
}

firstSample_ = false;

m_robot->calcForwardKinematics(true);
m_robot->calcForwardKinematics(true);

int k = estimator_.getCurrentTime();

hrp::Sensor *accel = m_robot->sensor(hrp::Sensor::ACCELERATION, 0);
hrp::Sensor *gyro = m_robot->sensor(hrp::Sensor::RATE_GYRO, 0);
hrp::Sensor *gyro = m_robot->sensor(hrp::Sensor::RATE_GYRO, 0);

so::Vector3 b = gyro->link->R * gyro->localPos;

so::Vector3 p_S_C = gyro->link->p + b;
so::Matrix3 R_S_C = gyro->link->R * gyro->localR;

so::Vector3 v_S_C = gyro->link->v + gyro->link->w.cross(b);
so::Vector3 w_S_C = gyro->link->w;

estimator_.setSensorPositionInC(p_S_C);
estimator_.setSensorOrientationInC(R_S_C);
estimator_.setSensorLinearVelocityInC(v_S_C);
estimator_.setSensorAngularVelocityInC(w_S_C);
estimator_.setControlOriginVelocityInW(v_C);
so::Vector3 gyroMeas(m_rate.data.avx, m_rate.data.avy, m_rate.data.avz);
so::Vector3 acceleroMeas(m_acc.data.ax, m_acc.data.ay, m_acc.data.az);

int k = estimator_.getCurrentTime();
estimator_.setMeasurement(p_S_C, v_S_C, acceleroMeas, gyroMeas, k + 1);

estimator_.setMeasurement(ya, yg, k + 1);

xk_ = estimator_.getEstimatedState(k + 1);

so::Vector3 tilt = xk_.tail(3);

so::Vector3 rpy = so::kine::rotationMatrixToRollPitchYaw(
so::kine::mergeTiltWithYaw(
tilt, so::kine::rollPitchYawToRotationMatrix(
m_rpyBEst.data.r,m_rpyBEst.data.p,m_rpyBEst.data.y)));
so::Vector3 rpy =
so::kine::rotationMatrixToRollPitchYaw(so::kine::mergeTiltWithYaw(
tilt, so::kine::rollPitchYawToRotationMatrix(
m_rpyBEst.data.r, m_rpyBEst.data.p, m_rpyBEst.data.y)));

m_rpyS.data.r = rpy(0);
m_rpyS.data.p = rpy(1);
Expand All @@ -331,11 +299,10 @@ RTC::ReturnCode_t TiltEstimator::onExecute(RTC::UniqueId ec_id)
m_rpyS.tm = m_q.tm;

m_rpySOut.write();

return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t TiltEstimator::onAborting(RTC::UniqueId ec_id)
{
Expand Down Expand Up @@ -371,16 +338,11 @@ RTC::ReturnCode_t TiltEstimator::onRateChanged(RTC::UniqueId ec_id)
}
*/

extern "C" {

extern "C"
{

void TiltEstimatorInit(RTC::Manager* manager)
{
RTC::Properties profile(TiltEstimator_spec);
manager->registerFactory(profile,
RTC::Create<TiltEstimator>,
RTC::Delete<TiltEstimator>);
}

void TiltEstimatorInit(RTC::Manager *manager) {
RTC::Properties profile(TiltEstimator_spec);
manager->registerFactory(profile, RTC::Create<TiltEstimator>,
RTC::Delete<TiltEstimator>);
}
};