RosMsgTrajectoryFactory.cpp

Go to the documentation of this file.

#include <robodyn_controllers/RosMsgTrajectoryFactory.h>

static const double eps = std::numeric_limits<float>::epsilon();

void RosMsgJointTrajectoryFactory::setSettings(const r2_msgs::ControllerJointSettings& settings)
{
    motionLimiter.setLimits(settings.joint_names, settings.jointVelocityLimits, settings.jointAccelerationLimits);
}

void RosMsgJointTrajectoryFactory::getSettings(r2_msgs::ControllerJointSettings& settings)
{
    motionLimiter.getAllLimits(settings.joint_names, settings.jointVelocityLimits, settings.jointAccelerationLimits);
}

void RosMsgJointTrajectoryFactory::setLimits(const std::vector<std::string>& jointNames, double velLim, double accLim)
{
    motionLimiter.setLimits(jointNames, velLim, accLim);
}

void RosMsgJointTrajectoryFactory::setPositionLimiter(boost::shared_ptr<JointNamePositionLimiter> posLimiter)
{
    positionLimiter = posLimiter;
}

boost::shared_ptr<RosMsgJointTrajectory> RosMsgJointTrajectoryFactory::getTrajectory(const sensor_msgs::JointState& jointPositions,
        const sensor_msgs::JointState& jointVels,
        const sensor_msgs::JointState& prevJointVels,
        const trajectory_msgs::JointTrajectory& goalTraj)
{
    boost::shared_ptr<RosMsgJointTrajectory> trajPtr(new RosMsgJointTrajectory);
    trajPtr->setGoalId(goalTraj.header.stamp, goalTraj.header.frame_id);
    trajPtr->setJointNames(goalTraj.joint_names);

    // reorder limits
    motionLimiter.reorderLimits(goalTraj.joint_names);

    KDL::JntArrayVel startVel(goalTraj.joint_names.size());
    KDL::JntArrayVel prevVel(goalTraj.joint_names.size());
    KDL::JntArrayAcc startPose(goalTraj.joint_names.size());

    // these can throw but we want to leave the catch up to the caller
    RosMsgConverter::JointStateToJntArray(jointPositions,   goalTraj.joint_names, startPose.q);
    RosMsgConverter::JointStateToJntArrayVel(jointVels,     goalTraj.joint_names, startVel);
    RosMsgConverter::JointStateToJntArrayVel(prevJointVels, goalTraj.joint_names, prevVel);

    startPose.qdot = startVel.qdot;
    ros::Duration elapsedTime(jointVels.header.stamp - prevJointVels.header.stamp);

    if (elapsedTime > ros::Duration(0.))
    {
        KDL::Subtract(startVel.qdot,   prevVel.qdot,        startPose.qdotdot);
        KDL::Divide(startPose.qdotdot, elapsedTime.toSec(), startPose.qdotdot);
    }

    std::vector<KDL::JntArrayAcc> goalPoses(goalTraj.points.size(), KDL::JntArrayAcc(goalTraj.joint_names.size()));
    std::vector<double>           durationTargets(goalTraj.points.size(), -1.);

    for (unsigned int i = 0; i < goalPoses.size(); ++i)
    {
        if (goalTraj.points[i].positions.size() != goalTraj.joint_names.size())
        {
            std::stringstream err;
            err << "RosMsgJointTrajectoryFactory::getTrajectory() - A goal trajectory pose has mismatched size for joint_names list";
            NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgJointTrajectoryFactory", log4cpp::Priority::ERROR, err.str());
            throw std::runtime_error(err.str());
        }

        for (unsigned int j = 0; j < goalTraj.joint_names.size(); ++j)
        {
            goalPoses[i].q(j) = goalTraj.points[i].positions[j];

            if (goalTraj.points[i].velocities.size() == goalTraj.joint_names.size())
            {
                goalPoses[i].qdot(j) = goalTraj.points[i].velocities[j];
            }

            if (goalTraj.points[i].accelerations.size() == goalTraj.joint_names.size())
            {
                goalPoses[i].qdotdot(j) = goalTraj.points[i].accelerations[j];
            }

            if (positionLimiter->hasLimits(goalTraj.joint_names[j]) &&
                    positionLimiter->limit(goalTraj.joint_names[j], goalPoses[i].q(j)))
            {
                goalPoses[i].qdot(j) = 0.;
                goalPoses[i].qdotdot(j) = 0.;
                KDL::JntArrayAcc iPose;

                if (i == 0)
                {
                    iPose = startPose;
                }
                else
                {
                    iPose = goalPoses[i - 1];
                }

                double aLim = 0;

                if (fabs(goalPoses[i].qdot(j) - iPose.qdot(j)) > eps)
                {
                    aLim = fabs(0.5 * iPose.qdot(j) * iPose.qdot(j) / (goalPoses[i].qdot(j) - iPose.qdot(j)));
                }

                if (aLim > motionLimiter.getAccelerationLimit(goalTraj.joint_names[j]))
                {
                    std::vector<std::string> jointName;
                    jointName.push_back(goalTraj.joint_names[j]);
                    motionLimiter.setLimits(jointName, motionLimiter.getVelocityLimit(goalTraj.joint_names[j]), aLim);
                    NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.RosMsgJointTrajectoryFactory") << log4cpp::Priority::NOTICE << "set acceleration limit to " << aLim;
                }

                std::stringstream ss;
                ss << "Goal Pose limited for " << goalTraj.joint_names[j];
                ss << "  calculated acceleration limit: " << aLim << std::endl;
                ss << "  current acceleration limit: " << motionLimiter.getAccelerationLimit(goalTraj.joint_names[j]) << std::endl;
                NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgJointTrajectoryFactory", log4cpp::Priority::INFO, ss.str());
            }
        }

        durationTargets[i] = goalTraj.points[i].time_from_start.toSec();
    }

    NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgJointTrajectoryFactory", log4cpp::Priority::INFO, "setting joint trajectory");
    trajPtr->setTrajectory(sequenceFactory.getTrajectory(startPose, goalPoses, durationTargets));

    return trajPtr;
}

void RosMsgCartesianTrajectoryFactory::setSettings(const r2_msgs::ControllerPoseSettings& settings)
{
    factory->setVelocityLimits(settings.maxLinearVelocity, settings.maxRotationalVelocity);
    factory->setAccelerationLimits(settings.maxLinearAcceleration, settings.maxRotationalAcceleration);
}

boost::shared_ptr<RosMsgSynchedCartesianTrajectory> RosMsgCartesianTrajectoryFactory::getTrajectory(const r2_msgs::PoseState& poseState,
        const r2_msgs::PoseTrajectory& goalTraj)
{
    boost::shared_ptr<RosMsgSynchedCartesianTrajectory> trajPtr(new RosMsgSynchedCartesianTrajectory);
    trajPtr->setGoalId(goalTraj.header.stamp, goalTraj.header.frame_id);

    // check trajectory validity
    if (goalTraj.nodes.empty())
    {
        std::stringstream err;
        err << "RosMsgCartesianTrajectoryFactory::getTrajectory() - No nodes provided";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgCartesianTrajectoryFactory", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }
    else if (goalTraj.refFrames.size() != goalTraj.nodes.size() && goalTraj.refFrames.size() != 1)
    {
        std::stringstream err;
        err << "RosMsgCartesianTrajectoryFactory::getTrajectory() - refFrames size invalid";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgCartesianTrajectoryFactory", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    trajPtr->setNodeNames(goalTraj.nodes);
    trajPtr->setRefFrames(goalTraj.refFrames);
    trajPtr->setPriorities(goalTraj.node_priorities);

    // get starting poses
    std::vector<KDL::FrameAcc> startPoses(goalTraj.nodes.size());
    KDL::Frame tempPose;

    for (unsigned int i = 0; i < goalTraj.nodes.size(); ++i)
    {
        if (goalTraj.refFrames.size() == 1)
        {
            // these can throw but we want to leave the catch up to the caller
            RosMsgConverter::PoseStateToFrame(poseState, goalTraj.refFrames.at(0), goalTraj.nodes[i], tempPose);
        }
        else
        {
            // these can throw but we want to leave the catch up to the caller
            RosMsgConverter::PoseStateToFrame(poseState, goalTraj.refFrames.at(i), goalTraj.nodes[i], tempPose);
        }

        startPoses[i].p.p = tempPose.p;
        startPoses[i].M.R = tempPose.M;
    }

    std::vector<std::vector<KDL::FrameAcc> > goalPoses(goalTraj.points.size(), std::vector<KDL::FrameAcc>(goalTraj.nodes.size()));
    std::vector<double>                      durationTargets(goalTraj.points.size(), -1.);

    for (unsigned int i = 0; i < goalPoses.size(); ++i)
    {
        if (goalTraj.points[i].positions.size() != goalTraj.nodes.size())
        {
            std::stringstream err;
            err << "RosMsgCartesianTrajectoryFactory::getTrajectory() - A goal trajectory pose has mismatched size for nodes list";
            NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgCartesianTrajectoryFactory", log4cpp::Priority::ERROR, err.str());
            throw std::runtime_error(err.str());
        }

        for (unsigned int j = 0; j < goalTraj.nodes.size(); ++j)
        {
            goalPoses[i][j].p.p.x(goalTraj.points[i].positions[j].position.x);
            goalPoses[i][j].p.p.y(goalTraj.points[i].positions[j].position.y);
            goalPoses[i][j].p.p.z(goalTraj.points[i].positions[j].position.z);
            goalPoses[i][j].M.R = KDL::Rotation::Quaternion(goalTraj.points[i].positions[j].orientation.x,
                                  goalTraj.points[i].positions[j].orientation.y,
                                  goalTraj.points[i].positions[j].orientation.z,
                                  goalTraj.points[i].positions[j].orientation.w);

            // velocity
            if (goalTraj.points[i].velocities.size() == goalTraj.nodes.size())
            {
                goalPoses[i][j].p.v.x(goalTraj.points[i].velocities[j].linear.x);
                goalPoses[i][j].p.v.y(goalTraj.points[i].velocities[j].linear.y);
                goalPoses[i][j].p.v.z(goalTraj.points[i].velocities[j].linear.z);
                goalPoses[i][j].M.w.x(goalTraj.points[i].velocities[j].angular.x);
                goalPoses[i][j].M.w.y(goalTraj.points[i].velocities[j].angular.y);
                goalPoses[i][j].M.w.z(goalTraj.points[i].velocities[j].angular.z);
            }

            // acceleration
            if (goalTraj.points[i].accelerations.size() == goalTraj.nodes.size())
            {
                goalPoses[i][j].p.dv.x(goalTraj.points[i].accelerations[j].linear.x);
                goalPoses[i][j].p.dv.y(goalTraj.points[i].accelerations[j].linear.y);
                goalPoses[i][j].p.dv.z(goalTraj.points[i].accelerations[j].linear.z);
                goalPoses[i][j].M.dw.x(goalTraj.points[i].accelerations[j].angular.x);
                goalPoses[i][j].M.dw.y(goalTraj.points[i].accelerations[j].angular.y);
                goalPoses[i][j].M.dw.z(goalTraj.points[i].accelerations[j].angular.z);
            }
        }

        durationTargets[i] = goalTraj.points[i].time_from_start.toSec();
    }

    NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgJointTrajectoryFactory", log4cpp::Priority::INFO, "setting cartesian trajectory");
    trajPtr->setTrajectory(sequenceFactory.getTrajectory(startPoses, goalPoses, durationTargets));

    return trajPtr;
}