RenderTargetFB::RenderTargetFB( const RenderContextSharedPtr &glContext ) : RenderTarget( glContext) , m_swapBuffersEnabled(false) , m_stereoEnabled( glContext->getFormat().isStereo() ) , m_stereoTarget( glContext->getFormat().isStereo() ? StereoTarget::LEFT_AND_RIGHT : StereoTarget::LEFT ) , m_clearMaskGL(0) , m_clearColorR(0.0f) , m_clearColorG(0.0f) , m_clearColorB(0.0f) , m_clearColorA(0.0f) { TmpCurrent tmpCurrent(this); //sync the viewport size GLint viewport[4]; glGetIntegerv( GL_VIEWPORT, viewport ); this->setPosition(viewport[0], viewport[1]); this->setSize(viewport[2]-viewport[0], viewport[3]-viewport[1]); }
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; }