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