コード例 #1
0
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;
}
コード例 #2
0
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");
	}
}