bool BodyMotionPoseProvider::updateMotion() { const int numFrames = motion->numFrames(); footLinkPositions->setDimension(numFrames, footLinks.size()); footLinkPositions->setFrameRate(motion->frameRate()); MultiValueSeqPtr qseq = motion->jointPosSeq(); MultiSE3SeqPtr pseq = motion->linkPosSeq(); zmpSeq = getZMPSeq(*motion); if(pseq->numParts() < 1){ return false; } Link* rootLink = body_->rootLink(); minNumJoints = std::min(body_->numJoints(), qseq->numParts()); qTranslated.resize(minNumJoints); for(int frame=0; frame < numFrames; ++frame){ const SE3& p = pseq->at(frame, 0); rootLink->p() = p.translation(); rootLink->R() = p.rotation().toRotationMatrix(); MultiValueSeq::Frame q = qseq->frame(frame); for(int i=0; i < minNumJoints; ++i){ body_->joint(i)->q() = q[i]; } body_->calcForwardKinematics(); for(size_t i=0; i < footLinks.size(); ++i){ Link* footLink = footLinks[i]; Affine3& p = footLinkPositions->at(frame, i); p.translation() = footLink->p(); p.linear() = footLink->R(); } } return true; }
int KinematicFaultCheckerImpl::checkFaults (BodyItem* bodyItem, BodyMotionItem* motionItem, std::ostream& os, bool checkPosition, bool checkVelocity, bool checkCollision, dynamic_bitset<> linkSelection, double beginningTime, double endingTime) { numFaults = 0; BodyPtr body = bodyItem->body(); BodyMotionPtr motion = motionItem->motion(); MultiValueSeqPtr qseq = motion->jointPosSeq();; MultiSE3SeqPtr pseq = motion->linkPosSeq(); if((!checkPosition && !checkVelocity && !checkCollision) || body->isStaticModel() || !qseq->getNumFrames()){ return numFaults; } BodyState orgKinematicState; if(USE_DUPLICATED_BODY){ body = body->clone(); } else { bodyItem->storeKinematicState(orgKinematicState); } CollisionDetectorPtr collisionDetector; WorldItem* worldItem = bodyItem->findOwnerItem<WorldItem>(); if(worldItem){ collisionDetector = worldItem->collisionDetector()->clone(); } else { int index = CollisionDetector::factoryIndex("AISTCollisionDetector"); if(index >= 0){ collisionDetector = CollisionDetector::create(index); } else { collisionDetector = CollisionDetector::create(0); os << _("A collision detector is not found. Collisions cannot be detected this time.") << endl; } } addBodyToCollisionDetector(*body, *collisionDetector); collisionDetector->makeReady(); const int numJoints = std::min(body->numJoints(), qseq->numParts()); const int numLinks = std::min(body->numLinks(), pseq->numParts()); frameRate = motion->frameRate(); double stepRatio2 = 2.0 / frameRate; angleMargin = radian(angleMarginSpin.value()); translationMargin = translationMarginSpin.value(); velocityLimitRatio = velocityLimitRatioSpin.value() / 100.0; int beginningFrame = std::max(0, (int)(beginningTime * frameRate)); int endingFrame = std::min((motion->numFrames() - 1), (int)lround(endingTime * frameRate)); lastPosFaultFrames.clear(); lastPosFaultFrames.resize(numJoints, std::numeric_limits<int>::min()); lastVelFaultFrames.clear(); lastVelFaultFrames.resize(numJoints, std::numeric_limits<int>::min()); lastCollisionFrames.clear(); if(checkCollision){ Link* root = body->rootLink(); root->p().setZero(); root->R().setIdentity(); } for(int frame = beginningFrame; frame <= endingFrame; ++frame){ int prevFrame = (frame == beginningFrame) ? beginningFrame : frame - 1; int nextFrame = (frame == endingFrame) ? endingFrame : frame + 1; for(int i=0; i < numJoints; ++i){ Link* joint = body->joint(i); double q = qseq->at(frame, i); joint->q() = q; if(joint->index() >= 0 && linkSelection[joint->index()]){ if(checkPosition){ bool fault = false; if(joint->isRotationalJoint()){ fault = (q > (joint->q_upper() - angleMargin) || q < (joint->q_lower() + angleMargin)); } else if(joint->isSlideJoint()){ fault = (q > (joint->q_upper() - translationMargin) || q < (joint->q_lower() + translationMargin)); } if(fault){ putJointPositionFault(frame, joint, os); } } if(checkVelocity){ double dq = (qseq->at(nextFrame, i) - qseq->at(prevFrame, i)) / stepRatio2; joint->dq() = dq; if(dq > (joint->dq_upper() * velocityLimitRatio) || dq < (joint->dq_lower() * velocityLimitRatio)){ putJointVelocityFault(frame, joint, os); } } } } if(checkCollision){ Link* link = body->link(0); if(!pseq->empty()) { const SE3& p = pseq->at(frame, 0); link->p() = p.translation(); link->R() = p.rotation().toRotationMatrix(); } else { link->p() = Vector3d(0., 0., 0.); link->R() = Matrix3d::Identity(); } body->calcForwardKinematics(); for(int i=1; i < numLinks; ++i){ link = body->link(i); if(!pseq->empty()) { const SE3& p = pseq->at(frame, i); link->p() = p.translation(); link->R() = p.rotation().toRotationMatrix(); } } for(int i=0; i < numLinks; ++i){ link = body->link(i); collisionDetector->updatePosition(i, link->position()); } collisionDetector->detectCollisions( boost::bind(&KinematicFaultCheckerImpl::putSelfCollision, this, body.get(), frame, _1, boost::ref(os))); } } if(!USE_DUPLICATED_BODY){ bodyItem->restoreKinematicState(orgKinematicState); } return numFaults; }