From 7cdc9d8aa435cbc78772bfc6c8fbb4d5e9de0a79 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 7 Dec 2024 16:35:23 -0500 Subject: [PATCH 01/12] wip --- scripts/Course.lua | 12 -- scripts/Waypoint.lua | 1 - scripts/ai/AIDriveStrategyBunkerSilo.lua | 2 +- scripts/ai/AIDriveStrategyFieldWorkCourse.lua | 2 +- scripts/ai/AIDriveStrategyUnloadCombine.lua | 2 +- scripts/ai/AIUtil.lua | 82 ++++++++++--- scripts/ai/turns/AITurn.lua | 13 ++- scripts/ai/turns/TurnManeuver.lua | 110 ++++++++---------- scripts/pathfinder/PathfinderUtil.lua | 8 +- 9 files changed, 131 insertions(+), 101 deletions(-) diff --git a/scripts/Course.lua b/scripts/Course.lua index c8fb4c41..75f395a0 100644 --- a/scripts/Course.lua +++ b/scripts/Course.lua @@ -559,18 +559,6 @@ function Course:getYRotationCorrectedForDirectionChanges(ix) end end --- This is the radius from the course generator. For now ony island bypass waypoints nodes have a --- radius. -function Course:getRadiusAtIx(ix) - local r = self.waypoints[ix].radius - if r ~= r then - -- radius can be nan - return nil - else - return r - end -end - -- This is the radius calculated when the course is created. function Course:getCalculatedRadiusAtIx(ix) local r = self.waypoints[ix].calculatedRadius diff --git a/scripts/Waypoint.lua b/scripts/Waypoint.lua index 834e976d..a96b1ff8 100644 --- a/scripts/Waypoint.lua +++ b/scripts/Waypoint.lua @@ -34,7 +34,6 @@ function Waypoint:init(wp) ---@type CourseGenerator.WaypointAttributes self.attributes = wp.attributes and wp.attributes:clone() or CourseGenerator.WaypointAttributes() self.angle = wp.angle or nil - self.radius = wp.radius or nil self.rev = wp.rev or wp.reverse or false self.rev = self.rev or wp.gear and wp.gear == Gear.Backward -- dynamically added/calculated properties diff --git a/scripts/ai/AIDriveStrategyBunkerSilo.lua b/scripts/ai/AIDriveStrategyBunkerSilo.lua index 3c93eb83..ead8452d 100644 --- a/scripts/ai/AIDriveStrategyBunkerSilo.lua +++ b/scripts/ai/AIDriveStrategyBunkerSilo.lua @@ -415,7 +415,7 @@ function AIDriveStrategyBunkerSilo:startTransitionToNextLane() end local path = PathfinderUtil.findAnalyticPath(ReedsSheppSolver(), self.vehicle:getAIDirectionNode(), - 0, self.turnNode, 0, 0, self.turningRadius) + 0, 0, self.turnNode, 0, 0, self.turningRadius) if not path or #path == 0 then self:debug("No valid turn was found!") self.vehicle:stopCurrentAIJob(AIMessageCpErrorNoPathFound.new()) diff --git a/scripts/ai/AIDriveStrategyFieldWorkCourse.lua b/scripts/ai/AIDriveStrategyFieldWorkCourse.lua index c4ec2224..d865284d 100644 --- a/scripts/ai/AIDriveStrategyFieldWorkCourse.lua +++ b/scripts/ai/AIDriveStrategyFieldWorkCourse.lua @@ -775,7 +775,7 @@ function AIDriveStrategyFieldWorkCourse:calculateTightTurnOffset() if self.state == self.states.WORKING or self.state == self.states.DRIVING_TO_WORK_START_WAYPOINT then -- when rounding small islands or to start on a course with curves self.tightTurnOffset = AIUtil.calculateTightTurnOffset(self.vehicle, self.turningRadius, self.course, - self.tightTurnOffset, true) + self.tightTurnOffset) else self.tightTurnOffset = 0 end diff --git a/scripts/ai/AIDriveStrategyUnloadCombine.lua b/scripts/ai/AIDriveStrategyUnloadCombine.lua index 9ef44703..25b58a0e 100644 --- a/scripts/ai/AIDriveStrategyUnloadCombine.lua +++ b/scripts/ai/AIDriveStrategyUnloadCombine.lua @@ -2829,7 +2829,7 @@ function AIDriveStrategyUnloadCombine:onFieldUnloadPositionReached() self:debug("Starting pathfinding to the reverse unload turn end node with align length: %.2f and steering length: %.2f, turn radius: %.2f", alignLength, steeringLength, self.turningRadius) local path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, self.fieldUnloadTurnStartNode, - 0, self.fieldUnloadTurnEndNode, 0, 3, self.turningRadius) + 0, 0, self.fieldUnloadTurnEndNode, 0, 3, self.turningRadius) if not path or #path == 0 then self:debug("Reverse alignment course creation failed!") else diff --git a/scripts/ai/AIUtil.lua b/scripts/ai/AIUtil.lua index 190336d7..a3907555 100644 --- a/scripts/ai/AIUtil.lua +++ b/scripts/ai/AIUtil.lua @@ -50,7 +50,7 @@ end --- making sure that the towed implement's trajectory remains closer to the --- course. ---@param course Course -function AIUtil.calculateTightTurnOffset(vehicle, vehicleTurningRadius, course, previousOffset, useCalculatedRadius) +function AIUtil.calculateTightTurnOffset(vehicle, vehicleTurningRadius, course, previousOffset) local tightTurnOffset local function smoothOffset(offset) @@ -58,12 +58,7 @@ function AIUtil.calculateTightTurnOffset(vehicle, vehicleTurningRadius, course, end -- first of all, does the current waypoint have radius data? - local r - if useCalculatedRadius then - r = course:getCalculatedRadiusAtIx(course:getCurrentWaypointIx()) - else - r = course:getRadiusAtIx(course:getCurrentWaypointIx()) - end + local r = course:getCalculatedRadiusAtIx(course:getCurrentWaypointIx()) if not r then return smoothOffset(0) end @@ -104,13 +99,53 @@ function AIUtil.calculateTightTurnOffset(vehicle, vehicleTurningRadius, course, -- smooth the offset a bit to avoid sudden changes tightTurnOffset = smoothOffset(offset) - CpUtil.debugVehicle(CpDebug.DBG_AI_DRIVER, vehicle, + CpUtil.debugVehicle(CpDebug.DBG_TURN, vehicle, 'Tight turn, r = %.1f, tow bar = %.1f m, currentAngle = %.0f, nextAngle = %.0f, offset = %.1f, smoothOffset = %.1f', r, towBarLength, currentAngle, nextAngle, offset, tightTurnOffset ) -- remember the last value for smoothing return tightTurnOffset end +function AIUtil.calculateTightTurnOffsetForTurnManeuver(vehicle, steeringLength, course, ix, previousOffset) + local tightTurnOffset + + local function smoothOffset(offset) + return (offset + 4 * (previousOffset or 0 )) / 5 + end + + -- first of all, does the current waypoint have radius data? + local r = course:getCalculatedRadiusAtIx(ix) + if not r then + return smoothOffset(0) + end + + -- Ok, looks like a tight turn, so we need to move a bit left or right of the course + -- to keep the tool on the course. Use a little less than the calculated, this is purely empirical and should probably + -- be reviewed why the calculated one seems to overshoot. + local offset = 1 * AIUtil.getTractorRadiusFromImplementRadius(r, steeringLength) - r + if offset ~= offset then + -- check for nan + return smoothOffset(0) + end + -- figure out left or right now? + local nextAngle = course:getWaypointAngleDeg(ix + 1) + local currentAngle = course:getWaypointAngleDeg(ix) + if not nextAngle or not currentAngle then + return smoothOffset(0) + end + + if CpMathUtil.getDeltaAngle(math.rad(nextAngle), math.rad(currentAngle)) > 0 then offset = -offset end + + -- smooth the offset a bit to avoid sudden changes + tightTurnOffset = smoothOffset(offset) + CpUtil.debugVehicle(CpDebug.DBG_TURN, vehicle, + 'Tight turn, r = %.1f, tow bar = %.1f m, currentAngle = %.0f, nextAngle = %.0f, offset = %.1f, smoothOffset = %.1f', + r, steeringLength, currentAngle, nextAngle, offset, tightTurnOffset ) + -- remember the last value for smoothing + return tightTurnOffset +end + + function AIUtil.getTowBarLength(vehicle) -- is there a wheeled implement behind the tractor and is it on a pivot? local implement = AIUtil.getFirstReversingImplementWithWheels(vehicle, true) @@ -141,8 +176,17 @@ function AIUtil.getSteeringParameters(vehicle) end function AIUtil.getOffsetForTowBarLength(r, towBarLength) + return AIUtil.getTractorRadiusFromImplementRadius(r, towBarLength) - r +end + +function AIUtil.getImplementRadiusFromTractorRadius(r, towBarLength) + local rImplement = math.sqrt( r * r - towBarLength * towBarLength ) -- the radius the tractor should be on + return rImplement +end + +function AIUtil.getTractorRadiusFromImplementRadius(r, towBarLength) local rTractor = math.sqrt( r * r + towBarLength * towBarLength ) -- the radius the tractor should be on - return rTractor - r + return rTractor end function AIUtil.getArticulatedAxisVehicleReverserNode(vehicle) @@ -359,9 +403,9 @@ function AIUtil.getFirstAttachedImplement(vehicle, suppressLog) -- the distance from the vehicle's root node to the front of the implement local _, _, d = localToLocal(implement.object.rootNode, AIUtil.getDirectionNode(vehicle), 0, 0, implement.object.size.length / 2 + implement.object.size.lengthOffset) - if implement.object.spec_leveler then + if implement.object.spec_leveler then local nodeData = ImplementUtil.getLevelerNode(implement.object) - if nodeData then + if nodeData then _, _, d = localToLocal(nodeData.node, AIUtil.getDirectionNode(vehicle), 0, 0, 0) end end @@ -388,9 +432,9 @@ function AIUtil.getLastAttachedImplement(vehicle,suppressLog) -- the distance from the vehicle's root node to the back of the implement local _, _, d = localToLocal(implement.object.rootNode, AIUtil.getDirectionNode(vehicle), 0, 0, - implement.object.size.length / 2 + implement.object.size.lengthOffset) - if implement.object.spec_leveler then + if implement.object.spec_leveler then local nodeData = ImplementUtil.getLevelerNode(implement.object) - if nodeData then + if nodeData then _, _, d = localToLocal(nodeData.node, AIUtil.getDirectionNode(vehicle), 0, 0, 0) end end @@ -452,7 +496,7 @@ function AIUtil.getNumberOfChildVehiclesWithSpecialization(vehicle, specializati return #vehicles end ---- Gets all child vehicles with a given specialization. +--- Gets all child vehicles with a given specialization. --- This can include the rootVehicle and implements --- that are not directly attached to the rootVehicle. ---@param vehicle table @@ -462,7 +506,7 @@ end ---@return boolean at least one vehicle/implement was found function AIUtil.getAllChildVehiclesWithSpecialization(vehicle, specialization, specializationReference) if vehicle == nil then - printCallstack() + printCallstack() CpUtil.info("Vehicle is nil!") return {}, false end @@ -661,7 +705,7 @@ function AIUtil.getLength(vehicle) if vehicle.getAIAgentSize then vehicle:updateAIAgentAttachments() local width, length, lengthOffset, frontOffset, height = vehicle:getAIAgentSize() - for _, attachment in ipairs(vehicle.spec_aiDrivable.attachments) do + for _, attachment in ipairs(vehicle.spec_aiDrivable.attachments) do length = length + attachment.length end return length @@ -697,7 +741,7 @@ function AIUtil.hasCutterOnTrailerAttached(vehicle) end --- Checks if a cutter is attached and it's not registered as a valid combine cutter. ---- A Example is the New Holland Superflex header, when it is attached as transport trailer. +--- A Example is the New Holland Superflex header, when it is attached as transport trailer. function AIUtil.hasCutterAsTrailerAttached(vehicle) local cutters, found = AIUtil.getAllChildVehiclesWithSpecialization(vehicle, Cutter) if not found then @@ -705,12 +749,12 @@ function AIUtil.hasCutterAsTrailerAttached(vehicle) return false end local combines, found = AIUtil.getAllChildVehiclesWithSpecialization(vehicle, Combine) - if not found then + if not found then --- No valid combine object was found. return false end local spec = combines[1].spec_combine - if spec.numAttachedCutters <= 0 then + if spec.numAttachedCutters <= 0 then --- The cutter is not available for threshing in this combination. return true end diff --git a/scripts/ai/turns/AITurn.lua b/scripts/ai/turns/AITurn.lua index f9524b0d..45c896a0 100644 --- a/scripts/ai/turns/AITurn.lua +++ b/scripts/ai/turns/AITurn.lua @@ -651,8 +651,8 @@ function CourseTurn:onWaypointChange(ix) if self.turnCourse then if self.forceTightTurnOffset or (self.enableTightTurnOffset and self.turnCourse:useTightTurnOffset(ix)) then -- adjust the course a bit to the outside in a curve to keep a towed implement on the course - self.tightTurnOffset = AIUtil.calculateTightTurnOffset(self.vehicle, self.turningRadius, self.turnCourse, - self.tightTurnOffset, true) + self.tightTurnOffset = AIUtil.calculateTightTurnOffsetForTurnManeuver(self.vehicle, self.steeringLength, + self.turnCourse, self.turnCourse:getCurrentWaypointIx(), self.tightTurnOffset) self.turnCourse:setOffset(self.tightTurnOffset, 0) else -- reset offset to 0 if tight turn offset is not on @@ -728,8 +728,13 @@ function CourseTurn:generateCalculatedTurn() -- TODO: the generated Dubins turn may not fit on the field and we we'll move it back, forcing the -- vehicle to reverse at the start and at the end of the turn. This will be very slow, and if the -- vehicle is able to reverse, a Reeds-Shepp would be lot faster - turnManeuver = DubinsTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), - self.turningRadius, self.workWidth, self.steeringLength, distanceToFieldEdge) + if self.steeringLength > 0 then + turnManeuver = TowedDubinsTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), + self.turningRadius, self.workWidth, self.steeringLength, distanceToFieldEdge) + else + turnManeuver = DubinsTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), + self.turningRadius, self.workWidth, self.steeringLength, distanceToFieldEdge) + end else turnManeuver = ReedsSheppTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), self.turningRadius, self.workWidth, self.steeringLength, distanceToFieldEdge) diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua index 3dbf1d59..39672621 100644 --- a/scripts/ai/turns/TurnManeuver.lua +++ b/scripts/ai/turns/TurnManeuver.lua @@ -17,7 +17,7 @@ TurnManeuver.CHANGE_TO_FWD_WHEN_REACHED = 'changeToFwdWhenReached' -- making sure it is lowered when we reach the start of the next row) TurnManeuver.LOWER_IMPLEMENT_AT_TURN_END = 'lowerImplementAtTurnEnd' -- Mark waypoints for dynamic tight turn offset -TurnManeuver.applyTightTurnOffset = true +TurnManeuver.tightTurnOffsetEnabled = false ---@param course Course function TurnManeuver.hasTurnControl(course, ix, control) @@ -44,6 +44,7 @@ function TurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRa -- how far the furthest point of the maneuver is from the vehicle's direction node, used to -- check if we can turn on the field self.dzMax = -math.huge + self.turnEndXOffset = self.turnEndXOffset or 0 end function TurnManeuver:getCourse() @@ -284,8 +285,9 @@ function TurnManeuver:adjustCourseToFitField(course, dBack, ixBeforeEndingTurnSe self:debug('Reverse to work start (implement in back)') -- vehicle in front of the work start node at turn end local forwardAfterTurn = Course.createFromNode(self.vehicle, self.turnContext.vehicleAtTurnEndNode, 0, - dFromTurnEnd + 1, dFromTurnEnd + 1 + self.steeringLength, 0.8, false) + dFromTurnEnd + 1 + self.steeringLength / 2, dFromTurnEnd + 1 + self.steeringLength, 0.8, false) courseWithReversing:append(forwardAfterTurn) + --self:applyTightTurnOffset(forwardAfterTurn:getLength()) -- allow early direction change when aligned TurnManeuver.setTurnControlForLastWaypoints(courseWithReversing, forwardAfterTurn:getLength(), TurnManeuver.CHANGE_DIRECTION_WHEN_ALIGNED, true, true) @@ -306,6 +308,7 @@ function TurnManeuver:adjustCourseToFitField(course, dBack, ixBeforeEndingTurnSe local forwardAfterTurn = Course.createFromNode(self.vehicle, self.turnContext.workStartNode, 0, dFromWorkStart, 1, 0.8, false) courseWithReversing:append(forwardAfterTurn) + --self:applyTightTurnOffset(forwardAfterTurn:getLength()) TurnManeuver.setTurnControlForLastWaypoints(courseWithReversing, forwardAfterTurn:getLength(), TurnManeuver.CHANGE_DIRECTION_WHEN_ALIGNED, true, true) end @@ -315,45 +318,43 @@ function TurnManeuver:adjustCourseToFitField(course, dBack, ixBeforeEndingTurnSe endingTurnLength = reverseAfterTurn:getLength() else self:debug('Reverse to work start not needed') - endingTurnLength = self.turnContext:appendEndingTurnCourse(courseWithReversing, self.steeringLength, self.applyTightTurnOffset) + endingTurnLength = self.turnContext:appendEndingTurnCourse(courseWithReversing, self.steeringLength, self.tightTurnOffsetEnabled) end return courseWithReversing, endingTurnLength end +function TurnManeuver:applyTightTurnOffset(length) + if self.tightTurnOffsetEnabled then + -- use the default length (a quarter circle) unless there is a configured value + length = length or self.turningRadius * math.pi + self.course:setUseTightTurnOffsetForLastWaypoints( + g_vehicleConfigurations:getRecursively(self.vehicle, 'tightTurnOffsetDistanceInTurns') or length) + end +end + ---@class AnalyticTurnManeuver : TurnManeuver AnalyticTurnManeuver = CpObject(TurnManeuver) function AnalyticTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) TurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength) self:debug('Start generating') - self:debug('r=%.1f, w=%.1f, steeringLength=%.1f, distanceToFieldEdge=%.1f', - turningRadius, workWidth, steeringLength, distanceToFieldEdge) - local turnEndNode, goalOffset = self.turnContext:getTurnEndNodeAndOffsets(self.steeringLength) - self.course = self:findAnalyticPath(vehicleDirectionNode, 0, turnEndNode, 0, goalOffset, self.turningRadius) + local turnEndNode, endZOffset = self.turnContext:getTurnEndNodeAndOffsets(self.steeringLength) + self:debug('r=%.1f, w=%.1f, steeringLength=%.1f, distanceToFieldEdge=%.1f, goalOffset=%.1f', + turningRadius, workWidth, steeringLength, distanceToFieldEdge, endZOffset) + self.course = self:findAnalyticPath(vehicleDirectionNode, 0, 0, turnEndNode, self.turnEndXOffset, endZOffset, self.turningRadius) local endingTurnLength local dBack = self:getDistanceToMoveBack(self.course, workWidth, distanceToFieldEdge) local canReverse = AIUtil.canReverse(vehicle) if dBack > 0 and canReverse then dBack = dBack < 2 and 2 or dBack self:debug('Not enough space on field, regenerating course back %.1f meters', dBack) - self.course = self:findAnalyticPath(vehicleDirectionNode, -dBack, turnEndNode, 0, goalOffset + dBack, self.turningRadius) - if self.applyTightTurnOffset then - self.course:setUseTightTurnOffsetForLastWaypoints( - g_vehicleConfigurations:getRecursively(vehicle, 'tightTurnOffsetDistanceInTurns') or 10) - end + self.course = self:findAnalyticPath(vehicleDirectionNode, 0, -dBack, turnEndNode, self.turnEndXOffset, endZOffset + dBack, self.turningRadius) + self:applyTightTurnOffset(1000) local ixBeforeEndingTurnSection = self.course:getNumberOfWaypoints() self.course, endingTurnLength = self:adjustCourseToFitField(self.course, dBack, ixBeforeEndingTurnSection) else - if self.applyTightTurnOffset then - self.course:setUseTightTurnOffsetForLastWaypoints( - g_vehicleConfigurations:getRecursively(vehicle, 'tightTurnOffsetDistanceInTurns') or 10) - end - endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, steeringLength, self.applyTightTurnOffset) - end - if self.applyTightTurnOffset then - -- make sure we use tight turn offset towards the end of the course so a towed implement is aligned with the new row - self.course:setUseTightTurnOffsetForLastWaypoints( - g_vehicleConfigurations:getRecursively(vehicle, 'tightTurnOffsetDistanceInTurns') or 10) + self:applyTightTurnOffset(1000) + endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, steeringLength, self.tightTurnOffsetEnabled) end TurnManeuver.setLowerImplements(self.course, endingTurnLength, true) end @@ -363,18 +364,17 @@ function AnalyticTurnManeuver:getDistanceToMoveBack(course, workWidth, distanceT local dzMax = self:getDzMax(course) local spaceNeededOnFieldForTurn = dzMax + workWidth / 2 distanceToFieldEdge = distanceToFieldEdge or 500 -- if not given, assume we have a lot of space - if self.turnContext:getTurnEndForwardOffset() < 0 then - -- in an offset turn, where the turn start (and thus, the vehicle) is on the longer leg, - -- so the turn end is behind the turn start, we have in reality less space, as we measured the - -- distance to the field edge from the turn start, but we need to measure it from the turn end, - -- where there's less space - distanceToFieldEdge = distanceToFieldEdge + self.turnContext:getTurnEndForwardOffset() - end + local turnEndForwardOffset = self.turnContext:getTurnEndForwardOffset() + -- in an offset turn, where the turn start (and thus, the vehicle) is on the longer leg, + -- so the turn end is behind the turn start, we have in reality less space, as we measured the + -- distance to the field edge from the turn start, but we need to measure it from the middle of the turn, + -- where there's less space + distanceToFieldEdge = distanceToFieldEdge + turnEndForwardOffset / 2 -- with a headland at angle, we have to move further back, so the left/right edge of the swath also stays on -- the field, not only the center distanceToFieldEdge = distanceToFieldEdge - (workWidth / 2 / math.abs(math.tan(self.turnContext:getHeadlandAngle()))) - self:debug('dzMax=%.1f, workWidth=%.1f, spaceNeeded=%.1f, distanceToFieldEdge=%.1f', dzMax, workWidth, - spaceNeededOnFieldForTurn, distanceToFieldEdge) + self:debug('dzMax=%.1f, workWidth=%.1f, spaceNeeded=%.1f, turnEndForwardOffset=%.1f, distanceToFieldEdge=%.1f', dzMax, workWidth, + spaceNeededOnFieldForTurn, turnEndForwardOffset, distanceToFieldEdge) return spaceNeededOnFieldForTurn - distanceToFieldEdge end @@ -383,46 +383,41 @@ DubinsTurnManeuver = CpObject(AnalyticTurnManeuver) function DubinsTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) self.debugPrefix = '(DubinsTurn): ' + self.turnEndXOffset = 0 AnalyticTurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) end -function DubinsTurnManeuver:findAnalyticPath(vehicleDirectionNode, startOffset, turnEndNode, - xOffset, goalOffset, turningRadius) +function DubinsTurnManeuver:findAnalyticPath(startNode, startXOffset, startZOffset, endNode, + endXOffset, endZOffset, turningRadius) local path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, - vehicleDirectionNode, startOffset, turnEndNode, 0, goalOffset, self.turningRadius) + startNode, startXOffset, startZOffset, endNode, endXOffset, endZOffset, self.turningRadius) return Course.createFromAnalyticPath(self.vehicle, path, true) end ---@class TowedDubinsTurnManeuver : DubinsTurnManeuver TowedDubinsTurnManeuver = CpObject(DubinsTurnManeuver) -TowedDubinsTurnManeuver.applyTightTurnOffset = false function TowedDubinsTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) self.debugPrefix = '(TowedDubinsTurn): ' - local offset = AIUtil.getOffsetForTowBarLength(turningRadius, steeringLength) - turningRadius = turningRadius - offset - self:debug('Towed implement, adjusting radius to %.1f to accommodate tight turn offset', turningRadius) + local xOffset = turningRadius - AIUtil.getImplementRadiusFromTractorRadius(turningRadius, steeringLength) + self.turnEndXOffset = turnContext:isLeftTurn() and -xOffset or xOffset + self:debug('Towed implement, offsetting turn end %.1f to accommodate tight turn ', xOffset) AnalyticTurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) + --self:calculateTractorCourse(self.course) end function TowedDubinsTurnManeuver:calculateTractorCourse(course) - for _, wp in ipairs(course.waypoints) do - local v = PathfinderUtil.getWaypointAsState3D(wp, 0, self.steeringLength) - wp.x, wp.z = v.x, -v.y + local offsetX = 0 + for ix, wp in ipairs(course.waypoints) do + offsetX = AIUtil.calculateTightTurnOffsetForTurnManeuver(self.vehicle, self.steeringLength, course, ix, offsetX) + wp:setOffsetPosition(offsetX, 0) end course:enrichWaypointData() return course end -function TowedDubinsTurnManeuver:findAnalyticPath(vehicleDirectionNode, startOffset, turnEndNode, - xOffset, goalOffset, turningRadius) - local path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, - vehicleDirectionNode, startOffset, turnEndNode, 0, goalOffset, self.turningRadius) - return self:calculateTractorCourse(Course.createFromAnalyticPath(self.vehicle, path, true)) -end - ---@class LeftTurnReedsSheppSolver : ReedsSheppSolver LeftTurnReedsSheppSolver = CpObject(ReedsSheppSolver) function LeftTurnReedsSheppSolver:solve(start, goal, turnRadius) @@ -451,8 +446,8 @@ function ReedsSheppTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, workWidth, steeringLength, distanceToFieldEdge) end -function ReedsSheppTurnManeuver:findAnalyticPath(vehicleDirectionNode, startOffset, turnEndNode, - xOffset, goalOffset, turningRadius) +function ReedsSheppTurnManeuver:findAnalyticPath(vehicleDirectionNode, startXOffset, startZOffset, turnEndNode, + endXOffset, endZOffset, turningRadius) local solver if self.turnContext:isLeftTurn() then self:debug('using LeftTurnReedsSheppSolver') @@ -461,12 +456,12 @@ function ReedsSheppTurnManeuver:findAnalyticPath(vehicleDirectionNode, startOffs self:debug('using RightTurnReedsSheppSolver') solver = RightTurnReedsSheppSolver() end - local path = PathfinderUtil.findAnalyticPath(solver, vehicleDirectionNode, startOffset, turnEndNode, - 0, goalOffset, self.turningRadius) + local path = PathfinderUtil.findAnalyticPath(solver, vehicleDirectionNode, startXOffset, startZOffset, turnEndNode, + 0, endZOffset, self.turningRadius) if not path or #path == 0 then self:debug('Could not find ReedsShepp path, retry with Dubins') - path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, vehicleDirectionNode, startOffset, - turnEndNode, 0, goalOffset, self.turningRadius) + path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, vehicleDirectionNode, startXOffset, startZOffset, + turnEndNode, 0, endZOffset, self.turningRadius) end local course = Course.createFromAnalyticPath(self.vehicle, path, true) course:adjustForTowedImplements(1.5 * self.steeringLength + 1) @@ -503,10 +498,7 @@ function TurnEndingManeuver:init(vehicle, turnContext, vehicleDirectionNode, tur self:generateStraightSection(endArc, endStraight) myCorner:delete() self.course = Course(vehicle, self.waypoints, true) - if self.applyTightTurnOffset then - self.course:setUseTightTurnOffsetForLastWaypoints( - g_vehicleConfigurations:getRecursively(vehicle, 'tightTurnOffsetDistanceInTurns') or 20) - end + self:applyTightTurnOffset() TurnManeuver.setLowerImplements(self.course, math.max(math.abs(turnContext.frontMarkerDistance), steeringLength)) end @@ -627,7 +619,7 @@ function VineTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turni turningRadius, workWidth, dz, startOffset, goalOffset) local path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, -- always move the goal a bit backwards to let the vehicle align - vehicleDirectionNode, startOffset, turnEndNode, 0, goalOffset - turnContext.frontMarkerDistance, self.turningRadius) + vehicleDirectionNode, startOffset, 0, turnEndNode, 0, goalOffset - turnContext.frontMarkerDistance, self.turningRadius) self.course = Course.createFromAnalyticPath(self.vehicle, path, true) local endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, 0, false) TurnManeuver.setLowerImplements(self.course, endingTurnLength, true) diff --git a/scripts/pathfinder/PathfinderUtil.lua b/scripts/pathfinder/PathfinderUtil.lua index 164552fb..f9800981 100644 --- a/scripts/pathfinder/PathfinderUtil.lua +++ b/scripts/pathfinder/PathfinderUtil.lua @@ -604,7 +604,9 @@ end ------------------------------------------------------------------------------------------------------------------------ ---@param solver AnalyticSolver for instance PathfinderUtil.dubinsSolver or PathfinderUtil.reedsSheppSolver ---@param vehicleDirectionNode number Giants node ----@param startOffset number offset in meters relative to the vehicle position (forward positive, backward negative) where +---@param startXOffset number offset in meters relative to the vehicle position (left positive, right negative) where +--- we want the turn to start +---@param startZOffset number offset in meters relative to the vehicle position (forward positive, backward negative) where --- we want the turn to start ---@param goalReferenceNode table node used to determine the goal ---@param xOffset number offset in meters relative to the goal node (left positive, right negative) @@ -613,9 +615,9 @@ end ---@param turnRadius number vehicle turning radius ---@return table|nil path ---@return number length -function PathfinderUtil.findAnalyticPath(solver, vehicleDirectionNode, startOffset, goalReferenceNode, +function PathfinderUtil.findAnalyticPath(solver, vehicleDirectionNode, startXOffset, startZOffset, goalReferenceNode, xOffset, zOffset, turnRadius) - local x, z, yRot = PathfinderUtil.getNodePositionAndDirection(vehicleDirectionNode, 0, startOffset or 0) + local x, z, yRot = PathfinderUtil.getNodePositionAndDirection(vehicleDirectionNode, startXOffset, startZOffset or 0) local start = State3D(x, -z, CpMathUtil.angleFromGame(yRot)) x, z, yRot = PathfinderUtil.getNodePositionAndDirection(goalReferenceNode, xOffset or 0, zOffset or 0) local goal = State3D(x, -z, CpMathUtil.angleFromGame(yRot)) From 5394a6bf6bafc837286449bcc3ef146478768c6c Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sun, 8 Dec 2024 13:58:07 -0500 Subject: [PATCH 02/12] wip --- scripts/ai/AIUtil.lua | 16 ++++++++++++++-- scripts/ai/turns/TurnContext.lua | 1 + scripts/ai/turns/TurnManeuver.lua | 24 ++++++++++++++++-------- 3 files changed, 31 insertions(+), 10 deletions(-) diff --git a/scripts/ai/AIUtil.lua b/scripts/ai/AIUtil.lua index a3907555..cfa6c6e2 100644 --- a/scripts/ai/AIUtil.lua +++ b/scripts/ai/AIUtil.lua @@ -179,13 +179,25 @@ function AIUtil.getOffsetForTowBarLength(r, towBarLength) return AIUtil.getTractorRadiusFromImplementRadius(r, towBarLength) - r end +--- When a tractor is towing an implement in a turn, on what radius will the implement be if +--- the radius the tractor is driving is known? +---@param r number the radius the tractor is on +---@param towBarLength number the length of the tow bar +---@return number the radius the implement will be on. Can be negative, meaning the implement will be +--- moving backwards in the turn function AIUtil.getImplementRadiusFromTractorRadius(r, towBarLength) - local rImplement = math.sqrt( r * r - towBarLength * towBarLength ) -- the radius the tractor should be on + local rSquared = r * r - towBarLength * towBarLength + local rImplement = rSquared > 0 and math.sqrt(rSquared) or -math.sqrt(-rSquared) return rImplement end +--- When a tractor is towing an implement in a turn, on what radius will the tractor be if +--- the radius the implement is known? +---@param r number the radius the implement is following +---@param towBarLength number the length of the tow bar +---@return number the radius the tractor will be on function AIUtil.getTractorRadiusFromImplementRadius(r, towBarLength) - local rTractor = math.sqrt( r * r + towBarLength * towBarLength ) -- the radius the tractor should be on + local rTractor = math.sqrt( r * r + towBarLength * towBarLength ) return rTractor end diff --git a/scripts/ai/turns/TurnContext.lua b/scripts/ai/turns/TurnContext.lua index ae6d3e9a..df2c9209 100644 --- a/scripts/ai/turns/TurnContext.lua +++ b/scripts/ai/turns/TurnContext.lua @@ -230,6 +230,7 @@ end ---@return number angle (radian) between the row and the headland, 90 degrees means the headland is perpendicular to the row function TurnContext:getHeadlandAngle() + print(self.turnEndWp.angle, self.turnStartWp.angle) return math.abs(CpMathUtil.getDeltaAngle(math.rad(self.turnEndWp.angle), math.rad(self.turnStartWp.angle))) end diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua index 39672621..2a6baa15 100644 --- a/scripts/ai/turns/TurnManeuver.lua +++ b/scripts/ai/turns/TurnManeuver.lua @@ -339,8 +339,13 @@ function AnalyticTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, t self:debug('Start generating') local turnEndNode, endZOffset = self.turnContext:getTurnEndNodeAndOffsets(self.steeringLength) - self:debug('r=%.1f, w=%.1f, steeringLength=%.1f, distanceToFieldEdge=%.1f, goalOffset=%.1f', - turningRadius, workWidth, steeringLength, distanceToFieldEdge, endZOffset) + local _, _, dz = localToLocal(vehicleDirectionNode, turnEndNode, 0, 0, 0) + -- zOffset from the turn end (work start). If there is a zOffset in the turn, that is, the turn end is behind the + -- turn start due to an angled headland, we still want to make the complete 180 turn es close to the field edge + -- as we can, so a towed implement, with an offset arc is turned 180 as soon as possible and has time to align + endZOffset = math.min(dz, endZOffset) + self:debug('r=%.1f, w=%.1f, steeringLength=%.1f, distanceToFieldEdge=%.1f, goalOffset=%.1f, dz=%.1f', + turningRadius, workWidth, steeringLength, distanceToFieldEdge, endZOffset, dz) self.course = self:findAnalyticPath(vehicleDirectionNode, 0, 0, turnEndNode, self.turnEndXOffset, endZOffset, self.turningRadius) local endingTurnLength local dBack = self:getDistanceToMoveBack(self.course, workWidth, distanceToFieldEdge) @@ -372,9 +377,11 @@ function AnalyticTurnManeuver:getDistanceToMoveBack(course, workWidth, distanceT distanceToFieldEdge = distanceToFieldEdge + turnEndForwardOffset / 2 -- with a headland at angle, we have to move further back, so the left/right edge of the swath also stays on -- the field, not only the center - distanceToFieldEdge = distanceToFieldEdge - (workWidth / 2 / math.abs(math.tan(self.turnContext:getHeadlandAngle()))) - self:debug('dzMax=%.1f, workWidth=%.1f, spaceNeeded=%.1f, turnEndForwardOffset=%.1f, distanceToFieldEdge=%.1f', dzMax, workWidth, - spaceNeededOnFieldForTurn, turnEndForwardOffset, distanceToFieldEdge) + local headlandAngle = self.turnContext:getHeadlandAngle() + distanceToFieldEdge = distanceToFieldEdge - + (headlandAngle > 0.0001 and (workWidth / 2 / math.abs(math.tan(headlandAngle))) or 0) + self:debug('dzMax=%.1f, workWidth=%.1f, spaceNeeded=%.1f, turnEndForwardOffset=%.1f, headlandAngle=%.1f, distanceToFieldEdge=%.1f', dzMax, workWidth, + spaceNeededOnFieldForTurn, turnEndForwardOffset, math.deg(headlandAngle), distanceToFieldEdge) return spaceNeededOnFieldForTurn - distanceToFieldEdge end @@ -400,12 +407,13 @@ TowedDubinsTurnManeuver = CpObject(DubinsTurnManeuver) function TowedDubinsTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) self.debugPrefix = '(TowedDubinsTurn): ' - local xOffset = turningRadius - AIUtil.getImplementRadiusFromTractorRadius(turningRadius, steeringLength) + self.vehicle = vehicle + local implementRadius = AIUtil.getImplementRadiusFromTractorRadius(turningRadius, steeringLength) + local xOffset = turningRadius - implementRadius self.turnEndXOffset = turnContext:isLeftTurn() and -xOffset or xOffset - self:debug('Towed implement, offsetting turn end %.1f to accommodate tight turn ', xOffset) + self:debug('Towed implement, offsetting turn end %.1f to accommodate tight turn, implement radius %.1f ', xOffset, implementRadius) AnalyticTurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) - --self:calculateTractorCourse(self.course) end function TowedDubinsTurnManeuver:calculateTractorCourse(course) From 197f52422fcedea57e625a13fc0c26314e58973c Mon Sep 17 00:00:00 2001 From: Tensuko Date: Mon, 9 Dec 2024 17:17:52 +0100 Subject: [PATCH 03/12] add piros to config --- config/VehicleConfigurations.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/config/VehicleConfigurations.xml b/config/VehicleConfigurations.xml index fb9794ab..388b1cea 100644 --- a/config/VehicleConfigurations.xml +++ b/config/VehicleConfigurations.xml @@ -270,6 +270,9 @@ You can define the following custom settings: toolOffsetX = "-1.8" noReverse = "true" /> + Date: Mon, 9 Dec 2024 12:02:09 -0500 Subject: [PATCH 04/12] wip --- scripts/ai/turns/TurnManeuver.lua | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua index 2a6baa15..c23ae470 100644 --- a/scripts/ai/turns/TurnManeuver.lua +++ b/scripts/ai/turns/TurnManeuver.lua @@ -17,7 +17,7 @@ TurnManeuver.CHANGE_TO_FWD_WHEN_REACHED = 'changeToFwdWhenReached' -- making sure it is lowered when we reach the start of the next row) TurnManeuver.LOWER_IMPLEMENT_AT_TURN_END = 'lowerImplementAtTurnEnd' -- Mark waypoints for dynamic tight turn offset -TurnManeuver.tightTurnOffsetEnabled = false +TurnManeuver.tightTurnOffsetEnabled = true ---@param course Course function TurnManeuver.hasTurnControl(course, ix, control) @@ -354,11 +354,11 @@ function AnalyticTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, t dBack = dBack < 2 and 2 or dBack self:debug('Not enough space on field, regenerating course back %.1f meters', dBack) self.course = self:findAnalyticPath(vehicleDirectionNode, 0, -dBack, turnEndNode, self.turnEndXOffset, endZOffset + dBack, self.turningRadius) - self:applyTightTurnOffset(1000) + self:applyTightTurnOffset() local ixBeforeEndingTurnSection = self.course:getNumberOfWaypoints() self.course, endingTurnLength = self:adjustCourseToFitField(self.course, dBack, ixBeforeEndingTurnSection) else - self:applyTightTurnOffset(1000) + self:applyTightTurnOffset() endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, steeringLength, self.tightTurnOffsetEnabled) end TurnManeuver.setLowerImplements(self.course, endingTurnLength, true) @@ -411,6 +411,7 @@ function TowedDubinsTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode local implementRadius = AIUtil.getImplementRadiusFromTractorRadius(turningRadius, steeringLength) local xOffset = turningRadius - implementRadius self.turnEndXOffset = turnContext:isLeftTurn() and -xOffset or xOffset + self.turnEndXOffset = 0 self:debug('Towed implement, offsetting turn end %.1f to accommodate tight turn, implement radius %.1f ', xOffset, implementRadius) AnalyticTurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) From 1bdde1eb414f34071cdba0ce06dba0d6a0ef9845 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Mon, 9 Dec 2024 13:02:16 -0500 Subject: [PATCH 05/12] fix: callstack in shop while selecting other variant --- scripts/specializations/CpCourseManager.lua | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/scripts/specializations/CpCourseManager.lua b/scripts/specializations/CpCourseManager.lua index f941826f..35ea9220 100644 --- a/scripts/specializations/CpCourseManager.lua +++ b/scripts/specializations/CpCourseManager.lua @@ -217,8 +217,6 @@ end function CpCourseManager:addCourse(course,noEventSend) local spec = self.spec_cpCourseManager - -- reset temporary offset field course, this will be regenerated based on the current settings when the job starts - spec.offsetFieldWorkCourse = nil course:setVehicle(self) table.insert(spec.courses,course) SpecializationUtil.raiseEvent(self,"onCpCourseChange",course,noEventSend) @@ -226,10 +224,11 @@ end function CpCourseManager:resetCourses() local spec = self.spec_cpCourseManager - spec.offsetFieldWorkCourse = nil - spec.courses = {} - spec.assignedCoursesID = nil - SpecializationUtil.raiseEvent(self,"onCpCourseChange") + if spec.courses then + spec.courses = {} + spec.assignedCoursesID = nil + SpecializationUtil.raiseEvent(self,"onCpCourseChange") + end end function CpCourseManager:resetCpCoursesFromGui() From 98cf8b5b0d2dede235812f0fa5b26828e3620cc5 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Thu, 12 Dec 2024 13:52:44 -0500 Subject: [PATCH 06/12] fix: apply tight turn offset to end of course --- scripts/ai/turns/AITurn.lua | 13 +++++------- scripts/ai/turns/TurnContext.lua | 4 ++-- scripts/ai/turns/TurnManeuver.lua | 35 ++++++++++++++----------------- 3 files changed, 23 insertions(+), 29 deletions(-) diff --git a/scripts/ai/turns/AITurn.lua b/scripts/ai/turns/AITurn.lua index 45c896a0..885f9071 100644 --- a/scripts/ai/turns/AITurn.lua +++ b/scripts/ai/turns/AITurn.lua @@ -728,13 +728,8 @@ function CourseTurn:generateCalculatedTurn() -- TODO: the generated Dubins turn may not fit on the field and we we'll move it back, forcing the -- vehicle to reverse at the start and at the end of the turn. This will be very slow, and if the -- vehicle is able to reverse, a Reeds-Shepp would be lot faster - if self.steeringLength > 0 then - turnManeuver = TowedDubinsTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), - self.turningRadius, self.workWidth, self.steeringLength, distanceToFieldEdge) - else - turnManeuver = DubinsTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), - self.turningRadius, self.workWidth, self.steeringLength, distanceToFieldEdge) - end + turnManeuver = DubinsTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), + self.turningRadius, self.workWidth, self.steeringLength, distanceToFieldEdge) else turnManeuver = ReedsSheppTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), self.turningRadius, self.workWidth, self.steeringLength, distanceToFieldEdge) @@ -774,7 +769,8 @@ function CourseTurn:onPathfindingDone(path) self.turnCourse = Course(self.vehicle, CpMathUtil.pointsToGameInPlace(path), true) -- make sure we use tight turn offset towards the end of the course so a towed implement is aligned with the new row self.turnCourse:setUseTightTurnOffsetForLastWaypoints(15) - local endingTurnLength = self.turnContext:appendEndingTurnCourse(self.turnCourse, nil, true) + local endingTurnLength = self.turnContext:appendEndingTurnCourse(self.turnCourse, nil) + self.turnCourse:setUseTightTurnOffsetForLastWaypoints(endingTurnLength) local x = AIUtil.getDirectionNodeToReverserNodeOffset(self.vehicle) self:debug('Extending course at direction switch for reversing to %.1f m (or at least 1m)', -x) self.turnCourse:adjustForReversing(math.max(1, -x)) @@ -1024,6 +1020,7 @@ function StartRowOnly:init(vehicle, driveStrategy, ppc, turnContext, startRowCou self.turnCourse:setUseTightTurnOffsetForLastWaypoints(15) -- add a turn ending section into the row to make sure the implements are lowered correctly local endingTurnLength = self.turnContext:appendEndingTurnCourse(self.turnCourse, 3, true) + self.turnCourse:setUseTightTurnOffsetForLastWaypoints(endingTurnLength) TurnManeuver.setLowerImplements(self.turnCourse, endingTurnLength, true) self.turnCourse:adjustForReversing(2) self.state = self.states.DRIVING_TO_ROW diff --git a/scripts/ai/turns/TurnContext.lua b/scripts/ai/turns/TurnContext.lua index df2c9209..c00846c0 100644 --- a/scripts/ai/turns/TurnContext.lua +++ b/scripts/ai/turns/TurnContext.lua @@ -383,7 +383,7 @@ end ---@param extraLength number add so many meters to the calculated course (for example to allow towed implements to align --- before reversing) ---@return number length added to the course in meters -function TurnContext:appendEndingTurnCourse(course, extraLength, useTightTurnOffset) +function TurnContext:appendEndingTurnCourse(course, extraLength) -- make sure course reaches the front marker node so end it well behind that node local _, _, dzFrontMarker = course:getWaypointLocalPosition(self.vehicleAtTurnEndNode, course:getNumberOfWaypoints()) local _, _, dzWorkStart = course:getWaypointLocalPosition(self.workStartNode, course:getNumberOfWaypoints()) @@ -399,7 +399,7 @@ function TurnContext:appendEndingTurnCourse(course, extraLength, useTightTurnOff dzFrontMarker, dzWorkStart, extraLength) for d = math.min(dzFrontMarker, dzWorkStart) + 1, extraLength, 1 do local x, y, z = localToWorld(startNode, 0, 0, d) - table.insert(waypoints, {x = x, y = y, z = z, useTightTurnOffset = useTightTurnOffset or nil}) + table.insert(waypoints, {x = x, y = y, z = z}) end local oldLength = course:getLength() course:appendWaypoints(waypoints) diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua index c23ae470..bdf58d78 100644 --- a/scripts/ai/turns/TurnManeuver.lua +++ b/scripts/ai/turns/TurnManeuver.lua @@ -287,7 +287,7 @@ function TurnManeuver:adjustCourseToFitField(course, dBack, ixBeforeEndingTurnSe local forwardAfterTurn = Course.createFromNode(self.vehicle, self.turnContext.vehicleAtTurnEndNode, 0, dFromTurnEnd + 1 + self.steeringLength / 2, dFromTurnEnd + 1 + self.steeringLength, 0.8, false) courseWithReversing:append(forwardAfterTurn) - --self:applyTightTurnOffset(forwardAfterTurn:getLength()) + self:applyTightTurnOffset(forwardAfterTurn:getLength()) -- allow early direction change when aligned TurnManeuver.setTurnControlForLastWaypoints(courseWithReversing, forwardAfterTurn:getLength(), TurnManeuver.CHANGE_DIRECTION_WHEN_ALIGNED, true, true) @@ -308,7 +308,7 @@ function TurnManeuver:adjustCourseToFitField(course, dBack, ixBeforeEndingTurnSe local forwardAfterTurn = Course.createFromNode(self.vehicle, self.turnContext.workStartNode, 0, dFromWorkStart, 1, 0.8, false) courseWithReversing:append(forwardAfterTurn) - --self:applyTightTurnOffset(forwardAfterTurn:getLength()) + self:applyTightTurnOffset(forwardAfterTurn:getLength()) TurnManeuver.setTurnControlForLastWaypoints(courseWithReversing, forwardAfterTurn:getLength(), TurnManeuver.CHANGE_DIRECTION_WHEN_ALIGNED, true, true) end @@ -318,14 +318,15 @@ function TurnManeuver:adjustCourseToFitField(course, dBack, ixBeforeEndingTurnSe endingTurnLength = reverseAfterTurn:getLength() else self:debug('Reverse to work start not needed') - endingTurnLength = self.turnContext:appendEndingTurnCourse(courseWithReversing, self.steeringLength, self.tightTurnOffsetEnabled) + endingTurnLength = self.turnContext:appendEndingTurnCourse(courseWithReversing, self.steeringLength) + self:applyTightTurnOffset(endingTurnLength) end return courseWithReversing, endingTurnLength end function TurnManeuver:applyTightTurnOffset(length) if self.tightTurnOffsetEnabled then - -- use the default length (a quarter circle) unless there is a configured value + -- use the default length (a half circle) unless there is a configured value length = length or self.turningRadius * math.pi self.course:setUseTightTurnOffsetForLastWaypoints( g_vehicleConfigurations:getRecursively(self.vehicle, 'tightTurnOffsetDistanceInTurns') or length) @@ -340,9 +341,12 @@ function AnalyticTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, t local turnEndNode, endZOffset = self.turnContext:getTurnEndNodeAndOffsets(self.steeringLength) local _, _, dz = localToLocal(vehicleDirectionNode, turnEndNode, 0, 0, 0) - -- zOffset from the turn end (work start). If there is a zOffset in the turn, that is, the turn end is behind the - -- turn start due to an angled headland, we still want to make the complete 180 turn es close to the field edge - -- as we can, so a towed implement, with an offset arc is turned 180 as soon as possible and has time to align + -- zOffset from the turn end (work start). If there is a negative zOffset in the turn, that is, the turn end is behind the + -- turn start due to an angled headland, we still want to make the complete 180 turn as close to the field edge + -- as we can, so a towed implement, with an offset arc is turned 180 as soon as possible and has time to align. + -- This way, the tight turn offset can make its magic during the 180 turn. Otherwise, the Dubins generated will split + -- the 180 into two turns, one over 120 at the turn start, and one less than 60 at the turn end. This latter one + -- is not enough direction change for the tight turn offset to work. endZOffset = math.min(dz, endZOffset) self:debug('r=%.1f, w=%.1f, steeringLength=%.1f, distanceToFieldEdge=%.1f, goalOffset=%.1f, dz=%.1f', turningRadius, workWidth, steeringLength, distanceToFieldEdge, endZOffset, dz) @@ -359,7 +363,8 @@ function AnalyticTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, t self.course, endingTurnLength = self:adjustCourseToFitField(self.course, dBack, ixBeforeEndingTurnSection) else self:applyTightTurnOffset() - endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, steeringLength, self.tightTurnOffsetEnabled) + endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, steeringLength) + self:applyTightTurnOffset(endingTurnLength) end TurnManeuver.setLowerImplements(self.course, endingTurnLength, true) end @@ -402,6 +407,9 @@ function DubinsTurnManeuver:findAnalyticPath(startNode, startXOffset, startZOffs return Course.createFromAnalyticPath(self.vehicle, path, true) end +-- This is an experiment to create turns with towed implements that better align with the next row. +-- Instead of relying on the dynamic tight turn offset, we offset the turn end already while generating the turn +-- to get the implement closer to the next row. ---@class TowedDubinsTurnManeuver : DubinsTurnManeuver TowedDubinsTurnManeuver = CpObject(DubinsTurnManeuver) function TowedDubinsTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, @@ -411,22 +419,11 @@ function TowedDubinsTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode local implementRadius = AIUtil.getImplementRadiusFromTractorRadius(turningRadius, steeringLength) local xOffset = turningRadius - implementRadius self.turnEndXOffset = turnContext:isLeftTurn() and -xOffset or xOffset - self.turnEndXOffset = 0 self:debug('Towed implement, offsetting turn end %.1f to accommodate tight turn, implement radius %.1f ', xOffset, implementRadius) AnalyticTurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) end -function TowedDubinsTurnManeuver:calculateTractorCourse(course) - local offsetX = 0 - for ix, wp in ipairs(course.waypoints) do - offsetX = AIUtil.calculateTightTurnOffsetForTurnManeuver(self.vehicle, self.steeringLength, course, ix, offsetX) - wp:setOffsetPosition(offsetX, 0) - end - course:enrichWaypointData() - return course -end - ---@class LeftTurnReedsSheppSolver : ReedsSheppSolver LeftTurnReedsSheppSolver = CpObject(ReedsSheppSolver) function LeftTurnReedsSheppSolver:solve(start, goal, turnRadius) From c730aab18f7c0143229714a9d5cf593c4d8a2072 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Fri, 13 Dec 2024 10:45:55 -0500 Subject: [PATCH 07/12] feat: tight turn offset for articulated vehicles Enable tight turn offset for these, just use more smoothing. --- scripts/ai/AIUtil.lua | 11 ++++++----- scripts/ai/turns/AITurn.lua | 8 +++----- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/scripts/ai/AIUtil.lua b/scripts/ai/AIUtil.lua index cfa6c6e2..bf0e6dc4 100644 --- a/scripts/ai/AIUtil.lua +++ b/scripts/ai/AIUtil.lua @@ -110,7 +110,11 @@ function AIUtil.calculateTightTurnOffsetForTurnManeuver(vehicle, steeringLength, local tightTurnOffset local function smoothOffset(offset) - return (offset + 4 * (previousOffset or 0 )) / 5 + -- smooth more for articulated axis or track vehicle + -- as those usually have a very small turn radius anyway, causing jackknifing + -- TODO: use the vehicle's solo radius instead? + local factor = AIUtil.hasArticulatedAxis(vehicle) and 6 or 4 + return (offset + factor * (previousOffset or 0 )) / (factor + 1) end -- first of all, does the current waypoint have radius data? @@ -119,10 +123,7 @@ function AIUtil.calculateTightTurnOffsetForTurnManeuver(vehicle, steeringLength, return smoothOffset(0) end - -- Ok, looks like a tight turn, so we need to move a bit left or right of the course - -- to keep the tool on the course. Use a little less than the calculated, this is purely empirical and should probably - -- be reviewed why the calculated one seems to overshoot. - local offset = 1 * AIUtil.getTractorRadiusFromImplementRadius(r, steeringLength) - r + local offset = AIUtil.getTractorRadiusFromImplementRadius(r, steeringLength) - r if offset ~= offset then -- check for nan return smoothOffset(0) diff --git a/scripts/ai/turns/AITurn.lua b/scripts/ai/turns/AITurn.lua index 885f9071..e08c9d7c 100644 --- a/scripts/ai/turns/AITurn.lua +++ b/scripts/ai/turns/AITurn.lua @@ -734,9 +734,8 @@ function CourseTurn:generateCalculatedTurn() turnManeuver = ReedsSheppTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), self.turningRadius, self.workWidth, self.steeringLength, distanceToFieldEdge) end - -- only use tight turn offset if we are towing something and not an articulated axis or track vehicle - -- as those usually have a very small turn radius anyway, causing jackknifing - if self.steeringLength > 0 and not AIUtil.hasArticulatedAxis(self.vehicle) then + -- only use tight turn offset if we are towing something + if self.steeringLength > 0 then self:debug('Enabling tight turn offset') self.enableTightTurnOffset = true end @@ -1014,8 +1013,7 @@ function StartRowOnly:init(vehicle, driveStrategy, ppc, turnContext, startRowCou self.forceTightTurnOffset = false local _, steeringLength = AIUtil.getSteeringParameters(self.vehicle) - self.enableTightTurnOffset = steeringLength > 0 and not AIUtil.hasArticulatedAxis(self.vehicle) - + self.enableTightTurnOffset = steeringLength > 0 -- TODO: do we need tight turn offset here? self.turnCourse:setUseTightTurnOffsetForLastWaypoints(15) -- add a turn ending section into the row to make sure the implements are lowered correctly From ee333db6652dc8521f630fcfc224e782567c2a8f Mon Sep 17 00:00:00 2001 From: Tensuko Date: Fri, 13 Dec 2024 19:36:00 +0100 Subject: [PATCH 08/12] Update VehicleConfigurations.xml --- config/VehicleConfigurations.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/config/VehicleConfigurations.xml b/config/VehicleConfigurations.xml index 388b1cea..d36fd84b 100644 --- a/config/VehicleConfigurations.xml +++ b/config/VehicleConfigurations.xml @@ -505,7 +505,6 @@ You can define the following custom settings: baleCollectorOffset = "1.6" /> Date: Sat, 14 Dec 2024 11:37:47 -0500 Subject: [PATCH 09/12] feat: tight turn offset improvements --- scripts/Course.lua | 21 +++++++++++----- scripts/Waypoint.lua | 8 +++++++ scripts/ai/turns/AITurn.lua | 2 +- scripts/ai/turns/TurnManeuver.lua | 40 +++++++++++++++++++++++++++++-- 4 files changed, 62 insertions(+), 9 deletions(-) diff --git a/scripts/Course.lua b/scripts/Course.lua index 75f395a0..e921fc4a 100644 --- a/scripts/Course.lua +++ b/scripts/Course.lua @@ -273,9 +273,12 @@ function Course:enrichWaypointData(startIx) 'Course with %d waypoints created/updated, %.1f meters, %d turns', #self.waypoints, self.length, self.totalTurns) end +function Course:getDeltaAngle(ix) + return CpMathUtil.getDeltaAngle(self.waypoints[ix].yRot, self.waypoints[ix - 1].yRot) +end + function Course:calculateSignedRadius(ix) - local deltaAngle = CpMathUtil.getDeltaAngle(self.waypoints[ix].yRot, self.waypoints[ix - 1].yRot) - return CpMathUtil.divide(self:getDistanceToNextWaypoint(ix), (2 * math.sin(deltaAngle / 2))) + return CpMathUtil.divide(self:getDistanceToNextWaypoint(ix), (2 * math.sin(self:getDeltaAngle(ix) / 2))) end function Course:calculateRadius(ix) @@ -493,8 +496,12 @@ function Course:getTurnControls(ix) return self.waypoints[ix].turnControls end -function Course:useTightTurnOffset(ix) - return self.waypoints[ix].useTightTurnOffset +function Course:setUseTightTurnOffset(ix) + return self.waypoints[ix]:setUseTightTurnOffset() +end + +function Course:getUseTightTurnOffset(ix) + return self.waypoints[ix]:getUseTightTurnOffset() end --- Returns the position of the waypoint at ix with the current offset applied. @@ -1230,7 +1237,9 @@ function Course:draw() Utils.renderTextAtWorldPosition(x, y + 3.2, z, tostring(i), getCorrectTextSize(0.012), 0, color) if i < self:getNumberOfWaypoints() then local nx, ny, nz = self:getWaypointPosition(i + 1) - DebugUtil.drawDebugLine(x, y + 3, z, nx, ny + 3, nz, 0, 0, 100) + -- cyan, darker blue with tight turn offset + color = self:getUseTightTurnOffset(i) and {0, 0, 0.5} or {0, 0.5, 0.5} + DebugUtil.drawDebugLine(x, y + 3, z, nx, ny + 3, nz, table.unpack(color)) end end end @@ -1275,7 +1284,7 @@ end function Course:setUseTightTurnOffsetForLastWaypoints(d) self:executeFunctionForLastWaypoints(d, function(wp) - wp.useTightTurnOffset = true + wp:setUseTightTurnOffset() end) end diff --git a/scripts/Waypoint.lua b/scripts/Waypoint.lua index a96b1ff8..e08b386f 100644 --- a/scripts/Waypoint.lua +++ b/scripts/Waypoint.lua @@ -285,6 +285,14 @@ function Waypoint:setOnConnectingPath(onConnectingPath) self.attributes:setOnConnectingPath(onConnectingPath) end +function Waypoint:setUseTightTurnOffset() + self.useTightTurnOffset = true +end + +function Waypoint:getUseTightTurnOffset() + return self.useTightTurnOffset +end + function Waypoint:copyRowData(other) self.attributes.rowNumber = other.attributes.rowNumber self.attributes.leftSideWorked = other.attributes.leftSideWorked diff --git a/scripts/ai/turns/AITurn.lua b/scripts/ai/turns/AITurn.lua index e08c9d7c..fd50e5d8 100644 --- a/scripts/ai/turns/AITurn.lua +++ b/scripts/ai/turns/AITurn.lua @@ -649,7 +649,7 @@ end function CourseTurn:onWaypointChange(ix) AITurn.onWaypointChange(self, ix) if self.turnCourse then - if self.forceTightTurnOffset or (self.enableTightTurnOffset and self.turnCourse:useTightTurnOffset(ix)) then + if self.forceTightTurnOffset or (self.enableTightTurnOffset and self.turnCourse:getUseTightTurnOffset(ix)) then -- adjust the course a bit to the outside in a curve to keep a towed implement on the course self.tightTurnOffset = AIUtil.calculateTightTurnOffsetForTurnManeuver(self.vehicle, self.steeringLength, self.turnCourse, self.turnCourse:getCurrentWaypointIx(), self.tightTurnOffset) diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua index bdf58d78..8a421620 100644 --- a/scripts/ai/turns/TurnManeuver.lua +++ b/scripts/ai/turns/TurnManeuver.lua @@ -333,6 +333,42 @@ function TurnManeuver:applyTightTurnOffset(length) end end +-- Apply tight turn offset to an analytically generated 180 turn section. The goal is to align a towed +-- implement properly with the next row +function TurnManeuver:applyTightTurnOffsetToAnalyticPath(course) + if self.tightTurnOffsetEnabled then + local totalDeltaAngle = 0 + local totalDistance = 0 + local previousDeltaAngle = course:getDeltaAngle(course:getNumberOfWaypoints()) + for i = course:getNumberOfWaypoints(), 2, -1 do + course:setUseTightTurnOffset(i) + local deltaAngle = course:getDeltaAngle(i) + totalDeltaAngle = totalDeltaAngle + deltaAngle + -- check for configured distance + totalDistance = totalDistance + course:getDistanceToNextWaypoint(i - 1) + if totalDistance > g_vehicleConfigurations:getRecursively(self.vehicle, 'tightTurnOffsetDistanceInTurns') then + self:debug('Total distance %.1f > configured, stop applying tight turn offset', totalDistance) + break + end + -- Check for direction change: this is to have offset only at the foot of an omega turn and not + -- around the body, only when the foot is significantly narrower than the body. + -- This is for the case when the turn diameter is significantly bigger than the working width + if math.abs(totalDeltaAngle) > math.pi / 6 and + math.abs(deltaAngle) > 0.01 and math.sign(deltaAngle) ~= math.sign(previousDeltaAngle) then + self:debug('Curve direction change at %d (total delta angle %.1f, stop applying tight turn offset', + i, math.deg(totalDeltaAngle)) + break + end + -- in all other cases, apply to half circle + if math.abs(totalDeltaAngle) > math.pi / 2 then + self:debug('Total direction change more than 90, stop applying tight turn offset') + break + end + previousDeltaAngle = deltaAngle + end + end +end + ---@class AnalyticTurnManeuver : TurnManeuver AnalyticTurnManeuver = CpObject(TurnManeuver) function AnalyticTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) @@ -358,11 +394,11 @@ function AnalyticTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, t dBack = dBack < 2 and 2 or dBack self:debug('Not enough space on field, regenerating course back %.1f meters', dBack) self.course = self:findAnalyticPath(vehicleDirectionNode, 0, -dBack, turnEndNode, self.turnEndXOffset, endZOffset + dBack, self.turningRadius) - self:applyTightTurnOffset() + self:applyTightTurnOffsetToAnalyticPath(self.course) local ixBeforeEndingTurnSection = self.course:getNumberOfWaypoints() self.course, endingTurnLength = self:adjustCourseToFitField(self.course, dBack, ixBeforeEndingTurnSection) else - self:applyTightTurnOffset() + self:applyTightTurnOffsetToAnalyticPath(self.course) endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, steeringLength) self:applyTightTurnOffset(endingTurnLength) end From 10ad0fa5fc0d04ee2dd6bf15f171839c01000285 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 14 Dec 2024 12:09:54 -0500 Subject: [PATCH 10/12] fix: stupid nil error --- scripts/ai/turns/TurnManeuver.lua | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua index 8a421620..1f28cb6d 100644 --- a/scripts/ai/turns/TurnManeuver.lua +++ b/scripts/ai/turns/TurnManeuver.lua @@ -346,8 +346,9 @@ function TurnManeuver:applyTightTurnOffsetToAnalyticPath(course) totalDeltaAngle = totalDeltaAngle + deltaAngle -- check for configured distance totalDistance = totalDistance + course:getDistanceToNextWaypoint(i - 1) - if totalDistance > g_vehicleConfigurations:getRecursively(self.vehicle, 'tightTurnOffsetDistanceInTurns') then - self:debug('Total distance %.1f > configured, stop applying tight turn offset', totalDistance) + if totalDistance > + (g_vehicleConfigurations:getRecursively(self.vehicle, 'tightTurnOffsetDistanceInTurns') or math.huge) then + selff:debug('Total distance %.1f > configured, stop applying tight turn offset', totalDistance) break end -- Check for direction change: this is to have offset only at the foot of an omega turn and not From 8233f93281a4f555ddc011338a8c79cfb76a723c Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 14 Dec 2024 12:23:15 -0500 Subject: [PATCH 11/12] fix: another stupid typo --- scripts/ai/turns/TurnManeuver.lua | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua index 1f28cb6d..d249e66a 100644 --- a/scripts/ai/turns/TurnManeuver.lua +++ b/scripts/ai/turns/TurnManeuver.lua @@ -348,7 +348,7 @@ function TurnManeuver:applyTightTurnOffsetToAnalyticPath(course) totalDistance = totalDistance + course:getDistanceToNextWaypoint(i - 1) if totalDistance > (g_vehicleConfigurations:getRecursively(self.vehicle, 'tightTurnOffsetDistanceInTurns') or math.huge) then - selff:debug('Total distance %.1f > configured, stop applying tight turn offset', totalDistance) + self:debug('Total distance %.1f > configured, stop applying tight turn offset', totalDistance) break end -- Check for direction change: this is to have offset only at the foot of an omega turn and not From fe372c57d39d60cba6ca1b2ec6e5d34d064fef69 Mon Sep 17 00:00:00 2001 From: Tensuko Date: Sat, 14 Dec 2024 18:47:53 +0100 Subject: [PATCH 12/12] Tool configs adjusted --- config/VehicleConfigurations.xml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/config/VehicleConfigurations.xml b/config/VehicleConfigurations.xml index d36fd84b..712a8126 100644 --- a/config/VehicleConfigurations.xml +++ b/config/VehicleConfigurations.xml @@ -246,6 +246,10 @@ You can define the following custom settings: raiseLate = "true" lowerEarly = "true" /> + +