extent_protocol::status
extent_client::getattr(extent_protocol::extentid_t eid,
extent_protocol::attr &attr) {
if (isdir(eid)) {
return cl->call(extent_protocol::getattr, eid, attr);
}
decltype(cache_.begin()) iter;
{
std::unique_lock<std::mutex> m_(cache_mtx_);
iter = cache_.find(eid);
if (cache_.end() != iter && (iter->second.status & extent_protocol::ATTR_CACHED) ) {
attr = iter->second.attr;
return extent_protocol::OK;
}
}
extent_protocol::status ret = extent_protocol::OK;
ret = cl->call(extent_protocol::getattr, eid, attr);
{
std::unique_lock<std::mutex> m_(cache_mtx_);
if (cache_.end() != iter) {
iter->second.attr = attr;
iter->second.status |= extent_protocol::ATTR_CACHED;
} else {
cache_[eid] = {eid, attr, std::string(), false, extent_protocol::ATTR_CACHED};
}
}
return ret;
}
extent_protocol::status
extent_client::get(extent_protocol::extentid_t eid, std::string &buf) {
decltype(cache_.begin()) iter;
{
std::unique_lock<std::mutex> m_(cache_mtx_);
iter = cache_.find(eid);
if (cache_.end() != iter && (iter->second.status == extent_protocol::ALL_CACHED) ) {
buf = iter->second.buf;
iter->second.attr.atime = std::time(nullptr);
return extent_protocol::OK;
}
}
cl->call(extent_protocol::get, eid, buf);
{
extent_protocol::attr attr;
getattr(eid, attr);
std::unique_lock<std::mutex> m_(cache_mtx_);
if (cache_.end() != iter) {
iter->second.buf = buf;
iter->second.attr.atime = std::time(nullptr);
iter->second.status = extent_protocol::ALL_CACHED;
} else {
cache_[eid] = {eid, attr, buf, false, extent_protocol::ALL_CACHED};
}
}
return extent_protocol::OK;
}
int main(int ac, char **av)
{
int i, j, k, n;
unsigned char dat0[8] = { 0x01, 0xf2, 0x03, 0x04, 0x05, 0x06, 0xf7, 0x08 };
long long *datp = (long long *)&dat0;
int16_t dat1[8] = { 0x10, 0x20, -0x130, -0x140, 0x50, -0x160, -0x170, 0x80 };
volatile uint8_t *rfp = dat0;
volatile int16_t *bp = dat1;
unsigned char ans1[8], ans2[8];
n = 0;
for( i=-32768; i<32768; ++i ) {
j = 0;
while( j < 256 ) {
for( k=0; k<8; ++k ) {
dat0[k] = i;
dat1[k] = j++;
}
movq_m2r(m_(&rfp[0]),mm1); /* rfp[0..7] */
pxor_r2r(mm3,mm3);
pxor_r2r(mm4,mm4);
movq_m2r(m_(&bp[0]),mm5); /* bp[0..3] */
movq_r2r(mm1,mm2);
movq_m2r(m_(&bp[4]),mm6); /* bp[4..7] */
punpcklbw_r2r(mm3,mm1); /* rfp[0,2,4,6] */
punpckhbw_r2r(mm3,mm2); /* rfp[1,3,5,7] */
paddsw_r2r(mm5,mm1); /* bp[0..3] */
paddsw_r2r(mm6,mm2); /* bp[4..7] */
pcmpgtw_r2r(mm1,mm3);
pcmpgtw_r2r(mm2,mm4);
pandn_r2r(mm1,mm3);
pandn_r2r(mm2,mm4);
packuswb_r2r(mm4,mm3);
movq_r2m(mm3,m_(&ans1[0]));
emms();
ans2[0] = clip(bp[0] + rfp[0]);
ans2[1] = clip(bp[1] + rfp[1]);
ans2[2] = clip(bp[2] + rfp[2]);
ans2[3] = clip(bp[3] + rfp[3]);
ans2[4] = clip(bp[4] + rfp[4]);
ans2[5] = clip(bp[5] + rfp[5]);
ans2[6] = clip(bp[6] + rfp[6]);
ans2[7] = clip(bp[7] + rfp[7]);
if( *(uint64_t *)&ans1[0] != *(uint64_t *)&ans2[0] )
{
printf(" i=%5d %02x %02x %02x %02x %02x %02x %02x %02x\n", i,
ans1[0], ans1[1], ans1[2], ans1[3], ans1[4], ans1[5], ans1[6], ans1[7]);
printf(" j=%5d %02x %02x %02x %02x %02x %02x %02x %02x\n", j,
ans2[0], ans2[1], ans2[2], ans2[3], ans2[4], ans2[5], ans2[6], ans2[7]);
// exit(0);
}
n += 8;
}
}
printf("n=%d\n",n);
return 0;
}
extent_protocol::status extent_client::flush(extent_protocol::extentid_t id) {
if (isdir(id) ) {
return extent_protocol::OK;
}
extent_cache cache;
{
std::unique_lock<std::mutex> m_(cache_mtx_);
auto iter = cache_.find(id);
if (cache_.end() != iter) {
cache = iter->second;
cache_.erase(iter);
} else {
return extent_protocol::OK;
}
}
if (cache.modified) {
int r;
cl->call(extent_protocol::flush, id, cache.buf, cache.attr, cache.status, r);
}
return extent_protocol::OK;
}
void
lock_server_cache_rsm::unmarshal_state(std::string state)
{
unmarshall rep(state);
using ull = unsigned long long;
std::lock_guard<std::mutex> m_(mtxtb);
locktb.clear();
ull tbsize;
rep >> tbsize;
for (size_t i = 0; i < tbsize; ++i) {
ull lid;
int status;
std::string owner;
ull retrysize;
rep >> lid >> status >> owner >> retrysize;
auto rlk = new server_lock(lid);
rlk->status = status;
rlk->owner = owner;
std::string retryer;
for (size_t i = 0; i < retrysize; ++i) {
rep >> retryer;
rlk->retry.push(retryer);
}
ull xid;
rep >> xid;
rlk->xid = xid;
locktb[lid] = rlk;
}
}
std::string
lock_server_cache_rsm::marshal_state()
{
//std::ostringstream ost;
//std::string r;
marshall rep;
using ull = unsigned long long;
std::lock_guard<std::mutex> m_(mtxtb);
rep << static_cast<ull>(locktb.size());
for (auto &lock : locktb) {
ull lid = lock.second->lid;
int status = lock.second->status;
std::string owner = lock.second->owner;
rep << lid << status << owner;
ull retrysize = lock.second->retry.size();
rep << retrysize;
for (size_t i = 0; i < retrysize; ++i) {
std::string retryer = lock.second->retry.front();
rep << retryer;
lock.second->retry.pop();
lock.second->retry.push(retryer);
}
ull xid = lock.second->xid;
rep << xid;
}
return rep.str();
}
CommonTime CommonTimeAddMinute(CommonTime m_time,double min)
{
cJulian m_(m_time.year,m_time.month,m_time.day,
m_time.hour,m_time.minute,m_time.second);
m_.addMin(min);
return cJulian2CommonTime(m_);
}
double
SpectrogramData::value( double x, double y ) const
{
size_t ix = dx( x );
size_t iy = dy( y );
return m_( ix, iy );
}
static void
compose_console_string (RIP_MANAGER_INFO *rmi, TRACK_INFO* ti)
{
mchar console_string[SR_MAX_PATH];
msnprintf (console_string, SR_MAX_PATH, m_S m_(" - ") m_S,
ti->artist, ti->title);
string_from_gstring (rmi, rmi->filename, SR_MAX_PATH, console_string,
CODESET_LOCALE);
}
void
SpectrogramData::updateData()
{
m_.clear();
size_t id1 = std::distance( spectra_->x().begin(), std::lower_bound( spectra_->x().begin(), spectra_->x().end(), xlimits_.first ) );
size_t id2 = std::distance( spectra_->x().begin(), std::lower_bound( spectra_->x().begin(), spectra_->x().end(), xlimits_.second ) );
size1_ = std::min( m_.size1(), id2 - id1 + 1 );
double z_max = std::numeric_limits<double>::lowest();
size_t id = 0;
for ( auto& ms: *spectra_ ) {
double x = spectra_->x()[ id++ ];
if ( xlimits_.first <= x && x <= xlimits_.second ) {
size_t ix = dx(x);
adcontrols::segment_wrapper< const adcontrols::MassSpectrum > segs( *ms );
for ( auto& seg: segs ) {
for ( size_t i = 0; i < seg.size(); ++i ) {
double m = seg.getMass( i );
if ( ylimits_.first < m && m < ylimits_.second ) {
size_t iy = dy(m);
m_( ix, iy ) += seg.getIntensity( i );
z_max = std::max( z_max, m_( ix, iy ) );
}
}
}
}
}
setInterval( Qt::XAxis, QwtInterval( spectra_->x_left(), spectra_->x_right() ) ); // time (sec -> min)
setInterval( Qt::YAxis, QwtInterval( spectra_->lower_mass(), spectra_->upper_mass() ) ); // m/z
#if 0 // normaize
m_ /= ( z_max / 1000.0 );
setInterval( Qt::ZAxis, QwtInterval( 0.0, 1000 ) );
#else
setInterval( Qt::ZAxis, QwtInterval( 0.0, z_max ) );
#endif
}
extent_protocol::status
extent_client::remove(extent_protocol::extentid_t eid) {
{
std::unique_lock<std::mutex> m_(cache_mtx_);
auto iter = cache_.find(eid);
if (cache_.end() != iter) {
cache_.erase(iter);
}
}
extent_protocol::status ret = extent_protocol::OK;
int r;
ret = cl->call(extent_protocol::remove, eid, r);
return ret;
}
void compose_metadata(RIP_MANAGER_INFO * rmi, TRACK_INFO * ti)
{
int num_bytes;
unsigned char num_16_bytes;
mchar w_composed_metadata[MAX_METADATA_LEN + 1];
if (ti->have_track_info) {
if (ti->artist[0]) {
msnprintf(w_composed_metadata, MAX_METADATA_LEN,
m_("StreamTitle='") m_S m_(" - ") m_S m_("';"), ti->artist, ti->title);
} else {
msnprintf(w_composed_metadata, MAX_METADATA_LEN, m_("StreamTitle='") m_S m_("';"), ti->title);
}
} else {
debug_printf("No track info when composing relay metadata\n");
}
debug_printf("Converting relay string to char\n");
num_bytes = string_from_gstring(rmi, &ti->composed_metadata[1],
MAX_METADATA_LEN, w_composed_metadata, CODESET_RELAY);
ti->composed_metadata[MAX_METADATA_LEN] = 0; // note, not LEN-1
num_16_bytes = (num_bytes + 15) / 16;
ti->composed_metadata[0] = num_16_bytes;
}
void ObjectBase::__removeFromDebugList(ObjectBase *base)
{
#ifdef OXYGINE_DEBUG_TRACE_LEAKS
if (_tracingLeaks)
{
MutexAutoLock m_(getMutex());
if (base->__traceLeak)
{
base->__traceLeak = false;
__createdObjects& objs = __getCreatedObjects();
__createdObjects::iterator i = std::find(objs.begin(), objs.end(), base);
OX_ASSERT(i != objs.end());
objs.erase(i);
}
}
#endif
}
extent_protocol::status
extent_client::put(extent_protocol::extentid_t eid, std::string buf) {
decltype(cache_.begin()) iter;
std::unique_lock<std::mutex> m_(cache_mtx_);
iter = cache_.find(eid);
if (cache_.end() != iter && (iter->second.status == extent_protocol::ALL_CACHED)) {
iter->second.buf = buf;
iter->second.attr.mtime = iter->second.attr.atime = std::time(nullptr);
iter->second.attr.size = buf.size();
iter->second.modified = true;
return extent_protocol::OK;
}
extent_protocol::status ret = extent_protocol::OK;
extent_protocol::attr attr;
attr.atime = attr.mtime = attr.ctime = std::time(nullptr);
attr.size = buf.size();
cache_[eid] = {eid, attr, buf, true, extent_protocol::ALL_CACHED};
return ret;
}
inline value_type& operator()(size_type i, size_type j) {
return m_(j,i);
}
/* This mega-function reads in the rules file, and loads
all the rules into the rmi->parse_rules data structure */
void init_metadata_parser(RIP_MANAGER_INFO * rmi, char *rules_file)
{
FILE *fp;
int ri; /* Rule index */
int rn; /* Number of rules allocated */
if (!rules_file || !*rules_file) {
use_default_rules(rmi);
return;
}
fp = fopen(rules_file, "r");
if (!fp) {
use_default_rules(rmi);
return;
}
rmi->parse_rules = 0;
ri = rn = 0;
while (1) {
char rule_buf[MAX_RULE_SIZE];
char match_buf[MAX_RULE_SIZE];
char subst_buf[MAX_RULE_SIZE];
mchar w_match_buf[MAX_RULE_SIZE];
mchar w_subst_buf[MAX_RULE_SIZE];
char *rbp;
char *rp;
int got_command;
int rc;
/* Allocate memory for rule, if necessary. */
/* If there are no more rules in the file, */
/* this rule will become the sentinel null rule */
if (ri + 1 != rn) {
rmi->parse_rules = realloc(rmi->parse_rules, (ri + 1) * sizeof(Parse_Rule));
memset(&rmi->parse_rules[ri], 0, sizeof(Parse_Rule));
rn = ri + 1;
}
/* Get next line from file */
rp = fgets(rule_buf, 2048, fp);
if (!rp)
break;
/* Skip leading whitespace */
rbp = rule_buf;
while (*rbp && isspace(*rbp))
rbp++;
if (!*rbp)
continue;
/* Get command */
got_command = 0;
switch (*rbp++) {
case 'm':
got_command = 1;
rmi->parse_rules[ri].cmd = PARSERULE_CMD_MATCH;
break;
case 's':
got_command = 1;
rmi->parse_rules[ri].cmd = PARSERULE_CMD_SUBST;
break;
case '#':
got_command = 0;
break;
default:
got_command = 0;
printf("Warning: malformed command in rules file:\n%s\n", rule_buf);
break;
}
if (!got_command)
continue;
/* Skip past fwd slash */
if (*rbp++ != '/') {
printf("Warning: malformed command in rules file:\n%s\n", rule_buf);
continue;
}
/* Parse match string */
rbp = parse_escaped_string(match_buf, rbp);
debug_printf("match_buf=%s\n", match_buf);
if (!rbp) {
printf("Warning: malformed command in rules file:\n%s\n", rule_buf);
continue;
}
/* Parse subst string */
if (rmi->parse_rules[ri].cmd == PARSERULE_CMD_SUBST) {
rbp = parse_escaped_string(subst_buf, rbp);
debug_printf("subst_buf=%s\n", subst_buf);
if (!rbp) {
printf("Warning: malformed command in rules file:\n%s\n", rule_buf);
continue;
}
}
/* Parse flags */
rc = parse_flags(&rmi->parse_rules[ri], rbp);
if (!rc) {
printf("Warning: malformed command in rules file:\n%s\n", rule_buf);
continue;
}
/* Compile the rule */
debug_printf("Compiling the rule\n");
gstring_from_string(rmi, w_match_buf, MAX_RULE_SIZE, match_buf, CODESET_UTF8);
if (!compile_rule(&rmi->parse_rules[ri], w_match_buf)) {
printf("Warning: malformed regular expression:\n%s\n", match_buf);
continue;
}
/* Copy rule strings */
debug_printf("Copying rule string (1)\n");
debug_mprintf(m_("String is ") m_S m_("\n"), w_match_buf);
rmi->parse_rules[ri].match = mstrdup(w_match_buf);
debug_printf("Copying rule string (2)\n");
if (rmi->parse_rules[ri].cmd == PARSERULE_CMD_SUBST) {
debug_printf("Copying rule string (3)\n");
gstring_from_string(rmi, w_subst_buf, MAX_RULE_SIZE, subst_buf, CODESET_UTF8);
debug_printf("Copying rule string (4)\n");
rmi->parse_rules[ri].subst = mstrdup(w_subst_buf);
debug_printf("Copying rule string (5)\n");
}
debug_printf("End of loop\n");
ri++;
}
fclose(fp);
}
void parse_metadata(RIP_MANAGER_INFO * rmi, TRACK_INFO * ti)
{
int i;
int eflags;
int rc;
int matched;
mchar query_string[MAX_TRACK_LEN];
Parse_Rule *rulep;
/* Has any m/.../s rule matched? */
BOOL save_track_matched = FALSE;
/* Has any m/.../x rule matched? */
BOOL exclude_track_matched = FALSE;
ti->artist[0] = 0;
ti->title[0] = 0;
ti->album[0] = 0;
ti->composed_metadata[0] = 0;
ti->save_track = TRUE;
/* Loop through rules, if we find a matching rule, then use it */
/* For now, only default rules supported with ascii
regular expressions. */
debug_printf("Converting query string to wide\n");
gstring_from_string(rmi, query_string, MAX_TRACK_LEN, ti->raw_metadata, CODESET_METADATA);
for (rulep = rmi->parse_rules; rulep->cmd; rulep++) {
#if !defined (USE_GLIB_REGEX)
regmatch_t pmatch[MAX_SUBMATCHES + 1];
#endif
eflags = 0;
if (rulep->cmd == PARSERULE_CMD_MATCH) {
debug_mprintf(m_("Testing match rule: ") m_S m_(" vs. ") m_S m_("\n"),
query_string, rulep->match);
if (rulep->flags & PARSERULE_SKIP) {
#if defined (USE_GLIB_REGEX)
rc = g_regex_match(rulep->reg, query_string, 0, NULL);
matched = rc;
#else
rc = mregexec(rulep->reg, query_string, 0, NULL, eflags);
matched = !rc;
#endif
if (!matched) {
continue;
}
/* GCS FIX: We need to return to the
caller that the metadata should be dropped. */
debug_printf("Skip rule matched\n");
ti->save_track = FALSE;
ti->have_track_info = 0;
return;
} else if (rulep->flags & PARSERULE_SAVE) {
#if defined (USE_GLIB_REGEX)
rc = g_regex_match(rulep->reg, query_string, 0, NULL);
matched = rc;
#else
rc = mregexec(rulep->reg, query_string, 0, NULL, eflags);
matched = !rc;
#endif
if (!matched) {
if (!save_track_matched)
ti->save_track = FALSE;
continue;
}
if (!exclude_track_matched) {
ti->save_track = TRUE;
save_track_matched = TRUE;
}
} else if (rulep->flags & PARSERULE_EXCLUDE) {
#if defined (USE_GLIB_REGEX)
rc = g_regex_match(rulep->reg, query_string, 0, NULL);
matched = rc;
#else
rc = mregexec(rulep->reg, query_string, 0, NULL, eflags);
matched = !rc;
#endif
if (matched && !save_track_matched) {
/* Rule matched => Exclude track */
ti->save_track = FALSE;
exclude_track_matched = TRUE;
}
} else {
#if defined (USE_GLIB_REGEX)
GMatchInfo *match_info;
gint nmatch;
rc = g_regex_match(rulep->reg, query_string, 0, &match_info);
if (rc == 0) {
/* Didn't match rule. */
continue;
}
nmatch = g_match_info_get_match_count(match_info);
debug_printf("Got %d matches\n", nmatch);
for (i = 0; i < nmatch; i++) {
gchar *match = g_match_info_fetch(match_info, i);
debug_printf("[%d] = %s\n", i, match);
g_free(match);
}
copy_rule_result(ti->artist, match_info, rulep->artist_idx);
copy_rule_result(ti->title, match_info, rulep->title_idx);
copy_rule_result(ti->album, match_info, rulep->album_idx);
copy_rule_result(ti->track_p, match_info, rulep->trackno_idx);
copy_rule_result(ti->year, match_info, rulep->year_idx);
g_match_info_free(match_info);
#else
eflags = 0;
rc = mregexec(rulep->reg, query_string, MAX_SUBMATCHES + 1, pmatch, eflags);
if (rc != 0) {
/* Didn't match rule. */
continue;
}
for (i = 0; i < MAX_SUBMATCHES + 1; i++) {
debug_printf("pmatch[%d]: (so,eo) = (%d,%d)\n", i,
pmatch[i].rm_so, pmatch[i].rm_eo);
}
copy_rule_result(ti->artist, query_string, pmatch, rulep->artist_idx);
copy_rule_result(ti->title, query_string, pmatch, rulep->title_idx);
copy_rule_result(ti->album, query_string, pmatch, rulep->album_idx);
copy_rule_result(ti->track_p, query_string, pmatch, rulep->trackno_idx);
copy_rule_result(ti->year, query_string, pmatch, rulep->year_idx);
#endif
ti->have_track_info = 1;
compose_metadata(rmi, ti);
debug_mprintf(m_("Parsed track info.\n")
m_("ARTIST: ") m_S m_("\n")
m_("TITLE: ") m_S m_("\n")
m_("ALBUM: ") m_S m_("\n")
m_("TRACK: ") m_S m_("\n")
m_("YEAR: ") m_S m_("\n"),
ti->artist, ti->title, ti->album, ti->track_p, ti->year);
return;
}
} else if (rulep->cmd == PARSERULE_CMD_SUBST) {
#if defined (USE_GLIB_REGEX)
GMatchInfo *match_info;
gint start_pos, end_pos;
gchar *tmp, *subst_string;
debug_mprintf(m_("Testing subst rule: ") m_S m_(" vs. ") m_S m_("\n"),
query_string, rulep->match);
rc = g_regex_match(rulep->reg, query_string, 0, &match_info);
if (rc == 0) {
/* Didn't match rule. */
continue;
}
rc = g_match_info_fetch_pos(match_info, 0, &start_pos, &end_pos);
if (!rc) {
debug_printf("g_match_info_fetch_pos returned 0\n");
g_match_info_free(match_info);
continue;
}
debug_printf("Matched at (%d,%d)\n", start_pos, end_pos);
if (start_pos == -1) {
g_match_info_free(match_info);
continue;
}
tmp = g_strndup(query_string, start_pos);
tmp[start_pos] = 0;
subst_string = g_strconcat(tmp, rulep->subst, &tmp[end_pos], NULL);
g_free(tmp);
g_match_info_free(match_info);
mstrncpy(query_string, subst_string, MAX_TRACK_LEN);
#else
mchar subst_string[MAX_TRACK_LEN];
int used, left;
debug_mprintf(m_("Testing subst rule: ") m_S m_(" vs. ") m_S m_("\n"),
query_string, rulep->match);
rc = mregexec(rulep->reg, query_string, 1, pmatch, eflags);
if (rc != 0) {
/* Didn't match rule. */
continue;
}
/* Update the query string and continue. */
debug_printf("Matched at (%d,%d)\n", pmatch[0].rm_so, pmatch[0].rm_eo);
mstrncpy(subst_string, query_string, pmatch[0].rm_so + 1);
debug_mprintf(m_("(1) subst_string = ") m_S m_("\n"), subst_string);
used = pmatch[0].rm_so;
left = MAX_TRACK_LEN - used;
mstrncpy(subst_string + used, rulep->subst, left);
debug_mprintf(m_("(2) subst_string = ") m_S m_("\n"), subst_string);
used += mstrlen(rulep->subst);
left = MAX_TRACK_LEN - used;
mstrncpy(subst_string + used, query_string + pmatch[0].rm_eo, left);
debug_mprintf(m_("(3) subst_string = ") m_S m_("\n"), subst_string);
mstrncpy(query_string, subst_string, MAX_TRACK_LEN);
debug_mprintf(m_("(4) query_string = ") m_S m_("\n"), query_string);
#endif
}
}
debug_printf("Fell through while parsing data...\n");
mstrncpy(ti->title, query_string, MAX_TRACK_LEN);
ti->have_track_info = 1;
compose_metadata(rmi, ti);
}
void AdmittanceController::initialize(const double Ts, const KDL::JntArray& q_min, const KDL::JntArray& q_max, const std::vector<double>& mass, const std::vector<double>& damping) {
// Resize relevant parameters
uint num_joints = 15;
Ts_ = Ts;
// ToDo: Make variables (use M and D as inputs?)
m_.resize(num_joints);
d_.resize(num_joints);
k_.resize(num_joints);
om_.resize(num_joints);
a_.resize(2,num_joints);
b_.resize(2,num_joints);
tau_previous_.resize(num_joints);
qdot_reference_previous_.resize(num_joints);
q_min_.resize(num_joints);
q_max_.resize(num_joints);
// Set parameters
/*(hardcoded)
for (uint i = 0; i<num_joints; i++) {
m_(i) = 0.1;
d_(i) = 1;
}
m_(7) = 1; // Torso
d_(7) = 10; // Torso
*/
for (uint i = 0; i < num_joints; i++) {
m_(i) = mass[i];
d_(i) = damping[i];
}
for (uint i = 0; i<num_joints; i++) {
k_(i) = 1/d_(i);
om_(i) = d_(i)/m_(i);
double wp = om_(i) + eps;
double alpha = wp/(tan(wp*Ts_/2));
double x1 = alpha/om_(i)+1;
double x2 = -alpha/om_(i)+1;
// Numerator and denominator of the filter
a_(0,i) = 1;
a_(1,i) = x2 / x1;
b_(0,i) = 1 / x1;
b_(1,i) = 1 / x1;
///ROS_INFO("a %i = %f, %f, b %i = %f, %f", i, a_(0,i), a_(1,i), i, b_(0,1), b_(1,i));
// Set previous in- and outputs to zero
tau_previous_(i) = 0;
qdot_reference_previous_(i) = 0;
// Set joint limits
q_min_(i) = q_min(i);
q_max_(i) = q_max(i);
}
// Set inputs, states, outputs to zero
ROS_INFO("Admittance controller initialized");
}
inline value_type const& operator()(size_type i, size_type j) const {
return m_(j,i);
}
