void get_joint_values()
{
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
std::cout << "current joints:";
for(int i = 0;i < 5;i++)
std::cout << joint_values[i] << " ";
std::cout << std::endl;
}
void moveit_ik(const Eigen::Affine3d end_effector_state)
{
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1);
if(found_ik)
{
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
for(std::size_t i=0; i < 5; ++i)
{
std::cout << "[" << i << "," << joint_values[i] << "],";
}
}
else
{
ROS_INFO("Did not find IK solution.\n");
}
}