C++ (Cpp) Trajectory::getMaxDimension Examples

Example #1

File: dynamicMovementPrimitive.cpp Project: xufango/contrib_bk

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;
}