Ask Your Question
1

How to use orientation constraints in moveit?

asked 2018-12-14 08:46:24 -0500

boandlgrama gravatar image

updated 2018-12-21 03:33:15 -0500

For a box picking task with moveit, I want to add an tilt to prevent losing the content of the Box. (Like moving a filled glass of water) Therefore I added an OrientationConstraint in the Move Group Python Interface:

    constraint = Constraints()
    constraint.name = "tilt constraint"
    tilt_constraint = OrientationConstraint()
    # 'base_link' is equal to the world link
    tilt_constraint.header.frame_id = "base_link"
    # The link that must be oriented upwards
    tilt_constraint.link_name = "box_gripper_link"
    tilt_constraint.orientation = Quaternion(0.5, 0.5, 0.5, 0.5)
    # Allow rotation of 45 degrees around the x and y axis
    tilt_constraint.absolute_x_axis_tolerance = 0.45 #Allow max rotation of 45 degrees
    tilt_constraint.absolute_y_axis_tolerance = 3.6 #Allow max rotation of 360 degrees
    tilt_constraint.absolute_z_axis_tolerance = 0.45 #Allow max rotation of 45 degrees
    # The tilt constraint is the only constraint
    tilt_constraint.weight = 1
    constraint.orientation_constraints = [tilt_constraint]
    self.robot_controller.move_group.set_path_constraints(constraint)

My Problem here is, that the path planner is partly completely ignoring the constraint. I am also not sure whether radians are the unit of the axis_tolerance.

Any ideas what am I doing wrong, or where the mistake could be?

Thanks in advance.

Addition: Picture of the planning scene with relevant frames (I don't have the permissions to upload a file): https://ibb.co/yXCvxXw

edit retag flag offensive close merge delete

Comments

tilt_constraint.header should be tilt_constraint.header.frame_id. Moreover, are you sure you want the orientation to be that of the "null" quaternion? I have no idea regarding the tolerances, I was using absolute numbers but it is underdocumented in that sense.

aPonza gravatar imageaPonza ( 2018-12-17 02:14:05 -0500 )edit

I was looking for unrelated things and found this so in position 0.0001 = 0.1 mm, in orientation 0.001 = ~0.1 deg

aPonza gravatar imageaPonza ( 2018-12-17 10:49:40 -0500 )edit

Thanks for your replay, the missing .frame_id was just a copying mistake. I corrected that. Regarding the quaternion, I think the correct values should be (0.5, 0.5, 0.5, 0.5). I've added a picture of the scenery here: https://ibb.co/yXCvxXw

boandlgrama gravatar imageboandlgrama ( 2018-12-18 02:57:28 -0500 )edit

If 0.001 is about 0.1 degrees, then my absolute tolerance with pi/4 would be ~78.5 deg. However, the max deviation of the planner was much higher, which was a full turn around the z-axis of the end effector.

boandlgrama gravatar imageboandlgrama ( 2018-12-18 03:05:19 -0500 )edit

I don't understand: you're setting tilt_constraint.absolute_z_axis_tolerance = 2 * pi so a full turn around the z-axis is allowed, isn't it?

aPonza gravatar imageaPonza ( 2018-12-18 03:50:34 -0500 )edit

Yes, but I thought the tolerance refers to the link named in header.frame_id or not? So I would allow a full turn around the z-axis of the 'world'-link.

boandlgrama gravatar imageboandlgrama ( 2018-12-18 03:55:07 -0500 )edit

As per the docs, The robot link this constraint refers to is OrientationConstraint::link_name

aPonza gravatar imageaPonza ( 2018-12-18 04:21:57 -0500 )edit

Ok, thanks, I adjusted this but the problem still occurs. I have noticed that the robot is executing the faulty turning while to rest of the robot is standing still. So only the last joint of the robot is turning. Is that maybe a clue for an other planning problem?

boandlgrama gravatar imageboandlgrama ( 2018-12-18 04:54:59 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-01-07 03:42:20 -0500

boandlgrama gravatar image

The answer/workaround is to add the enforce_joint_model_state_space: true parameter to your planning group in the ompl_planning.yaml (see #541). Furthermore the unit of the absolute_tolerances is [rad].

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-12-14 08:46:24 -0500

Seen: 1,342 times

Last updated: Jan 07