ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
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 image aPonza  ( 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 image aPonza  ( 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 image boandlgrama  ( 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 image boandlgrama  ( 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 image aPonza  ( 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 image boandlgrama  ( 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 image aPonza  ( 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 image boandlgrama  ( 2018-12-18 04:54:59 -0500 )edit

No further ideas?

boandlgrama gravatar image boandlgrama  ( 2018-12-20 23:50:19 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
1

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
0

answered 2020-07-15 02:07:41 -0500

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/de...). 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/de... - 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?

edit flag offensive delete link more

Question Tools

1 follower

Stats

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

Seen: 5,094 times

Last updated: Jan 07 '19