virtual bool initialize() {
if(!qseq){
string filename = getNativePathString(
boost::filesystem::path(shareDirectory())
/ "motion" / "SR1" / "SR1WalkPattern3.yaml");
BodyMotion motion;
if(!motion.loadStandardYAMLformat(filename)){
os() << motion.seqMessage() << endl;
return false;
}
qseq = motion.jointPosSeq();
if(qseq->numFrames() == 0){
os() << "Empty motion data." << endl;
return false;
}
timeStep_ = qseq->getTimeStep();
}
if(fabs(timeStep() - timeStep_) > 1.0e-6){
os() << "Time step must be " << timeStep_ << "." << endl;;
return false;
}
body = ioBody();
numJoints = body->numJoints();
if(numJoints != qseq->numParts()){
os() << "The number of joints must be " << qseq->numParts() << endl;
return false;
}
qold.resize(numJoints);
VectorXd q0(numJoints);
VectorXd q1(numJoints);
for(int i=0; i < numJoints; ++i){
qold[i] = q0[i] = body->joint(i)->q();
}
MultiValueSeq::Frame frame = qseq->frame(0);
for(int i=0; i < numJoints; ++i){
q1[i] = frame[i];
}
interpolator.clear();
interpolator.appendSample(0.0, q0);
interpolator.appendSample(2.0, q1);
interpolator.update();
qref_old = interpolator.interpolate(0.0);
currentFrame = 0;
phase = 0;
time = 0.0;
return true;
}
bool PoseSeqItem::updateKeyPosesWithBalancedTrajectories(std::ostream& os)
{
BodyMotionPtr motion = bodyMotionItem()->motion();
MultiValueSeqPtr qseq = motion->jointPosSeq();
MultiSE3SeqPtr pseq = motion->linkPosSeq();
double length = seq->endingTime();
if(qseq->timeLength() < length || pseq->timeLength() < length){
os << "Length of the interpolated trajectories is shorter than key pose sequence.";
return false;
}
if(pseq->numParts() < ownerBodyItem->body()->numLinks()){
os << "Not all link positions are available. Please do interpolate with \"Put all link positions\"";
return false;
}
beginEditing();
for(PoseSeq::iterator p = seq->begin(); p != seq->end(); ++p){
PosePtr pose = p->get<Pose>();
if(pose){
seq->beginPoseModification(p);
int nj = pose->numJoints();
int frame = qseq->frameOfTime(p->time());
MultiValueSeq::Frame q = qseq->frame(frame);
for(int i=0; i < nj; ++i){
if(pose->isJointValid(i)){
pose->setJointPosition(i, q[i]);
}
}
MultiSE3Seq::Frame pos = pseq->frame(frame);
for(Pose::LinkInfoMap::iterator q = pose->ikLinkBegin(); q != pose->ikLinkEnd(); ++q){
int linkIndex = q->first;
Pose::LinkInfo& info = q->second;
const Vector3& p = pos[linkIndex].translation();
// only update horizontal position
info.p[0] = p[0];
info.p[1] = p[1];
}
seq->endPoseModification(p);
}
}
endEditing();
updateInterpolation();
return true;
}
void HrpsysSequenceFileExportPlugin::HrpsysSequenceFileExport()
{
cout << "\x1b[31m" << "Start HrpsysSequenceFileExport" << "\x1b[m" << endl;
// BodyItem,BodyPtr
ItemList<BodyItem> bodyItems = ItemTreeView::mainInstance()->checkedItems<BodyItem>();
BodyPtr body = bodyItems[0]->body();// ロボットモデルは１個のみチェック
LeggedBodyHelperPtr lgh = getLeggedBodyHelper(body);
PoseSeqItem* poseSeqItem = ItemTreeView::mainInstance()->selectedItems<PoseSeqItem>()[0];
PoseSeqPtr poseSeq = poseSeqItem->poseSeq();
BodyMotionItem* bodyMotionItem = poseSeqItem->bodyMotionItem();
BodyMotionPtr motion = bodyMotionItem->motion();
string poseSeqPathString = poseSeqItem->filePath();
boost::filesystem::path poseSeqPath(poseSeqPathString);
cout << " parent_path:" << poseSeqPath.parent_path().string() << " basename:" << getBasename(poseSeqPath) << endl;
double dt = ((double) 1)/motion->frameRate();
int numFrames = motion->numFrames();
std::vector<string> extentionVec{"wrenches","optionaldata"};
for(auto ext : extentionVec){
MultiValueSeqItemPtr seqItem = bodyMotionItem->findSubItem<MultiValueSeqItem>(ext);
if(!seqItem){
cout << ext << " is not found in PoseSeq: " << poseSeqItem->name() << endl;
continue;
}
stringstream fnamess;
fnamess << poseSeqItem->name() << "." << ext;
ofstream ofs;
ofs.open(((filesystem::path) poseSeqPath.parent_path() / fnamess.str()).string().c_str(), ios::out);
MultiValueSeqPtr multiValueSeqPtr = seqItem->seq();
for(int i=0; i<numFrames; ++i){
ofs << i*dt;
MultiValueSeq::Frame frame = multiValueSeqPtr->frame(i);
for(int j=0; j<frame.size(); ++j){
ofs << " " << frame[j];
}
ofs << endl;
}
ofs.close();
}
cout << "\x1b[31m" << "Finished HrpsysSequenceFileExport" << "\x1b[m" << endl << endl;
}
static bool bodyMotionItemPreFilter(BodyMotionItem* protoItem, Item* parentItem)
{
BodyItemPtr bodyItem = dynamic_cast<BodyItem*>(parentItem);
if(!bodyItem){
bodyItem = parentItem->findOwnerItem<BodyItem>();
}
if(bodyItem){
MultiValueSeqPtr jointPosSeq = protoItem->motion()->jointPosSeq();
int prevNumJoints = jointPosSeq->numParts();
int numJoints = bodyItem->body()->numJoints();
if(numJoints != prevNumJoints){
jointPosSeq->setNumParts(numJoints, true);
}
}
return true;
}
bool BodyMotionPoseProvider::updateMotion()
{
const int numFrames = motion->numFrames();
footLinkPositions->setDimension(numFrames, footLinks.size());
footLinkPositions->setFrameRate(motion->frameRate());
MultiValueSeqPtr qseq = motion->jointPosSeq();
MultiSE3SeqPtr pseq = motion->linkPosSeq();
zmpSeq = getZMPSeq(*motion);
if(pseq->numParts() < 1){
return false;
}
Link* rootLink = body_->rootLink();
minNumJoints = std::min(body_->numJoints(), qseq->numParts());
qTranslated.resize(minNumJoints);
for(int frame=0; frame < numFrames; ++frame){
const SE3& p = pseq->at(frame, 0);
rootLink->p() = p.translation();
rootLink->R() = p.rotation().toRotationMatrix();
MultiValueSeq::Frame q = qseq->frame(frame);
for(int i=0; i < minNumJoints; ++i){
body_->joint(i)->q() = q[i];
}
body_->calcForwardKinematics();
for(size_t i=0; i < footLinks.size(); ++i){
Link* footLink = footLinks[i];
Affine3& p = footLinkPositions->at(frame, i);
p.translation() = footLink->p();
p.linear() = footLink->R();
}
}
return true;
}
bool GRobotControllerItem::onPlaybackInitialized(double time)
{
if(controller->isPlayingMotion()){
controller->stopMotion();
}
playbackConnections.disconnect();
if(bodyItem){
BodyMotionItemPtr motionItem =
ItemTreeView::mainInstance()->selectedSubItem<BodyMotionItem>(bodyItem);
if(motionItem){
MultiValueSeqPtr qseq = motionItem->jointPosSeq();
if(qseq->numFrames() > 0 && qseq->numParts() == controller->numJoints()){
if(controller->setMotion(&qseq->at(0, 0), qseq->numFrames(), qseq->getTimeStep())){
TimeBar* timeBar = TimeBar::instance();
playbackConnections.add(
timeBar->sigPlaybackStarted().connect(
bind(&GRobotControllerItem::onPlaybackStarted, this, _1)));
playbackConnections.add(
timeBar->sigPlaybackStopped().connect(
bind(&GRobotControllerItem::onPlaybackStopped, this)));
}
}
}
}
return true;
}
virtual bool control() {
switch(phase){
case 0 :
qref = interpolator.interpolate(time);
if(time > interpolator.domainUpper()){
phase = 1;
}
break;
case 1:
if(currentFrame < qseq->numFrames()){
MultiValueSeq::Frame frame = qseq->frame(currentFrame++);
for(int i=0; i < numJoints; ++i){
qref[i] = frame[i];
}
}else{
phase = 2;
}
break;
case 2 :
qref = interpolator.interpolate(time);
}
for(int i=0; i < body->numJoints(); ++i){
Link* joint = body->joint(i);
double q = joint->q();
double dq_ref = (qref[i] - qref_old[i]) / timeStep_;
double dq = (q - qold[i]) / timeStep_;
joint->u() = (qref[i] - q) * pgain[i] + (dq_ref - dq) * dgain[i];
qold[i] = q;
}
qref_old = qref;
time += timeStep_;
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;
}
virtual bool control() {
switch(phase){
case 0 :
qref = interpolator.interpolate(time);
if(time > interpolator.domainUpper()){
phase = 1;
}
break;
case 1:
if(currentFrame < qseq->numFrames()){
MultiValueSeq::Frame frame = qseq->frame(currentFrame++);
for(int i=0; i < numJoints; ++i){
qref[i] = frame[i];
}
}else{
interpolator.clear();
interpolator.appendSample(time, qref);
VectorXd q1(numJoints);
q1 = qref;
q1[rarm_shoulder_r] = -0.4;
q1[rarm_shoulder_p] = 0.75;
q1[rarm_elbow] = -2.0;
interpolator.appendSample(time + 3.0, q1);
q1[rarm_elbow] = -1.57;
q1[rarm_shoulder_p] = -0.2;
q1[rarm_wrist_r] = 1.5;
interpolator.appendSample(time + 5.0, q1);
q1[rarm_elbow] = -1.3;
q1[rarm_wrist_y] = -0.24;
interpolator.appendSample(time + 6.0, q1);
interpolator.update();
qref = interpolator.interpolate(time);
phase = 2;
}
break;
case 2 :
qref = interpolator.interpolate(time);
if(time > interpolator.domainUpper()){
interpolator.clear();
interpolator.appendSample(time, qref);
VectorXd q1(numJoints);
q1 = qref;
q1[rarm_wrist_y] = 0.0;
q1[rarm_shoulder_r] = 0.1;
interpolator.appendSample(time + 5.0, q1);
interpolator.update();
qref = interpolator.interpolate(time);
phase = 3;
}
break;
case 3:
qref = interpolator.interpolate(time);
if( rhsensor->F()[1] < -2 ) {
interpolator.clear();
interpolator.appendSample(time, qref);
VectorXd q1(numJoints);
q1 = qref;
q1[rarm_wrist_r] = -0.3;;
interpolator.appendSample(time + 2.0, q1);
interpolator.appendSample(time + 2.5, q1);
q1[rarm_shoulder_p] = -0.13;
q1[rarm_elbow] = -1.8;
interpolator.appendSample(time + 3.5, q1);
interpolator.update();
qref = interpolator.interpolate(time);
phase = 4;
}
break;
case 4 :
qref = interpolator.interpolate(time);
}
for(int i=0; i < body->numJoints(); ++i){
Link* joint = body->joint(i);
double q = joint->q();
double dq_ref = (qref[i] - qref_old[i]) / timeStep_;
double dq = (q - qold[i]) / timeStep_;
joint->u() = (qref[i] - q) * pgain[i] + (dq_ref - dq) * dgain[i];
qold[i] = q;
}
qref_old = qref;
time += timeStep_;
return true;
}
