constraints.name = "down"
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = group.get_planning_frame()
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.orientation = pose_target_1.orientation
orientation_constraint.absolute_x_axis_tolerance = 3.1415
orientation_constraint.absolute_y_axis_tolerance = 0.05
orientation_constraint.absolute_z_axis_tolerance = 0.05
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
group.set_path_constraints(constraints)
rospy.loginfo("Get Path Constraints:\n{}".format(
group.get_path_constraints()))
# Pose Target 2
rospy.loginfo("Start Pose Target 2")
pose_target_2 = Pose()
pose_target_2.position.x = 0.3
pose_target_2.position.y = -0.5
pose_target_2.position.z = 0.15
pose_target_2.orientation.x = 0.0
pose_target_2.orientation.y = -0.707
pose_target_2.orientation.z = 0.0
pose_target_2.orientation.w = 0.707
group.set_planner_id("RRTConnectkConfigDefault")
group.allow_replanning(True)
print plan
print fraction
right_arm.execute(plan, wait=True)
rospy.sleep(0.5)
raw_input("please input")
# [00000000]
print right_arm.get_joint_value_target()
print both_arms.get_joint_value_target()
# no this functions
# print right_arm.get_named_targets()
print right_arm.get_remembered_joint_values()
print both_arms.get_remembered_joint_values()
print right_arm.get_path_constraints()
print both_arms.get_path_constraints()
print right_arm.get_active_joints()
print both_arms.get_active_joints()
print right_arm.get_current_joint_values()
print right_arm.get_current_pose()
print right_arm.get_current_rpy()
print both_arms.get_current_joint_values()
print both_arms.get_current_pose()
print both_arms.get_current_rpy()
right_arm.clear_pose_targets()
