void gen_colmap(BodyPtr body, Link *link1, Link *link2)
{
JointPathPtr path = body->getJointPath(link1, link2);
if (path->numJoints() != 2){
std::cerr << "the number of joints between " << link1->name << " and "
<< link2->name << "isn't equal to 2" << std::endl;
return;
}
ColdetLinkPairPtr pair = new ColdetLinkPair(link1, link2);
Link *joint1=path->joint(0), *joint2=path->joint(1);
double th1 = joint1->llimit, th2;
#define DTH (M_PI/180);
std::string fname = link1->name + "_" + link2->name + ".dat";
std::ofstream ofs(fname.c_str());
ofs << link1->ulimit << " " << link2->ulimit << std::endl;
ofs << link1->ulimit << " " << link2->llimit << std::endl;
ofs << link1->llimit << " " << link2->llimit << std::endl;
ofs << link1->llimit << " " << link2->ulimit << std::endl;
ofs << link1->ulimit << " " << link2->ulimit << std::endl;
ofs << std::endl << std::endl;
int ntest=0, ncollision=0;
while (th1 < joint1->ulimit){
joint1->q = th1;
th2 = joint2->llimit;
while (th2 < joint2->ulimit){
joint2->q = th2;
path->calcForwardKinematics();
link1->coldetModel->setPosition(link1->attitude(), link1->p);
link2->coldetModel->setPosition(link2->attitude(), link2->p);
ntest++;
if (pair->checkCollision()){
ofs << th1 << " " << th2 << std::endl;
ncollision++;
}
th2 += DTH;
}
th1 += DTH;
}
std::cout << link1->name << "<->" << link2->name << ":" << ncollision
<< "/" << ntest << std::endl;
}
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;
}
