ForwardDynamicsABM::ForwardDynamicsABM(BodyPtr body) :
ForwardDynamics(body),
q0(body->numLinks()),
dq0(body->numLinks()),
dq(body->numLinks()),
ddq(body->numLinks())
{
}
bool PoseProviderToBodyMotionConverter::convert(BodyPtr 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();
MultiAffine3Seq& pseq = *motion.linkPosSeq();
Vector3Seq& relZmpSeq = *motion.relativeZmpSeq();
bool isZmpValid = false;
Link* rootLink = body->rootLink();
Link* baseLink = rootLink;
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;
}
Affine3 orgp;
orgp.translation() = rootLink->p;
orgp.linear() = 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)->find(baseLink, rootLink);
}
}
provider->getBaseLinkPosition(baseLink->p, baseLink->R);
}
MultiValueSeq::View qs = qseq.frame(frame);
provider->getJointPositions(jointPositions);
for(int i=0; i < numJoints; ++i){
const 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){
Affine3& p = pseq.at(frame, i);
Link* link = body->link(i);
p.translation() = link->p;
p.linear() = link->R;
}
optional<Vector3> zmp = provider->zmp();
if(zmp){
relZmpSeq[frame].noalias() = rootLink->R.transpose() * (*zmp - rootLink->p);
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 = orgp.translation();
rootLink->R = orgp.linear();
body->calcForwardKinematics();
return true;
}
int KinematicFaultCheckerImpl::checkFaults
(BodyItem* bodyItem, BodyMotionItem* motionItem, std::ostream& os,
bool checkPosition, bool checkVelocity, bool checkCollision, dynamic_bitset<> linkSelection,
double beginningTime, double endingTime)
{
numFaults = 0;
BodyPtr body = bodyItem->body();
BodyMotionPtr motion = motionItem->motion();
MultiValueSeqPtr qseq = motion->jointPosSeq();;
MultiSE3SeqPtr pseq = motion->linkPosSeq();
if((!checkPosition && !checkVelocity && !checkCollision) || body->isStaticModel() || !qseq->getNumFrames()){
return numFaults;
}
BodyState orgKinematicState;
if(USE_DUPLICATED_BODY){
body = body->clone();
} else {
bodyItem->storeKinematicState(orgKinematicState);
}
CollisionDetectorPtr collisionDetector;
WorldItem* worldItem = bodyItem->findOwnerItem<WorldItem>();
if(worldItem){
collisionDetector = worldItem->collisionDetector()->clone();
} else {
int index = CollisionDetector::factoryIndex("AISTCollisionDetector");
if(index >= 0){
collisionDetector = CollisionDetector::create(index);
} else {
collisionDetector = CollisionDetector::create(0);
os << _("A collision detector is not found. Collisions cannot be detected this time.") << endl;
}
}
addBodyToCollisionDetector(*body, *collisionDetector);
collisionDetector->makeReady();
const int numJoints = std::min(body->numJoints(), qseq->numParts());
const int numLinks = std::min(body->numLinks(), pseq->numParts());
frameRate = motion->frameRate();
double stepRatio2 = 2.0 / frameRate;
angleMargin = radian(angleMarginSpin.value());
translationMargin = translationMarginSpin.value();
velocityLimitRatio = velocityLimitRatioSpin.value() / 100.0;
int beginningFrame = std::max(0, (int)(beginningTime * frameRate));
int endingFrame = std::min((motion->numFrames() - 1), (int)lround(endingTime * frameRate));
lastPosFaultFrames.clear();
lastPosFaultFrames.resize(numJoints, std::numeric_limits<int>::min());
lastVelFaultFrames.clear();
lastVelFaultFrames.resize(numJoints, std::numeric_limits<int>::min());
lastCollisionFrames.clear();
if(checkCollision){
Link* root = body->rootLink();
root->p().setZero();
root->R().setIdentity();
}
for(int frame = beginningFrame; frame <= endingFrame; ++frame){
int prevFrame = (frame == beginningFrame) ? beginningFrame : frame - 1;
int nextFrame = (frame == endingFrame) ? endingFrame : frame + 1;
for(int i=0; i < numJoints; ++i){
Link* joint = body->joint(i);
double q = qseq->at(frame, i);
joint->q() = q;
if(joint->index() >= 0 && linkSelection[joint->index()]){
if(checkPosition){
bool fault = false;
if(joint->isRotationalJoint()){
fault = (q > (joint->q_upper() - angleMargin) || q < (joint->q_lower() + angleMargin));
} else if(joint->isSlideJoint()){
fault = (q > (joint->q_upper() - translationMargin) || q < (joint->q_lower() + translationMargin));
}
if(fault){
putJointPositionFault(frame, joint, os);
}
}
if(checkVelocity){
double dq = (qseq->at(nextFrame, i) - qseq->at(prevFrame, i)) / stepRatio2;
joint->dq() = dq;
if(dq > (joint->dq_upper() * velocityLimitRatio) || dq < (joint->dq_lower() * velocityLimitRatio)){
putJointVelocityFault(frame, joint, os);
}
}
}
}
if(checkCollision){
Link* link = body->link(0);
if(!pseq->empty())
{
const SE3& p = pseq->at(frame, 0);
link->p() = p.translation();
link->R() = p.rotation().toRotationMatrix();
}
else
{
link->p() = Vector3d(0., 0., 0.);
link->R() = Matrix3d::Identity();
}
body->calcForwardKinematics();
for(int i=1; i < numLinks; ++i){
link = body->link(i);
if(!pseq->empty())
{
const SE3& p = pseq->at(frame, i);
link->p() = p.translation();
link->R() = p.rotation().toRotationMatrix();
}
}
for(int i=0; i < numLinks; ++i){
link = body->link(i);
collisionDetector->updatePosition(i, link->position());
}
collisionDetector->detectCollisions(
boost::bind(&KinematicFaultCheckerImpl::putSelfCollision, this, body.get(), frame, _1, boost::ref(os)));
}
}
if(!USE_DUPLICATED_BODY){
bodyItem->restoreKinematicState(orgKinematicState);
}
return numFaults;
}
/**
@brief compute CoM Jacobian
@param base link fixed to the environment
@param J CoM Jacobian
@note Link::wc must be computed by calcCM() before calling
*/
void calcCMJacobian(const BodyPtr& body, Link* base, Eigen::MatrixXd& J)
{
// prepare subm, submwc
const int nj = body->numJoints();
vector<SubMass> subMasses(body->numLinks());
Link* rootLink = body->rootLink();
JointPath path;
if(!base) {
calcSubMass(rootLink, subMasses);
J.resize(3, nj + 6);
} else {
path.setPath(rootLink, base);
Link* skip = path.joint(0);
SubMass& sub = subMasses[skip->index()];
sub.m = rootLink->m();
sub.mwc = rootLink->m() * rootLink->wc();
for(Link* child = rootLink->child(); child; child = child->sibling()) {
if(child != skip) {
calcSubMass(child, subMasses);
subMasses[skip->index()] += subMasses[child->index()];
}
}
// assuming there is no branch between base and root
for(int i=1; i < path.numJoints(); i++) {
Link* joint = path.joint(i);
const Link* parent = joint->parent();
SubMass& sub = subMasses[joint->index()];
sub.m = parent->m();
sub.mwc = parent->m() * parent->wc();
sub += subMasses[parent->index()];
}
J.resize(3, nj);
}
// compute Jacobian
std::vector<int> sgn(nj, 1);
for(int i=0; i < path.numJoints(); i++) {
sgn[path.joint(i)->jointId()] = -1;
}
for(int i=0; i < nj; i++) {
Link* joint = body->joint(i);
if(joint->isRotationalJoint()) {
const Vector3 omega = sgn[joint->jointId()] * joint->R() * joint->a();
const SubMass& sub = subMasses[joint->index()];
const Vector3 arm = (sub.mwc - sub.m * joint->p()) / body->mass();
const Vector3 dp = omega.cross(arm);
J.col(joint->jointId()) = dp;
} else {
std::cerr << "calcCMJacobian() : unsupported jointType("
<< joint->jointType() << std::endl;
}
}
if(!base) {
const int c = nj;
J.block(0, c, 3, 3).setIdentity();
const Vector3 dp = subMasses[0].mwc / body->mass() - rootLink->p();
J.block(0, c + 3, 3, 3) <<
0.0, dp(2), -dp(1),
-dp(2), 0.0, dp(0),
dp(1), -dp(0), 0.0;
}
}