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
tilt_constraint.header
should betilt_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.I was looking for unrelated things and found this so in position 0.0001 = 0.1 mm, in orientation 0.001 = ~0.1 deg
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/yXCvxXwIf 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.
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?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.As per the docs,
The robot link this constraint refers to
isOrientationConstraint::link_name
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?