robot.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <iostream>

#include <se2ez/core.h>

using namespace se2ez;

int main()
{
    auto f_base = std::make_shared<Frame>("base", Eigen::Vector3d{0, 0, 0},   //
                                          Joint::FLOAT,                       //
                                          Eigen::Vector3d{-1, -1, math::pi},  //
                                          Eigen::Vector3d{1, 1, -math::pi});
    auto f_l1 = std::make_shared<Frame>("link_1", Eigen::Vector3d{1, 0, 0},  //
                                        Joint::REVOLUTE, -math::pi, math::pi);
    auto f_l2 = std::make_shared<Frame>("link_2", Eigen::Vector3d{0, 0, 0},  //
                                        Joint::PRISMATIC, 0, 1);
    auto f_l3 = std::make_shared<Frame>("link_3", Eigen::Vector3d{1, 0, 0},  //
                                        Joint::REVOLUTE, -math::pi, math::pi);
    auto f_e1 = std::make_shared<Frame>("ee_1", Eigen::Vector3d{0, 0, 0}, Joint::FIXED);
    auto f_e2 = std::make_shared<Frame>("ee_2", Eigen::Vector3d{0, 0, 0}, Joint::FIXED);

    auto robot = std::make_shared<Robot>();
    robot->attachFrame(f_base);
    robot->attachFrame(f_l1, f_base);
    robot->attachFrame(f_l2, f_l1);

    robot->attachFrame(f_l3, f_l1);
    robot->attachFrame(f_l3, f_l2);

    robot->attachFrame(f_e1, f_l1);
    robot->attachFrame(f_e1, f_l3);
    robot->attachFrame(f_e1, f_l2);

    robot->attachFrame(f_e2, f_l3);
    robot->attachFrame(f_e2, f_l2);
    robot->attachFrame(f_e2, f_l3);

    robot->compileTree();

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

    State s(robot);
    s.setFloatJoint(robot, "base", Eigen::Vector3d{0, 1, 0});
    s.setJoint(robot, "link_1", 0);
    s.setJoint(robot, "link_2", 1);
    s.setJoint(robot, "link_3", math::pi2);

    s.fk(robot);

    std::cout << s.printFrames(robot) << std::endl;
}