当我在 move_group_python_interface 中添加约束时，MoveIt 中生成的轨迹根本不遵守约束

Question

我目前正在尝试使用 ROS、moveit 和 UR3e 模型执行一些运动规划。 更准确地说，我正在使用教程脚本中的 move_group_python_interface.py 程序，并尝试添加一些约束，以便在计划从pose1到pose2的运动时，机器人方向的末端执行器不会改变。 （我想最终模拟焊接任务，因此工具在工作空间中的方向和位置很重要。） 但是，当我在手腕_3_link 上添加方向约束，并在启动 Moveit 时启动程序时，生成的轨迹根本不遵守约束。甚至很奇怪。

这是我添加约束的代码部分。

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)

如您所见，主要是教程中的代码。

我尝试更改约束条件并多次检查是否传递了正确的角度信息。 我还尝试更改底座和末端执行器定义中的链接。 添加这些约束肯定会改变运动规划，因此某些事情正在发生...... 如果您有任何可能有帮助的资源，我将非常乐意咨询他们。

Answer 1

我实际上面临着完全相同的问题，你找到解决方案了吗？