Robotics StackExchange | Archived questions

moveit rviz executes plan but not script

I am using ROS Melodic with Ubuntu 16 running virtual with Parallels on Macbook Pro

I have created xacro file that integrates the wx200 Trossen robot arm with a vertical gantry robot with a tray that extends horizontally. The arm and tray are defined as 2 move groups I have used the Moveit Setup Assistant to generate the launch files, yaml files, surdf file etc

I can use Rviz to drag the arm or tray into any reasonable position and execute the planned path successfully. However when I attempt to do them same with a python script as per http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html

It more often fails with the error

ABORTED: No motion plan found. No execution attempted.

On the Rviz terminal the error is "RRTConnect: Unable to sample any valid states for goal tree"

I have verified that the goal position of the move group matches the position in the goal state in rviz that rviz will successfully execute where the script fails.

What is RVIZ doing differently from my script and how do I get my script working?

The messages the come from the Rviz terminal after a successfully executed trajectory seem to mostly match those generated by the script attempting a the same goal but without the error message. There is also mention of

[ INFO] [1577256266.952056630]: Planning attempt 1 of at most 1
[ INFO] [1577256266.952701828]: Planner configuration 'tray' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1577256266.953182542]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1577256266.953430426]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1577256266.953914875]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1577256266.955898687]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1577256266.956269627]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1577256266.959214103]: RRTConnect: Created 6 states (2 start + 4 goal)
[ INFO] [1577256266.959952153]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1577256266.961281256]: RRTConnect: Created 6 states (2 start + 4 goal)
[ INFO] [1577256266.966632123]: ParallelPlan::solve(): Solution found by one or more threads in 0.013677 seconds
[ INFO] [1577256266.967154776]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1577256266.967338454]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1577256266.967794293]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1577256266.969386733]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1577256266.975225945]: RRTConnect: Created 6 states (2 start + 4 goal)
[ INFO] [1577256266.977646842]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1577256266.978931579]: RRTConnect: Created 6 states (2 start + 4 goal)
[ INFO] [1577256266.983391074]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1577256266.983618173]: ParallelPlan::solve(): Solution found by one or more threads in 0.016620 seconds
[ INFO] [1577256266.983871001]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1577256266.983963891]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1577256266.987698261]: RRTConnect: Created 6 states (2 start + 4 goal)
[ INFO] [1577256266.994881020]: RRTConnect: Created 6 states (2 start + 4 goal)
[ INFO] [1577256267.000797830]: ParallelPlan::solve(): Solution found by one or more threads in 0.017001 seconds
[ INFO] [1577256267.007318762]: SimpleSetup: Path simplification took 0.006400 seconds and changed from 3 to 2 states
[ INFO] [1577256267.053537866]: Fake execution of trajectory
[ INFO] [1577256269.152267818]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1577256276.852035843]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.

Setting the goal tolerance can stop the errors but the arm/tray only gets halfway to the goal

Both the tray and arm use the KDL IK solver as defined in kinematics.yaml , I have read that this is inappropriate for a 2DOF group like the tray (vertical prismatic and horizontal prismatic). However, why then is RVIZ continually executing planned paths without error?

# General Imports 
import sys
import copy
from math import pi
import time

# ROS Imports
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
from std_srvs.srv import Empty

# init
rospy.init_node('control_arm', anonymous=True)
moveit_commander.roscpp_initialize(sys.argv)


# init MoveIt
robot_name = rospy.get_param("/robot_name")
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()#ns="/"+robot_name)
arm_group_name = "arm"
arm_group = moveit_commander.MoveGroupCommander(arm_group_name)
tray_group_name = "tray"
tray_group = moveit_commander.MoveGroupCommander(tray_group_name)
print(tray_group.get_current_pose().pose)

eef_link = arm_group.get_end_effector_link()

rospy.loginfo("MoveIt initialized.")

while(not rospy.is_shutdown()):
    #tray_group.set_goal_tolerance(0.2)
    rospy.loginfo("moving tray to printer")
    pose_goal = tray_group.get_current_pose().pose
    pose_goal.position.z = 0.1
    tray_group.set_pose_target(pose_goal)
    plan = arm_group.go(wait=True)
    tray_group.stop()
    tray_group.clear_pose_targets()

    rospy.sleep(100.)

Asked by georgebuild on 2019-12-25 03:01:51 UTC

Comments

Have you managed to solve this in the meantime? It looks like the goal that you define in your Python script is not feasible, but the one from Rviz is. Have you checked the content of the MotionPlanRequest that Rviz initiates?

Asked by fvd on 2020-02-28 03:01:05 UTC

Any updates here? I am having the same issue.

Asked by raghava13 on 2020-10-14 16:57:50 UTC

Answers