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++) { 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_; }