Ask Your Question
0

moveit joint space constraints in python

asked 2018-10-03 08:40:52 -0500

danjo gravatar image

updated 2018-10-05 10:59:40 -0500

gvdhoorn gravatar image

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.

edit retag flag offensive close merge delete

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.

gvdhoorn gravatar imagegvdhoorn ( 2018-10-03 09:18:54 -0500 )edit
jarvisschultz gravatar imagejarvisschultz ( 2018-10-03 14:43:12 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2018-10-04 06:39:55 -0500

fvd gravatar image

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.

edit flag offensive delete link more
-1

answered 2018-10-04 10:11:27 -0500

AndyZe gravatar image

updated 2018-10-04 10:29:14 -0500

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.

edit flag offensive delete link more

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.

gvdhoorn gravatar imagegvdhoorn ( 2018-10-04 10:49:44 -0500 )edit

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

AndyZe gravatar imageAndyZe ( 2018-10-04 10:52:28 -0500 )edit

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.

gvdhoorn gravatar imagegvdhoorn ( 2018-10-04 10:55:15 -0500 )edit

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.

AndyZe gravatar imageAndyZe ( 2018-10-04 10:57:13 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2018-10-03 08:19:32 -0500

Seen: 1,160 times

Last updated: Oct 05 '18