인터넷에 공개된 MoveIt2 tutorial을 따라하다 보면 많은 사실을 학습할 수 있다. 다만 그대로 따라하면 그 감동이 적어, 내가 마음대로 해보기로 했다. ABB가 인터넷에 공개한 ABB_IRB2400 urdf 파일로 시작했다. 내가 한 삽질을 여기에 끄적인다.
setup assistant로 urdf 파일을 로딩하고, move_group, kinematics solver, pose 등을 설정하여 움직이는 Manuplator를 프로젝트 이름 + moveit_config 디렉토리에 저장했다. 기본으로 제공된 demo.launch.py을 실행하여 Rviz를 열고, 마우스 버튼으로 robotModel, MarkArray, panel을 추가했다. Planning with Path Constraints 부분에서 막혔는데,
start_state.setFromIK(joint_model_group, start_pose2);
를 실행하면
No kinematics solver instantiated for group '%s
메세지를 내고 프로그램이 죽는다. RobotState Class를 찾아보니 인자로 제공된 JointModelGroup에서 SolverInstance를 얻지 못하면 error 처리한다.
01278 bool moveit::core::RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose,
01279 unsigned int attempts, double timeout,
01280 const GroupStateValidityCallbackFn& constraint,
01281 const kinematics::KinematicsQueryOptions& options)
01282 {
01283 const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
01284 if (!solver)
01285 {
01286 logError("No kinematics solver instantiated for group '%s'", jmg->getName().c_str());
01287 return false;
01288 }
01289 return setFromIK(jmg, pose, solver->getTipFrame(), attempts, timeout, constraint, options);
01290 }
01291
JointModelGroup은 moveit setup assistanct가 설정하는 듯 한데, 어떻게 설정하는지 정확하게 설명되어 있지 않았다. moveit setup assistant로 설정할 때 kinematics solver를 선택하는 항목이 있는데, 나는 제대로 선택했는데 문제가 있다.
제공된 panda_arm 예제가 어떻게 돌아가는지 다시 살펴 보기로 했다. moveit2_tutorial에 panda_arm으로 제공된 항목을 찾아 move_group에 해당하는 부분을 launch 했다. moveit2_tutorial을 설치하면 moveit_resources에 panda_move_config가 있는데 여기 demo.launch.py를 실행해 보았다.
root@rygen3600:/home/ros2_test#ros2 launch moveit_resources_panda_moveit_config demo.launch.py ... [move_group-4] [INFO] [1690614580.226886350] [move_group.move_group]: [move_group-4] [move_group-4] ******************************************************** [move_group-4] * MoveGroup using: [move_group-4] * - ApplyPlanningSceneService [move_group-4] * - ClearOctomapService [move_group-4] * - CartesianPathService [move_group-4] * - ExecuteTrajectoryAction [move_group-4] * - GetPlanningSceneService [move_group-4] * - KinematicsService [move_group-4] * - MoveAction [move_group-4] * - MotionPlanService [move_group-4] * - QueryPlannersService [move_group-4] * - StateValidationService [move_group-4] ******************************************************** [move_group-4]
parameter가 어떻게 제공되는지 알아보기 위해 param list를 확인해 보고, 의심되는 kinematics_solver 값을 확인했다. robot_description에 solver를 설정할 수 있는데, moveit setup assistant가 설정한 값으로 동작하고 있다.
root@rygen3600:/home/ros2_test/src/hello_moveit/debug# ros2 param get /move_group robot_description_kinematics.panda_arm.kinematics_solver String value is: kdl_kinematics_plugin/KDLKinematicsPlugin
내가 만든 ABB 로봇은 해당 node가 없다. 이제 문제가 무엇인지 알았다. launch할 때 해당 값을 넣어줘야 하는데, 내가 빼먹고 넣어주지 않았다. 이제 panda_arm demo 파일을 보고, 어떻게 설정해야 하는지 알아봐야 한다.
panda demo를 자세히 보니, 노드를 만들어서 robot_description에 kinematics를 설정해 주었다. 나는 ros run으로 해당 프로그램만 실행해주어 kinematics_solver가 설정되지 않았다. 아래 형식으로 노드를 만들어 준다.
root@rygen3600:/home/ros2_test# cat src/hello_moveit/launch/my_demo.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
def generate_launch_description():
moveit_config = MoveItConfigsBuilder("hello").to_moveit_configs()
# MoveGroupInterface demo executable
move_group_demo = Node(
name="my_move_group_interface",
package="hello_moveit",
executable="planning",
output="screen",
parameters=[
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.robot_description_kinematics,
],
)
return LaunchDescription([move_group_demo])
이제 새로운 문제가 있다. orientation constraint를 설정하면 로봇이 제대로 움직이지 않는다. interactive marker는 쉽게 planning 하는데, 프로그램으로 하면 짧은 거리도 planning이 높은 확율로 실패한다.