ik.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <se2ez/core.h>

using namespace se2ez;

void testTree(const RobotPtr &robot)
{
    TreeIK ik(robot, robot->getEEs());
    SE2EZ_INFORM("Testing IK Tree");

    auto state = std::make_shared<State>(robot);

    tf::EigenMap<std::string, Eigen::Isometry2d> goal = {
        {"ee", tf::toIsometry(Eigen::Vector3d{2, 1, math::pi})}  //
    };

    std::map<std::string, IKSolver::Request> request = {
        {"ee", IKSolver::POSITION}  //
    };

    if (ik.solve(state, goal, request))
    {
        SE2EZ_INFORM("Tree IK Success!");
        state->fk(robot);

        std::cout << state->printFrames(robot) << std::endl;
        std::cout << tf::printVector(state->data) << std::endl;
    }
    else
        SE2EZ_ERROR("Tree IK Failed!");
}

void testChain(const RobotPtr &robot)
{
    ChainIK ik(robot, "ee");
    SE2EZ_INFORM("Testing IK Chain");

    auto state = std::make_shared<State>(robot);

    if (ik.solve(state, tf::toIsometry(Eigen::Vector3d{2, 1, math::pi}), IKSolver::POSITION))
    {
        SE2EZ_INFORM("Chain IK Success!");
        state->fk(robot);

        std::cout << state->printFrames(robot) << std::endl;
        std::cout << tf::printVector(state->data) << std::endl;
    }
    else
        SE2EZ_ERROR("Chain IK Failed!");
}

int main(int argc, char **argv)
{
    if (argc < 2)
    {
        std::cout << "Please give filename to load!" << std::endl;
        return -1;
    }

    auto robot = io::loadRobot(std::string(argv[1]));
    if (!robot)
        return -1;

    std::cout << robot->printTree() << std::endl;

    testTree(robot);
    testChain(robot);
}