void BodyMotionPoseProvider::initialize(BodyPtr body__, BodyMotionPtr motion) { body_ = body__->clone(); this->motion = motion; footLinkPositions.reset(new MultiAffine3Seq()); footLinks.clear(); ikPaths.clear(); LeggedBodyHelperPtr legged = getLeggedBodyHelper(body_); if(legged->isValid()){ for(int i=0; i < legged->numFeet(); ++i){ Link* link = legged->footLink(i); JointPathPtr ikPath = getCustomJointPath(body_, body_->rootLink(), link); if(ikPath){ if(ikPath->hasAnalyticalIK() || ikPath->numJoints() == 6){ footLinks.push_back(link); ikPaths.push_back(ikPath); } } } ZMP_ = legged->homeCopOfSoles(); } else { ZMP_.setZero(); } updateMotion(); }
bool CompositeIK::addBaseLink(Link* baseLink) { if(baseLink && targetLink_){ JointPathPtr path = getCustomJointPath(body_, targetLink_, baseLink); if(path){ hasAnalyticalIK_ = paths.empty() ? path->hasAnalyticalIK() : (hasAnalyticalIK_ && path->hasAnalyticalIK()); paths.push_back(path); return true; } } return false; }
bool CompositeIK::addBaseLink(Link* baseLink) { if(baseLink && targetLink_){ JointPathPtr path = getCustomJointPath(body_, targetLink_, baseLink); if(path){ isAnalytical_ = pathList.empty() ? path->hasAnalyticalIK() : (isAnalytical_ && path->hasAnalyticalIK()); PathInfo info; info.path = path; info.endLink = baseLink; pathList.push_back(info); return true; } } return false; }
void gen_colmap(BodyPtr body, Link *link1, Link *link2) { JointPathPtr path = body->getJointPath(link1, link2); if (path->numJoints() != 2){ std::cerr << "the number of joints between " << link1->name << " and " << link2->name << "isn't equal to 2" << std::endl; return; } ColdetLinkPairPtr pair = new ColdetLinkPair(link1, link2); Link *joint1=path->joint(0), *joint2=path->joint(1); double th1 = joint1->llimit, th2; #define DTH (M_PI/180); std::string fname = link1->name + "_" + link2->name + ".dat"; std::ofstream ofs(fname.c_str()); ofs << link1->ulimit << " " << link2->ulimit << std::endl; ofs << link1->ulimit << " " << link2->llimit << std::endl; ofs << link1->llimit << " " << link2->llimit << std::endl; ofs << link1->llimit << " " << link2->ulimit << std::endl; ofs << link1->ulimit << " " << link2->ulimit << std::endl; ofs << std::endl << std::endl; int ntest=0, ncollision=0; while (th1 < joint1->ulimit){ joint1->q = th1; th2 = joint2->llimit; while (th2 < joint2->ulimit){ joint2->q = th2; path->calcForwardKinematics(); link1->coldetModel->setPosition(link1->attitude(), link1->p); link2->coldetModel->setPosition(link2->attitude(), link2->p); ntest++; if (pair->checkCollision()){ ofs << th1 << " " << th2 << std::endl; ncollision++; } th2 += DTH; } th1 += DTH; } std::cout << link1->name << "<->" << link2->name << ":" << ncollision << "/" << ntest << std::endl; }
bool BodyMotionPoseProvider::seek (double time, int waistLinkIndex, const Vector3& waistTranslation, bool applyWaistTranslation) { int frame = lround(time * motion->frameRate()); if(frame >= motion->numFrames()){ frame = motion->numFrames() - 1; } const MultiValueSeq::Frame q = motion->jointPosSeq()->frame(frame); for(int i=0; i < minNumJoints; ++i){ qTranslated[i] = q[i]; } if(waistLinkIndex != 0){ return false; } const SE3& waist = motion->linkPosSeq()->at(frame, 0); p_waist = waist.translation(); R_waist = waist.rotation(); if(applyWaistTranslation){ p_waist += waistTranslation; for(size_t i=0; i < footLinks.size(); ++i){ const Affine3& foot = footLinkPositions->at(frame, i); JointPathPtr ikPath = ikPaths[i]; ikPath->calcInverseKinematics(p_waist, R_waist, foot.translation(), foot.linear()); for(int j=0; j < ikPath->numJoints(); ++j){ Link* joint = ikPath->joint(j); qTranslated[joint->jointId()] = joint->q(); } } } if(zmpSeq){ if(zmpSeq->isRootRelative()){ ZMP_.noalias() = R_waist * zmpSeq->at(frame) + p_waist; } else { ZMP_.noalias() = zmpSeq->at(frame); } } return true; }
bool LeggedBodyHelper::setStance(double width, Link* baseLink) { if(footInfos.size() != 2){ return false; } bool result = false; Link* foot[2]; double sign; if(footInfos[1].link == baseLink){ foot[0] = footInfos[1].link; foot[1] = footInfos[0].link; sign = -1.0; } else { foot[0] = footInfos[0].link; foot[1] = footInfos[1].link; sign = 1.0; } const Matrix3& R0 = foot[0]->R(); const Vector3 baseY(R0(0,1), sign * R0(1,1), 0.0); Link* waist = body_->rootLink(); foot[1]->p() = foot[0]->p() + baseY * width; Vector3 wp = (foot[0]->p() + foot[1]->p()) / 2.0; wp[2] = waist->p()[2]; JointPathPtr ikPath = getCustomJointPath(body_, foot[0], waist); if(ikPath && ikPath->calcInverseKinematics(wp, waist->R())){ ikPath = getCustomJointPath(body_, waist, foot[1]); if(ikPath && ikPath->calcInverseKinematics(foot[1]->p(), foot[1]->R())){ LinkTraverse fkTraverse(baseLink); fkTraverse.calcForwardKinematics(); result = true; } } return result; }
bool BodyMotionPoseProvider::seek (double time, int waistLinkIndex, const Vector3& waistTranslation, bool applyWaistTranslation) { int frame = lround(time * motion->frameRate()); if(frame >= motion->numFrames()){ frame = motion->numFrames() - 1; } const MultiValueSeq::View q = motion->jointPosSeq()->frame(frame); for(int i=0; i < minNumJoints; ++i){ qTranslated[i] = q[i]; } if(waistLinkIndex != 0){ return false; } const Affine3& waist = motion->linkPosSeq()->at(frame, 0); p_waist = waist.translation(); R_waist = waist.linear(); if(applyWaistTranslation){ p_waist += waistTranslation; for(size_t i=0; i < footLinks.size(); ++i){ const Affine3& foot = footLinkPositions->at(frame, i); JointPathPtr ikPath = ikPaths[i]; ikPath->calcInverseKinematics(p_waist, R_waist, foot.translation(), foot.linear()); for(int j=0; j < ikPath->numJoints(); ++j){ Link* joint = ikPath->joint(j); qTranslated[joint->jointId] = joint->q; } } } if(motion->hasRelativeZmpSeq()){ const Vector3& relZmp = motion->relativeZmpSeq()->at(frame); zmp_.noalias() = waist.linear() * relZmp + waist.translation(); // original world position } return true; }
void BodyMotionPoseProvider::initialize(BodyPtr body__, BodyMotionPtr motion) { body_ = body__->duplicate(); this->motion = motion; footLinkPositions.reset(new MultiAffine3Seq()); footLinks.clear(); ikPaths.clear(); YamlSequence& footLinkInfos = *body_->info()->findSequence("footLinks"); if(footLinkInfos.isValid()){ for(int i=0; i < footLinkInfos.size(); ++i){ const YamlMapping& footLinkInfo = *footLinkInfos[i].toMapping(); Link* link = body_->link(footLinkInfo["link"].toString()); JointPathPtr ikPath = body_->getJointPath(body_->rootLink(), link); if(ikPath && ikPath->hasAnalyticalIK()){ footLinks.push_back(link); ikPaths.push_back(ikPath); } } } updateMotion(); }
void Body::calcCMJacobian(Link *base, dmatrix &J) { // prepare subm, submwc JointPathPtr jp; if (base){ jp = getJointPath(rootLink(), base); Link *skip = jp->joint(0); skip->subm = rootLink()->m; skip->submwc = rootLink()->m*rootLink()->wc; Link *l = rootLink()->child; if (l){ if (l != skip) { l->calcSubMassCM(); skip->subm += l->subm; skip->submwc += l->submwc; } l = l->sibling; while(l){ if (l != skip){ l->calcSubMassCM(); skip->subm += l->subm; skip->submwc += l->submwc; } l = l->sibling; } } // assuming there is no branch between base and root for (int i=1; i<jp->numJoints(); i++){ l = jp->joint(i); l->subm = l->parent->m + l->parent->subm; l->submwc = l->parent->m*l->parent->wc + l->parent->submwc; } J.resize(3, numJoints()); }else{ rootLink()->calcSubMassCM(); J.resize(3, numJoints()+6); } // compute Jacobian std::vector<int> sgn(numJoints(), 1); if (jp) { for (int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1; } for (int i=0; i<numJoints(); i++){ Link *j = joint(i); switch(j->jointType){ case Link::ROTATIONAL_JOINT: { Vector3 omega(sgn[j->jointId]*j->R*j->a); Vector3 arm((j->submwc-j->subm*j->p)/totalMass_); Vector3 dp(omega.cross(arm)); J.col(j->jointId) = dp; break; } default: std::cerr << "calcCMJacobian() : unsupported jointType(" << j->jointType << std::endl; } } if (!base){ int c = numJoints(); J(0, c ) = 1.0; J(0, c+1) = 0.0; J(0, c+2) = 0.0; J(1, c ) = 0.0; J(1, c+1) = 1.0; J(1, c+2) = 0.0; J(2, c ) = 0.0; J(2, c+1) = 0.0; J(2, c+2) = 1.0; Vector3 dp(rootLink()->submwc/totalMass_ - rootLink()->p); J(0, c+3) = 0.0; J(0, c+4) = dp(2); J(0, c+5) = -dp(1); J(1, c+3) = -dp(2); J(1, c+4) = 0.0; J(1, c+5) = dp(0); J(2, c+3) = dp(1); J(2, c+4) = -dp(0); J(2, c+5) = 0.0; } }
int main(int argc, char* argv[]) { int i; string url = "file://"; // -urlでモデルのURLを指定 // for(i=0; i < argc; i++) { if( strcmp(argv[i], "-url") == 0 && i+1 < argc) url += argv[i+1]; } // モデルロード // BodyPtr body(new Body()); if(!loadBodyFromModelLoader(body, url.c_str(), argc, argv)){ cerr << "ModelLoader: " << url << " cannot be loaded" << endl; return 0; } body->calcForwardKinematics(); // OnlineViewer設定 // OnlineViewer_var olv = getOnlineViewer(argc, argv); try { olv->load(body->modelName().c_str(), url.c_str()); olv->setLogName("move_ankle"); } catch (CORBA::SystemException& ex) { cerr << "Failed to connect GrxUI." << endl; return 1; } // 特異点にならないよう最初は曲げておく // body->joint(1)->q = deg2rad(-10); body->joint(3)->q = deg2rad(20); body->joint(4)->q = deg2rad(-10); body->calcForwardKinematics(); // 腰から足首までのパスを設定 // Link* waist = body->link("WAIST"); Link* ankle = body->link("RLEG_ANKLE_R"); JointPathPtr path = body->getJointPath(waist, ankle); // WorldStateを作成する // WorldState world; world.characterPositions.length(1); world.collisions.length(0); // SampleRobot用CharacterPosition // //world.collisions.length(0); CharacterPosition& robot = world.characterPositions[0]; robot.characterName = CORBA::string_dup("SampleRobot"); // 時間は0 // world.time=0; while (1) { // 時間を進める // world.time+=0.01; // 少し動かす // Vector3 p = ankle->p; Matrix33 R = ankle->R; p(2) += 0.002; // もし逆運動学計算に失敗したら終わり // if (!path->calcInverseKinematics(p, R)) { break; } // LinkをWorldStateにコピーする。 // int n = body->numLinks(); robot.linkPositions.length(n); for (int i=0; i<n; i++) { Link* link = body->link(i); setVector3(link->p, robot.linkPositions[i].p); setMatrix33ToRowMajorArray(link->R, robot.linkPositions[i].R); } // OnlineViewer アップデート // try { olv->update(world); } catch (CORBA::SystemException& ex) { std::cerr << "OnlineViewer could not be updated." << endl; return 1; } } return 0; }
bool LeggedBodyHelper::doLegIkToMoveCm(const Vector3& c, bool onlyProjectionToFloor) { if(!isValid_){ return false; } static const int MAX_ITERATION = 100; static const double ERROR_THRESH_SQR = 1.0e-6 * 1.0e-6; Link* baseFoot = footInfos[0].link; Link* waist = body_->rootLink(); LinkTraverse fkTraverse(waist); Vector3 c0 = body_->calcCenterOfMass(); bool failed = false; int loopCounter = 0; while(true){ Vector3 e = c - c0; if(onlyProjectionToFloor){ e.z() = 0.0; } if(e.squaredNorm() < ERROR_THRESH_SQR){ break; } size_t numDone = 0; JointPathPtr baseToWaist = getCustomJointPath(body_, baseFoot, waist); if(baseToWaist && baseToWaist->calcInverseKinematics(waist->p() + e, waist->R())){ numDone++; for(size_t j=1; j < footInfos.size(); ++j){ Link* foot = footInfos[j].link; JointPathPtr waistToFoot = getCustomJointPath(body_, waist, foot); if(waistToFoot){ bool ikDone; if(waistToFoot->hasAnalyticalIK()){ ikDone = waistToFoot->calcInverseKinematics(foot->p(), foot->R()); } else { Vector3 p0 = foot->p(); Matrix3 R0 = foot->R(); waistToFoot->calcForwardKinematics(); ikDone = waistToFoot->calcInverseKinematics(p0, R0); } if(ikDone){ numDone++; } else { break; } } } } if(numDone < footInfos.size()){ failed = true; break; } if(++loopCounter < MAX_ITERATION){ fkTraverse.calcForwardKinematics(); c0 = body_->calcCenterOfMass(); } else { break; } } return !failed; }
void Body::calcAngularMomentumJacobian(Link *base, dmatrix &H) { // prepare subm, submwc JointPathPtr jp; dmatrix M; calcCMJacobian(base, M); M.conservativeResize(3, numJoints()); M *= totalMass(); if (base){ jp = getJointPath(rootLink(), base); Link *skip = jp->joint(0); skip->subm = rootLink()->m; skip->submwc = rootLink()->m*rootLink()->wc; Link *l = rootLink()->child; if (l){ if (l != skip) { l->calcSubMassCM(); skip->subm += l->subm; skip->submwc += l->submwc; } l = l->sibling; while(l){ if (l != skip){ l->calcSubMassCM(); skip->subm += l->subm; skip->submwc += l->submwc; } l = l->sibling; } } // assuming there is no branch between base and root for (unsigned int i=1; i<jp->numJoints(); i++){ l = jp->joint(i); l->subm = l->parent->m + l->parent->subm; l->submwc = l->parent->m*l->parent->wc + l->parent->submwc; } H.resize(3, numJoints()); }else{ rootLink()->calcSubMassCM(); H.resize(3, numJoints()+6); } // compute Jacobian std::vector<int> sgn(numJoints(), 1); if (jp) { for (unsigned int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1; } for (unsigned int i=0; i<numJoints(); i++){ Link *j = joint(i); switch(j->jointType){ case Link::ROTATIONAL_JOINT: { Vector3 omega(sgn[j->jointId]*j->R*j->a); Vector3 Mcol = M.col(j->jointId); Matrix33 jsubIw; j->calcSubMassInertia(jsubIw); Vector3 dp = jsubIw*omega; if (j->subm!=0) dp += (j->submwc/j->subm).cross(Mcol); H.col(j->jointId) = dp; break; } case Link::SLIDE_JOINT: { if(j->subm!=0){ Vector3 Mcol =M.col(j->jointId); Vector3 dp = (j->submwc/j->subm).cross(Mcol); H.col(j->jointId) = dp; } break; } default: std::cerr << "calcCMJacobian() : unsupported jointType(" << j->jointType << ")" << std::endl; } } if (!base){ int c = numJoints(); H.block(0, c, 3, 3).setZero(); Matrix33 Iw; rootLink_->calcSubMassInertia(Iw); H.block(0, c+3, 3, 3) = Iw; Vector3 cm = calcCM(); Matrix33 cm_cross; cm_cross << 0.0, -cm(2), cm(1), cm(2), 0.0, -cm(0), -cm(1), cm(0), 0.0; H.block(0,0,3,c) -= cm_cross * M; } }