本文整理汇总了C++中Trajectory::getEndPosition方法的典型用法代码示例。如果您正苦于以下问题:C++ Trajectory::getEndPosition方法的具体用法?C++ Trajectory::getEndPosition怎么用?C++ Trajectory::getEndPosition使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Trajectory
的用法示例。
在下文中一共展示了Trajectory::getEndPosition方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: learnFromTrajectory
bool DynamicMovementPrimitive::learnFromTrajectory(const Trajectory &trajectory)
{
if (!initialized_)
{
printf("ERROR: DMP motion unit is not initialized, not learning from trajectory.\n");
params_.isLearned_ = false;
return params_.isLearned_;
}
int numRows = trajectory.getLength();
if (numRows < MIN_NUM_DATA_POINTS)
{
printf("ERROR: Trajectory has %i rows, but should have at least %i.\n", numRows, MIN_NUM_DATA_POINTS);
params_.isLearned_ = false;
return params_.isLearned_;
}
double samplingFrequency = trajectory.getSamplingFrequency();
if (samplingFrequency <= 0)
{
printf("ERROR: Sampling frequency %f [Hz] of the trajectory is not valid.\n",samplingFrequency);
params_.isLearned_ = false;
return params_.isLearned_;
}
//set teaching duration to the duration of the trajectory
params_.teachingDuration_ = static_cast<double> (numRows) / static_cast<double> (samplingFrequency);
params_.deltaT_ = static_cast<double> (1.0) / samplingFrequency;
params_.initialDeltaT_ = params_.deltaT_;
params_.tau_ = params_.teachingDuration_;
params_.initialTau_ = params_.tau_;
//compute alpha_x such that the canonical system drops below the cutoff when the trajectory has finished
//alpha_x is the time constant for the phase system (second order asimptotically stable system)
params_.alphaX_ = -log(params_.canSysCutoff_);
double mseTotal = 0.0;
double normalizedMseTotal = 0.0;
for (int i = 0; i < params_.numTransformationSystems_; i++)
{
transformationSystems_[i].trajectoryTarget_.clear();
transformationSystems_[i].resetMSE();
}
trajectoryTargetFunctionInput_.clear();
//reset canonical system
resetCanonicalState();
//obtain start and goal position
VectorXd start = VectorXd::Zero(params_.numTransformationSystems_);
if (!trajectory.getStartPosition(start))
{
printf("ERROR: Could not get the start position of the trajectory\n");
params_.isLearned_ = false;
return params_.isLearned_;
}
VectorXd goal = VectorXd::Zero(params_.numTransformationSystems_);
if (!trajectory.getEndPosition(goal))
{
printf("ERROR: Could not get the goal position of the trajectory\n");
params_.isLearned_ = false;
return params_.isLearned_;
}
//set y0 to start state of trajectory and set goal to end of the trajectory
for (int i = 0; i < params_.numTransformationSystems_; i++)
{
//check whether all this is necessary (I don't think so...)
transformationSystems_[i].reset();
//set start and goal
transformationSystems_[i].setStart(start(i));
transformationSystems_[i].setGoal(goal(i));
//set current state to start state (position and velocity)
transformationSystems_[i].setState(start(i), 0.0);
}
for (int i = 0; i < params_.numTransformationSystems_; i++)
{
transformationSystems_[i].setInitialStart(transformationSystems_[i].y0_);
transformationSystems_[i].setInitialGoal(transformationSystems_[i].goal_);
}
//for each time step and for each dimension, perform supervised learning of the input trajectory
//Actually is not a "classical" learning problem...here the problem is how to encode the
//target trajectory in the dmp by representing it as a second order system modulated with
//a nonlinear function f.
for (int rowIndex = 0; rowIndex < numRows; rowIndex++)
{
//set transformation target:
//t_, td_ and tdd_ represent the current position, velocity and acceleration we want
//to learn throught supervised learning. f_ represents the current values of
//the nonlinear function used to modulate the dmp behaviour, while ft_ is the target
//value for such nonlinear function.
//NOTE: is f_ actually used anywhere?????
for (int i = 0; i < params_.numTransformationSystems_; i++)
{
//.........这里部分代码省略.........