MoveGroupCommander + pilz industrial motion planner not using the updated allowed collision matrix

[rr-robert]

Description

Asked by @felixvd to raise an issue ticket (see this ROS answers question)

My environment

• ROS Noetic in a docker container with Ubuntu 20.04
• Moveit! version 1.1.2
• ros-industrial-client with motoman driver
• motoman gp8 robot
• binary build

Problem

• Using MoveGroup commander (Python API) and Pilz Industrial Motion Planner, changes to the allowed collision matrix (ACM) are published to the planning scene but do not propagate to the planning interface.

Action Taken So Far

• Tested with an identical stack except with OMPL planner in the pipeline. Behaviour is not reproducible, changes propagate to the planning interface (perhaps through pre/post-processing plugins?).
• Tested with planning directly in rviz. Behaviour is not reproducible, changes propagate to the planning interface.

Steps to reproduce

My base application is a docker container and that is detailed below.

ARG ROS_DISTRO=noetic
ARG BASE_IMAGE=ros:${ROS_DISTRO}-ros-base
ARG UID=0
ARG GID=0
ARG HOME=/root

FROM ${BASE_IMAGE} AS base

RUN apt-get update && apt-get upgrade -y && \
  apt-get install -y --no-install-recommends \
  clang-format \
  clang-tidy \
  cppcheck \
  git-core \
  libgl1-mesa-dri \
  libgl1-mesa-glx \
  libmuparser-dev \
  libopenblas-dev \
  libxml2-utils \
  nodejs \
  npm \
  python3-pip \
  python3-psutil \
  python3-vcstool \
  python-is-python3 \
  ros-${ROS_DISTRO}-controller-interface \
  ros-${ROS_DISTRO}-controller-manager \
  ros-${ROS_DISTRO}-control-toolbox \
  ros-${ROS_DISTRO}-diagnostic-updater \
  ros-${ROS_DISTRO}-eigen-conversions \
  ros-${ROS_DISTRO}-filters \
  ros-${ROS_DISTRO}-hardware-interface \
  ros-${ROS_DISTRO}-joint-limits-interface \
  ros-${ROS_DISTRO}-joint-state-controller \
  ros-${ROS_DISTRO}-joint-state-publisher \
  ros-${ROS_DISTRO}-moveit-core \
  ros-${ROS_DISTRO}-moveit-commander \
  ros-${ROS_DISTRO}-moveit-fake-controller-manager \
  ros-${ROS_DISTRO}-moveit-planners-ompl \
  ros-${ROS_DISTRO}-moveit-ros-move-group \
  ros-${ROS_DISTRO}-moveit-ros-planning \
  ros-${ROS_DISTRO}-moveit-ros-planning-interface \
  ros-${ROS_DISTRO}-moveit-ros-visualization \
  ros-${ROS_DISTRO}-moveit-simple-controller-manager \
  ros-${ROS_DISTRO}-pilz-industrial-motion-planner \
  ros-${ROS_DISTRO}-plotjuggler \
  ros-${ROS_DISTRO}-robot-state-publisher \
  ros-${ROS_DISTRO}-ros-controllers \
  ros-${ROS_DISTRO}-roslint \
  ros-${ROS_DISTRO}-ros-pytest \
  ros-${ROS_DISTRO}-tf-conversions \
  ros-${ROS_DISTRO}-xacro \
  yamllint && apt-get clean && rm -rf /var/lib/apt/lists/*

ENV ROS_LANG_DISABLE genlisp:gennodejs:geneus

RUN echo ${ROS_DISTRO} > /etc/ros-distro

# Install editor config checker
RUN curl -L https://github.com/editorconfig-checker/editorconfig-checker/releases/download/2.1.0/ec-linux-amd64.tar.gz --output ec.tar.gz && \
  tar xzf ec.tar.gz && \
  rm -rf ec.tar.gz && \
  mv ./bin/ec-linux-amd64 /usr/bin/editorconfig-checker

# Install https://github.com/gavanderhoorn/rosbag_fixer for fixing legacy bags from iiwa stack topics
RUN curl -L https://raw.githubusercontent.com/gavanderhoorn/rosbag_fixer/master/fix_bag_msg_def.py \
    --output /usr/local/bin/fix_bag_msg_def.py && \
    chmod +x /usr/local/bin/fix_bag_msg_def.py

# Install python requirements
ADD requirements_dev.txt /tmp/
ADD requirements.txt /tmp/
RUN pip3 install -r /tmp/requirements_dev.txt && \
    pip3 install -r /tmp/requirements.txt && \
    jupyter labextension install @jupyter-widgets/jupyterlab-manager jupyterlab-plotly@4.14.1 && \
    chown -R ${UID}:${GID} $HOME

FROM base as app

ARG UID
ARG GID
ARG HOME

USER ${UID}:${GID}

RUN git config --global advice.detachedHead "false" && \
    mkdir -p $HOME/checkout && \
    echo "source $HOME/checkout/activate.sh" >> $HOME/.bashrc

WORKDIR $HOME/checkout

# This is set by activate.sh but necessary when working with the container in PyCharm
ENV PYTHONPATH $HOME/checkout/internal/devel/lib/python3/dist-packages:$HOME/checkout/external/devel/lib/python3/dist-packages:/opt/ros/noetic/lib/python3/dist-packages
ENV ROS_IP 127.0.0.1
ENV ROS_MASTER_URI http://$ROS_IP:11312

My planning_pipeline.launch is standard as below:

<?xml version="1.0"?>
<launch>
  <!-- Pilz Command Planner Plugin for MoveIt -->
  <arg name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner"/>
  <arg name="planning_adapters" default="default_planner_request_adapters/AddTimeParameterization     default_planner_request_adapters/FixWorkspaceBounds     default_planner_request_adapters/FixStartStateBounds     default_planner_request_adapters/FixStartStateCollision     default_planner_request_adapters/FixStartStatePathConstraints"/>
  <arg name="start_state_max_bounds_error" value="0.1"/>
  <param name="planning_plugin" value="$(arg planning_plugin)"/>
  <param name="request_adapters" value="$(arg planning_adapters)"/>
  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)"/>
  <param name="default_planner_config" value="PTP"/>
</launch>

The robot_description is a motoman gp8 but that can replaced by any package. Collision manager python script is below:

from typing import Dict

import pandas as pd
import rospy
from moveit_msgs.msg import AllowedCollisionMatrix, PlanningScene, PlanningSceneComponents
from moveit_msgs.srv import GetPlanningScene


class CollisionManager:
    def __init__(self, namespace: str = "") -> None:
        self.ns = namespace
        self.get_planning_scene = rospy.ServiceProxy(
            f"/{self.ns}/get_planning_scene", GetPlanningScene
        )
        self.scene = rospy.Publisher(
            f"/{self.ns}/move_group/monitored_planning_scene", PlanningScene, queue_size=0
        )

    def get_collision_matrix(self) -> AllowedCollisionMatrix:
        request = PlanningSceneComponents(
            components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX
        )
        return self.get_planning_scene(request).scene.allowed_collision_matrix

    def get_links(self, matrix: AllowedCollisionMatrix) -> Dict[str, int]:
        return {n: i for i, n in enumerate(matrix.entry_names)}

    def are_allowed(self, link_1: str, link_2: str) -> bool:
        matrix = self.get_collision_matrix()
        name_map = self.get_links(matrix)

        source_index = name_map[link_1]
        target_index = name_map[link_2]

        return bool(
            matrix.entry_values[source_index].enabled[target_index]
            and matrix.entry_values[target_index].enabled[source_index]
        )

    def toggle(self, link_1: str, link_2: str, allowed: bool) -> None:
        """
        Toggle collisions between two links in the URDF

        :param link_1:
        :param link_2:
        :param allowed:
        """
        matrix = self.get_collision_matrix()
        name_map = self.get_links(matrix)

        source_index = name_map[link_1]
        target_index = name_map[link_2]

        matrix.entry_values[source_index].enabled[target_index] = allowed
        matrix.entry_values[target_index].enabled[source_index] = allowed

        self.update_matrix(matrix)

    def update_matrix(self, matrix: AllowedCollisionMatrix) -> None:
        self.scene.publish(PlanningScene(is_diff=False, allowed_collision_matrix=matrix))

    @staticmethod
    def show_matrix(matrix: AllowedCollisionMatrix) -> object:
        # name_map = {i: n for i, n in enumerate(matrix.entry_names)}
        name_map = dict(enumerate(matrix.entry_names))
        rows = [["x", *matrix.entry_names]]
        for i, row in enumerate(matrix.entry_values):
            rows.append([name_map[i]] + row.enabled)
        pd.options.display.max_columns = None
        df = pd.DataFrame(rows)
        return df.rename(columns=df.iloc[0])

The python script where I define the move_group commander wrapper functions:

from __future__ import annotations

import os
import sys
from dataclasses import dataclass
from typing import Any, Callable, Dict, List, Optional, cast

import moveit_commander
from geometry_msgs.msg import PoseStamped
from moveit_commander import MoveItCommanderException
from moveit_msgs.msg import MoveItErrorCodes, RobotTrajectory

from .collision_manager import CollisionManager
from .util import clamp

PILZ_INDUSTRIAL = "Pilz Industrial Motion Planner"
PLANNING_ATTEMPTS = 1 if os.environ.get("PROFILE", "") == "test" else 20


class PlanningError(MoveItCommanderException):
    # Map numerical code to some human readable text
    _error_code_map = {
        getattr(MoveItErrorCodes, attr): attr
        for attr in dir(MoveItErrorCodes)
        if attr[0].isalpha() and attr[0].isupper()
    }

    def __init__(self, error_code: int) -> None:
        self.error_code = error_code
        self.message = self._error_code_map[error_code]
        super().__init__(self.message)


class SplinePlanningError(PlanningError):
    def __init__(self, message: str) -> None:
        super().__init__(-1)
        self.message = message


class MoveFailed(SystemError):
    pass


@dataclass
class Joint:
    name: str
    upper_bound: float
    lower_bound: float
    position: float

    @classmethod
    def from_robot(cls, joint: moveit_commander.RobotCommander.Joint) -> Joint:
        return cls(
            name=joint.name(),
            upper_bound=joint.max_bound(),
            lower_bound=joint.min_bound(),
            position=joint.value(),
        )


def require_trajectory(func: Callable) -> Callable:
    def inner(self: Any, *args: Any, **kwargs: Any) -> Any:
        if not self._trajectory:  # pylint: disable=protected-access
            raise ValueError("No trajectory, did you forget to plan?")
        return func(self, *args, **kwargs)

    return inner


# pylint: disable=too-many-public-methods
class MoveitController:
    planning_time: float
    tooltip_frame: str
    _trajectory: Optional[RobotTrajectory]
    _target_joint_values: Optional[List[float]]

    JOINT_ERROR_THRESHOLD = 0.001
    JOINT_MEAN_ERROR_THRESHOLD = 0.0005

    def __init__(self, planning_group: str = "gp8_only", namespace: str = "") -> None:
        moveit_commander.roscpp_initialize(sys.argv)
        self.planning_group = planning_group
        self.ns = namespace
        self._move_group = moveit_commander.MoveGroupCommander(
            self.planning_group, ns=f"/{self.ns}"
        )
        self.tooltip_frame = self._move_group.get_end_effector_link()
        self._robot = moveit_commander.RobotCommander()
        self.collisions = CollisionManager(self.ns)

        # If using pilz default to PTP
        self._pilz = PILZ_INDUSTRIAL == self._move_group.get_interface_description().name

    def _reset_state(self) -> None:
        self._move_group.stop()
        self._move_group.set_start_state_to_current_state()
        self._move_group.set_goal_tolerance(0.0001)
        self._move_group.set_num_planning_attempts(PLANNING_ATTEMPTS)

        if self._pilz:
            self._move_group.set_planner_id("PTP")

        self._move_group.clear_pose_targets()
        self._target_joint_values = None
        self._trajectory = None
        self.planning_time = 0.0

    def get_tooltip_pose(self) -> PoseStamped:
        posestamped = self._move_group.get_current_pose(self.tooltip_frame)

        # When moveit can't fetch robot state it silently returns a pose at 0 and logs to stderr
        # This checks that case and raises an error for the user
        p = posestamped.pose
        axis = [
            p.position.x,
            p.position.y,
            p.position.z,
            p.orientation.x,
            p.orientation.y,
            p.orientation.z,
        ]
        if all(axis) == 0 and p.orientation.w == 1:
            raise SystemError("Couldn't fetch robot state, please check logs")

        return posestamped

    def get_joint_positions(self) -> Dict[str, float]:
        return dict(
            zip(self._move_group.get_active_joints(), self._move_group.get_current_joint_values())
        )

    def _plan(self) -> None:
        success, self._trajectory, self.planning_time, error_code = self._move_group.plan()

        if not success:
            self._trajectory = None
            raise PlanningError(error_code.val)

        self._target_joint_values = self._trajectory.joint_trajectory.points[-1].positions

    def get_joint_limits(self) -> Dict[str, Joint]:
        return {
            j: Joint.from_robot(self._robot.get_joint(j))
            for j in self._move_group.get_active_joints()
        }

    def plan_home(self, velocity: float, acceleration: float) -> None:
        """
        Plan to home position
        """
        self._reset_state()
        self._move_group.set_max_acceleration_scaling_factor(acceleration)
        self._move_group.set_max_velocity_scaling_factor(velocity)
        self._move_group.set_named_target("home")
        self._plan()

    def execute_home(self, velocity: float, acceleration: float) -> None:
        """
        Move to the defined home position (blocking)
        """
        self.plan_home(velocity, acceleration)

        self.execute_plan(block=True)

    def plan_ptp(self, pose: PoseStamped, velocity: float, acceleration: float) -> None:
        """
        Plan path to a pose with joints conforming to the given velocity and acceleration limits

        :param pose: PoseStamped
        :param velocity: Percentage of the max joint velocity in joint_limits
        :param acceleration: Percentage of the max joint acceleration in joint_limits
        """
        self._reset_state()

        clamp("velocity", velocity, 0.0001, 1)
        clamp("acceleration", acceleration, 0.0001, 1)

        self._move_group.set_max_acceleration_scaling_factor(acceleration)
        self._move_group.set_max_velocity_scaling_factor(velocity)
        self._move_group.set_pose_reference_frame(pose.header.frame_id)
        self._move_group.set_pose_target(pose.pose)

        self._plan()

    def execute_ptp(
        self, pose: PoseStamped, velocity: float, acceleration: float, block: bool = True
    ) -> None:
        self.plan_ptp(pose, velocity, acceleration)
        self.execute_plan(block)

    def plan_lin(self, pose: PoseStamped, velocity: float, acceleration: float) -> None:
        """
        Plan path to a pose at a given velocity and acceleration

        :param pose: PoseStamped
        :param velocity: Percentage of the max cartesian velocity in cartesian_limits
        :param acceleration: Percentage of the max cartesian acceleration in cartesian_limits
        """
        self._reset_state()

        clamp("velocity", velocity, 0.0001, 1)
        clamp("acceleration", acceleration, 0.0001, 1)

        if not self._pilz:
            self.plan_spline([pose])
            return

        self._move_group.set_planner_id("LIN")

        self._move_group.set_max_acceleration_scaling_factor(acceleration)
        self._move_group.set_max_velocity_scaling_factor(velocity)
        self._move_group.set_pose_reference_frame(pose.header.frame_id)
        self._move_group.set_pose_target(pose.pose)

        self._plan()

    def execute_lin(self, pose: PoseStamped, velocity: float, acceleration: float) -> None:
        self.plan_lin(pose, velocity, acceleration)
        self.execute_plan(block=True)

    def plan_spline(self, waypoints: List[PoseStamped], step_size: float = 0.001) -> None:
        self._reset_state()

        (plan, fraction) = self._move_group.compute_cartesian_path(
            [w.pose for w in waypoints], step_size, 0.0
        )

        if fraction < 1.0:
            _percent = int(fraction * 100)
            raise SplinePlanningError(f"Could only achieve {_percent}% of the path")

        self._trajectory = plan
        self._target_joint_values = self._trajectory.joint_trajectory.points[-1].positions

    def execute_spline(self, waypoints: List[PoseStamped], step_size: float = 0.001) -> None:
        self.plan_spline(waypoints, step_size)
        self.execute_plan(block=True)

    def plan_joint_position(
        self, positions: List[float], velocity: float, acceleration: float
    ) -> None:
        """
        Plan path to a target set of joint positions at a given velocity and acceleration

        :param positions: List of 7 radians
        :param velocity: Percentage of the max joint velocity in joint_limits
        :param acceleration: Percentage of the max joint acceleration in joint_limits
        """
        self._reset_state()

        active_joints = self._move_group.get_active_joints()
        if len(positions) != len(active_joints):
            raise ValueError(
                f"positions array contains {len(positions)} joints"
                f" where as this move_group has {len(active_joints)} joints"
            )

        joint_limits = self.get_joint_limits()

        # Assert the positions are not outside our joint limits
        for i, val in enumerate(positions):
            joint_name = active_joints[i]
            joint = joint_limits[joint_name]
            clamp(f"positions[{i}] ({joint.name})", val, joint.lower_bound, joint.upper_bound)

        clamp("velocity", velocity, 0.0001, 1)
        clamp("acceleration", acceleration, 0.0001, 1)

        self._move_group.set_max_acceleration_scaling_factor(acceleration)
        self._move_group.set_max_velocity_scaling_factor(velocity)
        self._move_group.set_joint_value_target(positions)

        self._plan()

    def execute_joint_position(
        self, positions: List[float], velocity: float, acceleration: float, block: bool = True
    ) -> None:
        self.plan_joint_position(positions, velocity, acceleration)
        self.execute_plan(block)

    @require_trajectory
    def execute_plan(self, block: bool = True) -> None:
        self._move_group.execute(self._trajectory, block)

        if not block:
            return

        if not self._reached_pose():
            raise MoveFailed("The robot did not reach it's target pose")

    @require_trajectory
    def _reached_pose(self) -> bool:
        target_joint_values: List[float] = cast(List[float], self._target_joint_values)

        distance = [
            abs(target - current)
            for target, current in zip(target_joint_values, self.get_joint_positions().values())
        ]

        if [i for i in distance if i >= self.JOINT_ERROR_THRESHOLD]:
            return False

        return sum(distance) / len(target_joint_values) < self.JOINT_MEAN_ERROR_THRESHOLD

The rest of my code is standard implementation and calls to the functions above.

Expected behaviour

• Same as disabling collisions in srdf; which is a successful plan.

Actual behaviour

• Disable collision using collision manager and plan same path which produces error below (same behaviour when connected to real robot /yaskawa):

[ INFO] [1621497011.006225090] [/fake/move_group]: Generating PTP trajectory...
[ERROR] [1621497011.068327329] [/fake/move_group]: Inverse kinematics for pose
0.17201
0.633975
0.278817 has no solution.
[ERROR] [1621497011.068450128] [/fake/move_group]: No IK solution for goal pose

[welcome]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[gavanderhoorn]

Could you add the full error message, instead of paraphrasing it?

[rr-robert]

Edited issue to include error message (see section "Actual Behaviour")

[rr-robert]

I have some extra debug messages to share. Below are two traces for identical plans. One however was initiated from the MoveGroupCommander and the other from rviz GUI. Notice that the pipeline_id is set in the rviz trace but not in the MoveGroupCommander and the planning request is fully populated from rviz yet predominately empty from MoveGroupCommander.

MoveGroup Commander

[ INFO] [1621513504.298925721] [/fake/move_group]: Received event 'stop'
[DEBUG] [1621513504.299080820] [/fake/move_group]: The action server has received a new goal request
[DEBUG] [1621513504.299305419] [/fake/move_group]: A new goal has been recieved by the single goal action server
[DEBUG] [1621513504.299422718] [/fake/move_group]: Accepting a new goal
[DEBUG] [1621513504.299489517] [/fake/move_group]: Accepting goal, id: /fake/move_group_commander_wrappers_1621513484971512745-1-1621513504.298822822, stamp: 1621513504.30
[DEBUG] [1621513504.299549717] [/fake/move_group]: Publishing feedback for goal, id: /fake/move_group_commander_wrappers_1621513484971512745-1-1621513504.298822822, stamp: 1621513504.30
[DEBUG] [1621513504.299584417] [/fake/move_group]: Publishing feedback for goal with id: /fake/move_group_commander_wrappers_1621513484971512745-1-1621513504.298822822 and stamp: 1621513504.30
[DEBUG] [1621513504.299618716] [/fake/move_group]: Trying to publish message of type [moveit_msgs/MoveGroupActionFeedback/12232ef97486c7962f264c105aae2958] on a publisher with type [moveit_msgs/MoveGroupActionFeedback/12232ef97486c7962f264c105aae2958]
[DEBUG] [1621513504.299670216] [/fake/move_group]: sync robot state to: 4.300s
[ INFO] [1621513504.393283161] [/fake/move_group]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[DEBUG] [1621513504.393360960] [/fake/move_group]: Running 'Fix Workspace Bounds'
[DEBUG] [1621513504.393391560] [/fake/move_group]: It looks like the planning volume was not specified. Using default values.
[DEBUG] [1621513504.393415960] [/fake/move_group]: Running 'Fix Start State Bounds'
[DEBUG] [1621513504.393437060] [/fake/move_group]: Running 'Fix Start State In Collision'
[DEBUG] [1621513504.393815757] [/fake/move_group]: Start state is valid with respect to group gp8_toolchange
[DEBUG] [1621513504.393859257] [/fake/move_group]: Running 'Fix Start State Path Constraints'
[DEBUG] [1621513504.394245154] [/fake/move_group]: Path constraints are OK. Running usual motion plan.
[DEBUG] [1621513504.394376153] [/fake/move_group]: Loading PlanningContext for request
<request>
workspace_parameters:
  header:
    seq: 0
    stamp: 0.000000000
    frame_id:
  min_corner:
    x: -5
    y: -5
    z: -5
  max_corner:
    x: 5
    y: 5
    z: 5
start_state:
  joint_state:
    header:
      seq: 0
      stamp: 0.000000000
      frame_id:
    name[]
    position[]
    velocity[]
    effort[]
  multi_dof_joint_state:
    header:
      seq: 0
      stamp: 0.000000000
      frame_id:
    joint_names[]
    transforms[]
    twist[]
    wrench[]
  attached_collision_objects[]
  is_diff: 1
goal_constraints[]
  goal_constraints[0]:
    name:
    joint_constraints[]
    position_constraints[]
      position_constraints[0]:
            header:
          seq: 0
          stamp: 0.000000000
          frame_id: world
        link_name: axia80_mate
        target_point_offset:
          x: 0
          y: 0
          z: 0
        constraint_region:
          primitives[]
            primitives[0]:
                        type: 2
              dimensions[]
                dimensions[0]: 0.0001
          primitive_poses[]
            primitive_poses[0]:
                        position:
                x: 0.17201
                y: 0.633975
                z: 0.278817
              orientation:
                x: 0
                y: 0
                z: 0
                w: 1
          meshes[]
          mesh_poses[]
        weight: 1
    orientation_constraints[]
      orientation_constraints[0]:
            header:
          seq: 0
          stamp: 0.000000000
          frame_id: world
        orientation:
          x: -0.499615
          y: -0.481931
          z: -0.517101
          w: 0.500733
        link_name: axia80_mate
        absolute_x_axis_tolerance: 0.0001
        absolute_y_axis_tolerance: 0.0001
        absolute_z_axis_tolerance: 0.0001
        parameterization: 0
        weight: 1
    visibility_constraints[]
path_constraints:
  name:
  joint_constraints[]
  position_constraints[]
  orientation_constraints[]
  visibility_constraints[]
trajectory_constraints:
  constraints[]
reference_trajectories[]
pipeline_id:
planner_id: PTP
group_name: gp8_toolchange
num_planning_attempts: 20
allowed_planning_time: 5
max_velocity_scaling_factor: 0.1
max_acceleration_scaling_factor: 0.1
cartesian_speed_end_effector_link:
max_cartesian_speed: 0

</request>
[ INFO] [1621513504.394517752] [/fake/move_group]: Initialized Point-to-Point Trajectory Generator.
[DEBUG] [1621513504.394539452] [/fake/move_group]: Found planning context loader for PTP group:gp8_toolchange
[ INFO] [1621513504.394581452] [/fake/move_group]: Generating PTP trajectory...
[ERROR] [1621513504.455530025] [/fake/move_group]: Inverse kinematics for pose
 0.17201
0.633975
0.278817 has no solution.
[ERROR] [1621513504.455662724] [/fake/move_group]: No IK solution for goal pose
[DEBUG] [1621513504.455727723] [/fake/move_group]: Setting the current goal as aborted
[DEBUG] [1621513504.455748023] [/fake/move_group]: Setting status to aborted on goal, id: /fake/move_group_commander_wrappers_1621513484971512745-1-1621513504.298822822, stamp: 1621513504.30
[DEBUG] [1621513504.456109421] [/fake/move_group]: Publishing result for goal with id: /fake/move_group_commander_wrappers_1621513484971512745-1-1621513504.298822822 and stamp: 1621513504.30
[DEBUG] [1621513504.456233020] [/fake/move_group]: Trying to publish message of type [moveit_msgs/MoveGroupActionResult/e8f0259d9c66fae7820b6818cc47d807] on a publisher with type [moveit_msgs/MoveGroupActionResult/e8f0259d9c66fae7820b6818cc47d807]
[DEBUG] [1621513504.456423719] [/fake/move_group]: Publishing feedback for goal, id: /fake/move_group_commander_wrappers_1621513484971512745-1-1621513504.298822822, stamp: 1621513504.30
[DEBUG] [1621513504.456541218] [/fake/move_group]: Publishing feedback for goal with id: /fake/move_group_commander_wrappers_1621513484971512745-1-1621513504.298822822 and stamp: 1621513504.30
[DEBUG] [1621513504.507206463] [/fake/move_group]: Published full planning scene: ''
[DEBUG] [1621513504.507265763] [/fake/move_group]: scene update 4.50724 robot stamp: 0
[DEBUG] [1621513504.507286062] [/fake/move_group]: Setting new planning scene: ''
[DEBUG] [1621513504.507396862] [/fake/rviz]: scene update 4.50738 robot stamp: 0

rviz

[DEBUG] [1621513379.010266649] [/fake/move_group]: scene update 9.01025 robot stamp: 0
[DEBUG] [1621513379.010290849] [/fake/move_group]: Setting new planning scene: ''
[DEBUG] [1621513379.010369248] [/fake/rviz]: scene update 9.01034 robot stamp: 0
[DEBUG] [1621513379.010396648] [/fake/rviz]: Setting new planning scene: ''
[DEBUG] [1621513379.217117801] [/fake/move_group]: The action server has received a new goal request
[DEBUG] [1621513379.217200300] [/fake/move_group]: A new goal has been recieved by the single goal action server
[DEBUG] [1621513379.217278199] [/fake/move_group]: Accepting a new goal
[DEBUG] [1621513379.217306199] [/fake/move_group]: Accepting goal, id: /fake/rviz-1-1621513379.216763503, stamp: 1621513379.22
[DEBUG] [1621513379.217332999] [/fake/move_group]: Publishing feedback for goal, id: /fake/rviz-1-1621513379.216763503, stamp: 1621513379.22
[DEBUG] [1621513379.217343099] [/fake/move_group]: Publishing feedback for goal with id: /fake/rviz-1-1621513379.216763503 and stamp: 1621513379.22
[DEBUG] [1621513379.217355099] [/fake/move_group]: Trying to publish message of type [moveit_msgs/MoveGroupActionFeedback/12232ef97486c7962f264c105aae2958] on a publisher with type [moveit_msgs/MoveGroupActionFeedback/12232ef97486c7962f264c105aae2958]
[DEBUG] [1621513379.217390799] [/fake/move_group]: sync robot state to: 9.217s
[DEBUG] [1621513379.260305998] [/fake/move_group]: Published full planning scene: ''
[DEBUG] [1621513379.260369298] [/fake/move_group]: scene update 9.26035 robot stamp: 0
[DEBUG] [1621513379.260407397] [/fake/move_group]: Setting new planning scene: ''
[DEBUG] [1621513379.260443697] [/fake/rviz]: scene update 9.26043 robot stamp: 0
[DEBUG] [1621513379.260526997] [/fake/rviz]: Setting new planning scene: ''
[ INFO] [1621513379.273948703] [/fake/move_group]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1621513379.274011102] [/fake/move_group]: Using planning pipeline 'pilz_industrial_motion_planner'
[DEBUG] [1621513379.274083102] [/fake/move_group]: Running 'Fix Workspace Bounds'
[DEBUG] [1621513379.274100402] [/fake/move_group]: Running 'Fix Start State Bounds'
[DEBUG] [1621513379.274141201] [/fake/move_group]: Running 'Fix Start State In Collision'
[DEBUG] [1621513379.274498599] [/fake/move_group]: Start state is valid with respect to group gp8_toolchange
[DEBUG] [1621513379.274555798] [/fake/move_group]: Running 'Fix Start State Path Constraints'
[DEBUG] [1621513379.274960296] [/fake/move_group]: Path constraints are OK. Running usual motion plan.
[DEBUG] [1621513379.275105995] [/fake/move_group]: Loading PlanningContext for request
<request>
workspace_parameters:
  header:
    seq: 0
    stamp: 1621513379.216642004
    frame_id: world
  min_corner:
    x: -1
    y: -1
    z: -1
  max_corner:
    x: 1
    y: 1
    z: 1
start_state:
  joint_state:
    header:
      seq: 0
      stamp: 0.000000000
      frame_id: world
    name[]
      name[0]: positioner_fixture_plate
      name[1]: yaskawa_joint_1_s
      name[2]: yaskawa_joint_2_l
      name[3]: yaskawa_joint_3_u
      name[4]: yaskawa_joint_4_r
      name[5]: yaskawa_joint_5_b
      name[6]: yaskawa_joint_6_t
    position[]
      position[0]: 0
      position[1]: 1.39626
      position[2]: 0.610865
      position[3]: -0.959931
      position[4]: -1.39626
      position[5]: 1.5708
      position[6]: 0
    velocity[]
    effort[]
  multi_dof_joint_state:
    header:
      seq: 0
      stamp: 0.000000000
      frame_id: world
    joint_names[]
    transforms[]
    twist[]
    wrench[]
  attached_collision_objects[]
  is_diff: 0
goal_constraints[]
  goal_constraints[0]:
    name:
    joint_constraints[]
      joint_constraints[0]:
            joint_name: yaskawa_joint_1_s
        position: 1.16885
        tolerance_above: 0.0001
        tolerance_below: 0.0001
        weight: 1
      joint_constraints[1]:
            joint_name: yaskawa_joint_2_l
        position: 0.913773
        tolerance_above: 0.0001
        tolerance_below: 0.0001
        weight: 1
      joint_constraints[2]:
            joint_name: yaskawa_joint_3_u
        position: -0.0924986
        tolerance_above: 0.0001
        tolerance_below: 0.0001
        weight: 1
      joint_constraints[3]:
            joint_name: yaskawa_joint_4_r
        position: 0.462288
        tolerance_above: 0.0001
        tolerance_below: 0.0001
        weight: 1
      joint_constraints[4]:
            joint_name: yaskawa_joint_5_b
        position: 1.05515
        tolerance_above: 0.0001
        tolerance_below: 0.0001
        weight: 1
      joint_constraints[5]:
            joint_name: yaskawa_joint_6_t
        position: -0.24094
        tolerance_above: 0.0001
        tolerance_below: 0.0001
        weight: 1
    position_constraints[]
    orientation_constraints[]
    visibility_constraints[]
path_constraints:
  name:
  joint_constraints[]
  position_constraints[]
  orientation_constraints[]
  visibility_constraints[]
trajectory_constraints:
  constraints[]
reference_trajectories[]
pipeline_id: pilz_industrial_motion_planner
planner_id: PTP
group_name: gp8_toolchange
num_planning_attempts: 10
allowed_planning_time: 5
max_velocity_scaling_factor: 1
max_acceleration_scaling_factor: 1
cartesian_speed_end_effector_link:
max_cartesian_speed: 0

</request>
[ INFO] [1621513379.275331993] [/fake/move_group]: Initialized Point-to-Point Trajectory Generator.
[DEBUG] [1621513379.275374993] [/fake/move_group]: Found planning context loader for PTP group:gp8_toolchange
[ INFO] [1621513379.275439292] [/fake/move_group]: Generating PTP trajectory...
[DEBUG] [1621513379.275748390] [/fake/move_group]: Running 'Add Time Parameterization'
[DEBUG] [1621513379.276085388] [/fake/move_group]: Motion planner reported a solution path with 26 states
[DEBUG] [1621513379.279943861] [/fake/move_group]: Planned path was found to be valid when rechecked
[DEBUG] [1621513379.280052860] [/fake/move_group]: Trying to publish message of type [moveit_msgs/DisplayTrajectory/41936b74e168ba754279ae683ce3f121] on a publisher with type [moveit_msgs/DisplayTrajectory/41936b74e168ba754279ae683ce3f121]
[DEBUG] [1621513379.280157259] [/fake/move_group]: Setting the current goal as succeeded
[DEBUG] [1621513379.280239759] [/fake/move_group]: Setting status to succeeded on goal, id: /fake/rviz-1-1621513379.216763503, stamp: 1621513379.22
[DEBUG] [1621513379.280288258] [/fake/move_group]: Publishing result for goal with id: /fake/rviz-1-1621513379.216763503 and stamp: 1621513379.22
[DEBUG] [1621513379.280304158] [/fake/move_group]: Trying to publish message of type [moveit_msgs/MoveGroupActionResult/e8f0259d9c66fae7820b6818cc47d807] on a publisher with type [moveit_msgs/MoveGroupActionResult/e8f0259d9c66fae7820b6818cc47d807]
[DEBUG] [1621513379.280329658] [/fake/move_group]: Publishing feedback for goal, id: /fake/rviz-1-1621513379.216763503, stamp: 1621513379.22
[DEBUG] [1621513379.280338158] [/fake/move_group]: Publishing feedback for goal with id: /fake/rviz-1-1621513379.216763503 and stamp: 1621513379.22
[DEBUG] [1621513379.510386647] [/fake/move_group]: Published full planning scene: ''
[DEBUG] [1621513379.510453947] [/fake/move_group]: scene update 9.51044 robot stamp: 0
[DEBUG] [1621513379.510473747] [/fake/move_group]: Setting new planning scene: ''
[DEBUG] [1621513379.510554746] [/fake/rviz]: scene update 9.51052 robot stamp: 0
[DEBUG] [1621513379.510612046] [/fake/rviz]: Setting new planning scene: ''
[DEBUG] [1621513379.760481996] [/fake/move_group]: Published full planning scene: ''
[DEBUG] [1621513379.760583195] [/fake/move_group]: scene update 9.76057 robot stamp: 0
[DEBUG] [1621513379.760604095] [/fake/move_group]: Setting new planning scene: ''
[DEBUG] [1621513379.760648395] [/fake/rviz]: scene update 9.76063 robot stamp: 0
[DEBUG] [1621513379.760670795] [/fake/rviz]: Setting new planning scene: ''

[rr-robert]

Now using MoveIt 1.1.4. Could the author of the new API function get_planning_pipeline_id(self, planning_pipeline), @felixvd explain what type/ object 'planning_pipeline' is? Im still stuck with this issue and Im trying to get a better understanding of the API, but can't get the above function to return.

[felixvd]

Sorry for not getting back to you. I can't test this at the moment, and it doesn't look like you posted the lines that you are actually executing. Your classes seem to make sense (although I didn't read it all), but what is the order of operations?

The planning_pipeline_id and planner_id are both strings that need to match an active planning pipeline and a planner inside that pipeline. You can check the available ones by looking at the terminal when your move_group starts up, or by checking the dropdown menu in the Rviz MotionPlanning plugin. These lines should work for ompl and the pilz planner:

group = moveit_commander.MoveGroupCommander("my_robot")
group.set_planning_pipeline_id("pilz_industrial_motion_planner")
group.set_planner_id("LIN")
[...]
group.set_planning_pipeline_id("ompl")
group.set_planner_id("RRTConnect")

If you 1) change the allowed collision matrix as shown in your code, 2) publish the change, and 3) try planning with both planners from MoveGroupCommander, I don't see how the plans could be using different ACMs. The Rviz MotionPlanning plugin should also use the same ACM. Please post the misbehaving lines if you can reproduce the problem.

I have some extra debug messages to share.

This one makes me think that you may have called set_planner_id but not set_planning_pipeline_id.

My planning_pipeline.launch is standard as below:

Are you starting up both the ompl and pilz_industrial_motion_planner pipelines in your move_group? It looks like your moveit_config package is out of date. If you regenerate it with the setup assistant, your file should look like this, and the move_group.launch should contain a section like this:

  <!-- Planning Pipelines -->
  <group ns="move_group/planning_pipelines">

    <!-- OMPL -->
    <include ns="ompl" file="$(find o2ac_moveit_config)/launch/planning_pipeline.launch.xml">
      <arg name="pipeline" value="ompl" />
    </include>

    <!-- CHOMP -->
    <include ns="chomp" file="$(find o2ac_moveit_config)/launch/planning_pipeline.launch.xml">
      <arg name="pipeline" value="chomp" />
    </include>

    <!-- Pilz Industrial Motion-->
    <include ns="pilz_industrial_motion_planner" file="$(find moveit_resources_prbt_moveit_config)/launch/planning_pipeline.launch.xml">
      <arg name="pipeline" value="pilz_industrial_motion_planner" />
    </include>

    <!-- Support custom planning pipeline -->
    <include if="$(eval arg('pipeline') not in ['ompl', 'chomp', 'pilz_industrial_motion_planner'])"
	     file="$(find o2ac_moveit_config)/launch/planning_pipeline.launch.xml">
      <arg name="pipeline" value="$(arg pipeline)" />
    </include>
  </group>

If there was anything unclear in the tutorials, please note it now while it's hot on your mind and feel free to submit a PR.

[rr-robert]

Thank you for your reply, my config is exactly as you describe above and since updating to 1.1.4 the set_planning_pipeline_id eradicates one of my concerns. However, I now apologise in advance for this question, as I feel I'm missing something very obvious and being a bit dumb. The command below does not return:

group.get_planning_pipeline_id("pilz_industrial_motion_planner")

The error is:

ArgumentError: Python argument types in
    MoveGroupInterface.get_planning_pipeline_id(MoveGroupInterface, str)
did not match C++ signature:
    get_planning_pipeline_id(moveit::planning_interface::MoveGroupInterfaceWrapper {lvalue})

I feel like I should be passing it a planning_pipeline object but I don't currently create one. Perhaps my oversight here is something so obvious to you guys that it isn't particularly documented? If so I apologise for the dumb question, but could you tell me how to create this object.

[felixvd]

I guess it's because you're writing "get" instead of "set"?

[rr-robert]

I am trying to "get" the id; "set" works fine. As per the code you wrote, the docstring says to pass get the planning_pipeline.

[felixvd]

My bad, I will double check/fix that soon.

As a workaround for now, I would just set both the planning pipeline and planner to whatever you need before planning.

[rr-robert]

My bad, I will double check/fix that soon.

I'm just glad it wasn't me being dumb! 😃

it doesn't look like you posted the lines that you are actually executing. Your classes seem to make sense (although I didn't read it all), but what is the order of operations?

Video below showing my order of operations and showing the OMPL vs. Pilz issue from MoveGroupCommander (using your new set function 🚀 ). Remember Pilz does actually work as expected when planned from rviz which I don't understand 😕. Would really appreciate some guidance as I'm blocked at the moment.

acm_pilz_bug_sm.mp4

[felixvd]

1. Long shot, but are you sure that the OMPL plan for target B with collisions disabled is the same motion that the PTP plan should return? You can check the trajectory in the plan to see if there's more than one joint goal.

2. @jschleicher @ct2034 In my own system I've gotten spurious IK failures that often work out when I attempt the plan multiple times. Could this be related?

[rr-robert]

You can check the trajectory in the plan to see if there's more than one joint goal.

I hope the below is what you were after?

Last point in _trajectory for OMPL target b (collisions disabled via MGI manager)

• positions: [1.2297643327024208, 0.8866434517297366, -0.13153589567446222, 0.3947528471556745, 1.0533737209939544, -0.20323410382787466]
  velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  accelerations: [0.07811556058867675, -0.129385950679334, -0.3886550505995608, -0.8402842448804891, 0.2427570865380844, 0.09535058874000003]
  effort: []
  time_from_start: 
    secs: 6
    nsecs: 658554343
  

Last point in _trajectory for pilz target b (collisions disabled via srdf)

• positions: [1.2297754365406206, 0.8868086455407083, -0.1315005379184601, 0.39480578386032017, 1.0535808297249376, -0.20323103154208577]
  velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  effort: []
  time_from_start: 
    secs: 4
    nsecs: 781501524
  

[rr-robert]

@felixvd I have a breakthrough of sorts from your debug suggestion! 👊 Planning to a joint position rather than pose works as expected. The command is below for reference. Of course for my application I need to supply poses, not joint positions but perhaps now this will help you guys pin point the root cause? 🤞
jogger.moveit.plan_joint_position([1.2297754365406206, 0.8868086455407083, -0.1315005379184601, 0.39480578386032017, 1.0535808297249376, -0.20323103154208577],0.5, 0.5).

For reference: I did then change IK solver from IKFast to KDL but that doesn't make a difference

[rr-robert]

@jschleicher @ct2034 would you be able to help identify the root cause? We are more than happy to contribute to a fix but we didn't write the library, so it would be far more efficient if Pilz engineers pointed us in the right direction.

[rr-robert]

I made a workaround for PTP planner_id, which is to convert pose to joint positions under the hood before planning. Can't do this for LIN, as the source code checks in cartesian points, see here. I believe the root cause is somewhere in the isStateColliding function, see here. Please help with some guidance as to where I go from here. I'm solely using the python API.

[jschleicher]

@robertjbush I'm not into the collision system and have no clue, why the ACM could not be updated. The code you point to, just uses the planning scene to check for collisions. Maybe @felixvd can give a hint, what's wrong in the usage or needs to be done to update the collsiion matrix?
https://github.com/ros-planning/moveit/blob/3361b2d1b6b2feabc2d3e93c75653f5a00e87fa4/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp#L571-L574

[felixvd]

@robertjbush Have you encountered this problem since? If so, is there a minimal example to reproduce the problem?

[rr-robert]

Hi @felixvd, Ive stopped using Pilz as a result of this bug. Is the code and information I specified not sufficient to replicate? Have you not encountered this issue with Pilz?

On Thursday we have a new software engineer starting and this bug will be one of his tasks.

[felixvd]

The gold standard would be a .test file using one of the robots in moveit_resources, containing one plan that succeeds with OMPL and another that fails with a Pilz planner :)

But it looks like we were able to reproduce a similar (or the same) bug on our system. Using OMPL after disabling collisions for a part of the scene (actually the robot) works, but the LIN planning request fails. I'll look into it.

[cambel]

Hi, I am checking this problem too.

I was able to reproduce the issue. In our scenario, there is an object define directly from the robot's URDF so it is considered as part of self-collision, however we can disable the collision checking planning_scene_interface.py->allow_collisions. The thing is, this works correctly for the OMPL planners but does not work for the Pilz ->LIN/CIRC.

After reading the code, I think that the problem is that Pilz planner's check if there is self collision when computing an IK solution for the whole robot describe by the URDF, instead of just considering the chain of links relevant to the actual robot. Whether this is the right approach or not, I don't know. The solution would be to compute an IK solution without self collision validation and then let the PlannerScene validate the found plan, OMPL seems to be doing this (wild guess).

From the code:
The RobotState computeIK is called with the extra isStateColliding
https://github.com/ros-planning/moveit/blob/3361b2d1b6b2feabc2d3e93c75653f5a00e87fa4/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp#L75-L80

Without this extra check, the behavior is as expected. The Pilz planners can generate a plan over an obstacle that has collision allowed but fail otherwise with the error message:

Found a contact between 'a_bot_right_inner_finger_pad' (type 'Robot link') and 'tray' (type 'Robot link'), which constitutes a collision. Contact information is not stored.

I tested this by just returning true (thus not self collision, weird naming though) here:
https://github.com/ros-planning/moveit/blob/3361b2d1b6b2feabc2d3e93c75653f5a00e87fa4/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp#L576

[rr-robert]

We're going to test @cambel's approach tomorrow or Friday. Thanks for sharing. I'll report back with whether it fixes our bug asap.

[rr-tom-noble]

Hey all, I'm also taking a look at this in place of @robertjbush. I'm a little new to MoveIt and pilz so please forgive any silly questions!

@cambel I'd be interested in learning how you tested your modified code. Are there unit tests within pilz that could be updated to verify new functionality, or would we have to make the modifications, compile from source, then swap the standard pilz package out with the modified one?

It sounds like MoveIt will apply collision detection at the appropriate part of the planning pipeline, so I'm wondering if the collision detection logic in pilz can be removed completely?

[cambel]

Hi @aa-tom, we have installed moveit from source, so I just modified the code, compiled and test. I don't know if there is a unit test for it.

As far as I understand, moveit checks collisions for the plan generated by any planner. The extra check of self collision for each IK solution sounds like a good idea if it's limited to the robot's link involved and if it can consider the update collision matrix. So, removing the self collision entirely from the pilz is one option but I guess it'd be better to have the self collision check focus on the parts of the robot that matter.

[v4hn]

As far as I understand, moveit checks collisions for the plan generated by any planner.

Yes and no. It's [a feature that can be disabled](https://github.com/ros-planning/moveit/blob/master/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h#L100-L102), though it's enabled by default. We usually expect planners to do collision checking on their own. Pilz's approach is quite unsatisfying to me in that regard anyway, because they only check for self-collisions and only for LIN (not for PTP) planning. So LIN trajectories will stop before self-collisions, but PTP trajectories will not. Neither will stop before environment collisions because the planner straight ignores the `PlanningScene` it receives together with the planning request and instead [creates an empty one for collision checking](https://github.com/ros-planning/moveit/blob/master/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp#L574). I'm not sure whether there are reasons for this behavior, maybe @jschleicher can comment on that. So all in all, at the moment the feature *has* to be enabled for MoveIt anyway because of environment collisions, but it's still reasonable to pass a collision checker to the IK plugin (which could in theory still ignore it if that's what you want).

[rr-tom-noble]

@v4hn @jschleicher Do you know where about in the code the PlanningScene would be received by pilz from MoveIt? I'm thinking of passing it down to the isStateColliding() function and using it as the basis for collision detection rather than creating a new one from scratch, which I'm hoping will propagate the collision matrix.

I've been taking a look at some of the relevant methods, in particular TrajectoryGenerator.generate(), TrajectoryGenerator.plan(), generateJointTrajectory(), computePoseIK(), but as far as I can tell none of the parameters available to these methods can be used to access the PlanningScene. I'm also unable to tell where TrajectoryGenerator.generate() (which is the highest-level planning method I was able to find) is being called from in the code.

[v4hn]

this is the top-level entry point from MoveIt to the planner where the correct scene is passed.

For the service/action interface for sequence planning it's here instead.

[rr-tom-noble]

Excellent, thanks. Based on a look at the code it seems like the PlanningContext class is responsible for holding all relevant state required by a particular planner? My current idea is:

1. Modify PlanningContextBase to contain a reference to the PlanningScene *
2. Modify PlanningContextLoader::loadContext() to populate this reference with the scene (or maybe in pilz_industrial_motion::getPlanningContext() would be better) *
3. Starting at PlanningContextPtr::solve() here pass the scene reference down the method calls to the collision detection function TrajectoryGenerator::generate() -> TrajectoryGenerator::plan() -> pilz_industrial_motion_planner::generateJointTrajectory() -> pilz_industrial_motion_planner::computePoseIK() -> pilz_industrial_motion_planner::isStateColliding()
4. Use this scene to create the collision detection copy using the goal pose of the robot

Does this sound sensible?

*Edit: Just realised that setting the planning scene is already provided by planning_interface::PlanningContext, which is used here, so 1 and 2 aren't needed

[rr-tom-noble]

@v4hn I've implemented a fix based on the above approach. What's the process for raising a PR?

[v4hn]

What's the process for raising a PR?

I don't get the question. Create a pull-request on github, make sure CI succeeds, and in the best case add a test that is fixed by the PR.

[rr-robert]

This has been fixed by @aa-tom (with help from @cambel), but is waiting for @jschleicher to review #2803.