void EditableSceneBodyImpl::makeLinkAttitudeLevel()
{
if(pointedSceneLink){
Link* targetLink = outlinedLink->link();
InverseKinematicsPtr ik = bodyItem->getCurrentIK(targetLink);
if(ik){
const Position& T = targetLink->T();
const double theta = acos(T(2, 2));
const Vector3 z(T(0,2), T(1, 2), T(2, 2));
const Vector3 axis = z.cross(Vector3::UnitZ()).normalized();
const Matrix3 R2 = AngleAxisd(theta, axis) * T.linear();
bodyItem->beginKinematicStateEdit();
if(ik->calcInverseKinematics(targetLink->p(), R2)){
bodyItem->notifyKinematicStateChange(true);
bodyItem->acceptKinematicStateEdit();
}
}
}
}
bool CompositeIK::calcInverseKinematics(const Position& T)
{
const int n = body_->numJoints();
Position T0 = targetLink_->T();
q0.resize(n);
for(int i=0; i < n; ++i){
q0[i] = body_->joint(i)->q();
}
targetLink_->setPosition(T);
bool solved = true;
size_t pathIndex;
for(pathIndex=0; pathIndex < paths.size(); ++pathIndex){
JointPath& path = *paths[pathIndex];
Link* link = path.endLink();
Position T_end = link->T();
solved = path.setBaseLinkGoal(T).calcInverseKinematics(T_end);
if(!solved){
link->setPosition(T_end);
break;
}
}
if(!solved){
targetLink_->setPosition(T0);
for(int i=0; i < n; ++i){
body_->joint(i)->q() = q0[i];
}
for(size_t i=0; i < pathIndex; ++i){
paths[i]->calcForwardKinematics();
}
}
return solved;
}
bool PoseProviderToBodyMotionConverter::convert(Body* 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();
MultiSE3Seq& pseq = *motion.linkPosSeq();
ZMPSeq& zmpseq = *getOrCreateZMPSeq(motion);
bool isZmpValid = false;
Link* rootLink = body->rootLink();
Link* baseLink = rootLink;
std::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();
}
Vector3 p0 = rootLink->p();
Matrix3 R0 = 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)->setPath(baseLink, rootLink);
}
}
provider->getBaseLinkPosition(baseLink->T());
}
MultiValueSeq::Frame qs = qseq.frame(frame);
provider->getJointPositions(jointPositions);
for(int i=0; i < numJoints; ++i){
const boost::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){
SE3& p = pseq(frame, i);
Link* link = body->link(i);
p.set(link->p(), link->R());
}
boost::optional<Vector3> zmp = provider->ZMP();
if(zmp){
zmpseq[frame] = *zmp;
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() = p0;
rootLink->R() = R0;
body->calcForwardKinematics();
return true;
}
