C++ Trajectory::getEndPosition方法代码示例

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++)
        {
//.........这里部分代码省略.........