void XTCReader::read(Trajectory &trajectory) {
while (true) {
SmartPointer<Positions> positions = new Positions;
if (!read(*positions, trajectory.getTopology().get())) break;
trajectory.add(positions);
}
}
void TrajectoryTest::testSizeInc(void)
{
Trajectory<CvPoint> *t = new Trajectory<CvPoint> ();
for (unsigned i = 0; i < sampleTrajectory->size(); ++i)
{
CPPUNIT_ASSERT_EQUAL(t->size(), (unsigned int)i);
CvPoint point = (*sampleTrajectory)[i];
t->add(point);
}
CPPUNIT_ASSERT_EQUAL(t->size(), sampleTrajectory->size());
}
void TrajectoryTest::testAdd2(void)
{
Trajectory<CvPoint> trajectory;
for (unsigned i = 0; i < sampleTrajectory->size(); ++i)
{
CvPoint p1 = sampleTrajectory->getPosition(i);
trajectory.add(i + 1, p1);
const CvPoint p2 = trajectory.getPosition(i);
CPPUNIT_ASSERT (equal(p1,p2));
const unsigned frameNumber = trajectory.getFrameNumber(i);
CPPUNIT_ASSERT_EQUAL(frameNumber, i + 1);
}
CPPUNIT_ASSERT(equal(trajectory, *sampleTrajectory));
}
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;
}
bool MathHelper::generateMinJerkTrajectory(const yarp::sig::Vector &start, const yarp::sig::Vector &goal, const double duration,
const double deltaT, Trajectory &trajectory)
{
int trajectoryDimension = trajectory.getDimension();
if ((trajectoryDimension % POS_VEL_ACC) != 0)
{
printf("ERROR: Trajectory dimension (%i) must be a multiple of 3 to contain position, velocity, and acceleration information for each trace.\n",
trajectoryDimension);
return false;
}
trajectoryDimension /= POS_VEL_ACC;
if ((trajectoryDimension != start.size()) || (trajectoryDimension != goal.size()))
{
printf("ERROR: Trajectory dimension (%i), does not match start (%i) and goal (%i).\n", trajectoryDimension, start.size(), goal.size());
return false;
}
trajectory.clear();
int numSteps = static_cast<int> (duration / deltaT);
yarp::sig::Vector tmpCurrent = yarp::math::zeros(POS_VEL_ACC);
yarp::sig::Vector tmpGoal = yarp::math::zeros(POS_VEL_ACC);
yarp::sig::Vector tmpNext = yarp::math::zeros(POS_VEL_ACC);
yarp::sig::Vector next = yarp::math::zeros(trajectoryDimension * POS_VEL_ACC);
Eigen::VectorXd eigenNext;
//add first trajectory point
for (int j = 0; j < trajectoryDimension; j++)
{
next(j * POS_VEL_ACC + 0) = start(j);
next(j * POS_VEL_ACC + 1) = 0;
next(j * POS_VEL_ACC + 2) = 0;
}
yarpToEigenVector(next, eigenNext);
if (!trajectory.add(eigenNext))
{
printf("ERROR: Could not add first trajectory point.\n");
return false;
}
for (int i = 1; i < numSteps; i++)
{
for (int j = 0; j < trajectoryDimension; j++)
{
if (i == 1)
{
tmpCurrent(0) = start(j);
}
else
{
//update the current state
for (int k = 0; k < POS_VEL_ACC; k++)
{
tmpCurrent(k) = next(j * POS_VEL_ACC + k);
}
}
tmpGoal(0) = goal(j);
if (!MathHelper::calculateMinJerkNextStep(tmpCurrent, tmpGoal, duration - ((i - 1) * deltaT), deltaT, tmpNext))
{
printf("ERROR: Could not compute next minimum jerk trajectory point.\n");
return false;
}
for (int k = 0; k < POS_VEL_ACC; k++)
{
next(j * POS_VEL_ACC + k) = tmpNext(k);
}
}
yarpToEigenVector(next, eigenNext);
if (!trajectory.add(eigenNext))
{
printf("ERROR: Could not add next trajectory point.\n");
return false;
}
}
if(!trajectory.writeTrajectoryToFile("data/minJerkTrajectory.txt")) return false;
return true;
}
