本文整理汇总了C++中Interpolator::appendSample方法的典型用法代码示例。如果您正苦于以下问题：C++ Interpolator::appendSample方法的具体用法？C++ Interpolator::appendSample怎么用？C++ Interpolator::appendSample使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Interpolator的用法示例。
在下文中一共展示了Interpolator::appendSample方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: initialize
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;
}示例2: control
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;
}