bool SequencePlayer::setInitialState()
{
if (!m_seq->isEmpty()) return true;
if (m_qInit.data.length() == 0){
std::cerr << "can't determine initial posture" << std::endl;
return false;
}else{
m_seq->setJointAngles(m_qInit.data.get_buffer());
for (int i=0; i<m_robot->numJoints(); i++){
Link *l = m_robot->joint(i);
l->q = m_qInit.data[i];
}
Link *root = m_robot->rootLink();
root->p << m_basePosInit.data.x,
m_basePosInit.data.y,
m_basePosInit.data.z;
m_seq->setBasePos(root->p.data());
double rpy[] = {m_baseRpyInit.data.r,
m_baseRpyInit.data.p,
m_baseRpyInit.data.y};
m_seq->setBaseRpy(rpy);
calcRotFromRpy(root->R, rpy[0], rpy[1], rpy[2]);
#if 1
m_robot->calcForwardKinematics();
Vector3 com = m_robot->calcCM();
com[2] = 0.0;
Vector3 local_com(root->R.transpose()*(com - root->p));
double zmp[] = {local_com[0], local_com[1], local_com[2]};
m_seq->setZmp(zmp);
#endif
double zero[] = {0,0,0};
m_seq->setBaseAcc(zero);
return true;
}
}
bool SequencePlayer::setInitialState(double tm)
{
if ( m_debugLevel > 0 ) {
std::cerr << __PRETTY_FUNCTION__ << "m_seq-isEmpty() " << m_seq->isEmpty() << ", m_Init.data.length() " << m_qInit.data.length() << std::endl;
}
if (!m_seq->isEmpty()) return true;
if (m_qInit.data.length() == 0){
std::cerr << "can't determine initial posture" << std::endl;
return false;
}else{
m_seq->setJointAngles(m_qInit.data.get_buffer(), tm);
for (unsigned int i=0; i<m_robot->numJoints(); i++){
Link *l = m_robot->joint(i);
l->q = m_qInit.data[i];
m_qRef.data[i] = m_qInit.data[i]; // update m_qRef for setTargetPose()
}
Link *root = m_robot->rootLink();
root->p << m_basePosInit.data.x,
m_basePosInit.data.y,
m_basePosInit.data.z;
m_seq->setBasePos(root->p.data(), tm);
double rpy[] = {m_baseRpyInit.data.r,
m_baseRpyInit.data.p,
m_baseRpyInit.data.y};
m_seq->setBaseRpy(rpy, tm);
calcRotFromRpy(root->R, rpy[0], rpy[1], rpy[2]);
double zmp[] = {m_zmpRefInit.data.x, m_zmpRefInit.data.y, m_zmpRefInit.data.z};
m_seq->setZmp(zmp, tm);
double zero[] = {0,0,0};
m_seq->setBaseAcc(zero, tm);
return true;
}
}