//If the cart_hand_pose_index of the q_near node in the start tree is bigger than the
//cart_hand_pose_index of the last node q_connect of the goal tree
if (articulation_constraintactive == true && tree.name == "start_tree" && q_near.cart_hand_pose_index > q_connect.cart_hand_pose_index)
return TRAPPED;
//If the cart_hand_pose_index of the q_near node in the goal tree is smaller than the
//cart_hand_pose_index of the last node q_connect of the start tree
if (articulation_constraintactive == true && tree.name == "goal_tree" && q_near.cart_hand_pose_index < q_connect.cart_hand_pose_index)
return TRAPPED;
//If the cart_hand_pose_index of the q_near node in the start tree is bigger than the //cart_hand_pose_index of the last node q_connect of the goal tree if (articulation_constraintactive == true && tree.name == "start_tree" && q_near.cart_hand_pose_index > q_connect.cart_hand_pose_index) return TRAPPED; //If the cart_hand_pose_index of the q_near node in the goal tree is smaller than the //cart_hand_pose_index of the last node q_connect of the start tree if (articulation_constraintactive == true && tree.name == "goal_tree" && q_near.cart_hand_pose_index < q_connect.cart_hand_pose_index) return TRAPPED;