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

//.........这里部分代码省略.........
    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++)
        {
            transformationSystems_[i].t_ = trajectory.getTrajectoryPosition(rowIndex, i);
            transformationSystems_[i].td_ = trajectory.getTrajectoryVelocity(rowIndex, i);
            transformationSystems_[i].tdd_ = trajectory.getTrajectoryAcceleration(rowIndex, i);
            transformationSystems_[i].f_ = 0.0;
            transformationSystems_[i].ft_ = 0.0;
        }

        //fit the target function:
		//it computes the ideal value of f_ (i.e. ft_) which allows to exactly reproduce
		//the trajectory with the dmp
        if (!integrateAndFit())
        {
            printf("ERROR: Could not integrate system and fit the target function\n");
            params_.isLearned_ = false;
            return params_.isLearned_;
        }
    }

	if(!writeVectorToFile(trajectoryTargetFunctionInput_, "data/trajectory_target_function_input_.txt")) return false;
	
	if(!transformationSystems_[0].writeTrajectoryTargetToFile("data/trajectory_target_.txt")) return false;

    if (!learnTransformationTarget())
    {
        printf("ERROR: Could not learn transformation target.\n");
        params_.isLearned_ = false;
        return params_.isLearned_;
    }

    mseTotal = 0.0;
    normalizedMseTotal = 0.0;
    for (int i = 0; i < params_.numTransformationSystems_; i++)
    {
        double mse;
        double normalizedMse;
        if (transformationSystems_[i].getMSE(mse))
        {
            mseTotal += mse;
        }
        if (transformationSystems_[i].getNormalizedMSE(normalizedMse))
        {
            normalizedMseTotal += normalizedMse;
        }
        transformationSystems_[i].resetMSE();
    }

    printf("Successfully learned DMP from trajectory.\n");
    params_.isLearned_ = true;

    return params_.isLearned_;
}