RosMsgForceTrajectoryFactory.cpp

Go to the documentation of this file.

#include <robodyn_controllers/RosMsgForceTrajectoryFactory.h>

RosMsgForceTrajectoryFactory::RosMsgForceTrajectoryFactory()
    : logCategory("gov.nasa.controllers.RosMsgForceTrajectoryFactory")
{
}

boost::shared_ptr<RosMsgJointTrajectory> RosMsgForceTrajectoryFactory::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 r2_msgs::ForceControlAxisArray& forceControlAxes) 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<ForceTrajectory> ikTraj(new ForceTrajectory);
    ikTraj->setTreeIk(treeIk);

    // create the force control map for this trajectory
    std::map<std::string, std::pair<std::vector<int>, std::vector<double> > > forceControlMap;

    if (forceControlAxes.nodes.size() != forceControlAxes.axes.size())
    {
        std::stringstream err;
        err << "forceControlAxis nodes and axis must have the same size!" << forceControlAxes.nodes.size() << " vs " << forceControlAxes.axes.size();
        NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    for (unsigned int i = 0; i < forceControlAxes.nodes.size(); ++i)
    {
        if (forceControlAxes.axes[i].axis.size() != forceControlAxes.axes[i].magnitude.size())
        {
            std::stringstream err;
            err << "Node axis size and magnitude size must be the same! Axis size:" << forceControlAxes.axes[i].axis.size() << " Magnitude size: " << forceControlAxes.axes[i].magnitude.size();
            NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::ERROR, err.str());
            throw std::runtime_error(err.str());
        }

        if (!forceControlAxes.axes[i].axis.empty())
        {
            forceControlMap[forceControlAxes.nodes[i]] = std::make_pair(std::vector<int>(forceControlAxes.axes.at(i).axis.begin(), forceControlAxes.axes.at(i).axis.end()),
                    std::vector<double>(forceControlAxes.axes.at(i).magnitude.begin(), forceControlAxes.axes.at(i).magnitude.end()));
        }
    }

    forceController->createForceControlMap(forceControlMap);

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

    try
    {
        RosMsgConverter::JointStateToJntArray(jointPositions, jointNames, startJoints.q);
        RosMsgConverter::JointStateToJntArrayVel(jointVels, jointNames, startVel);
        RosMsgConverter::JointStateToJntArrayVel(prevJointVels, jointNames, prevVel);
    }
    catch (std::exception& e)
    {
        NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::ERROR, e.what());
    }

    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)
    {
        try
        {
            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]);
            }
        }
        catch (std::exception& e)
        {
            NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::ERROR, e.what());
        }
    }

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

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

            RosMsgConverter::PoseStateToFrameAcc(poseVels, treeIk->getBaseName(), goalTraj.nodes[i], startPose[i]);
            NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG
                    << "startPose[" << i << "] (" << goalTraj.nodes[i] << "): " << startPose[i].p.p.x() << ", " << startPose[i].p.p.y() << ", " << startPose[i].p.p.z()
                    << ", " << startPose[i].M.R.GetRot().x() << ", " << startPose[i].M.R.GetRot().y() << ", " << startPose[i].M.R.GetRot().z()
                    << "\n\t" << startPose[i].p.v.x() << ", " << startPose[i].p.v.y() << ", " << startPose[i].p.v.z()
                    << ", " << startPose[i].M.dw.x() << ", " << startPose[i].M.dw.y() << ", " << startPose[i].M.dw.z();
            RosMsgConverter::PoseStateToFrame(poseState, treeIk->getBaseName(), goalTraj.nodes[i], frame);
        }
        catch (std::exception& e)
        {
            NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::ERROR, e.what());
        }

        startPose[i].M.R = frame.M;
        startPose[i].p.p = frame.p;
        forceController->setInitialPose(goalTraj.nodes[i], frame);
    }

    // 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.RosMsgForceTrajectoryFactory", 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];
            NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG
                    << "goalPoses[" << i << "][" << j << "] (" << goalTraj.nodes[j] << "): " << goalPoses[i][j].p.p.x() << ", " << goalPoses[i][j].p.p.y() << ", " << goalPoses[i][j].p.p.z()
                    << ", " << goalPoses[i][j].M.R.GetRot().x() << ", " << goalPoses[i][j].M.R.GetRot().y() << ", " << goalPoses[i][j].M.R.GetRot().z();
        }

        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(logCategory, 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(logCategory, 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->setCartesianHybCntrl(forceController);
    NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgForceTrajectoryFactory", log4cpp::Priority::INFO, "setCartesianHybCntrl success");

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

    trajPtr->setTrajectory(ikTraj);

    return trajPtr;
}