Obstacle avoidance refactoring
This commit is contained in:
parent
4e7ef35280
commit
a6a5baa352
@ -32,9 +32,6 @@ public interface DubinsPose2D : DoubleVector2D {
|
|||||||
require(vector2D.x != 0.0 || vector2D.y != 0.0) { "Can't get bearing of zero vector" }
|
require(vector2D.x != 0.0 || vector2D.y != 0.0) { "Can't get bearing of zero vector" }
|
||||||
return atan2(vector2D.y, vector2D.x).radians
|
return atan2(vector2D.y, vector2D.x).radians
|
||||||
}
|
}
|
||||||
|
|
||||||
public fun of(point: DoubleVector2D, direction: DoubleVector2D): DubinsPose2D =
|
|
||||||
DubinsPose2D(point, vectorToBearing(direction))
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -72,4 +69,10 @@ public object DubinsPose2DSerializer : KSerializer<DubinsPose2D> {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
public fun DubinsPose2D(coordinate: DoubleVector2D, theta: Angle): DubinsPose2D = DubinsPose2DImpl(coordinate, theta)
|
public fun DubinsPose2D(coordinate: DoubleVector2D, bearing: Angle): DubinsPose2D = DubinsPose2DImpl(coordinate, bearing)
|
||||||
|
|
||||||
|
public fun DubinsPose2D(point: DoubleVector2D, direction: DoubleVector2D): DubinsPose2D =
|
||||||
|
DubinsPose2D(point, DubinsPose2D.vectorToBearing(direction))
|
||||||
|
|
||||||
|
public fun DubinsPose2D(x: Number, y: Number, bearing: Angle): DubinsPose2D =
|
||||||
|
DubinsPose2DImpl(Euclidean2DSpace.vector(x, y), bearing)
|
@ -17,8 +17,10 @@ public interface Obstacle {
|
|||||||
|
|
||||||
public fun intersects(segment: LineSegment2D): Boolean
|
public fun intersects(segment: LineSegment2D): Boolean
|
||||||
|
|
||||||
|
public fun intersects(circle: Circle2D): Boolean
|
||||||
|
|
||||||
public companion object {
|
public companion object {
|
||||||
public fun allPathsAvoiding(
|
public fun avoidObstacles(
|
||||||
start: DubinsPose2D,
|
start: DubinsPose2D,
|
||||||
finish: DubinsPose2D,
|
finish: DubinsPose2D,
|
||||||
trajectoryRadius: Double,
|
trajectoryRadius: Double,
|
||||||
@ -30,7 +32,7 @@ public interface Obstacle {
|
|||||||
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
|
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
|
||||||
}
|
}
|
||||||
|
|
||||||
public fun allPathsAvoiding(
|
public fun avoidPolygons(
|
||||||
start: DubinsPose2D,
|
start: DubinsPose2D,
|
||||||
finish: DubinsPose2D,
|
finish: DubinsPose2D,
|
||||||
trajectoryRadius: Double,
|
trajectoryRadius: Double,
|
||||||
@ -42,11 +44,43 @@ public interface Obstacle {
|
|||||||
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
|
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public fun avoidObstacles(
|
||||||
|
start: DubinsPose2D,
|
||||||
|
finish: DubinsPose2D,
|
||||||
|
trajectoryRadius: Double,
|
||||||
|
obstacles: Collection<Obstacle>,
|
||||||
|
): List<CompositeTrajectory2D> {
|
||||||
|
val obstacleShells: List<ObstacleShell> = obstacles.map { polygon ->
|
||||||
|
ObstacleShell(polygon.circles)
|
||||||
|
}
|
||||||
|
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
|
||||||
|
}
|
||||||
|
|
||||||
|
public fun avoidPolygons(
|
||||||
|
start: DubinsPose2D,
|
||||||
|
finish: DubinsPose2D,
|
||||||
|
trajectoryRadius: Double,
|
||||||
|
obstacles: Collection<Polygon<Double>>,
|
||||||
|
): List<CompositeTrajectory2D> {
|
||||||
|
val obstacleShells: List<ObstacleShell> = obstacles.map { polygon ->
|
||||||
|
ObstacleShell(polygon.points.map { Circle2D(it, trajectoryRadius) })
|
||||||
|
}
|
||||||
|
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public fun Obstacle.intersectsTrajectory(trajectory: Trajectory2D): Boolean = when (trajectory) {
|
||||||
|
is CircleTrajectory2D -> intersects(trajectory.circle)
|
||||||
|
is StraightTrajectory2D -> intersects(trajectory)
|
||||||
|
is CompositeTrajectory2D -> trajectory.segments.any { intersectsTrajectory(it) }
|
||||||
|
}
|
||||||
|
|
||||||
public fun Obstacle(vararg circles: Circle2D): Obstacle = ObstacleShell(listOf(*circles))
|
public fun Obstacle(vararg circles: Circle2D): Obstacle = ObstacleShell(listOf(*circles))
|
||||||
|
|
||||||
|
public fun Obstacle(points: List<Vector2D<Double>>, radius: Double): Obstacle =
|
||||||
|
ObstacleShell(points.map { Circle2D(it, radius) })
|
||||||
|
|
||||||
//public fun Trajectory2D.intersects(
|
//public fun Trajectory2D.intersects(
|
||||||
// polygon: Polygon<Double>,
|
// polygon: Polygon<Double>,
|
||||||
// radius: Double,
|
// radius: Double,
|
||||||
|
@ -14,7 +14,7 @@ internal data class ObstacleNode(
|
|||||||
val obstacle: Obstacle,
|
val obstacle: Obstacle,
|
||||||
val nodeIndex: Int,
|
val nodeIndex: Int,
|
||||||
val direction: Trajectory2D.Direction,
|
val direction: Trajectory2D.Direction,
|
||||||
){
|
) {
|
||||||
val circle get() = obstacle.circles[nodeIndex]
|
val circle get() = obstacle.circles[nodeIndex]
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -22,7 +22,7 @@ internal data class ObstacleTangent(
|
|||||||
val lineSegment: LineSegment2D,
|
val lineSegment: LineSegment2D,
|
||||||
val beginNode: ObstacleNode,
|
val beginNode: ObstacleNode,
|
||||||
val endNode: ObstacleNode,
|
val endNode: ObstacleNode,
|
||||||
) : LineSegment2D by lineSegment{
|
) : LineSegment2D by lineSegment {
|
||||||
val startCircle get() = beginNode.circle
|
val startCircle get() = beginNode.circle
|
||||||
val startDirection get() = beginNode.direction
|
val startDirection get() = beginNode.direction
|
||||||
val endCircle get() = endNode.circle
|
val endCircle get() = endNode.circle
|
||||||
@ -164,10 +164,13 @@ internal class ObstacleShell(
|
|||||||
shell.any { tangent -> segment.intersectsSegment(tangent) }
|
shell.any { tangent -> segment.intersectsSegment(tangent) }
|
||||||
|| circles.any { circle -> segment.intersectsCircle(circle) }
|
|| circles.any { circle -> segment.intersectsCircle(circle) }
|
||||||
|
|
||||||
|
override fun intersects(circle: Circle2D): Boolean =
|
||||||
|
shell.any{tangent -> tangent.intersectsCircle(circle) }
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Tangent to next obstacle node in given direction
|
* Tangent to next obstacle node in given direction
|
||||||
*/
|
*/
|
||||||
fun nextTangent(circleIndex:Int, direction: Trajectory2D.Direction): ObstacleTangent {
|
fun nextTangent(circleIndex: Int, direction: Trajectory2D.Direction): ObstacleTangent {
|
||||||
if (circleIndex == -1) error("Circle does not belong to this tangent")
|
if (circleIndex == -1) error("Circle does not belong to this tangent")
|
||||||
|
|
||||||
val nextCircleIndex = if (direction == this.shellDirection) {
|
val nextCircleIndex = if (direction == this.shellDirection) {
|
||||||
@ -181,15 +184,18 @@ internal class ObstacleShell(
|
|||||||
shell[nextCircleIndex].end,
|
shell[nextCircleIndex].end,
|
||||||
shell[nextCircleIndex].begin
|
shell[nextCircleIndex].begin
|
||||||
),
|
),
|
||||||
ObstacleNode(this,circleIndex, direction),
|
ObstacleNode(this, circleIndex, direction),
|
||||||
ObstacleNode(this,nextCircleIndex, direction),
|
ObstacleNode(this, nextCircleIndex, direction),
|
||||||
)
|
)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* All tangents in given direction
|
||||||
|
*/
|
||||||
internal fun tangentsAlong(
|
internal fun tangentsAlong(
|
||||||
initialCircleIndex: Int,
|
initialCircleIndex: Int,
|
||||||
direction: Trajectory2D.Direction,
|
|
||||||
finalCircleIndex: Int,
|
finalCircleIndex: Int,
|
||||||
|
direction: Trajectory2D.Direction,
|
||||||
): List<ObstacleTangent> {
|
): List<ObstacleTangent> {
|
||||||
return buildList {
|
return buildList {
|
||||||
var currentIndex = initialCircleIndex
|
var currentIndex = initialCircleIndex
|
||||||
@ -389,8 +395,7 @@ internal fun findAllPaths(
|
|||||||
finalRadius: Double,
|
finalRadius: Double,
|
||||||
obstacles: List<ObstacleShell>,
|
obstacles: List<ObstacleShell>,
|
||||||
): List<CompositeTrajectory2D> {
|
): List<CompositeTrajectory2D> {
|
||||||
fun DubinsPose2D.direction() =
|
fun DubinsPose2D.direction() = Euclidean2DSpace.vector(cos(bearing), sin(bearing))
|
||||||
Euclidean2DSpace.vector(space.kscience.kmath.geometry.cos(bearing), space.kscience.kmath.geometry.sin(bearing))
|
|
||||||
|
|
||||||
// two circles for the initial point
|
// two circles for the initial point
|
||||||
val initialCircles = constructTangentCircles(
|
val initialCircles = constructTangentCircles(
|
||||||
@ -439,11 +444,12 @@ internal fun findAllPaths(
|
|||||||
if (currentObstacle == finalObstacle) {
|
if (currentObstacle == finalObstacle) {
|
||||||
newPaths.add(tangentPath)
|
newPaths.add(tangentPath)
|
||||||
} else {
|
} else {
|
||||||
val tangentToFinal: ObstacleTangent = outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type(
|
val tangentToFinal: ObstacleTangent =
|
||||||
currentDirection,
|
outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type(
|
||||||
Trajectory2D.S,
|
currentDirection,
|
||||||
j
|
Trajectory2D.S,
|
||||||
)] ?: break
|
j
|
||||||
|
)] ?: break
|
||||||
|
|
||||||
// searching for the nearest obstacle that intersects with the direct path
|
// searching for the nearest obstacle that intersects with the direct path
|
||||||
val nextObstacle = sortedObstacles(currentObstacle, obstacles).find { obstacle ->
|
val nextObstacle = sortedObstacles(currentObstacle, obstacles).find { obstacle ->
|
||||||
@ -463,7 +469,7 @@ internal fun findAllPaths(
|
|||||||
}.values
|
}.values
|
||||||
|
|
||||||
for (tangent in nextTangents) {
|
for (tangent in nextTangents) {
|
||||||
val tangentsAlong: List<ObstacleTangent> = if (tangent.startCircle === tangentPath.last().endCircle) {
|
val tangentsAlong = if (tangent.startCircle === tangentPath.last().endCircle) {
|
||||||
//if the previous segment last circle is the same as first circle of the next segment
|
//if the previous segment last circle is the same as first circle of the next segment
|
||||||
|
|
||||||
//If obstacle consists of single circle, do not walk around
|
//If obstacle consists of single circle, do not walk around
|
||||||
@ -490,8 +496,8 @@ internal fun findAllPaths(
|
|||||||
if (lengthCalculated > lengthMaxPossible) {
|
if (lengthCalculated > lengthMaxPossible) {
|
||||||
currentObstacle.tangentsAlong(
|
currentObstacle.tangentsAlong(
|
||||||
currentNode.nodeIndex,
|
currentNode.nodeIndex,
|
||||||
currentDirection,
|
|
||||||
tangent.beginNode.nodeIndex,
|
tangent.beginNode.nodeIndex,
|
||||||
|
currentDirection,
|
||||||
)
|
)
|
||||||
} else {
|
} else {
|
||||||
emptyList()
|
emptyList()
|
||||||
@ -500,8 +506,8 @@ internal fun findAllPaths(
|
|||||||
} else {
|
} else {
|
||||||
currentObstacle.tangentsAlong(
|
currentObstacle.tangentsAlong(
|
||||||
currentNode.nodeIndex,
|
currentNode.nodeIndex,
|
||||||
currentDirection,
|
|
||||||
tangent.beginNode.nodeIndex,
|
tangent.beginNode.nodeIndex,
|
||||||
|
currentDirection,
|
||||||
)
|
)
|
||||||
}
|
}
|
||||||
newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent))
|
newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent))
|
||||||
@ -512,7 +518,7 @@ internal fun findAllPaths(
|
|||||||
}
|
}
|
||||||
|
|
||||||
trajectories += currentPaths.map { tangentPath ->
|
trajectories += currentPaths.map { tangentPath ->
|
||||||
val lastDirection: Trajectory2D.Direction = tangentPath.last().endDirection
|
// val lastDirection: Trajectory2D.Direction = tangentPath.last().endDirection
|
||||||
val end = Obstacle(finalCircles[j])
|
val end = Obstacle(finalCircles[j])
|
||||||
TangentPath(
|
TangentPath(
|
||||||
tangentPath.tangents +
|
tangentPath.tangents +
|
||||||
|
@ -7,6 +7,7 @@ package space.kscience.trajectory
|
|||||||
|
|
||||||
import space.kscience.kmath.geometry.Circle2D
|
import space.kscience.kmath.geometry.Circle2D
|
||||||
import space.kscience.kmath.geometry.Euclidean2DSpace.vector
|
import space.kscience.kmath.geometry.Euclidean2DSpace.vector
|
||||||
|
import space.kscience.kmath.geometry.degrees
|
||||||
import kotlin.test.Test
|
import kotlin.test.Test
|
||||||
import kotlin.test.assertEquals
|
import kotlin.test.assertEquals
|
||||||
|
|
||||||
@ -19,9 +20,9 @@ class ObstacleTest {
|
|||||||
val finalPoint = vector(20.0, 4.0)
|
val finalPoint = vector(20.0, 4.0)
|
||||||
val finalDirection = vector(1.0, -1.0)
|
val finalDirection = vector(1.0, -1.0)
|
||||||
|
|
||||||
val outputTangents = Obstacle.allPathsAvoiding(
|
val outputTangents = Obstacle.avoidObstacles(
|
||||||
DubinsPose2D.of(startPoint, startDirection),
|
DubinsPose2D(startPoint, startDirection),
|
||||||
DubinsPose2D.of(finalPoint, finalDirection),
|
DubinsPose2D(finalPoint, finalDirection),
|
||||||
startRadius,
|
startRadius,
|
||||||
Obstacle(Circle2D(vector(7.0, 1.0), 5.0))
|
Obstacle(Circle2D(vector(7.0, 1.0), 5.0))
|
||||||
)
|
)
|
||||||
@ -37,9 +38,9 @@ class ObstacleTest {
|
|||||||
val finalPoint = vector(20.0, 4.0)
|
val finalPoint = vector(20.0, 4.0)
|
||||||
val finalDirection = vector(1.0, -1.0)
|
val finalDirection = vector(1.0, -1.0)
|
||||||
|
|
||||||
val paths = Obstacle.allPathsAvoiding(
|
val paths = Obstacle.avoidObstacles(
|
||||||
DubinsPose2D.of(startPoint, startDirection),
|
DubinsPose2D(startPoint, startDirection),
|
||||||
DubinsPose2D.of(finalPoint, finalDirection),
|
DubinsPose2D(finalPoint, finalDirection),
|
||||||
radius,
|
radius,
|
||||||
Obstacle(
|
Obstacle(
|
||||||
Circle2D(vector(1.0, 6.5), 0.5),
|
Circle2D(vector(1.0, 6.5), 0.5),
|
||||||
@ -59,15 +60,15 @@ class ObstacleTest {
|
|||||||
|
|
||||||
@Test
|
@Test
|
||||||
fun nearPoints() {
|
fun nearPoints() {
|
||||||
val startPoint = vector(-1.0, 0.0)
|
val startPoint = vector(-1.0, -1.0)
|
||||||
val startDirection = vector(0.0, 1.0)
|
val startDirection = vector(0.0, 1.0)
|
||||||
val startRadius = 1.0
|
val startRadius = 1.0
|
||||||
val finalPoint = vector(0, -1)
|
val finalPoint = vector(-1, -1)
|
||||||
val finalDirection = vector(1.0, 0)
|
val finalDirection = vector(1.0, 0)
|
||||||
|
|
||||||
val paths = Obstacle.allPathsAvoiding(
|
val paths = Obstacle.avoidObstacles(
|
||||||
DubinsPose2D.of(startPoint, startDirection),
|
DubinsPose2D(startPoint, startDirection),
|
||||||
DubinsPose2D.of(finalPoint, finalDirection),
|
DubinsPose2D(finalPoint, finalDirection),
|
||||||
startRadius,
|
startRadius,
|
||||||
Obstacle(
|
Obstacle(
|
||||||
Circle2D(vector(0.0, 0.0), 1.0),
|
Circle2D(vector(0.0, 0.0), 1.0),
|
||||||
@ -81,6 +82,24 @@ class ObstacleTest {
|
|||||||
//assertEquals(28.9678224, length, 1e-6)
|
//assertEquals(28.9678224, length, 1e-6)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@Test
|
||||||
|
fun fromMap() {
|
||||||
|
|
||||||
|
val paths = Obstacle.avoidObstacles(
|
||||||
|
DubinsPose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = 3.401475378237137.degrees),
|
||||||
|
DubinsPose2D(x = 456663.8489126448, y = 2830054.1087567504, bearing = 325.32183928982727.degrees),
|
||||||
|
5000.0,
|
||||||
|
Obstacle(
|
||||||
|
Circle2D(vector(x=446088.2236175772, y=2895264.0759535935), radius=5000.0),
|
||||||
|
Circle2D(vector(x=455587.51549431164, y=2897116.5594902174), radius=5000.0),
|
||||||
|
Circle2D(vector(x=465903.08440141426, y=2893897.500160981), radius=5000.0),
|
||||||
|
Circle2D(vector(x=462421.19397653354, y=2879496.4842121634), radius=5000.0),
|
||||||
|
Circle2D(vector(x=449231.8047505464, y=2880132.403305273), radius=5000.0)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
val length = paths.minOf { it.length }
|
||||||
|
}
|
||||||
|
|
||||||
@Test
|
@Test
|
||||||
fun equalObstacles() {
|
fun equalObstacles() {
|
||||||
val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
|
val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
|
||||||
|
Loading…
Reference in New Issue
Block a user