Ros Moveit软件包的联合约束

Question

我有一个UR5机器人，尝试在更改机械手位置的同时使前3个关节稳定。但是，当我将联合限制设置为0时，moveit包并没有给出解决方案：

from universal_robot_kinematics import *
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
from tf.transformations import euler_from_quaternion, quaternion_from_euler


moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_manipulator', anonymous=True)

robot = moveit_commander.RobotCommander()

scene = moveit_commander.PlanningSceneInterface()

group = moveit_commander.MoveGroupCommander("manipulator")

display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)

## Get Joint Position list
joint_goal = group.get_current_joint_values()

print("First of the Joint Goals: ", joint_goal)

## Set Constraints of the system joints

constraint_0 = moveit_msgs.msg.JointConstraint()

constraint_0.joint_name = 'shoulder_pan_joint'
constraint_0.position = joint_goal[0]
constraint_0.tolerance_above = 0
constraint_0.tolerance_below = 0
constraint_0.weight = 1

constraint_1 = moveit_msgs.msg.JointConstraint()

constraint_1.joint_name = 'shoulder_lift_joint'
constraint_1.position = joint_goal[1]
constraint_1.tolerance_above = 0
constraint_1.tolerance_below = 0
constraint_1.weight = 2

constraint_2 = moveit_msgs.msg.JointConstraint()

constraint_2.joint_name = 'elbow_joint'
constraint_2.position = joint_goal[2]
constraint_2.tolerance_above = 0
constraint_2.tolerance_below = 0
constraint_2.weight = 3

## Append the system constraints

joint_limits = moveit_msgs.msg.Constraints()
joint_limits.joint_constraints.append(constraint_0)
joint_limits.joint_constraints.append(constraint_1)
joint_limits.joint_constraints.append(constraint_2)

## Set group Constraints

group.set_path_constraints(joint_limits)

rpy = group.get_current_rpy()

## Set pose target

group.set_rpy_target([rpy[0], rpy[1] , rpy[2] + 1*(pi/2)], end_effector_link="wrist_3_link")

plan = group.plan()

plan = group.go(wait=True)

display_trajectory = moveit_msgs.msg.DisplayTrajectory()

display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory);

print "============ Waiting while plan1 is visualized (again)..."
rospy.sleep(5)

在此代码中，我命令我的机器人在设置前三个关节稳定的同时移动其机械手。您认为这是正确的方法吗？请问有什么想法在改变机械手方向时如何使前三个关节保持稳定？