本文整理汇总了C++中VectorOf::getFirst方法的典型用法代码示例。如果您正苦于以下问题:C++ VectorOf::getFirst方法的具体用法?C++ VectorOf::getFirst怎么用?C++ VectorOf::getFirst使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类VectorOf
的用法示例。
在下文中一共展示了VectorOf::getFirst方法的9个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: setHeadCtrlModes
bool imuIdentifierThread::setHeadCtrlModes(const string &_s)
{
printMessage(1,"Setting %s mode for head joints..\n",_s.c_str());
if (_s!="position" && _s!="velocity")
return false;
VectorOf<int> jointsToSet;
jointsToSet.push_back(0);
jointsToSet.push_back(1);
jointsToSet.push_back(2);
VectorOf<int> modes;
if (_s=="position")
{
modes.push_back(VOCAB_CM_POSITION);
modes.push_back(VOCAB_CM_POSITION);
modes.push_back(VOCAB_CM_POSITION);
}
else if (_s=="velocity")
{
modes.push_back(VOCAB_CM_VELOCITY);
modes.push_back(VOCAB_CM_VELOCITY);
modes.push_back(VOCAB_CM_VELOCITY);
}
imodH -> setControlModes(jointsToSet.size(),
jointsToSet.getFirst(),
modes.getFirst());
Time::delay(0.1);
return true;
}
示例2: handHandler
void handHandler(const bool b)
{
if (b)
{
if (s1==idle)
s1=triggered;
else if (s1==triggered)
{
if (++c1*getPeriod()>0.5)
{
imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
s1=running;
}
}
else
ivel->velocityMove(joints.size(),joints.getFirst(),vels.data());
}
else
{
if (s1==triggered)
vels=-1.0*vels;
if (c1!=0)
ivel->stop(joints.size(),joints.getFirst());
s1=idle;
c1=0;
}
}
示例3: setCtrlModes
bool reactCtrlThread::setCtrlModes(const VectorOf<int> &jointsToSet,
const string &_p, const string &_s)
{
if (_s!="position" && _s!="velocity")
return false;
if (jointsToSet.size()==0)
return true;
VectorOf<int> modes;
for (size_t i=0; i<jointsToSet.size(); i++)
{
if (_s=="position")
{
modes.push_back(VOCAB_CM_POSITION);
}
else if (_s=="velocity")
{
modes.push_back(VOCAB_CM_VELOCITY);
}
}
if (_p=="arm")
{
imodA->setControlModes(jointsToSet.size(),
jointsToSet.getFirst(),
modes.getFirst());
}
else if (_p=="torso")
{
imodT->setControlModes(jointsToSet.size(),
jointsToSet.getFirst(),
modes.getFirst());
}
else
return false;
return true;
}
示例4: areJointsHealthyAndSet
bool reactCtrlThread::areJointsHealthyAndSet(VectorOf<int> &jointsToSet,
const string &_p, const string &_s)
{
VectorOf<int> modes;
if (_p=="arm")
{
modes=encsA->size();
imodA->getControlModes(modes.getFirst());
}
else if (_p=="torso")
{
modes=encsT->size();
imodT->getControlModes(modes.getFirst());
}
else
return false;
for (size_t i=0; i<modes.size(); i++)
{
if ((modes[i]==VOCAB_CM_HW_FAULT) || (modes[i]==VOCAB_CM_IDLE))
return false;
if (_s=="velocity")
{
if (modes[i]!=VOCAB_CM_MIXED || modes[i]!=VOCAB_CM_VELOCITY)
jointsToSet.push_back(i);
}
else if (_s=="position")
{
if (modes[i]!=VOCAB_CM_MIXED || modes[i]!=VOCAB_CM_POSITION)
jointsToSet.push_back(i);
}
}
return true;
}
示例5: close
bool close()
{
iarm->stopControl();
iarm->restoreContext(startup_context);
drvCart.close();
ivel->stop(joints.size(),joints.getFirst());
for (size_t i=0; i<modes.size(); i++)
modes[i]=VOCAB_CM_POSITION;
imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
drvHand.close();
if (simulator)
{
Bottle cmd,reply;
cmd.addString("world");
cmd.addString("del");
cmd.addString("all");
simPort.write(cmd,reply);
simPort.close();
}
if (gaze)
{
igaze->stopControl();
drvGaze.close();
}
igeo->stopFeedback();
igeo->setTransformation(eye(4,4));
drvGeomagic.close();
forceFbPort.close();
return true;
}
示例6: goHome
bool imuIdentifierThread::goHome()
{
setHeadCtrlModes("position");
// Vector pos0(6,0.0);
// iposH -> positionMove(pos0.data());
VectorOf<int> jointsToSet;
jointsToSet.push_back(0);
jointsToSet.push_back(1);
jointsToSet.push_back(2);
Vector poss(3,0.0);
iposH -> positionMove(jointsToSet.size(),
jointsToSet.getFirst(),
poss.data());
return true;
}
示例7: setJointsCtrlMode
void Controller::setJointsCtrlMode()
{
if (jointsToSet.size()==0)
return;
VectorOf<int> modes;
for (size_t i=0; i<jointsToSet.size(); i++)
{
if (jointsToSet[i]<eyesJoints[0])
{
if (neckPosCtrlOn)
modes.push_back(VOCAB_CM_POSITION_DIRECT);
else
modes.push_back(VOCAB_CM_VELOCITY);
}
else
modes.push_back(VOCAB_CM_MIXED);
}
modHead->setControlModes(jointsToSet.size(),jointsToSet.getFirst(),
modes.getFirst());
}
示例8: processWayPoint
bool imuIdentifierThread::processWayPoint()
{
processIMU();
if (wayPoints[currentWaypoint].name == "START " ||
wayPoints[currentWaypoint].name == "END " ||
wayPoints[currentWaypoint].name == "MIDDLE " )
{
if (posCtrlFlag)
{
printMessage(1,"Putting head in home position..\n");
goHome();
posCtrlFlag = false;
}
if (yarp::os::Time::now() - timeNow > CTRL_PERIOD)
{
posCtrlFlag = true;
return false;
}
}
else
{
Vector jls = wayPoints[currentWaypoint].jntlims;
Vector vls = wayPoints[currentWaypoint].vels;
bool flag = false;
iencsH->getEncoders(encsH->data());
yarp::sig::Vector head = *encsH;
// ivelH -> velocityMove(vls.data());
VectorOf<int> jointsToSet;
jointsToSet.push_back(0);
jointsToSet.push_back(1);
jointsToSet.push_back(2);
ivelH -> velocityMove(jointsToSet.size(),
jointsToSet.getFirst(),
vls.data());
for (int i = 0; i < 3; i++)
{
if (vls(i) > 0.0)
{
if (jls(i) - head(i) > 0.0)
{
flag = true;
}
}
else if (vls(i) < 0.0)
{
if (jls(i) - head(i) < 0.0)
{
flag = true;
}
}
}
return flag;
}
return true;
}
示例9: configure
//.........这里部分代码省略.........
simPort.close();
if (gaze)
drvGaze.close();
drvCart.close();
return false;
}
drvHand.view(imod);
drvHand.view(ipos);
drvHand.view(ivel);
iarm->storeContext(&startup_context);
iarm->restoreContext(0);
Vector dof(10,1.0);
if (!torso)
dof[0]=dof[1]=dof[2]=0.0;
else
dof[1]=0.0;
iarm->setDOF(dof,dof);
iarm->setTrajTime(Tp2p);
Vector accs,poss;
for (int i=0; i<9; i++)
{
joints.push_back(7+i);
modes.push_back(VOCAB_CM_POSITION);
accs.push_back(1e9);
vels.push_back(100.0);
poss.push_back(0.0);
}
poss[0]=20.0;
poss[1]=70.0;
imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
ipos->setRefAccelerations(joints.size(),joints.getFirst(),accs.data());
ipos->setRefSpeeds(joints.size(),joints.getFirst(),vels.data());
ipos->positionMove(joints.size(),joints.getFirst(),poss.data());
joints.clear();
modes.clear();
vels.clear();
for (int i=2; i<9; i++)
{
joints.push_back(7+i);
modes.push_back(VOCAB_CM_VELOCITY);
vels.push_back(40.0);
}
vels[vels.length()-1]=100.0;
s0=s1=idle;
c0=c1=0;
onlyXYZ=true;
stateStr[idle]="idle";
stateStr[triggered]="triggered";
stateStr[running]="running";
Matrix T=zeros(4,4);
T(0,1)=1.0;
T(1,2)=1.0;
T(2,0)=1.0;
T(3,3)=1.0;
igeo->setTransformation(SE3inv(T));
igeo->setCartesianForceMode();
igeo->getMaxFeedback(maxFeedback);