void TrajectoryResponse(MoverServiceResponse response) { if (response.trajectories.Length > 0) { Debug.Log("Trajectory returned."); StartCoroutine(ExecuteTrajectories(response)); } else { Debug.LogError("No trajectory returned from MoverService."); } }
void TrajectoryResponse(MoverServiceResponse response) { if (response.trajectories != null && response.trajectories.Length > 0) { Debug.Log("Trajectory returned."); StartCoroutine(ExecuteTrajectories(response)); } else { Debug.LogError("No trajectory returned from MoverService."); InitializeButton.interactable = true; RandomizeButton.interactable = true; ServiceButton.interactable = true; } }
/// <summary> /// Execute the returned trajectories from the MoverService. /// /// The expectation is that the MoverService will return four trajectory plans, /// PreGrasp, Grasp, PickUp, and Place, /// where each plan is an array of robot poses. A robot pose is the joint angle values /// of the six robot joints. /// /// Executing a single trajectory will iterate through every robot pose in the array while updating the /// joint values on the robot. /// /// </summary> /// <param name="response"> MoverServiceResponse received from ur3_moveit mover service running in ROS</param> /// <returns></returns> private IEnumerator ExecuteTrajectories(MoverServiceResponse response) { if (response.trajectories != null) { // For every trajectory plan returned for (int poseIndex = 0; poseIndex < response.trajectories.Length; poseIndex++) { // For every robot pose in trajectory plan for (int jointConfigIndex = 0; jointConfigIndex < response.trajectories[poseIndex].joint_trajectory.points.Length; jointConfigIndex++) { var jointPositions = response.trajectories[poseIndex].joint_trajectory.points[jointConfigIndex].positions; float[] result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray(); // Set the joint values for every joint for (int joint = 0; joint < jointArticulationBodies.Length; joint++) { var joint1XDrive = jointArticulationBodies[joint].xDrive; joint1XDrive.target = result[joint]; jointArticulationBodies[joint].xDrive = joint1XDrive; } // Wait for robot to achieve pose for all joint assignments yield return(new WaitForSeconds(jointAssignmentWait)); } // Close the gripper if completed executing the trajectory for the Grasp pose if (poseIndex == (int)Poses.Grasp) { StartCoroutine(IterateToGrip(true)); yield return(new WaitForSeconds(jointAssignmentWait)); } else if (poseIndex == (int)Poses.Place) { yield return(new WaitForSeconds(poseAssignmentWait)); // Open the gripper to place the target cube StartCoroutine(IterateToGrip(false)); } // Wait for the robot to achieve the final pose from joint assignment yield return(new WaitForSeconds(poseAssignmentWait)); } // Re-enable buttons InitializeButton.interactable = true; RandomizeButton.interactable = true; yield return(new WaitForSeconds(jointAssignmentWait)); } }