Robotics StackExchange | Archived questions

How to use orientation constraints in moveit?

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

Asked by boandlgrama on 2018-12-14 09:46:24 UTC

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.

Asked by aPonza on 2018-12-17 03:14:05 UTC

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

Asked by aPonza on 2018-12-17 11:49:40 UTC

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

Asked by boandlgrama on 2018-12-18 03:57:28 UTC

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.

Asked by boandlgrama on 2018-12-18 04:05:19 UTC

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?

Asked by aPonza on 2018-12-18 04:50:34 UTC

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.

Asked by boandlgrama on 2018-12-18 04:55:07 UTC

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

Asked by aPonza on 2018-12-18 05:21:57 UTC

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?

Asked by boandlgrama on 2018-12-18 05:54:59 UTC

No further ideas?

Asked by boandlgrama on 2018-12-21 00:50:19 UTC

Could you edit the post adding your updated constraint? Does a tight constraint do what it should (I have been using 0.001 tolerance for all axes with no issues up till now)? I feel like it should just be a matter of trial and error, going from a tighter constraint and loosening it progressively.

Asked by aPonza on 2018-12-21 03:34:52 UTC

To sum it up: Am I right that the orientation of the constraint is the desired orientation of the link_name-link expressed in the header.frame_id-link? And the absolute_tolerances define the max rotation of the link_name-link expressed in the header.frame_id-link?

Asked by boandlgrama on 2018-12-21 07:00:02 UTC

I noticed a new problem, maybe the cause of this problem here. Screencast of the problem: https://youtu.be/rINMG6N6iNE

Asked by boandlgrama on 2018-12-21 08:34:53 UTC

Please check q272759 or git issue #562. Esp. the video issue might be related to this last one. Also bear in mind tolerances are in [rad], apparently.

Asked by aPonza on 2018-12-21 11:47:55 UTC

Sorry for the late response, #541 works as a workaround and the unit of the absolute_tolerances is [rad]

Asked by boandlgrama on 2019-01-07 04:37:10 UTC

Answers

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].

Asked by boandlgrama on 2019-01-07 04:42:20 UTC

Comments

Hi, We're working on Panda robot, and we'd like to make moves with semi-constrained end-effector. We try to use your code with PickNik package (https://github.com/PickNikRobotics/descartes_capability). The robot's arm (in Gazebo) seems to work fine during most of the move but in some points the end-effector rotates 360 degrees, even if there is no obstacle forcing such a move.

We try to run this program: https://github.com/PickNikRobotics/descartes_capability/blob/master/scripts/example_cartesian_command.py - with constraints suggested here

We've also tried setting path constraints: self.move_group.set_path_constraints(constraint)

ROS distro: Melodic

Do you know how can we force orientation constraints?

Asked by MichalBZ on 2020-07-15 02:07:41 UTC

Comments