You can add constraints to the path that should be generated by MoveIt. The one that is probably of most interest is the orientation constraint. This requires that for each point along the path, the end-effector maintains a certain orientation. For example, if the robot is holding a glass of water or a tray, it should keep the object level. From previous labs, it should be apparent that the robot's arm can move every which way, so adding such a constraint is absolutely necessary.

Add an orientation constraint

MoveItGoalBuilder already includes a method called add_path_orientation_constraint(orientation_constraint), which takes in a constraint of type moveit_msgs/OrientationConstraint.

However, you need to update move_to_pose:

• Add a new orientation_constraint arg with a default value of None
• If the value is not none, set the orientation constraint on the goal builder

Finally, edit arm_obstacle_demo.py and add an orientation constraint to pose 2:

oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'wrist_roll_link'
oc.orientation.w = 1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0

This will constrain the gripper to stay upright.

To see this work, you will need to get rid of the divider. The more bells and whistles we add, the slower the motion planning takes. Unfortunately, planning with orientation constraints is very slow -- too slow in addition to attached objects and obstacle avoidance. Again, you will need to structure your system to only use a subset of MoveIt's capabilities at a time.