main.cpp

Go to the documentation of this file.

//#include <QCoreApplication>
#include "endorob.h"
#include <iostream>
#include <Eigen/StdVector>
 
 
#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif
 
 
int main(int argc, char *argv[])
{
//    QCoreApplication app(argc, argv);
 
 
    Endorob truc[12];
 
 
 
 
    for(auto i = 0; i < 12; i++)
    {
        truc[i].add_DH_entry(0,0,0,0,Joint_type::Revolute);
        truc[i].initialize();
    }
 
    Mat4d Tkuka_global = Mat4d::Identity(4,4);
    Tkuka_global << -1, 0,  0, 420,
              0, 1,  0,   0,
              0, 0, -1, 570,
              0, 0,  0,   1;
    std::cout << "T kuka global:\n" << Tkuka_global << std::endl << std::endl;
 
    Mat4d Tkuka_local = Mat4d::Identity(4,4);
    std::cout << "T kuka local:\n" << Tkuka_local << std::endl << std::endl;
 
    Mat4d Tbowl_local = Mat4d::Identity(4,4);
    Tbowl_local.block<3,3>(0,0) = rot_y(30*M_PI/180);
    Tbowl_local.block<3,1>(0,3) << 25.10, 0, 100.73;
    std::cout << "Tbowl local:\n" << Tbowl_local << std::endl << std::endl;
 
    Mat4d Tsnake_local = Mat4d::Identity(4,4);
    Tsnake_local.block<3,1>(0,3) << 0, 0, 0;
    Tsnake_local.block<3,3>(0,0) = rot_z(1*M_PI/180);
    std::cout << "Tsnake:\n" << Tsnake_local << std::endl << std::endl;
 
 
    Mat4d Tbowl_global = Tkuka_global * Tbowl_local ;
    std::cout << "bowl global:\n" << Tbowl_global << std::endl << std::endl;
 
    Mat4d goback = Tbowl_global * invert_transform( Tbowl_local );
    std::cout << "back to kuka:\n" << goback << std::endl << std::endl;
 
    Mat4d bowl_motion = Mat4d::Identity(4,4);
    bowl_motion.block<3,1>(0,3) << 0, 0, 0;
    bowl_motion.block<3,3>(0,0) = rot_z(1*M_PI/180);
 
    goback = Tbowl_global * invert_transform( Tbowl_local * bowl_motion );
    std::cout << "kuka motion:\n" << goback << std::endl << std::endl;
    std::cout << "kuka motion:\n" << goback << std::endl << std::endl;
 
 
 
 
 
 
 
 
//    Mat4d kukamotion = Mat4d::Identity(4,4);
//    kukamotion.block<3,1>(0,3) = rot_y(-30*M_PI/180) * snakemotion.block<3,1>(0,3) + Tkuka.block<3,1>(0,3);//R * vec + T;
//    kukamotion.block<3,3>(0,0) = snakemotion.block<3,3>(0,0) * Tkuka.block<3,3>(0,0);
//    std::cout << "transformedinkukaframe:\n" << kukamotion << std::endl << std::endl;
 
 
 
//    Mat4d kukamotionbis = Mat4d::Identity(4,4);
//    Mat4d ttt = get_transform_increment(start_point_in_bowl_frame,goal_point_in_bowl_frame);
 
 
//    kukamotionbis = Tbowl_to_kuka * ttt;
//    std::cout << "kukamotionbis:\n" << kukamotionbis << std::endl << std::endl;
 
//    kukamotionbis = invert_transform(Tkuka_to_bowl) * ttt;
//    std::cout << "kukamotionbis:\n" << kukamotionbis << std::endl << std::endl;
 
 
    Mat4d tmpp = Mat4d::Identity(4,4);
    tmpp.block<3,1>(0,3) << 12,23,56;
    tmpp.block<3,3>(0,0) << rot_z(M_PI);
 
    Mat4d start = Mat4d::Identity(4,4), goal = Mat4d::Identity(4,4), tmp = Mat4d::Identity(4,4);
    start.block<3,1>(0,3) << -13,9,23;
    start.block<3,3>(0,0) << rot_y(M_PI/2);
 
    tmp = tmpp * start;
 
    goal = invert_transform(tmpp) * tmp;
 
    std::cout << start << std::endl << std::endl;
    std::cout << tmp << std::endl << std::endl;
    std::cout << goal << std::endl << std::endl;
 
    std::cout << tmpp << std::endl << std::endl;
    std::cout << invert_transform(tmpp) << std::endl << std::endl;
 
    std::cout << std::endl;
 
 
    //    Endorob machin;
 
    //    //machin.set_DH_convention(DH_type::DH_modified);
    //    machin.set_DH_convention(DH_type::DH_standard);
    //    double a = 0.00, l = 0.05, a_end = 0.03, a_bis = 0.011;
 
    //    machin.add_DH_entry(0,M_PI/2,0,0, Joint_type::Prismatic);
    //    machin.add_DH_entry(0,M_PI/2,0,0, Joint_type::Revolute);
    //    machin.add_DH_entry(0,M_PI/2,0.1,0, Joint_type::Revolute);
    //    machin.add_DH_entry(0,M_PI/2,0,0, Joint_type::Prismatic);
    //    machin.add_DH_entry(0,M_PI/2,0.3,0, Joint_type::Revolute);
 
    //    // Set solver type
    //    machin.set_solver_type(Solver_type::Jacobian_DLS);
 
 
    //    // Set joint coupling matrix
    //    //Vec coup(14);
    //    //coup << 0, 1, 2, 3, 2, 3, 4, 5, 4, 5, 6, 7, 6, 7;
 
    //   // machin.set_joint_coupling(coup);
 
    //    // Start solver
    //    // DO NOT FORGET THIS LINE!!!
    //    machin.initialize();
    //    Vec qv(5);
    //    qv << 0.8, -0.9, -0.1, 0.4, 0.9;
    //    machin.set_q_vec(qv);
 
    //    Mat full = machin.get_full_kinematics();
    //    std::cout << full << std::endl << std::endl;
 
    //    Mat result = machin.get_forward_kinematics();
    //    std::cout << result << std::endl << std::endl;
 
    //    Mat j = machin.get_Jacobian();
    //    std::cout << j << std::endl << std::endl;
 
 
    //    //machin.set_solver_weight(0,5);
    //    //Motion_RW truc(&machin);
    //    //truc.set_write_frequency(250);
    //    //truc.start_recording();
    //    //truc.set_read_frequency(1000);
    //    //truc.load_file("output_data.csv");
 
    //    Vec tq = machin.get_q_vec();
    //    std::cout << tq << std::endl << std::endl;
 
    //    // get random vector
    //    Vec qr = machin.get_random_q_vec();
    //    std::cout << qr;
 
 
 
 
    //    std::vector<Mat4d, Eigen::aligned_allocator<Mat4d>>fwdK;
    //    //fwdK = machin.calculate_full_forward_kinematics();
 
 
 
    //    Mat4d Te, Td,Ttest, Tcheck;
    //    Vec q;
 
    //    Tcheck << 0,-1, 0,0,
    //              0, 0,-1,0,
    //              1, 0, 0,0.08,
    //              0, 0, 0,1;
    //    q.resize(machin.dof());
    //    q.resize(machin.joints());
    //    q = Mat::Zero(q.rows(),1);
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
    //    q.resize(machin.dof());
    //    q = machin.get_random_q_vec();
    //    std::cout << "\n\nrandom q:\n"<< q ;
 
    //    machin.set_q_vec(q);
    //    Td = machin.get_tip_pose();
    //    std::cout << "\n\ncorresponding tip pose:\n"<< Td ;
 
    //    q.resize(machin.dof());
    //    q = Mat::Zero(q.rows(),1);
 
    //    machin.set_q_vec(q);
    //    //std::cout << "\n\nnew q:\n"<< q ;
    //    Ttest = machin.get_tip_pose();
    //    std::cout << "\n\ncorresponding tip pose:\n"<< Ttest ;
 
    //    machin.set_solver_target(Td);
 
    //    q = machin.get_IK_solution();
    //    std::cout << "\n\n solution q:\n"<< q ;
 
    //    Td = machin.get_tip_pose();
    //    std::cout << "\n\ncorresponding tip pose:\n"<< Td ;
 
 
 
 
 
    //    //q = machin.get_inversekinematics(Te);
 
    //    //    // Mask debug
    //    //    Vec mask(6);
    //    //    mask << 1, 1, 1, 0, 0, 0;
    //    //    machin.set_mask(mask);
 
 
    //    //machin.set_solver_target(Td);
    //    //    std::cout << "Asking with mask...\n";
    //    //    q = machin.get_IK_solution();
    //    //    std::cout << "Got it with mask...\n";
    //    //    std::cout << q;
 
 
 
 
 
 
    // //   std::cout << "Asking final...\n";
 
 
 
 
 
 
 
 
 
 
 
    // Create IK
        Endorob inverse_kinematics;  // todo: Endorob as _p
        inverse_kinematics.set_solver_type(Solver_type::Jacobian_DLS);
        inverse_kinematics.add_DH_entry(0, M_PI / 2, 0, M_PI / 2, Joint_type::Revolute);
        inverse_kinematics.set_joint_limits(-0.8, 0.8);
        inverse_kinematics.add_DH_entry(3.25, M_PI / 2, 0, 0, Joint_type::Revolute);
        inverse_kinematics.set_joint_limits(-0.8, 0.8);
        inverse_kinematics.add_DH_entry(8.75, 0, 0, 0, Joint_type::Revolute);
        inverse_kinematics.set_joint_limits(-0.8, 0.8);
        inverse_kinematics.add_DH_entry(6.75, 0, 0, 0, Joint_type::Revolute);
        inverse_kinematics.set_joint_limits(-0.8, 0.8);
        inverse_kinematics.add_DH_entry(3.25, 0, 0, 0, Joint_type::Revolute);
        inverse_kinematics.set_joint_limits(-0.8, 0.8);
        inverse_kinematics.add_DH_entry(4.42, 0, 0, 0, Joint_type::Revolute);
        inverse_kinematics.set_joint_limits(-0.8, 0.8);
        inverse_kinematics.add_DH_entry(0, M_PI / 2, 0, M_PI / 2, Joint_type::Revolute);
        inverse_kinematics.set_joint_limits(-0.8, 0.8);
        inverse_kinematics.add_DH_entry(0, M_PI / 2, 0, 0, Joint_type::Revolute);
        inverse_kinematics.set_joint_limits(-0.8, 0.8);
        Mat4d tool;
        tool << 1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 10.95,
            0, 0, 0, 1;
        inverse_kinematics.set_tool_frame(tool);
        inverse_kinematics.initialize();
 
        // Get Random target point
        Vec q = inverse_kinematics.get_random_q_vec();
        Mat4d Tt = inverse_kinematics.get_forward_kinematics(q);
        inverse_kinematics.set_q_vec(q);
        Eigen::Matrix4d target = inverse_kinematics.get_tip_pose();
        std::cout << "Wanted result qvec:" << std::endl << std::endl << inverse_kinematics.get_q_vec() << std::endl << std::endl;
        std::cout << "q:" << std::endl << std::endl << q << std::endl << std::endl;
 
 
        inverse_kinematics.reset_q_vec();
        inverse_kinematics.set_solver_target(target);
        //Sleep(2000);
        //inverse_kinematics.get_instant_IK_solution();
        inverse_kinematics.get_IK_solution();
        std::cout << "Target:" << std::endl << std::endl << target << std::endl << std::endl;
        std::cout << "Tt:" << std::endl << std::endl << Tt << std::endl << std::endl;
        std::cout << "Result:" << std::endl << std::endl << inverse_kinematics.get_tip_pose() << std::endl << std::endl;
        std::cout << "qvec:" << std::endl << std::endl << inverse_kinematics.get_q_vec() << std::endl << std::endl;
        std::cout << "Error:" << std::endl << std::endl << inverse_kinematics.get_error_norm() << std::endl << std::endl;
 
        std::cout << "Continue with ENTER" << std::endl;
        //std::cin.get();
 
 
//        Endorob *machin;
//        machin = new Endorob;
//        machin->add_DH_entry(0, M_PI / 2, 0, M_PI / 2, Joint_type::Revolute);
//        machin->set_joint_limits(-0.8, 0.8);
//        machin->add_DH_entry(3.25, M_PI / 2, 0, 0, Joint_type::Revolute);
//        machin->set_joint_limits(-0.8, 0.8);
//        machin->add_DH_entry(8.75, 0, 0, 0, Joint_type::Revolute);
//        machin->set_joint_limits(-0.8, 0.8);
//        machin->initialize();
//        machin->set_solver_target(target);
//        machin->get_IK_solution();
//        delete machin;
 
 
 
    std::this_thread::sleep_for(std::chrono::seconds(10));
 
 
    return 1;
}