How to benchmark Moveit! planning with constraints?
I want to benchmark several planners for moving a box with a 6 axis robot. Therefore I want to add an Orientation Constraint.
In the Moveit tutorial for benchmarking it says:
path_constraints: [Regex for the path constraints to benchmark] # Default ""
But how can I add named constraints? When I start my moveit launch file (equivalent to the demo.launch
from the setup assistant) with db:=true
and then add a Path Constraint with the moveit python interface it's
not listed in the Path Constraints drop down menu of the moveit rviz plugin. The constraints themselves work.
For adding the constraint I use these commands:
def add_tilt_constaint(self):
rospy.loginfo("add tilt constraint")
current_pose = self.robot_controller.move_group.get_current_pose()
constraint = Constraints()
constraint.name = "tilt constraint"
tilt_constraint = OrientationConstraint()
tilt_constraint.header.frame_id = "base_link"
tilt_constraint.link_name = "box_gripper_link"
tilt_constraint.orientation = Quaternion(0.5, 0.5, 0.5, 0.5)
tilt_constraint.absolute_x_axis_tolerance = pi / 3
tilt_constraint.absolute_y_axis_tolerance = pi
tilt_constraint.absolute_z_axis_tolerance = pi / 4
tilt_constraint.weight = 1
constraint.orientation_constraints = [tilt_constraint]
self.robot_controller.move_group.set_path_constraints(constraint)
rospy.loginfo("Known path constraints:\n{}".format(
self.robot_controller.move_group.get_path_constraints()))
I'm using Kinetic on Ubuntu 16.04
EDIT: I also successfully connected to the database before (in the rviz plugin and with the python interface)