How to benchmark Moveit! planning with constraints?

asked 2019-01-18 01:54:13 -0500

boandlgrama gravatar image

updated 2019-01-22 00:08:00 -0500

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)

edit retag flag offensive close merge delete