RosMsgTreeIkTrajectoryFactory.cpp

Go to the documentation of this file.

#include <robodyn_controllers/RosMsgTreeIkTrajectoryFactory.h>

boost::shared_ptr<RosMsgJointTrajectory> RosMsgTreeIkTrajectoryFactory::getTrajectory(const sensor_msgs::JointState& jointPositions,
        const sensor_msgs::JointState& jointVels,
        const sensor_msgs::JointState& prevJointVels,
        const r2_msgs::PoseState& poseState,
        const r2_msgs::PoseState& poseVels,
        const r2_msgs::PoseTrajectory& goalTraj) const
{
    boost::shared_ptr<RosMsgJointTrajectory> trajPtr(new RosMsgJointTrajectory);
    trajPtr->setGoalId(goalTraj.header.stamp, goalTraj.header.frame_id);
    std::vector<std::string> jointNames;
    treeIk->getJointNames(jointNames);
    trajPtr->setJointNames(jointNames);

    boost::shared_ptr<TreeIkTrajectory> ikTraj(new TreeIkTrajectory);
    ikTraj->setTreeIk(treeIk);

    // get startJoints
    KDL::JntArrayVel startVel(jointNames.size());
    KDL::JntArrayVel prevVel(jointNames.size());
    KDL::JntArrayAcc startJoints(jointNames.size());

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

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

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

    // get refPoses
    std::vector<KDL::FrameAcc> refPoses(goalTraj.nodes.size());

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

    // get startPose
    std::vector<KDL::FrameAcc> startPose(goalTraj.nodes.size());
    KDL::Frame                 frame;

    for (unsigned int i = 0; i < goalTraj.nodes.size(); ++i)
    {

        // these can throw but we want to leave the catch up to the caller
        RosMsgConverter::PoseStateToFrameAcc(poseVels, treeIk->getBaseName(), goalTraj.nodes[i], startPose[i]);
        RosMsgConverter::PoseStateToFrame(poseState, treeIk->getBaseName(), goalTraj.nodes[i], frame);

        startPose[i].M.R = frame.M;
        startPose[i].p.p = frame.p;
    }

    // get goalPoses
    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 << "RosMsgTreeIkTrajectoryFactory::getTrajectory() - A goal trajectory pose has mismatched size for nodes list";
            NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgTreeIkTrajectoryFactory", 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);
            }

            goalPoses[i][j] = refPoses[j] * goalPoses[i][j];
        }

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

    // get priorities
    std::vector<KdlTreeIk::NodePriority> priorities(goalTraj.nodes.size());

    if (goalTraj.node_priorities.size() != 0 && goalTraj.node_priorities.size() != goalTraj.nodes.size())
    {
        std::stringstream err;
        err << "invalid node_priorities size";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgTreeIkTrajectoryFactory", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    for (unsigned int nodeIndex = 0; nodeIndex < goalTraj.node_priorities.size(); ++nodeIndex)
    {
        if (goalTraj.node_priorities[nodeIndex].axis_priorities.size() == 1)
        {
            for (unsigned int axisIndex = 0; axisIndex < 6; ++axisIndex)
            {
                priorities[nodeIndex][axisIndex] = static_cast<int>(goalTraj.node_priorities[nodeIndex].axis_priorities[0]);
            }
        }
        else
        {
            if (goalTraj.node_priorities[nodeIndex].axis_priorities.size() != 6)
            {
                std::stringstream err;
                err << "invalid axis_priorities size at node_priorities[" << nodeIndex << "]";
                NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgTreeIkTrajectoryFactory", log4cpp::Priority::ERROR, err.str());
                throw std::runtime_error(err.str());
            }

            for (unsigned int axisIndex = 0; axisIndex < 6; ++axisIndex)
            {
                priorities[nodeIndex][axisIndex] = static_cast<int>(goalTraj.node_priorities[nodeIndex].axis_priorities[axisIndex]);
            }
        }
    }

    ikTraj->setCartesianTrajectory(sequenceFactory->getTrajectory(startPose, goalPoses, durationTargets));
    ikTraj->setInitialJoints(startJoints);
    ikTraj->setNodeNames(goalTraj.nodes);
    ikTraj->setNodePriorities(priorities);

    trajPtr->setTrajectory(ikTraj);

    return trajPtr;
}