Last active
June 12, 2018 09:28
-
-
Save christgau/03119d7905c79f4501f07794e22fa8da to your computer and use it in GitHub Desktop.
MWE for reproducing MoveIt's "No Sampler was constructed" error message
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
[ INFO] [1528795560.431024430]: Loading robot model 'panda'... | |
[ INFO] [1528795560.431098252]: No root/virtual joint specified in SRDF. Assuming fixed joint | |
[ INFO] [1528795560.537700015]: Loading robot model 'panda'... | |
[ INFO] [1528795560.537766247]: No root/virtual joint specified in SRDF. Assuming fixed joint | |
[ INFO] [1528795561.598329916]: Ready to take commands for planning group panda_arm. | |
[ INFO] [1528795561.809107519]: Ready to take commands for planning group panda_arm_hand. | |
[ INFO] [1528795562.007847478]: Ready to take commands for planning group hand. | |
[ WARN] [1528795568.882201009]: Fail: ABORTED: No motion plan found. No execution attempted. |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env python2 | |
import sys | |
from math import pi | |
import rospy | |
import moveit_commander | |
import geometry_msgs.msg | |
from geometry_msgs.msg import Pose, Point, Quaternion | |
from tf.transformations import quaternion_from_euler | |
PLACE_TARGET = (0, 0.5, 0.0) | |
CUBE = {'position': (+0.5, 0.0, 0.025), 'size': 0.05} | |
def moveto(pos): | |
pose_target = Pose() | |
# point downwards | |
pose_target.orientation = Quaternion(*quaternion_from_euler(pi, 0, 0)) | |
# avoid moving below ground level/or the object | |
pose_target.position = Point(*pos) | |
pose_target.position.z += 0.15 | |
arm.set_pose_target(pose_target) | |
arm.go() | |
def init_scene(): | |
rospy.sleep(1) | |
# add table to scene | |
TABLE_WIDTH = 1.0 | |
TABLE_LENGTH = 2.0 | |
PANDA_OFFSET = 0.145 | |
add_box('table', (TABLE_WIDTH / 2 - PANDA_OFFSET, 0, 0), (TABLE_WIDTH, TABLE_LENGTH, -0.001)) | |
add_box('cube', CUBE['position'], (CUBE['size'], CUBE['size'], CUBE['size'])) | |
def add_box(name, pos, size): | |
p = geometry_msgs.msg.PoseStamped() | |
p.header.frame_id = robot.get_planning_frame() | |
p.pose.orientation.w = 1.0 | |
p.pose.position = Point(*pos) | |
scene.add_box(name, p, size) | |
# -- main ---------------------------- | |
moveit_commander.roscpp_initialize(sys.argv) | |
rospy.init_node('picktest') | |
robot = moveit_commander.RobotCommander() | |
scene = moveit_commander.PlanningSceneInterface() | |
# hand and arm fail with 'no end-effector specified for pick action' | |
arm = moveit_commander.MoveGroupCommander('panda_arm') | |
arm_and_hand = moveit_commander.MoveGroupCommander('panda_arm_hand') | |
hand = moveit_commander.MoveGroupCommander('hand') | |
init_scene() | |
# this one fails with MoveIt "no sampler was constructed" error message | |
moveto((0.2, 0, 0.5)) | |
moveto(CUBE['position']) | |
arm_and_hand.pick('cube') | |
moveit_commander.roscpp_shutdown() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
[ INFO] [1528795568.879915844]: Planning attempt 1 of at most 1 | |
[ INFO] [1528795568.880117443]: Added plan for pipeline 'pick'. Queue is now of size 1 | |
[ERROR] [1528795568.880618580]: No sampler was constructed | |
[ INFO] [1528795568.880699576]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 1 | |
[ WARN] [1528795568.881070471]: All supplied grasps failed. Retrying last grasp in verbose mode. | |
[ INFO] [1528795568.881194245]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1 | |
[ INFO] [1528795568.881401932]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0 | |
[ INFO] [1528795568.881726837]: Pickup planning completed after 0.001074 seconds |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment