bool LeggedBodyHelper::setStance(double width, Link* baseLink)
{
if(footInfos.size() != 2){
return false;
}
bool result = false;
Link* foot[2];
double sign;
if(footInfos[1].link == baseLink){
foot[0] = footInfos[1].link;
foot[1] = footInfos[0].link;
sign = -1.0;
} else {
foot[0] = footInfos[0].link;
foot[1] = footInfos[1].link;
sign = 1.0;
}
const Matrix3& R0 = foot[0]->R();
const Vector3 baseY(R0(0,1), sign * R0(1,1), 0.0);
Link* waist = body_->rootLink();
foot[1]->p() = foot[0]->p() + baseY * width;
Vector3 wp = (foot[0]->p() + foot[1]->p()) / 2.0;
wp[2] = waist->p()[2];
JointPathPtr ikPath = getCustomJointPath(body_, foot[0], waist);
if(ikPath && ikPath->calcInverseKinematics(wp, waist->R())){
ikPath = getCustomJointPath(body_, waist, foot[1]);
if(ikPath && ikPath->calcInverseKinematics(foot[1]->p(), foot[1]->R())){
LinkTraverse fkTraverse(baseLink);
fkTraverse.calcForwardKinematics();
result = true;
}
}
return result;
}
bool BodyMotionPoseProvider::seek
(double time, int waistLinkIndex, const Vector3& waistTranslation, bool applyWaistTranslation)
{
int frame = lround(time * motion->frameRate());
if(frame >= motion->numFrames()){
frame = motion->numFrames() - 1;
}
const MultiValueSeq::Frame q = motion->jointPosSeq()->frame(frame);
for(int i=0; i < minNumJoints; ++i){
qTranslated[i] = q[i];
}
if(waistLinkIndex != 0){
return false;
}
const SE3& waist = motion->linkPosSeq()->at(frame, 0);
p_waist = waist.translation();
R_waist = waist.rotation();
if(applyWaistTranslation){
p_waist += waistTranslation;
for(size_t i=0; i < footLinks.size(); ++i){
const Affine3& foot = footLinkPositions->at(frame, i);
JointPathPtr ikPath = ikPaths[i];
ikPath->calcInverseKinematics(p_waist, R_waist, foot.translation(), foot.linear());
for(int j=0; j < ikPath->numJoints(); ++j){
Link* joint = ikPath->joint(j);
qTranslated[joint->jointId()] = joint->q();
}
}
}
if(zmpSeq){
if(zmpSeq->isRootRelative()){
ZMP_.noalias() = R_waist * zmpSeq->at(frame) + p_waist;
} else {
ZMP_.noalias() = zmpSeq->at(frame);
}
}
return true;
}
bool BodyMotionPoseProvider::seek
(double time, int waistLinkIndex, const Vector3& waistTranslation, bool applyWaistTranslation)
{
int frame = lround(time * motion->frameRate());
if(frame >= motion->numFrames()){
frame = motion->numFrames() - 1;
}
const MultiValueSeq::View q = motion->jointPosSeq()->frame(frame);
for(int i=0; i < minNumJoints; ++i){
qTranslated[i] = q[i];
}
if(waistLinkIndex != 0){
return false;
}
const Affine3& waist = motion->linkPosSeq()->at(frame, 0);
p_waist = waist.translation();
R_waist = waist.linear();
if(applyWaistTranslation){
p_waist += waistTranslation;
for(size_t i=0; i < footLinks.size(); ++i){
const Affine3& foot = footLinkPositions->at(frame, i);
JointPathPtr ikPath = ikPaths[i];
ikPath->calcInverseKinematics(p_waist, R_waist, foot.translation(), foot.linear());
for(int j=0; j < ikPath->numJoints(); ++j){
Link* joint = ikPath->joint(j);
qTranslated[joint->jointId] = joint->q;
}
}
}
if(motion->hasRelativeZmpSeq()){
const Vector3& relZmp = motion->relativeZmpSeq()->at(frame);
zmp_.noalias() = waist.linear() * relZmp + waist.translation(); // original world position
}
return true;
}
int
main(int argc, char* argv[])
{
int i;
string url = "file://";
// -urlでモデルのURLを指定 //
for(i=0; i < argc; i++) {
if( strcmp(argv[i], "-url") == 0 && i+1 < argc) url += argv[i+1];
}
// モデルロード //
BodyPtr body(new Body());
if(!loadBodyFromModelLoader(body, url.c_str(), argc, argv)){
cerr << "ModelLoader: " << url << " cannot be loaded" << endl;
return 0;
}
body->calcForwardKinematics();
// OnlineViewer設定 //
OnlineViewer_var olv = getOnlineViewer(argc, argv);
try {
olv->load(body->modelName().c_str(), url.c_str());
olv->setLogName("move_ankle");
} catch (CORBA::SystemException& ex) {
cerr << "Failed to connect GrxUI." << endl;
return 1;
}
// 特異点にならないよう最初は曲げておく //
body->joint(1)->q = deg2rad(-10);
body->joint(3)->q = deg2rad(20);
body->joint(4)->q = deg2rad(-10);
body->calcForwardKinematics();
// 腰から足首までのパスを設定 //
Link* waist = body->link("WAIST");
Link* ankle = body->link("RLEG_ANKLE_R");
JointPathPtr path = body->getJointPath(waist, ankle);
// WorldStateを作成する //
WorldState world;
world.characterPositions.length(1);
world.collisions.length(0);
// SampleRobot用CharacterPosition //
//world.collisions.length(0);
CharacterPosition& robot = world.characterPositions[0];
robot.characterName = CORBA::string_dup("SampleRobot");
// 時間は0 //
world.time=0;
while (1) {
// 時間を進める //
world.time+=0.01;
// 少し動かす //
Vector3 p = ankle->p;
Matrix33 R = ankle->R;
p(2) += 0.002;
// もし逆運動学計算に失敗したら終わり //
if (!path->calcInverseKinematics(p, R)) {
break;
}
// LinkをWorldStateにコピーする。 //
int n = body->numLinks();
robot.linkPositions.length(n);
for (int i=0; i<n; i++) {
Link* link = body->link(i);
setVector3(link->p, robot.linkPositions[i].p);
setMatrix33ToRowMajorArray(link->R, robot.linkPositions[i].R);
}
// OnlineViewer アップデート //
try {
olv->update(world);
} catch (CORBA::SystemException& ex) {
std::cerr << "OnlineViewer could not be updated." << endl;
return 1;
}
}
return 0;
}
bool LeggedBodyHelper::doLegIkToMoveCm(const Vector3& c, bool onlyProjectionToFloor)
{
if(!isValid_){
return false;
}
static const int MAX_ITERATION = 100;
static const double ERROR_THRESH_SQR = 1.0e-6 * 1.0e-6;
Link* baseFoot = footInfos[0].link;
Link* waist = body_->rootLink();
LinkTraverse fkTraverse(waist);
Vector3 c0 = body_->calcCenterOfMass();
bool failed = false;
int loopCounter = 0;
while(true){
Vector3 e = c - c0;
if(onlyProjectionToFloor){
e.z() = 0.0;
}
if(e.squaredNorm() < ERROR_THRESH_SQR){
break;
}
size_t numDone = 0;
JointPathPtr baseToWaist = getCustomJointPath(body_, baseFoot, waist);
if(baseToWaist && baseToWaist->calcInverseKinematics(waist->p() + e, waist->R())){
numDone++;
for(size_t j=1; j < footInfos.size(); ++j){
Link* foot = footInfos[j].link;
JointPathPtr waistToFoot = getCustomJointPath(body_, waist, foot);
if(waistToFoot){
bool ikDone;
if(waistToFoot->hasAnalyticalIK()){
ikDone = waistToFoot->calcInverseKinematics(foot->p(), foot->R());
} else {
Vector3 p0 = foot->p();
Matrix3 R0 = foot->R();
waistToFoot->calcForwardKinematics();
ikDone = waistToFoot->calcInverseKinematics(p0, R0);
}
if(ikDone){
numDone++;
} else {
break;
}
}
}
}
if(numDone < footInfos.size()){
failed = true;
break;
}
if(++loopCounter < MAX_ITERATION){
fkTraverse.calcForwardKinematics();
c0 = body_->calcCenterOfMass();
} else {
break;
}
}
return !failed;
}
