bool BodyItemImpl::doLegIkToMoveCm(const Vector3& c, bool onlyProjectionToFloor)
{
bool result = false;
LeggedBodyHelperPtr legged = getLeggedBodyHelper(body);
if(legged->isValid()){
BodyState orgKinematicState;
self->storeKinematicState(orgKinematicState);
self->beginKinematicStateEdit();
result = legged->doLegIkToMoveCm(c, onlyProjectionToFloor);
if(result){
self->notifyKinematicStateChange();
self->acceptKinematicStateEdit();
updateFlags.set(UF_CM);
} else {
self->restoreKinematicState(orgKinematicState);
}
}
return result;
}
void BodyItemImpl::getParticularPosition(BodyItem::PositionType position, boost::optional<Vector3>& pos)
{
if(position == BodyItem::ZERO_MOMENT_POINT){
pos = zmp;
} else {
if(position == BodyItem::CM_PROJECTION){
pos = self->centerOfMass();
} else {
LeggedBodyHelperPtr legged = getLeggedBodyHelper(body);
if(legged->isValid()){
if(position == BodyItem::HOME_COP){
pos = legged->homeCopOfSoles();
} else if(position == BodyItem::RIGHT_HOME_COP || position == BodyItem::LEFT_HOME_COP) {
if(legged->numFeet() == 2){
pos = legged->homeCopOfSole((position == BodyItem::RIGHT_HOME_COP) ? 0 : 1);
}
}
}
}
if(pos){
(*pos).z() = 0.0;
}
}
}
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 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();
}
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));
}
bool BodyItemImpl::setStance(double width)
{
bool result = false;
LeggedBodyHelperPtr legged = getLeggedBodyHelper(body);
if(legged->isValid()){
BodyState orgKinematicState;
self->storeKinematicState(orgKinematicState);
self->beginKinematicStateEdit();
result = legged->setStance(width, currentBaseLink);
if(result){
self->notifyKinematicStateChange();
self->acceptKinematicStateEdit();
} else {
self->restoreKinematicState(orgKinematicState);
}
}
return result;
}
