Unstable planning when use Stomp

[Juzhan]

My system is Ubuntu 14.04, the ROS distribution is indigo. Recently I am working with Fetch robot in gazebo simulation and I want to use stomp for better motion planning.

I install stomp from apt: apt-get ros-indigo-stomp-moveit .Then I modify the move_group.launch to use stomp as default motion planner.

When I use stomp, I found that it can't work well all the time. If I start a new empty world in gazebo with a Fetch robot, most of the time it can plan a trajectory to execute, but after a while, once it plan fails, it can't plan any motion anymore, even though the target poses it used to plan. And I have to restart the whole world instead of move_group.launch. Once it planed fail, it will print these error:

[ERROR] [1566302768.596134715, 42.778000000]: STOMP Start joint pose is out of bounds
[ERROR] [1566302768.596181439, 42.778000000]: STOMP failed to get the start and goal positions

Here is my python code to test stomp on Fetch robot, I wonder what's wrong with my code and how to fix the unstable situation. This code works fine with OMPL.

#!/usr/bin/env python
#-*- coding=utf-8 -*-

import rospy
import moveit_commander
from geometry_msgs.msg import Quaternion, Point, Pose, PoseStamped
from moveit_msgs.msg import MoveItErrorCodes
from std_msgs.msg import Header
from moveit_msgs.srv import GetPositionIK, GetPositionIKRequest
from moveit_msgs.msg import RobotState
from sensor_msgs.msg import JointState

class IKer(object):
    def __init__(self):
        self.ik_client = rospy.ServiceProxy('compute_ik', GetPositionIK)

    def compute_ik(self, pose_stamped, joints_name=None, timeout=rospy.Duration(10)):
        """Computes inverse kinematics for the given pose.

        Args:
            pose_stamped: geometry_msgs/PoseStamped.
            timeout: rospy.Duration. How long to wait before giving up on the
                IK solution.
        """
        request = GetPositionIKRequest()
        request.ik_request.pose_stamped = pose_stamped
        request.ik_request.group_name = 'arm_with_torso'
        request.ik_request.timeout = timeout
        response = self.ik_client(request)
        # error_str = response.error_code.val)
        success = response.error_code.val == MoveItErrorCodes.SUCCESS
        if not success:
            return None
        joints_state = response.solution.joint_state
        joints_position = []
        if joints_name is not None:
            for index, name in enumerate(joints_state.name):
                if name in joints_name:
                    joints_position.append(joints_state.position[index])

            joints_state.name = joints_name
            joints_state.position = joints_position
        
        return joints_state

rospy.init_node("hand_up")

ik = IKer()
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm_with_torso")
arm.set_pose_reference_frame("base_link")
arm.clear_pose_targets()

points = [
    Point(0.609014743623, 0.129241553455, 0.786647925569),
    Point(0.609014743623, 0.029241553455, 0.986647925569),
    Point(0.709014743623, -0.129241553455, 0.786647925569)
]

pos = Point(0.609014743623, 0.229241553455, 0.686647925569)
qua = Quaternion(0,0,0,1)
pose = Pose(pos, qua)
ps = PoseStamped()
ps.pose = pose
ps.header.frame_id = "base_link"

robot_state = RobotState()
joint_state = JointState()
joint_state.header = Header()
for p in points:
    ps.pose.position = p
    ps.header.stamp = rospy.Time.now()
    
    joints_value = ik.compute_ik(ps, arm.get_joints())
    joints_value.header = Header()
    joints_value.header.stamp = rospy.Time.now()

    joint_state.header.stamp = rospy.Time.now()
    joint_state.name = arm.get_joints()
    joint_state.position = arm.get_current_joint_values()
    robot_state.joint_state = joint_state

    arm.set_start_state(robot_state)
    arm.set_joint_value_target(joints_value)
    arm.allow_replanning(True)
    arm.set_planning_time(1000)

    plan = arm.plan()
    if plan is not None:
        arm.execute(plan)

    arm.clear_pose_targets()