bool SequencePlayer::playPatternOfGroup(const char *gname, const dSequenceSequence& pos, const dSequence& tm)
{
if ( m_debugLevel > 0 ) {
std::cerr << __PRETTY_FUNCTION__ << std::endl;
}
Guard guard(m_mutex);
if (!setInitialState()) return false;
std::vector<const double *> v_pos;
std::vector<double> v_tm;
for ( unsigned int i = 0; i < pos.length(); i++ ) v_pos.push_back(pos[i].get_buffer());
for ( unsigned int i = 0; i < tm.length() ; i++ ) v_tm.push_back(tm[i]);
return m_seq->playPatternOfGroup(gname, v_pos, v_tm, m_qInit.data.get_buffer(), pos.length()>0?pos[0].length():0);
}
void SequencePlayer::playPattern(const dSequenceSequence& pos, const dSequenceSequence& rpy, const dSequenceSequence& zmp, const dSequence& tm)
{
if ( m_debugLevel > 0 ) {
std::cerr << __PRETTY_FUNCTION__ << std::endl;
}
Guard guard(m_mutex);
if (!setInitialState()) return;
std::vector<const double *> v_pos, v_rpy, v_zmp;
std::vector<double> v_tm;
for ( unsigned int i = 0; i < pos.length(); i++ ) v_pos.push_back(pos[i].get_buffer());
for ( unsigned int i = 0; i < rpy.length(); i++ ) v_rpy.push_back(rpy[i].get_buffer());
for ( unsigned int i = 0; i < zmp.length(); i++ ) v_zmp.push_back(zmp[i].get_buffer());
for ( unsigned int i = 0; i < tm.length() ; i++ ) v_tm.push_back(tm[i]);
return m_seq->playPattern(v_pos, v_rpy, v_zmp, v_tm, m_qInit.data.get_buffer(), pos.length()>0?pos[0].length():0);
}
void SequencePlayer::playPattern(const dSequenceSequence& pos, const dSequenceSequence& rpy, const dSequenceSequence& zmp, const dSequence& tm)
{
Guard guard(m_mutex);
if (!setInitialState()) return;
const double *q=NULL, *z=NULL, *a=NULL, *p=NULL, *e=NULL; double t=0;
unsigned int len = (pos.length()>0?pos[0].length():0);
double *v = new double[len];
for (unsigned int i=0; i<pos.length(); i++){
q = pos[i].get_buffer();
if (i < pos.length() - 1 ) {
double t0, t1;
if (tm.length() == pos.length()) {
t0 = tm[i]; t1 = tm[i+1];
} else {
t0 = t1 = tm[0];
}
const double *q_next = pos[i+1].get_buffer();
const double *q_prev
= i==0 ? m_qInit.data.get_buffer() : pos[i-1].get_buffer();
for (unsigned int j = 0; j < len; j++) {
double d0, d1, v0, v1;
d0 = (q[j] - q_prev[j]);
d1 = (q_next[j] - q[j]);
v0 = d0/t0;
v1 = d1/t1;
if ( v0 * v1 >= 0 ) {
v[j] = 0.5 * (v0 + v1);
} else {
v[j] = 0;
}
}
} else {
for (unsigned int j = 0; j < len; j++) { v[j] = 0.0; }
}
if (i < zmp.length()) z = zmp[i].get_buffer();
if (i < rpy.length()) e = rpy[i].get_buffer();
if (i < tm.length()) t = tm[i];
m_seq->go(q, z, a, p, e,
v, NULL, NULL, NULL, NULL,
t, false);
}
m_seq->sync();
delete [] v;
}
