lebai-motion-control  3.1.6
lebai motion control C interface
example_urdf_group.cc

Examples of urdf group usage. group, you can see the source code in example_urdf_group.cc

#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;
}