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(); }
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)); }
void PoseSeqItem::onPositionChanged() { if(!sigInterpolationParametersChangedConnection.connected()){ sigInterpolationParametersChangedConnection = generationBar->sigInterpolationParametersChanged().connect( std::bind(&PoseSeqItem::updateInterpolationParameters, this)); updateInterpolationParameters(); } BodyItemPtr prevBodyItem = ownerBodyItem; ownerBodyItem = findOwnerItem<BodyItem>(); if(ownerBodyItem == prevBodyItem){ return; } if(!ownerBodyItem){ interpolator_->setBody(0); } else { Body* body = ownerBodyItem->body(); if(seq->targetBodyName().empty()){ seq->setTargetBodyName(body->name()); } else if(prevBodyItem && (seq->targetBodyName() != body->name())){ convert(prevBodyItem->body()); } interpolator_->setBody(body); const Listing& linearInterpolationJoints = *ownerBodyItem->body()->info()->findListing("linearInterpolationJoints"); if(linearInterpolationJoints.isValid()){ for(int i=0; i < linearInterpolationJoints.size(); ++i){ Link* link = body->link(linearInterpolationJoints[i].toString()); if(link){ interpolator_->setLinearInterpolationJoint(link->jointId()); } } } LeggedBodyHelperPtr legged = getLeggedBodyHelper(ownerBodyItem->body()); if(legged->isValid()){ for(int i=0; i < legged->numFeet(); ++i){ interpolator_->addFootLink(legged->footLink(i)->index(), legged->centerOfSoleLocal(i)); } } interpolator_->setLipSyncShapes(*ownerBodyItem->body()->info()->findMapping("lipSyncShapes")); bodyMotionItem_->motion()->setNumJoints(interpolator_->body()->numJoints()); if(generationBar->isAutoGenerationForNewBodyEnabled()){ updateTrajectory(true); } else { bodyMotionItem_->notifyUpdate(); } } }
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)); }