PanadaArm triggers self_collision_avoidance_violation

[samterra]

Description

Frequently when I am using moveit to plan paths, the execution fails due to
libfranka: Move command aborted: motion aborted by reflex! ["self_collision_avoidance_violation"]. I was under the impression that moveit should provide paths that avoid self collisions (especially since this arm is used in the tutorials)?

Your environment

• ROS Distro: Kinetic
• OS Version: Ubuntu 16.04
• Source or Binary build? Binary, version 0.9.15

Steps to reproduce

Below is a file I have written with a simple class ,based on the python tutorial.

#!/usr/bin/env python
import sys
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list

class PandaRobotController(object):
  def __init__(self):
    super(PandaRobotController, self).__init__()

    self.plan = None

    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_to_front', anonymous=True)

    self.robot = moveit_commander.RobotCommander()
    self.scene = moveit_commander.PlanningSceneInterface()
    self.group_name = 'panda_arm'
    self.move_group = moveit_commander.MoveGroupCommander(self.group_name)
    self.setup()

  def setup(self):
    self.add_floor()
    self.add_wall(1)
    self.move_group.set_max_acceleration_scaling_factor(0.1)
    self.move_group.set_max_velocity_scaling_factor(0.1)
    self.move_group.set_goal_orientation_tolerance(3.14/20)
    self.move_group.set_goal_position_tolerance(0.05)

  def add_floor(self):
    table_pose = geometry_msgs.msg.PoseStamped()
    table_pose.header.frame_id = 'panda_link0'
    table_pose.pose.orientation.w = 1.0
    size = [2, 2, 0.1]
    self.scene.add_box('table', table_pose, size)

  def add_wall(self, side=0):
    ''' side = 0: blocks positive y
            = 1 blocks positive x
            = 2 blocks negative y
            = 3 blocks negative x
    '''
    wall_pose = geometry_msgs.msg.PoseStamped()
    wall_pose.header.frame_id = 'panda_link0'
    size = [1, 1, 1]
    if side == 0:
      wall_pose.pose.position.y = 0.5
      size = [2, 0.1, 2]
    elif side == 1:
      wall_pose.pose.position.x = 0.5
      size = [0.1, 2, 2]
    elif side == 2:
      wall_pose.pose.position.y = -0.5
      size = [2, 0.1, 2]
    elif side == 3:
      wall_pose.pose.position.x = -0.5
      size = [0.1, 2, 2]
    else:
      raise ValueError('walls can only be on sides 0-3')
    wall_pose.pose.orientation.w = 1.0
    self.scene.add_box('wall' + str(side), wall_pose, size)

  def plan_pose_goal(self, x=-0.3, y=-0.3, z=0.3,rot_x = 0.5, rot_y=0.5, rot_z=0.5,  w=1.0):
    pose_goal = [x, y, z, rot_x, rot_y, rot_z]
    self.move_group.set_pose_target(pose_goal)
    self.plan = self.move_group.plan()
    #self.move_group.clear_pose_targets()

  def increment_plan(self, axis, value):
    self.move_group.shift_pose_target(axis, value)
    self.plan = self.move_group.plan()

  def execute_plan(self):
    self.move_group.execute(self.plan, wait=True)
    self.move_group.stop()

  def print_state(self):
    print self.move_group.get_current_pose()

Then in one terminal window, I write
roslaunch panda_moveit_config panda_control_moveit_rviz.launch load_gripper:=true robot_ip:=172.16.0.2
and in another window I open a python console and write:

>>> import move_to_front
>>> control = move_to_front.PandaRobotController()
>>> move = control.move_group
>>> wpose = move.get_current_pose().pose
>>> wpose.position.x -= 0.2
>>> wpose.position.y -= 0.1
>>> waypoints =[wpose]
>>> plan, fraction= move.compute_cartesian_path(waypoints, 0.01, 0.0)
>>> move.execute(plan)
True
>>> wpose.position.y += 0.5
>>> waypoints = [wpose]
>>> plan, fraction= move.compute_cartesian_path(waypoints, 0.01, 0.0)
>>> move.execute(plan)
[ INFO] [1542755244.263602120]: ABORTED: Solution found but controller failed during execution
False

Expected behaviour

I think that it should either successfully move the arm (it looks like it won't be in collision), or fail in the planning phase due to self_collision

Actual behaviour

It gives a plan, then the plan fails in execution stage.

[welcome]

Thanks for reporting an issue. We will have a look asap. If you can think of a fix, please consider providing it as a pull request.

[rhaschke]

Is libfranka even stronger in collision checking (e.g. adding more padding)?

[samterra]

That's possible. Libfranka defines a way to alter the collision behavior but since those values are torque and force, I think that is only for collisions with other objects.
The self collision avoidance happens before the robot actually hits itself (or even looks like it will hit itself, I had it running at a very slow speed and the path looked clear).
Is there some way that I can increase the self-collision padding in MoveIt so that it can match better with libfranka?

[fwalch]

See frankaemika/franka_ros#39