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 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 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 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;
}
}
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;
}
}
