diff --git a/config/VehicleConfigurations.xml b/config/VehicleConfigurations.xml index 65890fa2f..b58b8e625 100644 --- a/config/VehicleConfigurations.xml +++ b/config/VehicleConfigurations.xml @@ -185,7 +185,6 @@ You can define the following custom settings: - @@ -406,7 +402,6 @@ You can define the following custom settings: turnRadius = "7.5" implementWheelAlwaysOnGround = "true" workingWidth = "10.5" - noReverse = "true" /> 0) ---@return boolean, boolean, number the second one is true when the first is valid, and the distance to the work start --- in meters (<0) when driving forward, nil when driving backwards. -function AIDriveStrategyFieldWorkCourse:shouldLowerThisImplement(object, turnEndNode, reversing) +function AIDriveStrategyFieldWorkCourse:shouldLowerThisImplement(object, workStartNode, reversing) local aiLeftMarker, aiRightMarker, aiBackMarker = WorkWidthUtil.getAIMarkers(object, true) if not aiLeftMarker then return false, false, nil end - local dxLeft, _, dzLeft = localToLocal(aiLeftMarker, turnEndNode, 0, 0, 0) - local dxRight, _, dzRight = localToLocal(aiRightMarker, turnEndNode, 0, 0, 0) - local dxBack, _, dzBack = localToLocal(aiBackMarker, turnEndNode, 0, 0, 0) + local dxLeft, _, dzLeft = localToLocal(aiLeftMarker, workStartNode, 0, 0, 0) + local dxRight, _, dzRight = localToLocal(aiRightMarker, workStartNode, 0, 0, 0) + local dxBack, _, dzBack = localToLocal(aiBackMarker, workStartNode, 0, 0, 0) local loweringDistance if AIUtil.hasAIImplementWithSpecialization(self.vehicle, SowingMachine) then -- sowing machines are stopped while lowering, but leave a little reserve to allow for stopping @@ -362,10 +362,14 @@ function AIDriveStrategyFieldWorkCourse:shouldLowerThisImplement(object, turnEnd loweringDistance = math.min(self.vehicle.lastSpeed, self.settings.turnSpeed:getValue() / 3600) * self.loweringDurationMs + 0.5 -- vehicle.lastSpeed is in meters per millisecond end - local dzFront = (dzLeft + dzRight) / 2 + local aligned = CpMathUtil.isSameDirection(object.rootNode, workStartNode, 15) + -- some implements, especially plows may have the left and right markers offset longitudinally + -- so if the implement is aligned with the row direction already, then just take the front one + -- if not aligned, work with an average + local dzFront = aligned and math.max(dzLeft, dzRight) or (dzLeft + dzRight) / 2 local dxFront = (dxLeft + dxRight) / 2 - self:debug('%s: dzLeft = %.1f, dzRight = %.1f, dzFront = %.1f, dxFront = %.1f, dzBack = %.1f, loweringDistance = %.1f, reversing %s', - CpUtil.getName(object), dzLeft, dzRight, dzFront, dxFront, dzBack, loweringDistance, tostring(reversing)) + self:debug('%s: dzLeft = %.1f, dzRight = %.1f, aligned = %s, dzFront = %.1f, dxFront = %.1f, dzBack = %.1f, loweringDistance = %.1f, reversing %s', + CpUtil.getName(object), dzLeft, dzRight, aligned, dzFront, dxFront, dzBack, loweringDistance, tostring(reversing)) local dz = self:getImplementLowerEarly() and dzFront or dzBack if reversing then return dz < 0, true, nil @@ -397,7 +401,7 @@ end function AIDriveStrategyFieldWorkCourse:isThisImplementAligned(object, node) local aiFrontMarker, _, _ = WorkWidthUtil.getAIMarkers(object, true) if not aiFrontMarker then return true end - return CpMathUtil.isSameDirection(aiFrontMarker, node, 2) + return CpMathUtil.isSameDirection(aiFrontMarker, node, 5) end ----------------------------------------------------------------------------------------------------------------------- diff --git a/scripts/ai/AIDriveStrategyPlowCourse.lua b/scripts/ai/AIDriveStrategyPlowCourse.lua index 71a0c998d..178b4c5fc 100644 --- a/scripts/ai/AIDriveStrategyPlowCourse.lua +++ b/scripts/ai/AIDriveStrategyPlowCourse.lua @@ -40,6 +40,9 @@ function AIDriveStrategyPlowCourse.new(customMt) local self = AIDriveStrategyFieldWorkCourse.new(customMt) AIDriveStrategyFieldWorkCourse.initStates(self, AIDriveStrategyPlowCourse.myStates) self.debugChannel = CpDebug.DBG_FIELDWORK + -- the plow offset is automatically calculated on each waypoint and if it wasn't calculated for a while + -- or when some event (like a turn) invalidated it + self.plowOffsetUnknown = CpTemporaryObject(true) return self end @@ -86,6 +89,9 @@ function AIDriveStrategyPlowCourse:getDriveData(dt, vX, vY, vZ) self:debug('Plow is unfolded and ready to start') end end + if self.plowOffsetUnknown:get() then + self:updatePlowOffset() + end return AIDriveStrategyFieldWorkCourse.getDriveData(self, dt, vX, vY, vZ) end @@ -104,15 +110,23 @@ end function AIDriveStrategyPlowCourse:updatePlowOffset() local xOffset = 0 for _, controller in pairs(self.controllers) do - if controller.getAutomaticXOffset then - xOffset = xOffset + controller:getAutomaticXOffset() + if controller.getAutomaticXOffset then + local autoOffset = controller:getAutomaticXOffset() + if autoOffset == nil then + self:debugSparse('Plow offset can\'t be calculated now, leaving offset at %.2f', self.aiOffsetX) + return + end + xOffset = xOffset + autoOffset end end local oldOffset = self.aiOffsetX -- set to the average of old and new to smooth a little bit to avoid oscillations - self.aiOffsetX = (0.5 * self.aiOffsetX + 1.5 * xOffset) / 2 - self:debug("Plow offset calculated was %.2f and it changed from %.2f to %.2f", - xOffset, oldOffset, self.aiOffsetX) + -- when we have a valid previous value + self.aiOffsetX = self.plowOffsetUnknown:get() and xOffset or ((0.5 * self.aiOffsetX + 1.5 * xOffset) / 2) + if math.abs(oldOffset - xOffset) > 0.05 then + self:debug("Plow offset calculated was %.2f and it changed from %.2f to %.2f", xOffset, oldOffset, self.aiOffsetX) + end + self.plowOffsetUnknown:set(false, 3000) end --- Is a plow currently rotating? @@ -173,8 +187,6 @@ end --- When we return from a turn, the offset is reverted and should immediately set, not waiting --- for the first waypoint to pass as it is on the wrong side right after the turn function AIDriveStrategyPlowCourse:resumeFieldworkAfterTurn(ix) - -- call twice to trick the smoothing and reach the desired value sooner. - self:updatePlowOffset() - self:updatePlowOffset() + self.plowOffsetUnknown:reset() AIDriveStrategyPlowCourse.superClass().resumeFieldworkAfterTurn(self, ix) end \ No newline at end of file diff --git a/scripts/ai/AIReverseDriver.lua b/scripts/ai/AIReverseDriver.lua index d29df38cc..a8c4eb3da 100644 --- a/scripts/ai/AIReverseDriver.lua +++ b/scripts/ai/AIReverseDriver.lua @@ -18,6 +18,22 @@ along with this program. If not, see . The AIReverseDriver takes over the steering if there is a towed implement or a trailer to be reversed. +We control the Giants vehicles by passing a goal point (in the vehicle's reference frame) +to the AIVehicleUtil.driveToPoint() function. The goal point is calculated by the +PurePursuitController and when driving forward or backward without a towed implement, +it can directly be used. + +When driving backwards with a towed implement, we want the implement to follow the path +so the PurePursuitController uses a reference node on the implement to calculate a goal point +towards which the implement needs to be steered to stay on the path. + +We can't use this goal point to directly control the tractor, we need to calculate a +'virtual' goal point for the tractor to be able to use AIVehicleUtil.driveToPoint(). + +This works by calculating the hitch angle (angle between the tractor and the trailer) that +would result in turning the implement towards the goal point. For the details, see the +papers referenced below. + ]]-- ---@class AIReverseDriver @@ -25,231 +41,239 @@ AIReverseDriver = CpObject() ---@param course Course function AIReverseDriver:init(vehicle, ppc) - self.vehicle = vehicle - self.settings = vehicle:getCpSettings() - ---@type PurePursuitController - self.ppc = ppc - -- the main implement (towed) or trailer we are controlling - self.reversingImplement = AIUtil.getFirstReversingImplementWithWheels(self.vehicle) - if self.reversingImplement then - self.steeringLength = AIUtil.getTowBarLength(self.vehicle) - self:setReversingProperties(self.reversingImplement) - else - self:debug('No towed implement found.') - end - self:debug('AIReverseDriver created.') - -- TODO_22 - -- handle HookLift + self.vehicle = vehicle + self.settings = vehicle:getCpSettings() + ---@type PurePursuitController + self.ppc = ppc + -- the main implement (towed) or trailer we are controlling + self.reversingImplement = AIUtil.getFirstReversingImplementWithWheels(self.vehicle) + if self.reversingImplement then + self.steeringLength = AIUtil.getTowBarLength(self.vehicle) + self:setReversingProperties(self.reversingImplement) + else + self:debug('No towed implement found.') + end + self:debug('AIReverseDriver created.') + -- TODO_22 + -- handle HookLift end function AIReverseDriver:debug(...) - CpUtil.debugVehicle(CpDebug.DBG_REVERSE, self.vehicle, ...) + CpUtil.debugVehicle(CpDebug.DBG_REVERSE, self.vehicle, ...) end function AIReverseDriver:getDriveData() - if self.reversingImplement == nil then - -- no wheeled implement, simple reversing the PPC can handle by itself - return nil - end - - local trailerNode = self.reversingImplement.steeringAxleNode - local xTipper, yTipper, zTipper = getWorldTranslation(trailerNode); - - local trailerFrontNode = self.reversingImplement.reversingProperties.frontNode - local xFrontNode,yFrontNode,zFrontNode = getWorldTranslation(trailerFrontNode) - - local tx, ty, tz = self.ppc:getGoalPointPosition() - - local lxTipper, lzTipper = AIVehicleUtil.getDriveDirection(trailerNode, tx, ty, tz) - - self:showDirection(trailerNode, lxTipper, lzTipper, 1, 0, 0) - - local lxFrontNode, lzFrontNode = AIVehicleUtil.getDriveDirection(trailerFrontNode, xTipper, yTipper, zTipper) - - local lxTractor, lzTractor = 0, 0 - - local maxTractorAngle = math.rad(75) - - -- for articulated vehicles use the articulated axis' rotation node as it is a better indicator or the - -- vehicle's orientation than the direction node which often turns/moves with an articulated vehicle part - -- TODO: consolidate this with AITurn:getTurnNode() and if getAIDirectionNode() considers this already - local tractorNode - local useArticulatedAxisRotationNode = - SpecializationUtil.hasSpecialization(ArticulatedAxis, self.vehicle.specializations) and self.vehicle.spec_articulatedAxis.rotationNode - if useArticulatedAxisRotationNode then - tractorNode = self.vehicle.spec_articulatedAxis.rotationNode - else - tractorNode = self.vehicle:getAIDirectionNode() - end - - local lx, lz, angleDiff - - if self.reversingImplement.reversingProperties.isPivot then - self:showDirection(trailerFrontNode, lxFrontNode, lzFrontNode, 0, 1, 0) - - lxTractor, lzTractor = AIVehicleUtil.getDriveDirection(tractorNode, xFrontNode, yFrontNode, zFrontNode) - self:showDirection(tractorNode,lxTractor, lzTractor, 0, 0.7, 0) - - local rotDelta = (self.reversingImplement.reversingProperties.nodeDistance * - (0.5 - (0.023 * self.reversingImplement.reversingProperties.nodeDistance - 0.073))) - local trailerToWaypointAngle = self:getLocalYRotationToPoint(trailerNode, tx, ty, tz, -1) * rotDelta - trailerToWaypointAngle = MathUtil.clamp(trailerToWaypointAngle, -math.rad(90), math.rad(90)) - - local dollyToTrailerAngle = self:getLocalYRotationToPoint(trailerFrontNode, xTipper, yTipper, zTipper, -1) - - local tractorToDollyAngle = self:getLocalYRotationToPoint(tractorNode, xFrontNode, yFrontNode, zFrontNode, -1) - - local rearAngleDiff = (dollyToTrailerAngle - trailerToWaypointAngle) - rearAngleDiff = MathUtil.clamp(rearAngleDiff, -math.rad(45), math.rad(45)) - - local frontAngleDiff = (tractorToDollyAngle - dollyToTrailerAngle) - frontAngleDiff = MathUtil.clamp(frontAngleDiff, -math.rad(45), math.rad(45)) - - angleDiff = (frontAngleDiff - rearAngleDiff) * - (1.5 - (self.reversingImplement.reversingProperties.nodeDistance * 0.4 - 0.9) + rotDelta) - angleDiff = MathUtil.clamp(angleDiff, -math.rad(45), math.rad(45)) - - lx, lz = MathUtil.getDirectionFromYRotation(angleDiff) - else - local crossTrackError, orientationError, curvatureError, currentHitchAngle = self:calculateErrors(tractorNode, trailerNode) - angleDiff = self:calculateHitchCorrectionAngle(crossTrackError, orientationError, curvatureError, currentHitchAngle) - angleDiff = MathUtil.clamp(angleDiff, -maxTractorAngle, maxTractorAngle) - - lx, lz = MathUtil.getDirectionFromYRotation(angleDiff) - end - - self:showDirection(tractorNode, lx, lz, 0.7, 0, 1) - -- do a little bit of damping if using the articulated axis as lx tends to oscillate around 0 which results in the - -- speed adjustment kicking in and slowing down the vehicle. - if useArticulatedAxisRotationNode and math.abs(lx) < 0.04 then lx = 0 end - -- construct an artificial goal point to drive to - lx, lz = -lx * self.ppc:getLookaheadDistance(), -lz * self.ppc:getLookaheadDistance() - -- AIDriveStrategy wants a global position to drive to (which it later converts to local, but whatever...) - local gx, _, gz = localToWorld(self.vehicle:getAIDirectionNode(), lx, 0, lz) - return gx, gz, false, self.settings.reverseSpeed:getValue() + if self.reversingImplement == nil then + -- no wheeled implement, simple reversing the PPC can handle by itself + return nil + end + + -- if there's a reverser node on the tool, use that, otherwise the steering node + -- the reverser direction node, if exists, works better for tools with offset or for + -- rotating plows where it remains oriented and placed correctly + local trailerNode = AIVehicleUtil.getAIToolReverserDirectionNode(self.vehicle) or self.reversingImplement.steeringAxleNode + local trailerFrontNode = self.reversingImplement.reversingProperties.frontNode + + local tx, ty, tz = self.ppc:getGoalPointPosition() + local lxTrailer, lzTrailer = AIVehicleUtil.getDriveDirection(trailerNode, tx, ty, tz) + self:showDirection(trailerNode, lxTrailer, lzTrailer, 1, 0, 0) + + local maxTractorAngle = math.rad(75) + + -- for articulated vehicles use the articulated axis' rotation node as it is a better indicator or the + -- vehicle's orientation than the direction node which often turns/moves with an articulated vehicle part + -- TODO: consolidate this with AITurn:getTurnNode() and if getAIDirectionNode() considers this already + local tractorNode + local useArticulatedAxisRotationNode = SpecializationUtil.hasSpecialization(ArticulatedAxis, self.vehicle.specializations) and self.vehicle.spec_articulatedAxis.rotationNode + if useArticulatedAxisRotationNode then + tractorNode = self.vehicle.spec_articulatedAxis.rotationNode + else + tractorNode = self.vehicle:getAIDirectionNode() + end + + local lx, lz, angleDiff + + if self.reversingImplement.reversingProperties.isPivot then + -- The trailer/implement has a front axle (or dolly) with a draw bar. + -- The current Courseplay dev team has no idea how this works :), this is magic + -- from the old code, written by Satissis (Claus). + -- TODO: adapt a documented algorithm for these trailers + local xTrailer, yTrailer, zTrailer = getWorldTranslation(trailerNode); + local xFrontNode, yFrontNode, zFrontNode = getWorldTranslation(trailerFrontNode) + local lxFrontNode, lzFrontNode = AIVehicleUtil.getDriveDirection(trailerFrontNode, xTrailer, yTrailer, zTrailer) + self:showDirection(trailerFrontNode, lxFrontNode, lzFrontNode, 0, 1, 0) + + local lxTractor, lzTractor = AIVehicleUtil.getDriveDirection(tractorNode, xFrontNode, yFrontNode, zFrontNode) + self:showDirection(tractorNode, lxTractor, lzTractor, 0, 0.7, 0) + + local rotDelta = (self.reversingImplement.reversingProperties.nodeDistance * + (0.5 - (0.023 * self.reversingImplement.reversingProperties.nodeDistance - 0.073))) + local trailerToWaypointAngle = self:getLocalYRotationToPoint(trailerNode, tx, ty, tz, -1) * rotDelta + trailerToWaypointAngle = MathUtil.clamp(trailerToWaypointAngle, -math.rad(90), math.rad(90)) + + local dollyToTrailerAngle = self:getLocalYRotationToPoint(trailerFrontNode, xTrailer, yTrailer, zTrailer, -1) + + local tractorToDollyAngle = self:getLocalYRotationToPoint(tractorNode, xFrontNode, yFrontNode, zFrontNode, -1) + + local rearAngleDiff = (dollyToTrailerAngle - trailerToWaypointAngle) + rearAngleDiff = MathUtil.clamp(rearAngleDiff, -math.rad(45), math.rad(45)) + + local frontAngleDiff = (tractorToDollyAngle - dollyToTrailerAngle) + frontAngleDiff = MathUtil.clamp(frontAngleDiff, -math.rad(45), math.rad(45)) + + angleDiff = (frontAngleDiff - rearAngleDiff) * + (1.5 - (self.reversingImplement.reversingProperties.nodeDistance * 0.4 - 0.9) + rotDelta) + angleDiff = MathUtil.clamp(angleDiff, -math.rad(45), math.rad(45)) + + lx, lz = MathUtil.getDirectionFromYRotation(angleDiff) + else + -- the trailer/implement is like a semi-trailer, has a rear axle only, the front of the implement + -- is supported by the tractor + local crossTrackError, orientationError, curvatureError, currentHitchAngle = self:calculateErrors(tractorNode, trailerNode) + angleDiff = self:calculateHitchCorrectionAngle(crossTrackError, orientationError, curvatureError, currentHitchAngle) + angleDiff = MathUtil.clamp(angleDiff, -maxTractorAngle, maxTractorAngle) + + lx, lz = MathUtil.getDirectionFromYRotation(angleDiff) + end + + self:showDirection(tractorNode, lx, lz, 0.7, 0, 1) + -- do a little bit of damping if using the articulated axis as lx tends to oscillate around 0 which results in the + -- speed adjustment kicking in and slowing down the vehicle. + if useArticulatedAxisRotationNode and math.abs(lx) < 0.04 then + lx = 0 + end + -- construct an artificial goal point to drive to + lx, lz = -lx * self.ppc:getLookaheadDistance(), -lz * self.ppc:getLookaheadDistance() + -- AIDriveStrategy wants a global position to drive to (which it later converts to local, but whatever...) + local gx, _, gz = localToWorld(self.vehicle:getAIDirectionNode(), lx, 0, lz) + return gx, gz, false, self.settings.reverseSpeed:getValue() end function AIReverseDriver:getLocalYRotationToPoint(node, x, y, z, direction) - direction = direction or 1 - local dx, _, dz = worldToLocal(node, x, y, z) - dx = dx * direction - dz = dz * direction - return MathUtil.getYRotationFromDirection(dx, dz) + direction = direction or 1 + local dx, _, dz = worldToLocal(node, x, y, z) + dx = dx * direction + dz = dz * direction + return MathUtil.getYRotationFromDirection(dx, dz) end function AIReverseDriver:showDirection(node, lx, lz, r, g, b) - if CpDebug:isChannelActive(CpDebug.DBG_REVERSE, self.vehicle) then - local x,y,z = getWorldTranslation(node) - local tx,_, tz = localToWorld(node,lx*5,y,lz*5) - DebugUtil.drawDebugLine(x, y+5, z, tx, y+5, tz, r or 1, g or 0, b or 0) - end + if CpDebug:isChannelActive(CpDebug.DBG_REVERSE, self.vehicle) then + local x, y, z = getWorldTranslation(node) + local tx, _, tz = localToWorld(node, lx * 5, y, lz * 5) + DebugUtil.drawDebugLine(x, y + 5, z, tx, y + 5, tz, r or 1, g or 0, b or 0) + end end +--- Another Claus magic code to determine if the trailer has a front axle and find a node +--- for it to control. ---@param implement table implement.object function AIReverseDriver:setReversingProperties(implement) - if implement.reversingProperties and implement.reversingProperties.frontNode then return end - self:debug('setReversingProperties for %s', CpUtil.getName(implement)) - - implement.reversingProperties = {} - - local attacherVehicle = self.reversingImplement:getAttacherVehicle() - - if attacherVehicle == self.vehicle or ImplementUtil.isAttacherModule(attacherVehicle) then - implement.reversingProperties.frontNode = ImplementUtil.getRealTrailerFrontNode(implement) - else - implement.reversingProperties.frontNode = ImplementUtil.getRealDollyFrontNode(attacherVehicle) - if implement.reversingProperties.frontNode then - self:debug('--> self.reversingImplement %s has dolly', CpUtil.getName(implement)) - else - self:debug('--> self.reversingImplement %s has invalid dolly -> use implement own front node', - CpUtil.getName(implement)) - implement.reversingProperties.frontNode = ImplementUtil.getRealTrailerFrontNode(implement) - end - end - - implement.reversingProperties.nodeDistance = ImplementUtil.getRealTrailerDistanceToPivot(implement) - self:debug("--> tz: %.1f real trailer distance to pivot: %s", - implement.reversingProperties.nodeDistance, tostring(implement.steeringAxleNode)) - - if implement.steeringAxleNode == implement.reversingProperties.frontNode then - self:debug('--> implement.steeringAxleNode == implement.reversingProperties.frontNode') - implement.reversingProperties.isPivot = false - else - implement.reversingProperties.isPivot = true - end - - self:debug('--> isPivot=%s, frontNode=%s', - tostring(implement.reversingProperties.isPivot), tostring(implement.reversingProperties.frontNode)) + if implement.reversingProperties and implement.reversingProperties.frontNode then + return + end + self:debug('setReversingProperties for %s', CpUtil.getName(implement)) + + implement.reversingProperties = {} + + local attacherVehicle = self.reversingImplement:getAttacherVehicle() + + if attacherVehicle == self.vehicle or ImplementUtil.isAttacherModule(attacherVehicle) then + implement.reversingProperties.frontNode = ImplementUtil.getRealTrailerFrontNode(implement) + else + implement.reversingProperties.frontNode = ImplementUtil.getRealDollyFrontNode(attacherVehicle) + if implement.reversingProperties.frontNode then + self:debug('--> self.reversingImplement %s has dolly', CpUtil.getName(implement)) + else + self:debug('--> self.reversingImplement %s has invalid dolly -> use implement own front node', + CpUtil.getName(implement)) + implement.reversingProperties.frontNode = ImplementUtil.getRealTrailerFrontNode(implement) + end + end + + implement.reversingProperties.nodeDistance = ImplementUtil.getRealTrailerDistanceToPivot(implement) + self:debug("--> tz: %.1f real trailer distance to pivot: %s", + implement.reversingProperties.nodeDistance, tostring(implement.steeringAxleNode)) + + if implement.steeringAxleNode == implement.reversingProperties.frontNode then + self:debug('--> implement.steeringAxleNode == implement.reversingProperties.frontNode') + implement.reversingProperties.isPivot = false + else + implement.reversingProperties.isPivot = true + end + + self:debug('--> isPivot=%s, frontNode=%s', + tostring(implement.reversingProperties.isPivot), tostring(implement.reversingProperties.frontNode)) end --- The reversing algorithm here is based on the papers: ---- Peter Ridley and Peter Corke. Load haul dump vehicle kinematics and control. ---- Journal of dynamic systems, measurement, and control, 125(1):54–59, 2003. +--- Peter Ridley and Peter Corke. Load haul dump vehicle kinematics and control. +--- Journal of dynamic systems, measurement, and control, 125(1):54–59, 2003. --- and ---- Amro Elhassan. Autonomous driving system for reversing an articulated vehicle, 2015 - +--- Amro Elhassan. Autonomous driving system for reversing an articulated vehicle, 2015 --- Calculate the path following errors (also called path disturbance inputs in the context of a controller) function AIReverseDriver:calculateErrors(tractorNode, trailerNode) - -- PPC already has the cross track error (lateral error) - local crossTrackError = self.ppc:getCrossTrackError() + -- PPC already has the cross track error (lateral error) + local crossTrackError = self.ppc:getCrossTrackError() --+ self.settings.toolOffsetX:getValue() - -- Calculate the orientation error, the angle between the trailers current direction and - -- the path direction - local referencePathAngle = self.ppc:getCurrentWaypointYRotation() + -- Calculate the orientation error, the angle between the trailers current direction and + -- the path direction + local referencePathAngle = self.ppc:getCurrentWaypointYRotation() - local dx, _, dz = localDirectionToWorld(trailerNode, 0, 0, -1) - local trailerAngle = MathUtil.getYRotationFromDirection(dx, dz) + local dx, _, dz = localDirectionToWorld(trailerNode, 0, 0, -1) + local trailerAngle = MathUtil.getYRotationFromDirection(dx, dz) - local orientationError = getDeltaAngle(trailerAngle, referencePathAngle) + local orientationError = getDeltaAngle(trailerAngle, referencePathAngle) - -- The curvature (1/r) error is between the curvature of the path and the curvature of the tractor-trailer. - -- This is really needed only when we are trying to follow a curved path in reverse - local _, tractorAngle, _ = getWorldRotation(tractorNode) - dx, _, dz = localDirectionToWorld(tractorNode, 0, 0, -1) - tractorAngle = MathUtil.getYRotationFromDirection(dx, dz) + -- The curvature (1/r) error is between the curvature of the path and the curvature of the tractor-trailer. + -- This is really needed only when we are trying to follow a curved path in reverse + local _, tractorAngle, _ = getWorldRotation(tractorNode) + dx, _, dz = localDirectionToWorld(tractorNode, 0, 0, -1) + tractorAngle = MathUtil.getYRotationFromDirection(dx, dz) - local currentHitchAngle = getDeltaAngle(tractorAngle, trailerAngle) + local currentHitchAngle = getDeltaAngle(tractorAngle, trailerAngle) - local curvature = ( 2 * math.sin(currentHitchAngle / 2 )) / calcDistanceFrom(tractorNode, trailerNode) - local currentWp = self.ppc:getCurrentWaypoint() - local curvatureError = currentWp.curvature - curvature + local curvature = (2 * math.sin(currentHitchAngle / 2)) / calcDistanceFrom(tractorNode, trailerNode) + local currentWp = self.ppc:getCurrentWaypoint() + local curvatureError = currentWp.curvature - curvature - return crossTrackError, orientationError, curvatureError, currentHitchAngle + return crossTrackError, orientationError, curvatureError, currentHitchAngle end --- Based on the current errors, calculate the required correction (in controller terms, use different gains for --- different disturbances to calculate the controller's output, which in our case is just an angle the tractor --- needs to drive to, in the tractor's local coordinate system.) function AIReverseDriver:calculateHitchCorrectionAngle(crossTrackError, orientationError, curvatureError, currentHitchAngle) - -- the following constants must be tuned based on experiments. - - -- base cross track error gain. 0.6-0.7 for longer implements, 0.5 for shorter ones, should be adjusted based on - -- the steering length - local kXeBase = -0.5 - self.steeringLength / 50 - -- base orientation error gain - local kOeBase = 6 - -- base curvature error gain. 0 for now, as currently we only drive straight reverse - local kCeBase = 0 - - -- gain correction - local gainCorrection = 1.5 - - local hitchAngle = gainCorrection * ( - kXeBase * crossTrackError + - kOeBase * orientationError + - kCeBase * curvatureError - ) - hitchAngle = MathUtil.clamp(hitchAngle, -math.rad(35), math.rad(35)) - - local correctionAngle = -(hitchAngle - currentHitchAngle) - - if CpDebug:isChannelActive(CpDebug.DBG_REVERSE, self.vehicle) then - local text = string.format('xte=%.1f oe=%.1f ce=%.1f current=%.1f reference=%.1f correction=%.1f', - crossTrackError, math.deg(orientationError), curvatureError, math.deg(currentHitchAngle), math.deg(hitchAngle), math.deg(correctionAngle)) - setTextColor(1, 1, 0, 1) - renderText(0.3, 0.3, 0.015, text) - end - - return correctionAngle + -- the following constants must be tuned based on experiments. + + -- base cross track error gain. 0.6-0.7 for longer implements, 0.5 for shorter ones, should be adjusted based on + -- the steering length + local kXeBase = -0.5 - self.steeringLength / 50 + -- base orientation error gain + local kOeBase = 6 + -- base curvature error gain. 0 for now, as currently we only drive straight reverse + local kCeBase = 0 + + -- gain correction + local gainCorrection = 1.5 + + local hitchAngle = gainCorrection * ( + kXeBase * crossTrackError + + kOeBase * orientationError + + kCeBase * curvatureError + ) + local maxHitchAngle = math.rad(35) + hitchAngle = MathUtil.clamp(hitchAngle, -maxHitchAngle, maxHitchAngle) + + local correctionAngle = -(hitchAngle - currentHitchAngle) + + if CpDebug:isChannelActive(CpDebug.DBG_REVERSE, self.vehicle) then + local text = string.format('xte=%.1f oe=%.1f ce=%.1f current=%.1f reference=%.1f correction=%.1f', + crossTrackError, math.deg(orientationError), curvatureError, math.deg(currentHitchAngle), math.deg(hitchAngle), math.deg(correctionAngle)) + setTextColor(1, 1, 0, 1) + renderText(0.3, 0.3, 0.015, text) + end + + return correctionAngle end diff --git a/scripts/ai/AIUtil.lua b/scripts/ai/AIUtil.lua index 7ce7464ca..8ec5dfb2a 100644 --- a/scripts/ai/AIUtil.lua +++ b/scripts/ai/AIUtil.lua @@ -54,7 +54,7 @@ function AIUtil.calculateTightTurnOffset(vehicle, vehicleTurningRadius, course, local tightTurnOffset local function smoothOffset(offset) - return (offset + 3 * (previousOffset or 0 )) / 4 + return (offset + 4 * (previousOffset or 0 )) / 5 end -- first of all, does the current waypoint have radius data? @@ -157,12 +157,12 @@ end function AIUtil.getReverserNode(vehicle, reversingImplement, suppressLog) local reverserNode, debugText -- if there's a reverser node on the tool, use that - reversingImplement = reversingImplement and reversingImplement or AIUtil.getFirstReversingImplementWithWheels(vehicle, suppressLog) - if reversingImplement and reversingImplement.steeringAxleNode then - reverserNode, debugText = reversingImplement.steeringAxleNode, 'implement steering axle node' - end + reverserNode, debugText = AIVehicleUtil.getAIToolReverserDirectionNode(vehicle), 'AIToolReverserDirectionNode' if not reverserNode then - reverserNode, debugText = AIVehicleUtil.getAIToolReverserDirectionNode(vehicle), 'AIToolReverserDirectionNode' + reversingImplement = reversingImplement and reversingImplement or AIUtil.getFirstReversingImplementWithWheels(vehicle, suppressLog) + if reversingImplement and reversingImplement.steeringAxleNode then + reverserNode, debugText = reversingImplement.steeringAxleNode, 'implement steering axle node' + end end if not reverserNode and vehicle.getAIReverserNode then reverserNode, debugText = vehicle:getAIReverserNode(), 'AIReverserNode' diff --git a/scripts/ai/PurePursuitController.lua b/scripts/ai/PurePursuitController.lua index 3d187801a..9aa92d1c4 100644 --- a/scripts/ai/PurePursuitController.lua +++ b/scripts/ai/PurePursuitController.lua @@ -241,15 +241,6 @@ function PurePursuitController:switchControlledNode() end end ---- Compatibility function to return the original waypoint index as in vehicle.Waypoints. This --- is the same as self.currentWpNode.ix unless we have combined courses where the legacy CP code --- concatenates all courses into one Waypoints array (as opposed to the AIDriver which splits these --- combined courses into its parts). The rest of the CP code however (HUD, reverse, etc.) works with --- vehicle.Waypoints and vehicle.cp.waypointIndex and therefore expects the combined index -function PurePursuitController:getCurrentOriginalWaypointIx() - return self.course.waypoints[self:getCurrentWaypointIx()].cpIndex -end - function PurePursuitController:update() if not self.course then self:debugSparse('no course set.') diff --git a/scripts/ai/controllers/ImplementController.lua b/scripts/ai/controllers/ImplementController.lua index 85db23932..753cef243 100644 --- a/scripts/ai/controllers/ImplementController.lua +++ b/scripts/ai/controllers/ImplementController.lua @@ -90,7 +90,7 @@ end function ImplementController:onFinishRow(isHeadlandTurn) end -function ImplementController:onTurnEndProgress(workStartNode, isLeftTurn) +function ImplementController:onTurnEndProgress(workStartNode, reversing, shouldLower, isLeftTurn) end --- Any object this controller wants us to ignore, can register here a callback at the proximity controller diff --git a/scripts/ai/controllers/PlowController.lua b/scripts/ai/controllers/PlowController.lua index c2af7c043..3ee105001 100644 --- a/scripts/ai/controllers/PlowController.lua +++ b/scripts/ai/controllers/PlowController.lua @@ -6,16 +6,28 @@ PlowController = CpObject(ImplementController) function PlowController:init(vehicle, implement) ImplementController.init(self, vehicle, implement) self.plowSpec = self.implement.spec_plow + -- towed (not hitch mounted) and can reverse + self.towed = AIUtil.getFirstReversingImplementWithWheels(vehicle, true) + -- temporarily store the direction of the last turn. This hack is for the case when for some reason + -- the plow was not rotated into the working position before lowering. If that's the case, onLowering will + -- rotate it but it needs to know which direction to lower, which, however is not known at that point + -- anymore + self.lastTurnIsLeftTurn = CpTemporaryObject() end +---@return number|nil if an X offset could be calculated, return that, otherwise nil function PlowController:getAutomaticXOffset() + if self:isRotatablePlow() and not self:isFullyRotated() then + self:debugSparse('Plow is not fully rotated, not calculating offset') + return nil + end local aiLeftMarker, aiRightMarker, aiBackMarker = self.implement:getAIMarkers() if aiLeftMarker and aiBackMarker and aiRightMarker then local attacherJoint = self.implement:getActiveInputAttacherJoint() local referenceNode = attacherJoint and attacherJoint.node or self.vehicle:getAIDirectionNode() -- find out the left/right AI markers distance from the attacher joint (or, if does not exist, the - -- vehicle's root node) to calculate the offset. + -- vehicle's direction node) to calculate the offset. self.plowReferenceNode = referenceNode local leftMarkerDistance, rightMarkerDistance = self:getOffsets(referenceNode, aiLeftMarker, aiRightMarker) @@ -28,7 +40,7 @@ function PlowController:getAutomaticXOffset() leftMarkerDistance, rightMarkerDistance = -rightMarkerDistance, -leftMarkerDistance end local newToolOffsetX = -(leftMarkerDistance + rightMarkerDistance) / 2 - self:debug('Current Offset left = %.1f, right = %.1f, leftDx = %.1f, rightDx = %.1f, new = %.1f', + self:debugSparse('Current Offset left = %.1f, right = %.1f, leftDx = %.1f, rightDx = %.1f, new = %.1f', leftMarkerDistance, rightMarkerDistance, leftDx, rightDx, newToolOffsetX) return newToolOffsetX end @@ -98,17 +110,44 @@ function PlowController:onFinishRow(isHeadlandTurn) end end + +--- making sure the plow is in the working position when lowering +-- TODO: this whole magic hack would not be necessary if we moved the actual lowering into onTurnEndProgress() +function PlowController:onLowering() + -- if we just turned (that is, not starting to work) + local lastTurnIsLeftTurn = self.lastTurnIsLeftTurn:get() + if lastTurnIsLeftTurn ~= nil and + self:isRotatablePlow() and not self:isFullyRotated() and not self:isRotationActive() then + self:debug('Lowering, rotating plow to working position (last turn is left %s).', lastTurnIsLeftTurn) + -- rotation direction depends on the direction of the last turn + self.implement:setRotationMax(lastTurnIsLeftTurn) + end +end + --- This is called in every loop when we approach the start of the row, the location where --- the plow must be lowered. Currently AIDriveStrategyFieldworkCourse takes care of the lowering, --- here we only make sure that the plow is rotated to the work position (from the center position) --- in time. ---@param workStartNode number node where the work starts as calculated by TurnContext +---@param reversing boolean driving in reverse now +---@param shouldLower boolean the implement should be lowered as we are close to the work start (this +--- should most likely be calculated here in the controller, but for now, we get it from an argument ---@param isLeftTurn boolean is this a left turn? -function PlowController:onTurnEndProgress(workStartNode, isLeftTurn) +function PlowController:onTurnEndProgress(workStartNode, reversing, shouldLower, isLeftTurn) + self.lastTurnIsLeftTurn:set(isLeftTurn or false, 2000) if self:isRotatablePlow() and not self:isFullyRotated() and not self:isRotationActive() then -- more or less aligned with the first waypoint of the row, start rotating to working position - if CpMathUtil.isSameDirection(self.implement.rootNode, workStartNode, 30) then - self.implement:setRotationMax(isLeftTurn) + if CpMathUtil.isSameDirection(self.implement.rootNode, workStartNode, 30) or shouldLower then + if self.towed then + -- let towed plows remain in the center position while reversing to the start of the row + if not reversing then + self:debug('Rotating towed plow to working position.') + self.implement:setRotationMax(isLeftTurn) + end + else + self:debug('Rotating hitch-mounted plow to working position.') + self.implement:setRotationMax(isLeftTurn) + end end end end diff --git a/scripts/ai/turns/AITurn.lua b/scripts/ai/turns/AITurn.lua index 21c4d99e8..2118efaee 100644 --- a/scripts/ai/turns/AITurn.lua +++ b/scripts/ai/turns/AITurn.lua @@ -584,9 +584,9 @@ end ---@return boolean true if it is ok the continue driving, false when the vehicle should stop function CourseTurn:endTurn(dt) -- keep driving on the turn course until we need to lower our implements - self.driveStrategy:raiseControllerEvent(AIDriveStrategyCourse.onTurnEndProgressEvent, - self.turnContext.workStartNode, self.turnContext:isLeftTurn()) local shouldLower, dz = self.driveStrategy:shouldLowerImplements(self.turnContext.workStartNode, self.ppc:isReversing()) + self.driveStrategy:raiseControllerEvent(AIDriveStrategyCourse.onTurnEndProgressEvent, + self.turnContext.workStartNode, self.ppc:isReversing(), shouldLower, self.turnContext:isLeftTurn()) if shouldLower then if not self.implementsLowered then -- have not started lowering implements yet diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua index ef9884989..fa1510c70 100644 --- a/scripts/ai/turns/TurnManeuver.lua +++ b/scripts/ai/turns/TurnManeuver.lua @@ -235,14 +235,11 @@ end --- is to make sure that when the course changes to reverse and there is a reverse node, the first reverse --- waypoint is behind the reverser node. Otherwise we'll just keep backing up until the emergency brake is triggered. function TurnManeuver:getReversingOffset() - if self.steeringLength == 0 then - local reverserNode, debugText = AIUtil.getReverserNode(self.vehicle) - if reverserNode then - local _, _, dz = localToLocal(reverserNode, self.vehicleDirectionNode, 0, 0, 0) - self:debug('Using the vehicle\'s steering length 0, use reverser node (%s) distance %.1f instead', - debugText, dz) - return math.abs(dz) - end + local reverserNode, debugText = AIUtil.getReverserNode(self.vehicle) + if reverserNode then + local _, _, dz = localToLocal(reverserNode, self.vehicleDirectionNode, 0, 0, 0) + self:debug('Using reverser node (%s) distance %.1f', debugText, dz) + return math.abs(dz) end return self.steeringLength end @@ -283,9 +280,13 @@ function TurnManeuver:moveCourseBack(course, dBack, ixBeforeEndingTurnSection, e -- allow early direction change when aligned TurnManeuver.setTurnControlForLastWaypoints(movedCourse, endingTurnLength, TurnManeuver.CHANGE_DIRECTION_WHEN_ALIGNED, true, true) + -- go all the way to the back marker distance so there's plenty of room for lower early too, also, the + -- reversingOffset may be even behind the back marker, especially for vehicles which have a AIToolReverserDirectionNode + -- which is then used as the PPC controlled node, and thus it must be far enough that we reach the lowering + -- point before the controlled node reaches the end of the course local reverseAfterTurn = Course.createFromNode(self.vehicle, self.turnContext.vehicleAtTurnEndNode, 0, dFromTurnEnd + self.steeringLength, - math.min(dFromTurnEnd, self.turnContext.frontMarkerDistance), -0.8, true) + math.min(dFromTurnEnd, self.turnContext.backMarkerDistance, -reversingOffset), -0.8, true) movedCourse:append(reverseAfterTurn) elseif self.turnContext.turnEndForwardOffset <= 0 and dFromTurnEnd >= 0 then self:debug('Reverse to work start (implement in front)') diff --git a/scripts/dev/DevHelper.lua b/scripts/dev/DevHelper.lua index e91645158..00c8fcc7a 100644 --- a/scripts/dev/DevHelper.lua +++ b/scripts/dev/DevHelper.lua @@ -47,10 +47,6 @@ function DevHelper:update() -- make sure not calling this for something which does not have courseplay installed (only ones with spec_aiVehicle) if g_currentMission.controlledVehicle and g_currentMission.controlledVehicle.spec_aiVehicle then - if self.vehicle ~= g_currentMission.controlledVehicle then - --self.vehicleData = PathfinderUtil.VehicleData(g_currentMission.controlledVehicle, true) - end - self.vehicle = g_currentMission.controlledVehicle self.node = g_currentMission.controlledVehicle:getAIDirectionNode() lx, _, lz = localDirectionToWorld(self.node, 0, 0, 1) @@ -169,6 +165,8 @@ function DevHelper:keyEvent(unicode, sym, modifier, isDown) if ok then self.course = course end + elseif bitAND(modifier, Input.MOD_LALT) ~= 0 and isDown and sym == Input.KEY_n then + self:togglePpcControlledNode() end end @@ -182,11 +180,13 @@ function DevHelper:draw() for key, value in pairs(self.data) do table.insert(data, {name = key, value = value}) end - DebugUtil.renderTable(0.65, 0.3, 0.013, data, 0.05) + DebugUtil.renderTable(0.65, 0.27, 0.013, data, 0.05) self:showFillNodes() self:showAIMarkers() + self:showDriveData() + if not self.tNode then self.tNode = createTransformGroup("devhelper") link(g_currentMission.terrainRootNode, self.tNode) @@ -268,6 +268,25 @@ function DevHelper:showAIMarkers() end +function DevHelper:togglePpcControlledNode() + if not self.vehicle then return end + local strategy = self.vehicle:getCpDriveStrategy() + if not strategy then return end + if strategy.ppc:getControlledNode() == AIUtil.getReverserNode(self.vehicle) then + strategy.pcc:resetControlledNode() + else + strategy.ppc:setControlledNode(AIUtil.getReverserNode(self.vehicle)) + end +end + +function DevHelper:showDriveData() + if not self.vehicle then return end + local strategy = self.vehicle:getCpDriveStrategy() + if not strategy then return end + strategy.ppc:update() + strategy.reverser:getDriveData() +end + -- make sure to recreate the global dev helper whenever this script is (re)loaded if g_devHelper then g_devHelper:delete() diff --git a/scripts/util/CpRemainingTime.lua b/scripts/util/CpRemainingTime.lua index b037067b2..c6e2c6a53 100644 --- a/scripts/util/CpRemainingTime.lua +++ b/scripts/util/CpRemainingTime.lua @@ -52,7 +52,7 @@ end function CpRemainingTime:update(dt) if g_currentMission.controlledVehicle == self.vehicle and self.DEBUG_ACTIVE and CpDebug:isChannelActive(self.debugChannel, self.vehicle) then - DebugUtil.renderTable(0.4, 0.4, 0.018, { + DebugUtil.renderTable(0.2, 0.2, 0.018, { {name = "time", value = CpGuiUtil.getFormatTimeText(self.time)}, {name = "optimal speed", value = MathUtil.mpsToKmh(self:getOptimalSpeed())}, {name = "optimal time", value = CpGuiUtil.getFormatTimeText(self:getOptimalCourseTime(self.course, self.lastIx))},