MinJerkTrajectory.cpp

Go to the documentation of this file.

#include <robodyn_controllers/MinJerkTrajectory.h>

#include "nasa_common_logging/Logger.h"
#include <tf/tf.h>
#include <tf_conversions/tf_kdl.h>
#include <limits>

void MinJerkUtility::TimeUtility::setTime(double timeIn)
{
    if (timeIn < 0. || timeIn > 1.)
    {
        std::stringstream err;
        err << "MinJerkUtility::TimeUtility::setTime() - invalid time value (" << timeIn << ")";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.MinJerkUtility", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
        return;
    }

    t  = timeIn;
    t2 = t * t;
    t3 = t2 * t;
    t4 = t3 * t;
    t5 = t4 * t;
}

void MinJerkUtility::setConstraints(double pinit, double pfinal, double vinit, double vfinal, double ainit, double afinal)
{
    vinit = 0.;
    vfinal = 0.;
    ainit = 0.;
    afinal = 0.;

    // p1 - p0
    dist = pfinal - pinit;
    // a0   = p0
    a0   = pinit;
    // a1   = v0
    a1   = vinit;
    // a2   = vdot0/2
    a2   = ainit / 2.;
    // a3   = 10(p1-p0) - 6a1 - 3a2 - 4v1 + vdot1/2
    a3   = 10. * dist - 6. * a1 - 3. * a2 - 4. * vfinal + afinal / 2.;
    // a4   = 7v1 - vdot1 - 15(p1-p0) + 8a1 + 3a2
    a4   = 7. * vfinal - afinal - 15.*dist + 8. * a1 + 3. * a2;
    // a5   = vdot1/2 - 3v1 - a2 - 3a1 + 6(p1-p0)
    a5   = afinal / 2. - 3. * vfinal - a2 - 3. * a1 + 6. * dist;

    dist = fabs(dist);
}

double MinJerkUtility::duration(double vmax) const
{
    static const double eps = std::numeric_limits<float>::epsilon();

    if (vmax <= eps)
    {
        if (dist > eps)
        {
            // movement needed but no movement allowed by velocity
            std::stringstream err;
            err << "MinJerkUtility::duration() - move requested but velocity limited to zero";
            NasaCommonLogging::Logger::log("gov.nasa.controllers.MinJerkUtility", log4cpp::Priority::ERROR, err.str());
            throw std::runtime_error(err.str());
            return 0.;
        }
        else
        {
            return 0.;
        }
    }
    else
    {
        return dist * 1.875 / vmax;
    }
}

double MinJerkUtility::p(const TimeUtility& t) const
{
    return a0 + a1 * t.t + a2 * t.t2 + a3 * t.t3 + a4 * t.t4 + a5 * t.t5;
}

double MinJerkUtility::v(const TimeUtility& t) const
{
    return a1 + 2.*a2 * t.t + 3.*a3 * t.t2 + 4.*a4 * t.t3 + 5.*a5 * t.t4;
}

double MinJerkUtility::a(const TimeUtility& t) const
{
    return 2.*a2 + 6.*a3 * t.t + 12.*a4 * t.t2 + 20.*a5 * t.t3;
}

MinJerkJointTrajectory::MinJerkJointTrajectory(const JointVelocityLimiter& jvl, const KDL::JntArrayAcc& startPose,
        const KDL::JntArrayAcc& goalPose, double durationIn)
    : JointTrajectory(), JointVelocityLimiter(jvl)
{
    if (startPose.q.rows() != goalPose.q.rows())
    {
        std::stringstream err;
        err << "MinJerkJointTrajectory::MinJerkJointTrajectory() - startPose and goalPose lengths don't match";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.MinJerkJointTrajectory", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
        return;
    }

    utils.resize(startPose.q.rows());

    for (unsigned int i = 0; i < startPose.q.rows(); ++i)
    {
        utils[i].setConstraints(startPose.q(i), goalPose.q(i), startPose.qdot(i), goalPose.qdot(i), startPose.qdotdot(i), goalPose.qdotdot(i));

        double jointDur = utils[i].duration(getVelocityLimit(i));

        if (jointDur > durationIn)
        {
            durationIn = jointDur;
        }
    }

    setDuration(durationIn);
}

MinJerkJointTrajectory::~MinJerkJointTrajectory()
{

}

void MinJerkJointTrajectory::getPose(double time, KDL::JntArrayAcc& pose)
{
    MinJerkUtility::TimeUtility timeU;
    double dur = getDuration();

    if (dur == 0)
    {
        if (time > 0)
        {
            throw std::runtime_error("MinJerkJointTrajectory::getPose cannot get a pose for a zero duration trajectory with nonzero time");
            return;
        }
        else
        {
            timeU.setTime(0.);
        }
    }
    else
    {
        timeU.setTime(time / getDuration());
    }

    pose.resize(utils.size());

    for (unsigned int i = 0; i < utils.size(); ++i)
    {
        pose.q(i)       = utils[i].p(timeU);
        pose.qdot(i)    = utils[i].v(timeU) / dur;
        pose.qdotdot(i) = utils[i].a(timeU) / (dur * dur);
    }
}

MinJerkCartesianTrajectory::MinJerkCartesianTrajectory(const CartesianVelocityLimiter& cvl,
        const KDL::FrameAcc& startPose, const KDL::FrameAcc& goalPose, double durationIn)
    : CartesianTrajectory(), CartesianVelocityLimiter(cvl)
{
    utils.resize(2);

    // linear
    startVec = startPose.p.p;
    moveVec  = goalPose.p.p - startVec;

    utils[0].setConstraints(0., 1., startPose.p.v.Norm(), goalPose.p.v.Norm(), startPose.p.dv.Norm(), startPose.p.dv.Norm());
    double linDuration = moveVec.Norm() * utils[0].duration(getLinearVelocityLimit());

    // angular
    double x, y, z, w;

    startPose.M.R.GetQuaternion(x, y, z, w);
    startQ.setValue(x, y, z, w);
    goalPose.M.R.GetQuaternion(x, y, z, w);
    goalQ.setValue(x, y, z, w);

    double angle = startQ.angleShortestPath(goalQ);

    utils[1].setConstraints(0., 1., startPose.M.w.Norm(), goalPose.M.w.Norm(), startPose.M.dw.Norm(), startPose.M.dw.Norm());
    double rotDuration = angle * utils[1].duration(getRotationalVelocityLimit());

    setDuration(std::max(std::max(linDuration, rotDuration), durationIn));
}

MinJerkCartesianTrajectory::~MinJerkCartesianTrajectory()
{

}

void MinJerkCartesianTrajectory::getPose(double time, KDL::FrameAcc& pose)
{
    MinJerkUtility::TimeUtility timeU;
    timeU.setTime(time / getDuration());

    double p = utils[0].p(timeU);
    pose.p   = startVec + moveVec * p;
    p = utils[1].p(timeU);
    tf::Quaternion stepQ = startQ.slerp(goalQ, p).normalize();
    pose.M               = KDL::Rotation::Quaternion(stepQ.x(), stepQ.y(), stepQ.z(), stepQ.w());
}