DummyTrajectoryGenerator.cpp

Go to the documentation of this file.

#include "DummyTrajectoryGenerator.h"

double sitting[6] = {90, 90, 90, 90, 0, 0};
double standing[6] = {180, 180, 0, 0, 0, 0};

DummyTrajectoryGenerator::DummyTrajectoryGenerator(int NumOfJoints) {
    numJoints = NumOfJoints;
}

bool DummyTrajectoryGenerator::initialiseTrajectory() {
    currTraj = SIT;
    trajTime = 2;
    return true;
}

bool DummyTrajectoryGenerator::initialiseTrajectory(Trajectory traj, double time) {
    currTraj = traj;
    trajTime = time;
    return true;
}

std::vector<double> DummyTrajectoryGenerator::getSetPoint(double time) {
    double progress = time / trajTime;
    std::vector<double> angles;

    if (currTraj == SIT) {
        for (int i = 0; i < numJoints; i++) {
            if (progress > 1) {
                angles.push_back(sitting[i]);
            } else {
                angles.push_back(standing[i] + progress * (sitting[i] - standing[i]));
            }
        }
    } else {
        for (int i = 0; i < numJoints; i++) {
            if (progress > 1) {
                angles.push_back(standing[i]);
            } else {
                angles.push_back(sitting[i] + progress * (standing[i] - sitting[i]));
            }
        }
    }
    lastProgress = progress;
    return angles;
}

bool DummyTrajectoryGenerator::isTrajectoryFinished() {
    if (lastProgress > 1.0) {
        return true;
    } else {
        return false;
    }
}