moveit joint space constraints in python

edit

we have successfully done some pick and place motion planning for a ur10 robot using moveit. we can successfully add boxes and see them in rviz and it moves around them, etc. But what we'd really like to do is find a way to limit the joint angles and also establish some end effector orientation constraints, a la "keep the water glass upright"...

i found this joint constraint in the c++ docs http://docs.ros.org/indigo/api/moveit...

anything similar exist in python? and any examples of it in use out there?

setup: ros kinetic gazebo moveit rviz ubuntu 16.04 ur10 default ompl parameters.

*we did try to limit joint angles in the .xacro file but that seems to be post OMPL solver. ie. you would expect solver to really speed up on a simple path if you shrink the state space but changing .xacro file seems to actually slow it down. which makes me speculate that the OMPL RRT algorithm random path planning is still trying random steps within the whole joint space and then checking to see if that is within the limits set in .xacro rather than using the limits in .xacro to more intelligently guess... *

thanks! danjo

Edit: We got this simple constraint for one joint working(the base/shoulder of the ur10).

   def init_upright_path_constraints(self):
    self.upright_constraints = Constraints()
    joint_constraint = JointConstraint()

    self.upright_constraints.name = "upright"

    joint_constraint.position = 0
    joint_constraint.tolerance_above = .1
    joint_constraint.tolerance_below = 3.14/2
    joint_constraint.weight = 1

    joint_constraint.joint_name = "shoulder_lift_joint"
    self.upright_constraints.joint_constraints.append(joint_constraint)

This constraint is not exactly "upright" but it does constrain the motion of the shoulder and gives us much nicer looking path plans without really slowing the solution time down.

For end effector uprightness, it certainly seems that this tutorial

moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "panda_link7";
ocm.header.frame_id = "panda_link0";
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.1;
ocm.weight = 1.0;

and this post

def init_upright_path_constraints(self,pose):

    self.upright_constraints = Constraints()
    self.upright_constraints.name = "upright"
    orientation_constraint = OrientationConstraint()
    orientation_constraint.header = pose.header
    orientation_constraint.link_name = self.arm.get_end_effector_link()
    orientation_constraint.orientation = pose.pose.orientation
    orientation_constraint.absolute_x_axis_tolerance = 0.4
    orientation_constraint.absolute_y_axis_tolerance = 0.4
    orientation_constraint.absolute_z_axis_tolerance = 0.4
    #orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis
    orientation_constraint.weight = 1

    self.upright_constraints.orientation_constraints.append(orientation_constraint)


def enable_upright_path_constraints(self):
    self.arm.set_path_constraints(self.upright_constraints)

def disable_upright_path_constraints(self):
    self.arm.set_path_constraints(None)

are relevant but all our efforts towards orientation constraints of the end effector lead the path planner to stop finding solutions... We spent time looking into the various includes, the names of links, the matching of start position and constraint, etc. I'll post an update if we get it working. But in the meantime, if anyone has a snippet for the ur10 like the ones above please post.