/// <summary> /// Execute the trajectory coming from MoveIt /// </summary> private IEnumerator ExecuteTrajectories(RosMessageTypes.Trajectory.JointTrajectory joint_trajectory) { // The joint order of MoveIt is not regular so we must read the names to map // the right joint index. string[] jointNamesFromROS = joint_trajectory.joint_names; // For each trajectory returned by MoveIt set the xDrive and wait for completion for (int jointConfigIndex = 0; jointConfigIndex < joint_trajectory.points.Length; jointConfigIndex++) { // Get the joint position in radians and transform to degrees var jointPositions = joint_trajectory.points[jointConfigIndex].positions; float[] result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray(); // No points if (result.Length == 0) { Debug.Log("Joint trajectory returned 0 positions! Skipping"); continue; } // Set the joint positions into the xDrive of the ArticulationBody for (int joint = 0; joint < result.Length - 1; joint++) { var jointName = jointNamesFromROS[joint]; var jointIndex = (int)jointPositionIndex[jointName]; var joint1XDrive = jointArticulationBodies[jointIndex].xDrive; joint1XDrive.target = result[joint]; jointArticulationBodies[jointIndex].xDrive = joint1XDrive; } // Wait until the execution is complete yield return(new WaitForSeconds(jointAssignmentWait)); } }
/// <summary> /// Start a separate routine which executes the trajectory /// </summary> public void TrajectoryHandler(RosMessageTypes.Trajectory.JointTrajectory response) { StartCoroutine(ExecuteTrajectories(response)); }