example_urdf_group.cc

Go to the documentation of this file.

#include "motion_control/lmc_group.h"
#include <math.h>
#include <iostream>

int main(int argc, char **argv)
{
    lmc_group_model_t *group_model = lmc_group_model_new();
    char filename[] = "urdf/lm3.urdf";
    lmc_group_model_init_from_urdf_file(group_model, filename);
    
    lmc_group_joint_info_t joint_info;
    lmc_group_model_get_joint_info(group_model, &joint_info);

    lmc_group_state_t *group_state = lmc_group_state_new();
    lmc_group_state_init(group_state, group_model);

    lmc_group_state_collide(group_state, NULL, NULL);

    double joint_pos[6] = {-11.0 / 180.0 * M_PI, 157.0 / 180.0 * M_PI, -209.0 / 180.0 * M_PI, 88.0 / 180.0 * M_PI, 100.0 / 180.0 * M_PI, -76.0 / 180.0 * M_PI};
    lmc_group_state_set_jnt_pos(group_state, joint_pos);
    
    lmc_group_state_update(group_state, group_model);
    
    lmc_group_collide_req_t req;
    lmc_group_collide_resp_t resp;
    req.enable_contact = 1;
    req.num_max_contacts = 2;
    lmc_group_state_collide(group_state, &req, &resp);
    for(size_t i = 0; i < resp.num_contacts; ++i)
    {
        std::cout<<"contact pair: "<<resp.contacts[i].link_a<<" "<<resp.contacts[i].link_b<<"\n";
    }
    // EXPECT_STREQ("link_6", resp.contacts[0].link_a);
    // EXPECT_STREQ("link_2", resp.contacts[0].link_b);
    // EXPECT_STREQ("link_6", resp.contacts[1].link_a);
    // EXPECT_STREQ("link_2", resp.contacts[1].link_b);
    lmc_group_state_delete(&group_state);
    lmc_group_model_delete(&group_model);
    return 0;
}