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; } }