Robotics StackExchange | Archived questions

moveit joint space constraints in python

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_core/html/classkinematic__constraints_1_1JointConstraint.html

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.

Asked by danjo on 2018-10-03 08:19:32 UTC

Comments

re: changing limits: what's probably happening is that RRT et al. are not very good at handling "narrow passages" in the search space. Reducing joint limits can introduce narrow passages leading to longer planning times.

Asked by gvdhoorn on 2018-10-03 09:18:54 UTC

Answers

As posted on the discourse thread:

Does this tutorial solve your problem? The corresponding set_path_constraints function exists in the Python MoveGroupCommander.

Looks like @jarvisschultz link from the comment helped.

Asked by fvd on 2018-10-04 06:39:55 UTC

Comments

Since you have a UR10, the jog_arm package will work on your robot. It's not a point-to-point planner (yet) but you publish velocity commands to it. By definition, the orientation of the end-effector will be constant (unless you publish a roll/pitch/yaw velocity).

And we have recreated the Boston Dynamics "upright wine glass" video -- maintaining an end effector pose as the base of the robot moves. This cannot be done with point-to-point planners. So that's a cool ability, but it's still a bit slow and jerky. (video) The jerkiness is probably due to nav stack localization inaccuracies.

jog_arm will not violate whatever joint constraints are defined in the SRDF. You can add a bit of padding around the joint limits, if you want (maybe 5 degrees).

If you decide to go this route, send me a message and I will set you up with a config file for a UR10.

Asked by AndyZe on 2018-10-04 10:11:27 UTC

Comments

I like jog_arm as much as anyone else, but the OP seems to be asking for a way to plan motions with constraints. Afaik jog_arm is not a planner, nor can it be used as one -- as you already mention yourself.

Asked by gvdhoorn on 2018-10-04 10:49:44 UTC

Debatable, I think. It might be useful because you don't need to define constraints.

Asked by AndyZe on 2018-10-04 10:52:28 UTC

Can I give jog_arm a start and end pose and will it then plan trajectories that avoid collisions?

I know "planner" is an ambiguous term, but in the context of ROS and the context the OP describes it seems very likely they are (trying to) us(ing) moveit with constraints.

Asked by gvdhoorn on 2018-10-04 10:55:15 UTC

Start and end pose -- yes with C++ api. Plan around collisions -- no. It only succeeds if there's a straight, clear path between start and end pose.

Asked by AndyZe on 2018-10-04 10:57:13 UTC