I am currently trying to perform some motion planning using ROS, moveit and UR3e model. More precisely, I am using the move_group_python_interface.py program from tutorials scripts and am trying to add some constraints so that when the motion from pose1 to pose2 is planned, the end effector of the robot orientation doesn't change. (I want to eventually simulate a welding task, so the tool orientation and position in the work space are important.) However when I add orientation constraint on the wrist_3_link, and launch the program while Moveit is launched, the resulting trajectory is not respecting the constraints at all. It is even very weird.
Here is the part of the code where I added the constraints.
def go_to_pose_goal2(self):
move_group = self.move_group
## BEGIN_SUB_TUTORIAL plan_to_pose
##
## Planning to a Pose Goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector:
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.x = 1e-6
pose_goal.orientation.y = 1.00000
pose_goal.orientation.z = 1e-6
pose_goal.orientation.w = 1e-6
pose_goal.position.x = -0.13
pose_goal.position.y = 0.45
pose_goal.position.z = 0.16
move_group.set_pose_target(pose_goal)
#set constraint on gripper orientation
orientation_constraint = moveit_msgs.msg.OrientationConstraint()
orientation_constraint.header.frame_id = "base_link" #must give reference frame
orientation_constraint.link_name = "wrist_3_link" #must give end effector frame
orientation_constraint.orientation.x = 1e-6
orientation_constraint.orientation.y = 1.00000
orientation_constraint.orientation.z = 1e-6
orientation_constraint.orientation.w = 1e-6
orientation_constraint.absolute_x_axis_tolerance = 0.6 # Set the tolerance for the x-axis orientation
orientation_constraint.absolute_y_axis_tolerance = 0.6 # Set the tolerance for the y-axis orientation
orientation_constraint.absolute_z_axis_tolerance = 0.6 # Set the tolerance for the z-axis orientation
position_constraint = moveit_msgs.msg.PositionConstraint()
#position_constraint.position.z = 0.3
constraints = moveit_msgs.msg.Constraints()
constraints.orientation_constraints.append(orientation_constraint)
move_group.set_path_constraints(constraints)
## Now, we call the planner to compute the plan and execute it.
# `go()` returns a boolean indicating whether the planning and execution was successful.
success = move_group.go(wait=True)
# Calling `stop()` ensures that there is no residual movement
move_group.stop()
# It is always good to clear your targets after planning with poses.
# Note: there is no equivalent function for clear_joint_value_targets().
move_group.clear_pose_targets()
## END_SUB_TUTORIAL
# For testing:
# Note that since this section of code will not be included in the tutorials
# we use the class variable rather than the copied state variable
current_pose = self.move_group.get_current_pose().pose
return all_close(pose_goal, current_pose, 0.01)
As you can see, it is mostly the code from the tutorial.
I have tried changing my constraints and checked several time that I was passing the correct angle information. Also I tried changing links in the base and end effector definitions. Adding these constraints definitely changes the motion planning so something is happening... If you have any resources that could be helpful I will be very happy to consult them.