void EditableSceneBodyImpl::attachPositionDragger(Link* link) { SceneLink* sceneLink = self->sceneLink(link->index()); double r = sceneLink->untransformedBoundingBox().boundingSphereRadius(); positionDragger->setRadius(r); sceneLink->addChild(positionDragger); }
EditableSceneBodyImpl::EditableSceneBodyImpl(EditableSceneBody* self, BodyItemPtr& bodyItem) : self(self), bodyItem(bodyItem), kinematicsBar(KinematicsBar::instance()), modified(SgUpdate::MODIFIED) { pointedSceneLink = 0; outlinedLink = 0; targetLink = 0; positionDragger = new PositionDragger; positionDragger->setDraggerAlwaysShown(true); positionDragger->sigDragStarted().connect(boost::bind(&EditableSceneBodyImpl::onDraggerDragStarted, this)); positionDragger->sigPositionDragged().connect(boost::bind(&EditableSceneBodyImpl::onDraggerDragged, this)); positionDragger->sigDragFinished().connect(boost::bind(&EditableSceneBodyImpl::onDraggerDragFinished, this)); dragMode = DRAG_NONE; isDragging = false; isEditMode = false; markerGroup = new SgGroup; markerGroup->setName("Marker"); self->addChild(markerGroup); double radius = 0.0; const int n = self->numSceneLinks(); for(int i=0; i < n; ++i){ SceneLink* sLink = self->sceneLink(i); BoundingBox bb = sLink->boundingBox(); double radius0 = bb.size().norm() / 2.0; if(radius0 > radius){ radius = radius0; } } cmMarker = new CrossMarker(radius, Vector3f(0.0f, 1.0f, 0.0f), 2.0); cmMarker->setName("centerOfMass"); isCmVisible = false; ppcomMarker = new CrossMarker(radius, Vector3f(1.0f, 0.5f, 0.0f), 2.0); ppcomMarker->setName("ProjectionPointCoM"); isPpcomVisible = false; forcedPositionMode = NO_FORCED_POSITION; virtualElasticStringLine = new SgLineSet; virtualElasticStringLine->getOrCreateVertices()->resize(2); virtualElasticStringLine->addLine(0, 1); LeggedBodyHelperPtr legged = getLeggedBodyHelper(self->body()); if(legged->isValid() && legged->numFeet() > 0){ Link* footLink = legged->footLink(0); const double r = calcLinkMarkerRadius(self->sceneLink(footLink->index())); zmpMarker = new SphereMarker(r, Vector3f(0.0f, 1.0f, 0.0f), 0.3); zmpMarker->setName("ZMP"); zmpMarker->addChild(new CrossMarker(r * 2.5, Vector3f(0.0f, 1.0f, 0.0f), 2.0f)); } else { zmpMarker = new SphereMarker(0.1, Vector3f(0.0f, 1.0f, 0.0f), 0.3); } isZmpVisible = false; self->sigGraphConnection().connect(boost::bind(&EditableSceneBodyImpl::onSceneGraphConnection, this, _1)); }
EditableSceneBodyImpl::EditableSceneBodyImpl(EditableSceneBody* self, BodyItemPtr& bodyItem) : self(self), bodyItem(bodyItem), kinematicsBar(KinematicsBar::instance()), modified(SgUpdate::MODIFIED) { pointedSceneLink = 0; outlinedLink = 0; targetLink = 0; positionDragger = new PositionDragger; positionDragger->setDraggerAlwaysShown(true); positionDragger->sigDragStarted().connect(boost::bind(&EditableSceneBodyImpl::onDraggerIKstarted, this)); positionDragger->sigPositionDragged().connect(boost::bind(&EditableSceneBodyImpl::onDraggerIKdragged, this)); dragMode = DRAG_NONE; isDragging = false; isEditMode = false; markerGroup = new SgGroup; markerGroup->setName("Marker"); self->addChild(markerGroup); double radius = 0; const int n = self->numSceneLinks(); for(int i=0; i < n; ++i){ SceneLink* sLink = self->sceneLink(i); BoundingBox bb = sLink->boundingBox(); double radius0 = (bb.max() - bb.center()).norm(); if(radius0 > radius){ radius = radius0; } } cmMarker = new CrossMarker(0.25, Vector3f(0.0f, 1.0f, 0.0f), 2.0); cmMarker->setName("centerOfMass"); cmMarker->setSize(radius); isCmVisible = false; LeggedBodyHelperPtr legged = getLeggedBodyHelper(self->body()); if(legged->isValid() && legged->numFeet() > 0){ Link* footLink = legged->footLink(0); const double r = calcLinkMarkerRadius(self->sceneLink(footLink->index())); zmpMarker = new SphereMarker(r, Vector3f(0.0f, 1.0f, 0.0f), 0.3); zmpMarker->setName("ZMP"); zmpMarker->addChild(new CrossMarker(r * 2.5, Vector3f(0.0f, 1.0f, 0.0f), 2.0f)); } else { zmpMarker = new SphereMarker(0.1, Vector3f(0.0f, 1.0f, 0.0f), 0.3); } isZmpVisible = false; self->sigGraphConnection().connect(boost::bind(&EditableSceneBodyImpl::onSceneGraphConnection, this, _1)); }
SgCamera* VisionRenderer::initializeCamera() { SgCamera* sceneCamera = 0; SceneBody* sceneBody = sceneBodies[bodyIndex]; if(camera){ SceneDevice* sceneDevice = sceneBody->getSceneDevice(device); if(sceneDevice){ sceneCamera = sceneDevice->findNodeOfType<SgCamera>(); pixelWidth = camera->resolutionX(); pixelHeight = camera->resolutionY(); double frameRate = std::max(0.1, std::min(camera->frameRate(), simImpl->maxFrameRate)); cycleTime = 1.0 / frameRate; if(simImpl->isVisionDataRecordingEnabled){ camera->setImageStateClonable(true); } } } else if(rangeSensor){ const double thresh = (170.0 * PI / 180.0); // 170[deg] bool isWithinPossibleRanges = (rangeSensor->yawRange() < thresh) && (rangeSensor->pitchRange() < thresh); if(isWithinPossibleRanges){ SceneLink* sceneLink = sceneBody->sceneLink(rangeSensor->link()->index()); if(sceneLink){ SgPerspectiveCamera* persCamera = new SgPerspectiveCamera; sceneCamera = persCamera; persCamera->setNearDistance(rangeSensor->minDistance()); persCamera->setFarDistance(rangeSensor->maxDistance()); SgPosTransform* cameraPos = new SgPosTransform(); cameraPos->setTransform(rangeSensor->T_local()); cameraPos->addChild(persCamera); sceneLink->addChild(cameraPos); if(rangeSensor->yawRange() > rangeSensor->pitchRange()){ pixelWidth = rangeSensor->yawResolution() * simImpl->rangeSensorPrecisionRatio; if(rangeSensor->pitchRange() == 0.0){ pixelHeight = 1; double r = tan(rangeSensor->yawRange() / 2.0) * 2.0 / pixelWidth; persCamera->setFieldOfView(atan2(r / 2.0, 1.0) * 2.0); } else { pixelHeight = rangeSensor->pitchResolution() * simImpl->rangeSensorPrecisionRatio; persCamera->setFieldOfView(rangeSensor->pitchRange()); } } else { pixelHeight = rangeSensor->pitchResolution() * simImpl->rangeSensorPrecisionRatio; if(rangeSensor->yawRange() == 0.0){ pixelWidth = 1; double r = tan(rangeSensor->pitchRange() / 2.0) * 2.0 / pixelHeight; persCamera->setFieldOfView(atan2(r / 2.0, 1.0) * 2.0); } else { pixelWidth = rangeSensor->yawResolution() * simImpl->rangeSensorPrecisionRatio; persCamera->setFieldOfView(rangeSensor->yawRange()); } } double frameRate = std::max(0.1, std::min(rangeSensor->frameRate(), simImpl->maxFrameRate)); cycleTime = 1.0 / frameRate; if(simImpl->isVisionDataRecordingEnabled){ rangeSensor->setRangeDataStateClonable(true); } depthError = simImpl->depthError; } } } return sceneCamera; }
void EditableSceneBodyImpl::attachPositionDragger(Link* link) { SceneLink* sceneLink = self->sceneLink(link->index()); positionDragger->adjustSize(sceneLink->untransformedBoundingBox()); sceneLink->addChild(positionDragger); }