bool EditableModelItemImpl::loadModelFile(const std::string& filename) { BodyPtr newBody; MessageView* mv = MessageView::instance(); mv->beginStdioRedirect(); bodyLoader.setMessageSink(mv->cout(true)); newBody = bodyLoader.load(filename); mv->endStdioRedirect(); if(newBody){ newBody->initializeState(); newBody->calcForwardKinematics(); Link* link = newBody->rootLink(); AbstractBodyLoaderPtr loader = bodyLoader.lastActualBodyLoader(); VRMLBodyLoader* vloader = dynamic_cast<VRMLBodyLoader*>(loader.get()); if (vloader) { // VRMLBodyLoader supports retriveOriginalNode function setLinkTree(link, vloader); } else { // Other loaders dont, so we wrap with inline node VRMLProtoInstance* proto = new VRMLProtoInstance(new VRMLProto("")); MFNode* children = new MFNode(); VRMLInlinePtr inl = new VRMLInline(); inl->urls.push_back(filename); children->push_back(inl); proto->fields["children"] = *children; // first, create joint item JointItemPtr item = new JointItem(link); item->originalNode = proto; self->addChildItem(item); ItemTreeView::instance()->checkItem(item, true); // next, create link item under the joint item LinkItemPtr litem = new LinkItem(link); litem->originalNode = proto; litem->setName("link"); item->addChildItem(litem); ItemTreeView::instance()->checkItem(litem, true); } for (int i = 0; i < newBody->numDevices(); i++) { Device* dev = newBody->device(i); SensorItemPtr sitem = new SensorItem(dev); Item* parent = self->findItem<Item>(dev->link()->name()); if (parent) { parent->addChildItem(sitem); ItemTreeView::instance()->checkItem(sitem, true); } } } return (newBody); }
void BodyState::set(BodyPtr i_body) { Link *root = i_body->rootLink(); p = root->p; R = root->R; q.resize(i_body->numJoints()); for (int i=0; i<i_body->numJoints(); i++){ Link *joint = i_body->joint(i); if (joint){ q[i] = joint->q; } } }
void MeshShapeItemImpl::onUpdated() { sceneLink->translation() = self->absTranslation; sceneLink->rotation() = self->absRotation; if (path != ""){ if (shape) { sceneLink->removeChild(shape); shape = NULL; } BodyLoader bodyLoader; BodyPtr newBody = bodyLoader.load(path); if (!newBody) return; shape = newBody->rootLink()->visualShape(); sceneLink->addChildOnce(shape); sceneLink->notifyUpdate(); } }
void BodyLinkViewImpl::update() { currentLink = 0; if(!currentBodyItem){ nameLabel.setText(""); return; } propertyWidgetConnections.block(); stateWidgetConnections.block(); BodyPtr body = currentBodyItem->body(); const vector<int>& selectedLinkIndices = LinkSelectionView::mainInstance()->getSelectedLinkIndices(currentBodyItem); if(selectedLinkIndices.empty()){ currentLink = body->rootLink(); } else { currentLink = body->link(selectedLinkIndices.front()); } if(currentLink){ nameLabel.setText(QString("%1 / %2").arg(body->name().c_str()).arg(currentLink->name().c_str())); updateLink(); } else { nameLabel.setText(body->name().c_str()); } if(currentBodyItem->isLeggedBody()){ zmpBox.show(); } else { zmpBox.hide(); } updateKinematicState(false); updateCollisions(); stateWidgetConnections.unblock(); propertyWidgetConnections.unblock(); }
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; }
int main(int argc, char *argv[]) { srand((unsigned)time(NULL)); const char *robotURL = NULL; std::vector<std::string> obstacleURL; std::vector<Vector3> obstacleP; std::vector<Vector3> obstacleRpy; for(int i = 1 ; i < argc; i++) { if (strcmp(argv[i], "-robot") == 0) { robotURL = argv[++i]; } else if (strcmp(argv[i], "-obstacle") == 0) { obstacleURL.push_back(argv[++i]); Vector3 p, rpy; for (int j=0; j<3; j++) p[j] = atof(argv[++i]); obstacleP.push_back(p); for (int j=0; j<3; j++) rpy[j] = atof(argv[++i]); obstacleRpy.push_back(rpy); } } if (robotURL == NULL) { std::cerr << "please specify URL of VRML model by -robot option" << std::endl; return 1; } BodyPtr robot = new Body(); loadBodyFromModelLoader(robot, robotURL, argc, argv, true); problem prob; prob.addRobot("robot", robotURL, robot); std::vector<BodyPtr> obstacles; for (unsigned int i=0; i<obstacleURL.size(); i++) { char buf[20]; sprintf(buf, "obstacle%02d", i); obstacles.push_back(prob.addObstacle(buf, obstacleURL[i])); } // This must be called after all bodies are added prob.initOLV(argc, argv); PathEngine::Configuration::size(4+6+6); PathEngine::Configuration::bounds(0, 0.2, 0.8); // body z PathEngine::Configuration::bounds(1, -0.5, 0.5); // body roll PathEngine::Configuration::bounds(2, -0.0, 0.5); // body pitch PathEngine::Configuration::bounds(3, -0.5, 0.5); // body yaw int arm; PathEngine::Configuration goalCfg; std::ifstream ifs("goal.txt"); ifs >> arm; for (unsigned int i=0; i<PathEngine::Configuration::size(); i++) { ifs >> goalCfg[i]; } #if 1 PathEngine::Configuration::weight(0) = 0.1; // z PathEngine::Configuration::weight(1) = 1; // roll PathEngine::Configuration::weight(2) = 1; // pitch PathEngine::Configuration::weight(3) = 1; // yaw PathEngine::Configuration::weight(4) = 0.8; PathEngine::Configuration::weight(5) = 0.6; PathEngine::Configuration::weight(6) = 0.4; PathEngine::Configuration::weight(7) = 0.3; PathEngine::Configuration::weight(8) = 0.2; PathEngine::Configuration::weight(9) = 0.1; PathEngine::Configuration::weight(10) = 0.8; PathEngine::Configuration::weight(11) = 0.6; PathEngine::Configuration::weight(12) = 0.4; PathEngine::Configuration::weight(13) = 0.3; PathEngine::Configuration::weight(14) = 0.2; PathEngine::Configuration::weight(15) = 0.1; #endif JointPathPtr armPath[2]; Link *chest = robot->link("CHEST_JOINT1"); Link *wrist[2] = {robot->link("RARM_JOINT5"), robot->link("LARM_JOINT5") }; for (int k=0; k<2; k++) { armPath[k] = robot->getJointPath(chest, wrist[k]); for (int i=0; i<armPath[k]->numJoints(); i++) { Link *j = armPath[k]->joint(i); PathEngine::Configuration::bounds(4+k*6+i, j->llimit, j->ulimit); } } PathEngine::PathPlanner *planner = prob.planner(); planner->setMobilityName("OmniWheel"); planner->setAlgorithmName("RRT"); PathEngine::RRT *rrt = (PathEngine::RRT *)planner->getAlgorithm(); rrt->extendFromGoal(true); planner->getAlgorithm()->setProperty("eps", "0.1"); planner->getAlgorithm()->setProperty("max-trials", "50000"); planner->getAlgorithm()->setProperty("interpolation-distance", "0.05"); prob.initCollisionCheckPairs(); prob.initPlanner(); for (unsigned int i=0; i<obstacles.size(); i++) { Link *root = obstacles[i]->rootLink(); root->p = obstacleP[i]; root->R = rotFromRpy(obstacleRpy[i]); root->coldetModel->setPosition(root->R, root->p); obstacles[i]->calcForwardKinematics(); } // set halfconf dvector halfconf; halfconf.setZero(robot->numJoints()); #define ToRad(x) ((x)*M_PI/180) halfconf[2] = halfconf[ 8] = ToRad(-40); halfconf[3] = halfconf[ 9] = ToRad( 78); halfconf[4] = halfconf[10] = ToRad(-38); double leg_link_len1=0, leg_link_len2=0; halfconf[16] = halfconf[23] = ToRad(20); halfconf[17] = ToRad(-10); halfconf[24] = -halfconf[17]; halfconf[19] = halfconf[26] = ToRad(-30); if (robot->modelName() == "HRP2") { halfconf[20] = ToRad(80); halfconf[27] = -halfconf[20]; halfconf[22] = halfconf[29] = -1.0; } leg_link_len1 = leg_link_len2 = 0.3; double waistHeight = leg_link_len1*cos(halfconf[2]) + leg_link_len2*cos(halfconf[4]) + 0.105; robot->rootLink()->p(2) = waistHeight; for (int i=0; i<robot->numJoints(); i++) { robot->joint(i)->q = halfconf[i]; } robot->calcForwardKinematics(); myCfgSetter setter(robot); PathEngine::Configuration startCfg; startCfg[0] = robot->rootLink()->p[2]; startCfg[1] = startCfg[2] = startCfg[3] = 0; for (int j=0; j<2; j++) { for (int i=0; i<armPath[j]->numJoints(); i++) { startCfg[4+j*6+i] = armPath[j]->joint(i)->q; } } planner->setStartConfiguration(startCfg); planner->setGoalConfiguration(goalCfg); planner->setApplyConfigFunc(boost::bind(&myCfgSetter::set, &setter, _1, _2)); planner->setConfiguration(startCfg); prob.updateOLV(); planner->setConfiguration(goalCfg); prob.updateOLV(); #if 0 int earm; ifs >> earm; while (!ifs.eof()) { PathEngine::Configuration cfg; for (unsigned int i=0; i<PathEngine::Configuration::size(); i++) { ifs >> cfg[i]; } if (arm == earm) { rrt->addExtraGoal(cfg); std::cout << "added an extra goal" << std::endl; planner->setConfiguration(cfg); prob.updateOLV(); } ifs >> earm; } #endif CustomCD cd(robot, "hrp2.shape", "hrp2.pairs", obstacles[0], "plant.pc"); prob.planner()->setCollisionDetector(&cd); struct timeval tv1, tv2; gettimeofday(&tv1, NULL); // plan bool ret = planner->calcPath(); if (ret) { for (int i=0; i<5; i++) { planner->optimize("Shortcut"); planner->optimize("RandomShortcut"); } } gettimeofday(&tv2, NULL); if (ret) { const std::vector<PathEngine::Configuration>& postures = planner->getWayPoints(); std::ofstream ofs("path.txt"); ofs << arm << std::endl; for (unsigned int i=0; i<postures.size(); i++) { planner->setConfiguration(postures[i]); for (int j=0; j<3; j++) { ofs << robot->rootLink()->p[j] << " "; } for (int j=0; j<3; j++) { for (int k=0; k<3; k++) { ofs << robot->rootLink()->R(j,k) << " "; } } for (int j=0; j<robot->numJoints(); j++) { ofs << robot->joint(j)->q << " "; } ofs << std::endl; prob.updateOLV(); } } else { PathEngine::Roadmap *roadmap = planner->getRoadmap(); for (unsigned int i=0; i<roadmap->nNodes(); i++) { PathEngine::RoadmapNode *node = roadmap->node(i); planner->setConfiguration(node->position()); prob.updateOLV(); } } double time = (tv2.tv_sec - tv1.tv_sec) + ((double)(tv2.tv_usec - tv1.tv_usec))/1e6; std::cout << "total time = " << time << "[s]" << std::endl; double cdtime = prob.planner()->timeCollisionCheck(); std::cout << "time spent in collision detection:" << cdtime << "[s](" << cdtime/time*100 << "[%]), " << cdtime*1e3/prob.planner()->countCollisionCheck() << "[ms/call]" << std::endl; std::cout << "profile:"; setter.profile(); return 0; }
bool PoseProviderToBodyMotionConverter::convert(BodyPtr body, PoseProvider* provider, BodyMotion& motion) { const double frameRate = motion.frameRate(); const int beginningFrame = static_cast<int>(frameRate * std::max(provider->beginningTime(), lowerTime)); const int endingFrame = static_cast<int>(frameRate * std::min(provider->endingTime(), upperTime)); const int numJoints = body->numJoints(); const int numLinksToPut = (allLinkPositionOutputMode ? body->numLinks() : 1); motion.setDimension(endingFrame + 1, numJoints, numLinksToPut, true); MultiValueSeq& qseq = *motion.jointPosSeq(); MultiAffine3Seq& pseq = *motion.linkPosSeq(); Vector3Seq& relZmpSeq = *motion.relativeZmpSeq(); bool isZmpValid = false; Link* rootLink = body->rootLink(); Link* baseLink = rootLink; shared_ptr<LinkTraverse> fkTraverse; if(allLinkPositionOutputMode){ fkTraverse.reset(new LinkTraverse(baseLink, true, true)); } else { fkTraverse.reset(new LinkPath(baseLink, rootLink)); } // store the original state vector<double> orgq(numJoints); for(int i=0; i < numJoints; ++i){ orgq[i] = body->joint(i)->q; } Affine3 orgp; orgp.translation() = rootLink->p; orgp.linear() = rootLink->R; std::vector< boost::optional<double> > jointPositions(numJoints); for(int frame = beginningFrame; frame <= endingFrame; ++frame){ provider->seek(frame / frameRate); const int baseLinkIndex = provider->baseLinkIndex(); if(baseLinkIndex >= 0){ if(baseLinkIndex != baseLink->index){ baseLink = body->link(baseLinkIndex); if(allLinkPositionOutputMode){ fkTraverse->find(baseLink, true, true); } else { static_pointer_cast<LinkPath>(fkTraverse)->find(baseLink, rootLink); } } provider->getBaseLinkPosition(baseLink->p, baseLink->R); } MultiValueSeq::View qs = qseq.frame(frame); provider->getJointPositions(jointPositions); for(int i=0; i < numJoints; ++i){ const optional<double>& q = jointPositions[i]; qs[i] = q ? *q : 0.0; body->joint(i)->q = qs[i]; } if(allLinkPositionOutputMode || baseLink != rootLink){ fkTraverse->calcForwardKinematics(); } for(int i=0; i < numLinksToPut; ++i){ Affine3& p = pseq.at(frame, i); Link* link = body->link(i); p.translation() = link->p; p.linear() = link->R; } optional<Vector3> zmp = provider->zmp(); if(zmp){ relZmpSeq[frame].noalias() = rootLink->R.transpose() * (*zmp - rootLink->p); isZmpValid = true; } } if(!isZmpValid){ //bodyMotionItem->clearRelativeZmpSeq(); } // restore the original state for(int i=0; i < numJoints; ++i){ body->joint(i)->q = orgq[i]; } rootLink->p = orgp.translation(); rootLink->R = orgp.linear(); body->calcForwardKinematics(); return true; }
/** @brief compute CoM Jacobian @param base link fixed to the environment @param J CoM Jacobian @note Link::wc must be computed by calcCM() before calling */ void calcCMJacobian(const BodyPtr& body, Link* base, Eigen::MatrixXd& J) { // prepare subm, submwc const int nj = body->numJoints(); vector<SubMass> subMasses(body->numLinks()); Link* rootLink = body->rootLink(); JointPath path; if(!base) { calcSubMass(rootLink, subMasses); J.resize(3, nj + 6); } else { path.setPath(rootLink, base); Link* skip = path.joint(0); SubMass& sub = subMasses[skip->index()]; sub.m = rootLink->m(); sub.mwc = rootLink->m() * rootLink->wc(); for(Link* child = rootLink->child(); child; child = child->sibling()) { if(child != skip) { calcSubMass(child, subMasses); subMasses[skip->index()] += subMasses[child->index()]; } } // assuming there is no branch between base and root for(int i=1; i < path.numJoints(); i++) { Link* joint = path.joint(i); const Link* parent = joint->parent(); SubMass& sub = subMasses[joint->index()]; sub.m = parent->m(); sub.mwc = parent->m() * parent->wc(); sub += subMasses[parent->index()]; } J.resize(3, nj); } // compute Jacobian std::vector<int> sgn(nj, 1); for(int i=0; i < path.numJoints(); i++) { sgn[path.joint(i)->jointId()] = -1; } for(int i=0; i < nj; i++) { Link* joint = body->joint(i); if(joint->isRotationalJoint()) { const Vector3 omega = sgn[joint->jointId()] * joint->R() * joint->a(); const SubMass& sub = subMasses[joint->index()]; const Vector3 arm = (sub.mwc - sub.m * joint->p()) / body->mass(); const Vector3 dp = omega.cross(arm); J.col(joint->jointId()) = dp; } else { std::cerr << "calcCMJacobian() : unsupported jointType(" << joint->jointType() << std::endl; } } if(!base) { const int c = nj; J.block(0, c, 3, 3).setIdentity(); const Vector3 dp = subMasses[0].mwc / body->mass() - rootLink->p(); J.block(0, c + 3, 3, 3) << 0.0, dp(2), -dp(1), -dp(2), 0.0, dp(0), dp(1), -dp(0), 0.0; } }