bool DynamicMovementPrimitive::propagateFull(Trajectory& trajectory, const double samplingDuration, const int numSamples) { if ((!params_.isLearned_) || (!params_.isSetup_) || (!params_.isStartSet_)) { if(!params_.isLearned_) printf("ERROR: DMP is not learned from demonstration.\n"); if(!params_.isSetup_) printf("ERROR: DMP with is not setup. Need to set start, goal, and duration first.\n"); return false; } if(trajectory.getMaxDimension() < params_.numTransformationSystems_ * POS_VEL_ACC) return false; if(trajectory.getMaxLength() <= numSamples) return false; double specialSamplingFrequency = static_cast<double> (numSamples) / (samplingDuration); if (!trajectory.setSamplingFrequency(specialSamplingFrequency)) { printf("ERROR: Could not set sampling frequency.\n"); return false; } VectorXd desiredCoordinates = VectorXd::Zero(params_.numTransformationSystems_ * POS_VEL_ACC); bool movementFinished = false; while (!movementFinished) { if (!propagateStep(desiredCoordinates, movementFinished, samplingDuration, numSamples)) { printf("ERROR: Could not propagate dmp.\n"); return false; } if (!trajectory.add(desiredCoordinates)) { printf("ERROR: Could not add point to trajectory.\n"); return false; } } return true; }