ROS2 :: Moveit2 Python API에 대해 알아보기 (Rolling)

Motion Planning Python API(링크)​

▶ Moveit planning Scene Configuration

무브잇은 기본적으로 다수의 플래너를 제공한다. 이런 플래너마다 세팅과 파라미터를 설정해줘야하는데, ROS2에서는 configuration yaml 파일을 작성하면 된다. moveit_py 노드와 맞는 cofiguration 파일을 보자.

planning_scene_monitor_options:
        name: "planning_scene_monitor"
        robot_description: "robot_description"
        joint_state_topic: "/joint_states"
        attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
        publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
        monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
        wait_for_initial_state_timeout: 10.0

플래닝 씬 모니터링 옵션이 있다. 여기서는 모니터링 이름과, 어떤 로봇에 대한 정보를 받을지, 조인트 상태 토픽은 무엇인지를 설정한다.

추가로 붙어 있는 충돌 물체의 토픽과 플래닝 씬 자체의 토픽도 설정할 수 있다. 또한 모니터링 하고 있는 씬의 토픽도 설정할 수 있는데, 씬에 관련 된 토픽은 정확히 더 확인이 필요할 듯 하다.

planning_pipelines:
        pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp", "ompl_rrt_star"]

plan_request_params:
        planning_attempts: 1
        planning_pipeline: ompl
        max_velocity_scaling_factor: 1.0
        max_acceleration_scaling_factor: 1.0

ompl_rrtc:
        plan_request_params:
                planning_attempts: 1
                planning_pipeline: ompl
                planner_id: "RRTConnectkConfigDefault"
                max_velocity_scaling_factor: 1.0
                max_acceleration_scaling_factor: 1.0
                planning_time: 1.0

ompl_rrt_star:
        plan_request_params:
                planning_attempts: 1
                planning_pipeline: ompl_rrt_star # Different OMPL pipeline name!
                planner_id: "RRTstarkConfigDefault"
                max_velocity_scaling_factor: 1.0
                max_acceleration_scaling_factor: 1.0
                planning_time: 1.5

pilz_lin:
        plan_request_params:
                planning_attempts: 1
                planning_pipeline: pilz_industrial_motion_planner
                planner_id: "PTP"
                max_velocity_scaling_factor: 1.0
                max_acceleration_scaling_factor: 1.0
                planning_time: 0.8

chomp:
        plan_request_params:
                planning_attempts: 1
                planning_pipeline: chomp
                max_velocity_scaling_factor: 1.0
                max_acceleration_scaling_factor: 1.0
                planning_time: 1.5

가장 중요하다면 가장 중요한 planner 설정이다.

각각 플래너에는 planner_id 같은 것들을 설정해줘야 한다.

---

▶ moveit_py 과 planning 요소 선언

​

가장 먼저 moveit_py 노드를 선언한다. 거기에 planning component인 panda_arm을 선언한다.

당연하게도 logger도 선언해준다.

rclpy.init()
logger = rclpy.logging.get_logger("moveit_py.pose_goal")

# instantiate MoveItPy instance and get planning component
panda = MoveItPy(node_name="moveit_py")
panda_arm = panda.get_planning_component("panda_arm")
logger.info("MoveItPy instance created")

motion planning 함수를 만들자. 로봇과 planning_component가 일반적으로 들어가면 된다.

파라미터에 따라 다르게 plan result를 내게 helper function도 꾸민다. 실제 최종적으로 나오는 것은 robot_trajectory

def plan_and_execute(
        robot,
        planning_component,
        logger,
        single_plan_parameters=None,
        multi_plan_parameters=None,
        ):
        """A helper function to plan and execute a motion."""
        # plan to goal
        logger.info("Planning trajectory")
        if multi_plan_parameters is not None:
                plan_result = planning_component.plan(
                        multi_plan_parameters=multi_plan_parameters
                )
        elif single_plan_parameters is not None:
                plan_result = planning_component.plan(
                        single_plan_parameters=single_plan_parameters
                )
        else:
                plan_result = planning_component.plan()

        # execute the plan
        if plan_result:
                logger.info("Executing plan")
                robot_trajectory = plan_result.trajectory
                robot.execute(robot_trajectory, blocking=True, controllers=[])
        else:
                logger.error("Planning failed")

▶ Single pipeline planning - 기본 설정

기존에 먼저 정의된 state로 plan을 시작해보자.

 

# set plan start state using predefined state
panda_arm.set_start_state(configuration_name="ready")

# set pose goal using predefined state
panda_arm.set_goal_state(configuration_name="extended")

# plan to goal
plan_and_execute(panda, panda_arm, logger)

▶ Single pipeline planning - 로봇 상태

그 다음으로는 로봇의 상태를 plan한다. 조인트 값에 따라 로봇 상태를 바꿀 수 있기에 유연하게 적용할 수 있다.

우선 랜덤하게 로봇의 상태를 정의해보자. 주의깊게 봐야할 함수는 set_start_state_to_current_state()와 set_goal_state(robot_state)이다.

# instantiate a RobotState instance using the current robot model
robot_model = panda.get_robot_model()
robot_state = RobotState(robot_model)

# randomize the robot state
robot_state.set_to_random_positions()

# set plan start state to current state
panda_arm.set_start_state_to_current_state()

# set goal state to the initialized robot state
logger.info("Set goal state to the initialized robot state")
panda_arm.set_goal_state(robot_state=robot_state)

# plan to goal
plan_and_execute(panda, panda_arm, logger)

▶ Single pipeline planning - 도착 위치

도착 위치의 경우 ROS 메세지로 나타내주면 된다.

# set plan start state to current state
panda_arm.set_start_state_to_current_state()

# set pose goal with PoseStamped message
pose_goal = PoseStamped()
pose_goal.header.frame_id = "panda_link0"
pose_goal.pose.orientation.w = 1.0
pose_goal.pose.position.x = 0.28
pose_goal.pose.position.y = -0.2
pose_goal.pose.position.z = 0.5
panda_arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link="panda_link8")

# plan to goal
plan_and_execute(panda, panda_arm, logger)

▶ Single pipeline planning - 제약 조건

플래닝 단계에서 제약 조건을 바꿔줄 수 있는 기능이 새로 생겼다. 정확히 어떻게 활용할지는 모르겠지만, 특정 모션을 특정 조인트는 움직이지 않는 형태로 플래닝 하도록 하는 작업들은 가능할 것 같다.

# set plan start state to current state
panda_arm.set_start_state_to_current_state()

# set constraints message
joint_values = {
        "panda_joint1": -1.0,
        "panda_joint2": 0.7,
        "panda_joint3": 0.7,
        "panda_joint4": -1.5,
        "panda_joint5": -0.7,
        "panda_joint6": 2.0,
        "panda_joint7": 0.0,
}
robot_state.joint_positions = joint_values
joint_constraint = construct_joint_constraint(
        robot_state=robot_state,
        joint_model_group=panda.get_robot_model().get_joint_model_group("panda_arm"),
)
panda_arm.set_goal_state(motion_plan_constraints=[joint_constraint])

# plan to goal
plan_and_execute(panda, panda_arm, logger)

▶ Multi pipeline planning

기존 moveit과 다르게 moveit2에서는 다수의 플래닝 파이프 라인을 만들 수 있다. 여러 플래너를 병렬로 실행시켜서 생성 된 모션 플랜 중에서 원하는 목적에 가장 부합하는 플랜을 고를 수 있다.

# set plan start state to current state
panda_arm.set_start_state_to_current_state()

# set pose goal with PoseStamped message
panda_arm.set_goal_state(configuration_name="ready")

# initialise multi-pipeline plan request parameters
multi_pipeline_plan_request_params = MultiPipelinePlanRequestParameters(
        panda, ["ompl_rrtc", "pilz_lin", "chomp", "ompl_rrt_star"]
)

# plan to goal
plan_and_execute(
        panda,
        panda_arm,
        logger,
        multi_plan_parameters=multi_pipeline_plan_request_params,
)

# execute the plan
if plan_result:
        logger.info("Executing plan")
        panda_arm.execute()

이번 moveit2에 들어오면서 바뀐점은 planning pipeline을 작성할 수 있다는 점, 혹은 Task Constructor를 작성할 수 있다는 점이다. 또한 다수의 플래너를 병렬로 사용하여 원하는 결과를 선택할 수 있다는 점이다. (근데 원하는 플래너의 trajectory 선택하는 부분은 어디에....?)

​

코드 깃허브(https://github.com/ros-planning/moveit2_tutorials/tree/main/doc/examples/motion_planning_python_api)