/// <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 niryo_moveit mover service running in ROS</param>
/// <returns></returns>
private IEnumerator ExecuteTrajectories(GotoServiceResponse 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));
}
// Wait for the robot to achieve the final pose from joint assignment
yield return(new WaitForSeconds(poseAssignmentWait));
}
}
}
void TrajectoryResponse(GotoServiceResponse response)
{
if (response.trajectories.Length > 0)
{
Debug.Log("Trajectory returned.");
StartCoroutine(ExecuteTrajectories(response));
}
else
{
Debug.LogError("No trajectory returned from GotoService.");
}
}
