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();
}
bool CompositeIK::addBaseLink(Link* baseLink)
{
if(baseLink && targetLink_){
JointPathPtr path = getCustomJointPath(body_, targetLink_, baseLink);
if(path){
hasAnalyticalIK_ = paths.empty() ? path->hasAnalyticalIK() : (hasAnalyticalIK_ && path->hasAnalyticalIK());
paths.push_back(path);
return true;
}
}
return false;
}
bool CompositeIK::addBaseLink(Link* baseLink)
{
if(baseLink && targetLink_){
JointPathPtr path = getCustomJointPath(body_, targetLink_, baseLink);
if(path){
isAnalytical_ = pathList.empty() ? path->hasAnalyticalIK() : (isAnalytical_ && path->hasAnalyticalIK());
PathInfo info;
info.path = path;
info.endLink = baseLink;
pathList.push_back(info);
return true;
}
}
return false;
}
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 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 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::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;
}
void BodyMotionPoseProvider::initialize(BodyPtr body__, BodyMotionPtr motion)
{
body_ = body__->duplicate();
this->motion = motion;
footLinkPositions.reset(new MultiAffine3Seq());
footLinks.clear();
ikPaths.clear();
YamlSequence& footLinkInfos = *body_->info()->findSequence("footLinks");
if(footLinkInfos.isValid()){
for(int i=0; i < footLinkInfos.size(); ++i){
const YamlMapping& footLinkInfo = *footLinkInfos[i].toMapping();
Link* link = body_->link(footLinkInfo["link"].toString());
JointPathPtr ikPath = body_->getJointPath(body_->rootLink(), link);
if(ikPath && ikPath->hasAnalyticalIK()){
footLinks.push_back(link);
ikPaths.push_back(ikPath);
}
}
}
updateMotion();
}
void Body::calcCMJacobian(Link *base, dmatrix &J)
{
// prepare subm, submwc
JointPathPtr jp;
if (base){
jp = getJointPath(rootLink(), base);
Link *skip = jp->joint(0);
skip->subm = rootLink()->m;
skip->submwc = rootLink()->m*rootLink()->wc;
Link *l = rootLink()->child;
if (l){
if (l != skip) {
l->calcSubMassCM();
skip->subm += l->subm;
skip->submwc += l->submwc;
}
l = l->sibling;
while(l){
if (l != skip){
l->calcSubMassCM();
skip->subm += l->subm;
skip->submwc += l->submwc;
}
l = l->sibling;
}
}
// assuming there is no branch between base and root
for (int i=1; i<jp->numJoints(); i++){
l = jp->joint(i);
l->subm = l->parent->m + l->parent->subm;
l->submwc = l->parent->m*l->parent->wc + l->parent->submwc;
}
J.resize(3, numJoints());
}else{
rootLink()->calcSubMassCM();
J.resize(3, numJoints()+6);
}
// compute Jacobian
std::vector<int> sgn(numJoints(), 1);
if (jp) {
for (int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1;
}
for (int i=0; i<numJoints(); i++){
Link *j = joint(i);
switch(j->jointType){
case Link::ROTATIONAL_JOINT:
{
Vector3 omega(sgn[j->jointId]*j->R*j->a);
Vector3 arm((j->submwc-j->subm*j->p)/totalMass_);
Vector3 dp(omega.cross(arm));
J.col(j->jointId) = dp;
break;
}
default:
std::cerr << "calcCMJacobian() : unsupported jointType("
<< j->jointType << std::endl;
}
}
if (!base){
int c = numJoints();
J(0, c ) = 1.0; J(0, c+1) = 0.0; J(0, c+2) = 0.0;
J(1, c ) = 0.0; J(1, c+1) = 1.0; J(1, c+2) = 0.0;
J(2, c ) = 0.0; J(2, c+1) = 0.0; J(2, c+2) = 1.0;
Vector3 dp(rootLink()->submwc/totalMass_ - rootLink()->p);
J(0, c+3) = 0.0; J(0, c+4) = dp(2); J(0, c+5) = -dp(1);
J(1, c+3) = -dp(2); J(1, c+4) = 0.0; J(1, c+5) = dp(0);
J(2, c+3) = dp(1); J(2, c+4) = -dp(0); J(2, c+5) = 0.0;
}
}
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;
}
void Body::calcAngularMomentumJacobian(Link *base, dmatrix &H)
{
// prepare subm, submwc
JointPathPtr jp;
dmatrix M;
calcCMJacobian(base, M);
M.conservativeResize(3, numJoints());
M *= totalMass();
if (base){
jp = getJointPath(rootLink(), base);
Link *skip = jp->joint(0);
skip->subm = rootLink()->m;
skip->submwc = rootLink()->m*rootLink()->wc;
Link *l = rootLink()->child;
if (l){
if (l != skip) {
l->calcSubMassCM();
skip->subm += l->subm;
skip->submwc += l->submwc;
}
l = l->sibling;
while(l){
if (l != skip){
l->calcSubMassCM();
skip->subm += l->subm;
skip->submwc += l->submwc;
}
l = l->sibling;
}
}
// assuming there is no branch between base and root
for (unsigned int i=1; i<jp->numJoints(); i++){
l = jp->joint(i);
l->subm = l->parent->m + l->parent->subm;
l->submwc = l->parent->m*l->parent->wc + l->parent->submwc;
}
H.resize(3, numJoints());
}else{
rootLink()->calcSubMassCM();
H.resize(3, numJoints()+6);
}
// compute Jacobian
std::vector<int> sgn(numJoints(), 1);
if (jp) {
for (unsigned int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1;
}
for (unsigned int i=0; i<numJoints(); i++){
Link *j = joint(i);
switch(j->jointType){
case Link::ROTATIONAL_JOINT:
{
Vector3 omega(sgn[j->jointId]*j->R*j->a);
Vector3 Mcol = M.col(j->jointId);
Matrix33 jsubIw;
j->calcSubMassInertia(jsubIw);
Vector3 dp = jsubIw*omega;
if (j->subm!=0) dp += (j->submwc/j->subm).cross(Mcol);
H.col(j->jointId) = dp;
break;
}
case Link::SLIDE_JOINT:
{
if(j->subm!=0){
Vector3 Mcol =M.col(j->jointId);
Vector3 dp = (j->submwc/j->subm).cross(Mcol);
H.col(j->jointId) = dp;
}
break;
}
default:
std::cerr << "calcCMJacobian() : unsupported jointType("
<< j->jointType << ")" << std::endl;
}
}
if (!base){
int c = numJoints();
H.block(0, c, 3, 3).setZero();
Matrix33 Iw;
rootLink_->calcSubMassInertia(Iw);
H.block(0, c+3, 3, 3) = Iw;
Vector3 cm = calcCM();
Matrix33 cm_cross;
cm_cross <<
0.0, -cm(2), cm(1),
cm(2), 0.0, -cm(0),
-cm(1), cm(0), 0.0;
H.block(0,0,3,c) -= cm_cross * M;
}
}
