From bd4468bf452ded16dc86ea55221afdbf31b4fece Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Mon, 24 Jul 2023 17:10:42 -0700 Subject: [PATCH 01/38] Implement the first couple Transform methods --- motionplan/tpspace/ptg.go | 5 ++ motionplan/tpspace/ptgC.go | 47 +++++++++++++++++ motionplan/tpspace/ptgCC.go | 33 +++++++++++- .../tpspace}/ptgFrame.go | 50 +++++++++---------- motionplan/tpspace/ptgGridSim.go | 1 - 5 files changed, 109 insertions(+), 27 deletions(-) rename {referenceframe => motionplan/tpspace}/ptgFrame.go (73%) diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index 2ef1e86479a..9c25b01d429 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -6,9 +6,12 @@ import ( "github.com/golang/geo/r3" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) +const floatEpsilon = 0.0001 // If floats are closer than this consider them equal + // PTG is a Parameterized Trajectory Generator, which defines how to map back and forth from cartesian space to TP-space // PTG coordinates are specified in polar coordinates (alpha, d) // One of these is needed for each sort of motion that can be done. @@ -24,6 +27,8 @@ type PTG interface { // Returns the set of trajectory nodes for alpha index K Trajectory(uint) []*TrajNode + + Transform([]referenceframe.Input) (spatialmath.Pose, error) } // PTGProvider is something able to provide a set of PTGs associsated with it. For example, a frame which precomputes diff --git a/motionplan/tpspace/ptgC.go b/motionplan/tpspace/ptgC.go index 4d0861b6181..4c463ea7384 100644 --- a/motionplan/tpspace/ptgC.go +++ b/motionplan/tpspace/ptgC.go @@ -1,7 +1,13 @@ package tpspace import ( + "fmt" "math" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" + + "github.com/golang/geo/r3" ) // ptgDiffDriveC defines a PTG family composed of circular trajectories with an alpha-dependent radius. @@ -30,3 +36,44 @@ func (ptg *ptgDiffDriveC) PTGVelocities(alpha, t, x, y, phi float64) (float64, f w := (alpha / math.Pi) * ptg.maxRPS * ptg.k return v, w, nil } + +// Transform will return the pose for the given inputs. The first input is [-pi, pi]. This corresponds to the radius of the curve, +// where 0 is straight ahead, pi is turning at min turning radius to the right, and a value between 0 and pi represents turning at a radius +// of (input/pi)*minradius. A negative value denotes turning left. The second input is the distance traveled along this arc. +func (ptg *ptgDiffDriveC) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + turnRad := ptg.maxMMPS / ptg.maxRPS + + if len(inputs) != 2 { + return nil, fmt.Errorf("ptgC takes 2 inputs, but received %d", len(inputs)) + } + alpha := inputs[0].Value + dist := inputs[1].Value + + // Check for OOB within FP error + if math.Pi - math.Abs(alpha) > floatEpsilon { + return nil, fmt.Errorf("ptgC input 0 is limited to [-pi, pi] but received %f", inputs[0]) + } + + if alpha > math.Pi { + alpha = math.Pi + } + if alpha < -1 * math.Pi { + alpha = -1 * math.Pi + } + + pt := r3.Vector{0, dist, 0} // Straight line, +Y is "forwards" + angleRads := 0. + if alpha != 0 { + arcRadius := math.Pi * turnRad / math.Abs(alpha) // radius of arc + angleRads = dist / turnRad // number of radians to travel along arc + pt = r3.Vector{arcRadius * (1 - math.Cos(angleRads)), arcRadius * math.Sin(angleRads), 0} + if alpha > 0 { + // positive alpha = positive rotation = left turn = negative X + pt.X *= -1 + angleRads *= -1 + } + } + pose := spatialmath.NewPose(pt, &spatialmath.OrientationVector{OZ: 1, Theta: angleRads}) + + return pose, nil +} diff --git a/motionplan/tpspace/ptgCC.go b/motionplan/tpspace/ptgCC.go index 02cb13998cc..98b53f41050 100644 --- a/motionplan/tpspace/ptgCC.go +++ b/motionplan/tpspace/ptgCC.go @@ -2,6 +2,9 @@ package tpspace import ( "math" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" ) // ptgDiffDriveCC defines a PTG family combined of two stages; first reversing while turning at radius, then moving forwards while turning @@ -11,14 +14,20 @@ type ptgDiffDriveCC struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius k float64 // k = +1 for forwards, -1 for backwards + + circle *ptgDiffDriveC } // NewCCPTG creates a new PrecomputePTG of type ptgDiffDriveCC. func NewCCPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { + + circle := NewCirclePTG(maxMMPS, maxRPS, k).(*ptgDiffDriveC) + return &ptgDiffDriveCC{ maxMMPS: maxMMPS, maxRPS: maxRPS, k: k, + circle: circle, } } @@ -43,7 +52,7 @@ func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, t, x, y, phi float64) (float64, w = ptg.maxRPS } - // Turn in the opposite direction?? + // Turn in the opposite direction if alpha < 0 { w *= -1 } @@ -53,3 +62,25 @@ func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, t, x, y, phi float64) (float64, return v, w, nil } + +func (ptg *ptgDiffDriveCC) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + alpha := inputs[0].Value + dist := inputs[1].Value + + reverseDistance := math.Abs(alpha) * 0.5 + flip := math.Copysign(1., alpha) // left or right + direction := math.Copysign(1., dist) // forwards or backwards + + revPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {-1. * direction * math.Min(dist, reverseDistance)}}) + if err != nil { + return nil, err + } + if dist < reverseDistance { + return revPose, nil + } + fwdPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {direction * (dist - reverseDistance)}}) + if err != nil { + return nil, err + } + return spatialmath.Compose(revPose, fwdPose), nil +} diff --git a/referenceframe/ptgFrame.go b/motionplan/tpspace/ptgFrame.go similarity index 73% rename from referenceframe/ptgFrame.go rename to motionplan/tpspace/ptgFrame.go index a54582830de..0cbf987076f 100644 --- a/referenceframe/ptgFrame.go +++ b/motionplan/tpspace/ptgFrame.go @@ -1,4 +1,4 @@ -package referenceframe +package tpspace import ( "fmt" @@ -6,7 +6,7 @@ import ( pb "go.viam.com/api/component/arm/v1" - "go.viam.com/rdk/motionplan/tpspace" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -21,26 +21,26 @@ const ( distanceAlongTrajectoryIndex ) -type ptgFactory func(float64, float64, float64) tpspace.PrecomputePTG +type ptgFactory func(float64, float64, float64) PrecomputePTG var defaultPTGs = []ptgFactory{ - tpspace.NewCirclePTG, - tpspace.NewCCPTG, - tpspace.NewCCSPTG, - tpspace.NewCSPTG, - tpspace.NewAlphaPTG, + NewCirclePTG, + NewCCPTG, + NewCCSPTG, + NewCSPTG, + NewAlphaPTG, } type ptgGridSimFrame struct { name string - limits []Limit + limits []referenceframe.Limit geometries []spatialmath.Geometry - ptgs []tpspace.PTG + ptgs []PTG } -// NewPTGFrameFromTurningRadius will create a new Frame which is also a tpspace.PTGProvider. It will precompute the default set of +// NewPTGFrameFromTurningRadius will create a new Frame which is also a PTGProvider. It will precompute the default set of // trajectories out to a given distance, or a default distance if the given distance is <= 0. -func NewPTGFrameFromTurningRadius(name string, velocityMMps, turnRadMeters, simDist float64, geoms []spatialmath.Geometry) (Frame, error) { +func NewPTGFrameFromTurningRadius(name string, velocityMMps, turnRadMeters, simDist float64, geoms []spatialmath.Geometry) (referenceframe.Frame, error) { if velocityMMps <= 0 { return nil, fmt.Errorf("cannot create ptg frame, movement velocity %f must be >0", velocityMMps) } @@ -62,7 +62,7 @@ func NewPTGFrameFromTurningRadius(name string, velocityMMps, turnRadMeters, simD pf.geometries = geoms - pf.limits = []Limit{ + pf.limits = []referenceframe.Limit{ {Min: 0, Max: float64(len(pf.ptgs) - 1)}, {Min: 0, Max: float64(defaultAlphaCnt)}, {Min: 0, Max: simDist}, @@ -71,7 +71,7 @@ func NewPTGFrameFromTurningRadius(name string, velocityMMps, turnRadMeters, simD return pf, nil } -func (pf *ptgGridSimFrame) DoF() []Limit { +func (pf *ptgGridSimFrame) DoF() []referenceframe.Limit { return pf.limits } @@ -85,7 +85,7 @@ func (pf *ptgGridSimFrame) MarshalJSON() ([]byte, error) { } // Inputs are: [0] index of PTG to use, [1] index of the trajectory within that PTG, and [2] distance to travel along that trajectory. -func (pf *ptgGridSimFrame) Transform(inputs []Input) (spatialmath.Pose, error) { +func (pf *ptgGridSimFrame) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { ptgIdx := int(math.Round(inputs[ptgIndex].Value)) trajIdx := uint(math.Round(inputs[trajectoryIndexWithinPTG].Value)) traj := pf.ptgs[ptgIdx].Trajectory(trajIdx) @@ -102,15 +102,15 @@ func (pf *ptgGridSimFrame) Transform(inputs []Input) (spatialmath.Pose, error) { return lastPose, nil } -func (pf *ptgGridSimFrame) InputFromProtobuf(jp *pb.JointPositions) []Input { - n := make([]Input, len(jp.Values)) +func (pf *ptgGridSimFrame) InputFromProtobuf(jp *pb.JointPositions) []referenceframe.Input { + n := make([]referenceframe.Input, len(jp.Values)) for idx, d := range jp.Values { - n[idx] = Input{d} + n[idx] = referenceframe.Input{d} } return n } -func (pf *ptgGridSimFrame) ProtobufFromInput(input []Input) *pb.JointPositions { +func (pf *ptgGridSimFrame) ProtobufFromInput(input []referenceframe.Input) *pb.JointPositions { n := make([]float64, len(input)) for idx, a := range input { n[idx] = a.Value @@ -118,9 +118,9 @@ func (pf *ptgGridSimFrame) ProtobufFromInput(input []Input) *pb.JointPositions { return &pb.JointPositions{Values: n} } -func (pf *ptgGridSimFrame) Geometries(inputs []Input) (*GeometriesInFrame, error) { +func (pf *ptgGridSimFrame) Geometries(inputs []referenceframe.Input) (*referenceframe.GeometriesInFrame, error) { if len(pf.geometries) == 0 { - return NewGeometriesInFrame(pf.Name(), nil), nil + return referenceframe.NewGeometriesInFrame(pf.Name(), nil), nil } transformedPose, err := pf.Transform(inputs) @@ -131,22 +131,22 @@ func (pf *ptgGridSimFrame) Geometries(inputs []Input) (*GeometriesInFrame, error for _, geom := range pf.geometries { geoms = append(geoms, geom.Transform(transformedPose)) } - return NewGeometriesInFrame(pf.name, geoms), nil + return referenceframe.NewGeometriesInFrame(pf.name, geoms), nil } -func (pf *ptgGridSimFrame) PTGs() []tpspace.PTG { +func (pf *ptgGridSimFrame) PTGs() []PTG { return pf.ptgs } func (pf *ptgGridSimFrame) initPTGs(maxMps, maxRPS, simDist float64) error { - ptgs := []tpspace.PTG{} + ptgs := []PTG{} for _, ptg := range defaultPTGs { for _, k := range []float64{1., -1.} { // Positive K calculates trajectories forwards, negative k calculates trajectories backwards ptgGen := ptg(maxMps, maxRPS, k) if ptgGen != nil { // irreversible trajectories, e.g. alpha, will return nil for negative k - newptg, err := tpspace.NewPTGGridSim(ptgGen, defaultAlphaCnt, simDist) + newptg, err := NewPTGGridSim(ptgGen, defaultAlphaCnt, simDist) if err != nil { return err } diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index e3eac18f576..555e357e5a6 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -10,7 +10,6 @@ const ( defaultMaxTime = 15. defaultDiffT = 0.005 defaultMinDist = 3. - defaultAlphaCnt uint = 121 defaultSearchRadius = 10. From 98bc9ba3676400e5478ecab5edea168ffe5eb032 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 2 Aug 2023 07:07:06 -0700 Subject: [PATCH 02/38] Update CS and CCS ptgs --- .../base/kinematicbase/ptgKinematics_test.go | 50 +++++++++++++++++++ motionplan/tpspace/ptgCCS.go | 49 ++++++++++++++++-- motionplan/tpspace/ptgCS.go | 46 +++++++++++++++-- motionplan/tpspace/simPtgAlpha.go | 32 ++++++++++++ 4 files changed, 168 insertions(+), 9 deletions(-) diff --git a/components/base/kinematicbase/ptgKinematics_test.go b/components/base/kinematicbase/ptgKinematics_test.go index e065b3b690a..3dcb1a56d0d 100644 --- a/components/base/kinematicbase/ptgKinematics_test.go +++ b/components/base/kinematicbase/ptgKinematics_test.go @@ -49,3 +49,53 @@ func TestPTGKinematics(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, plan, test.ShouldNotBeNil) } + +func TestPTGKinematicsWithGeom(t *testing.T) { + logger := golog.NewTestLogger(t) + + name, err := resource.NewFromString("is:a:fakebase") + test.That(t, err, test.ShouldBeNil) + + baseGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{1, 1, 1}, "") + test.That(t, err, test.ShouldBeNil) + + b := &fake.Base{ + Named: name.AsNamed(), + Geometry: []spatialmath.Geometry{baseGeom}, + WidthMeters: 0.2, + TurningRadius: 0.3, + } + + ctx := context.Background() + + kbOpt := NewKinematicBaseOptions() + kbOpt.AngularVelocityDegsPerSec = 0 + kb, err := WrapWithKinematics(ctx, b, logger, nil, nil, kbOpt) + test.That(t, err, test.ShouldBeNil) + test.That(t, kb, test.ShouldNotBeNil) + ptgBase, ok := kb.(*ptgBaseKinematics) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, ptgBase, test.ShouldNotBeNil) + + dstPIF := referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewPoseFromPoint(r3.Vector{X: 2000, Y: 0, Z: 0})) + + fs := referenceframe.NewEmptyFrameSystem("test") + f := kb.Kinematics() + test.That(t, err, test.ShouldBeNil) + fs.AddFrame(f, fs.World()) + inputMap := referenceframe.StartPositions(fs) + + obstacle, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{1000, 0, 0}), r3.Vector{1, 1, 1}, "") + test.That(t, err, test.ShouldBeNil) + + geoms := []spatialmath.Geometry{obstacle} + worldState, err := referenceframe.NewWorldState( + []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, geoms)}, + nil, + ) + test.That(t, err, test.ShouldBeNil) + + plan, err := motionplan.PlanMotion(ctx, logger, dstPIF, f, inputMap, fs, worldState, nil, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, plan, test.ShouldNotBeNil) +} diff --git a/motionplan/tpspace/ptgCCS.go b/motionplan/tpspace/ptgCCS.go index 1d3ffc5c6bc..5657ff96d8b 100644 --- a/motionplan/tpspace/ptgCCS.go +++ b/motionplan/tpspace/ptgCCS.go @@ -2,6 +2,9 @@ package tpspace import ( "math" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" ) // ptgDiffDriveCCS defines a PTG family combining the CC and CS trajectories, essentially executing the CC trajectory @@ -10,14 +13,22 @@ type ptgDiffDriveCCS struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius k float64 // k = +1 for forwards, -1 for backwards + r float64 + circle *ptgDiffDriveC } // NewCCSPTG creates a new PrecomputePTG of type ptgDiffDriveCCS. func NewCCSPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { + + r := maxMMPS / maxRPS + circle := NewCirclePTG(maxMMPS, maxRPS, k).(*ptgDiffDriveC) + return &ptgDiffDriveCCS{ maxMMPS: maxMMPS, maxRPS: maxRPS, k: k, + r: r, + circle: circle, } } @@ -27,16 +38,14 @@ func NewCCSPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { func (ptg *ptgDiffDriveCCS) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { u := math.Abs(alpha) * 0.5 - r := ptg.maxMMPS / ptg.maxRPS - v := ptg.maxMMPS w := 0. - if t < u*r/ptg.maxMMPS { + if t < u*ptg.r/ptg.maxMMPS { // l- v = -ptg.maxMMPS w = ptg.maxRPS - } else if t < (u+math.Pi/2)*r/ptg.maxMMPS { + } else if t < (u+math.Pi/2)*ptg.r/ptg.maxMMPS { // l+ pi/2 v = ptg.maxMMPS w = ptg.maxRPS @@ -52,3 +61,35 @@ func (ptg *ptgDiffDriveCCS) PTGVelocities(alpha, t, x, y, phi float64) (float64, return v, w, nil } + +func (ptg *ptgDiffDriveCCS) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + alpha := inputs[0].Value + dist := inputs[1].Value + + reverseDistance := math.Abs(alpha) * 0.5 + fwdArcDistance := (reverseDistance + math.Pi/2) * ptg.r + flip := math.Copysign(1., alpha) // left or right + direction := math.Copysign(1., dist) // forwards or backwards + + revPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {-1. * direction * math.Min(dist, reverseDistance)}}) + if err != nil { + return nil, err + } + if dist < reverseDistance { + return revPose, nil + } + fwdPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {direction * (math.Min(dist, fwdArcDistance) - reverseDistance)}}) + if err != nil { + return nil, err + } + arcPose := spatialmath.Compose(revPose, fwdPose) + if dist < reverseDistance + fwdArcDistance { + return arcPose, nil + } + + finalPose, err := ptg.circle.Transform([]referenceframe.Input{{0}, {direction * (dist - (fwdArcDistance + reverseDistance))}}) + if err != nil { + return nil, err + } + return spatialmath.Compose(arcPose, finalPose), nil +} diff --git a/motionplan/tpspace/ptgCS.go b/motionplan/tpspace/ptgCS.go index 01d7e1c80f0..d9f3656e113 100644 --- a/motionplan/tpspace/ptgCS.go +++ b/motionplan/tpspace/ptgCS.go @@ -2,6 +2,13 @@ package tpspace import ( "math" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" +) + +const ( + turnStraightConst = 1.2 // turn at max for this many radians, then go straight, depending on alpha ) // ptgDiffDriveCS defines a PTG family combined of two stages; first driving forwards while turning at radius, going straight. @@ -10,14 +17,24 @@ type ptgDiffDriveCS struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius k float64 // k = +1 for forwards, -1 for backwards + + r float64 // turning radius + turnStraight float64 + circle *ptgDiffDriveC } // NewCSPTG creates a new PrecomputePTG of type ptgDiffDriveCS. func NewCSPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { + r := maxMMPS / maxRPS + turnStraight := 1.2 * r + circle := NewCirclePTG(maxMMPS, maxRPS, k).(*ptgDiffDriveC) return &ptgDiffDriveCS{ maxMMPS: maxMMPS, maxRPS: maxRPS, k: k, + r: r, + turnStraight: turnStraight, + circle: circle, } } @@ -25,16 +42,13 @@ func NewCSPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { // Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased // because they will have zero linear velocity through their turns, not max. func (ptg *ptgDiffDriveCS) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { - r := ptg.maxMMPS / ptg.maxRPS - // Magic number; rotate this much before going straight - // Bigger value = more rotation - turnStraight := 1.2 * math.Sqrt(math.Abs(alpha)) * r / ptg.maxMMPS + turnSecs := math.Sqrt(math.Abs(alpha)) * ptg.turnStraight / ptg.maxMMPS v := ptg.maxMMPS w := 0. - if t < turnStraight { + if t < turnSecs { // l+ v = ptg.maxMMPS w = ptg.maxRPS * math.Min(1.0, 1.0-math.Exp(-1*alpha*alpha)) @@ -49,3 +63,25 @@ func (ptg *ptgDiffDriveCS) PTGVelocities(alpha, t, x, y, phi float64) (float64, w *= ptg.k return v, w, nil } + +func (ptg *ptgDiffDriveCS) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + alpha := inputs[0].Value + dist := inputs[1].Value + + arcDistance := ptg.turnStraight * math.Sqrt(math.Abs(alpha)) + flip := math.Copysign(1., alpha) // left or right + direction := math.Copysign(1., dist) // forwards or backwards + + arcPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {direction * math.Min(dist, arcDistance)}}) + if err != nil { + return nil, err + } + if dist < arcDistance { + return arcPose, nil + } + fwdPose, err := ptg.circle.Transform([]referenceframe.Input{{0}, {direction * (dist - arcDistance)}}) + if err != nil { + return nil, err + } + return spatialmath.Compose(arcPose, fwdPose), nil +} diff --git a/motionplan/tpspace/simPtgAlpha.go b/motionplan/tpspace/simPtgAlpha.go index e5899d02818..046a9ef7868 100644 --- a/motionplan/tpspace/simPtgAlpha.go +++ b/motionplan/tpspace/simPtgAlpha.go @@ -36,3 +36,35 @@ func (ptg *simPTGAlpha) PTGVelocities(alpha, t, x, y, phi float64) (float64, flo return v, w, nil } + +//~ func (ptg *simPTGAlpha) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + //~ alpha := inputs[0].Value + //~ dist := inputs[1].Value + + //~ reverseDistance := math.Abs(alpha) * 0.5 + //~ fwdArcDistance := (reverseDistance + math.Pi/2) * ptg.r + //~ flip := math.Copysign(1., alpha) // left or right + //~ direction := math.Copysign(1., dist) // forwards or backwards + + //~ revPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {-1. * direction * math.Min(dist, reverseDistance)}}) + //~ if err != nil { + //~ return nil, err + //~ } + //~ if dist < reverseDistance { + //~ return revPose, nil + //~ } + //~ fwdPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {direction * (math.Min(dist, fwdArcDistance) - reverseDistance)}}) + //~ if err != nil { + //~ return nil, err + //~ } + //~ arcPose := spatialmath.Compose(revPose, fwdPose) + //~ if dist < reverseDistance + fwdArcDistance { + //~ return arcPose, nil + //~ } + + //~ finalPose, err := ptg.circle.Transform([]referenceframe.Input{{0}, {direction * (dist - (fwdArcDistance + reverseDistance))}}) + //~ if err != nil { + //~ return nil, err + //~ } + //~ return spatialmath.Compose(arcPose, finalPose), nil +//~ } From 1d13d50f826da4252482e381096e3a05064cc0a3 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 2 Aug 2023 08:19:28 -0700 Subject: [PATCH 03/38] remove duplicate const --- motionplan/tpspace/ptgFrame.go | 1 - 1 file changed, 1 deletion(-) diff --git a/motionplan/tpspace/ptgFrame.go b/motionplan/tpspace/ptgFrame.go index a128c590f59..d314b10fa18 100644 --- a/motionplan/tpspace/ptgFrame.go +++ b/motionplan/tpspace/ptgFrame.go @@ -12,7 +12,6 @@ import ( const ( defaultSimDistMM = 2000. - defaultAlphaCnt uint = 121 ) const ( From 952b6750527578130a17f49a7311eeae271e0cfe Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Fri, 4 Aug 2023 07:45:04 -0700 Subject: [PATCH 04/38] add IK ptg file (placeholder) --- motionplan/tpspace/ptg.go | 10 +- motionplan/tpspace/ptgGridSim.go | 8 +- motionplan/tpspace/ptgIK.go | 209 ++++++++++++++++++++++++++++++ motionplan/tpspace/simPtgAlpha.go | 38 +----- 4 files changed, 225 insertions(+), 40 deletions(-) create mode 100644 motionplan/tpspace/ptgIK.go diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index 9c25b01d429..f87ea66ec18 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -16,19 +16,14 @@ const floatEpsilon = 0.0001 // If floats are closer than this consider them equa // PTG coordinates are specified in polar coordinates (alpha, d) // One of these is needed for each sort of motion that can be done. type PTG interface { - // CToTP Converts an (x, y) cartesian coord to a (k, d) TP-space coord - // d is the distance along a trajectory, k is a discretized uint index corresponding to an alpha value in [-pi, pi] - // See `index2alpha` for more - // Also returns a bool representing whether the xy is a within-traj match vs an extrapolation - CToTP(x, y float64) []*TrajNode + // CToTP Converts a pose to a (k, d) TP-space trajectory, returning the set of trajectory nodes + CToTP(spatialmath.Pose) []*TrajNode // RefDistance returns the maximum distance that a single precomputed trajectory may travel RefDistance() float64 // Returns the set of trajectory nodes for alpha index K Trajectory(uint) []*TrajNode - - Transform([]referenceframe.Input) (spatialmath.Pose, error) } // PTGProvider is something able to provide a set of PTGs associsated with it. For example, a frame which precomputes @@ -42,6 +37,7 @@ type PTGProvider interface { type PrecomputePTG interface { // PTGVelocities returns the linear and angular velocity at a specific point along a trajectory PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) + Transform([]referenceframe.Input) (spatialmath.Pose, error) } // TrajNode is a snapshot of a single point in time along a PTG trajectory, including the distance along that trajectory, diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index dc743fdfbca..d8919681b34 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -9,7 +9,6 @@ import ( const ( defaultMaxTime = 15. defaultDiffT = 0.005 - defaultMinDist = 3. defaultAlphaCnt uint = 121 defaultSearchRadius = 10. @@ -61,7 +60,12 @@ func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64) (PTG, error return ptg, nil } -func (ptg *ptgGridSim) CToTP(x, y float64) []*TrajNode { +func (ptg *ptgGridSim) CToTP(pose spatialmath.Pose) []*TrajNode { + + point := pose.Point() + x := point.X + y := point.Y + nearbyNodes := []*TrajNode{} // First, try to do a quick grid-based lookup diff --git a/motionplan/tpspace/ptgIK.go b/motionplan/tpspace/ptgIK.go new file mode 100644 index 00000000000..705381e349e --- /dev/null +++ b/motionplan/tpspace/ptgIK.go @@ -0,0 +1,209 @@ +package tpspace + + +import ( + "math" + + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/referenceframe" +) + + +// ptgGridSim will take a PrecomputePTG, and simulate out a number of trajectories through some requested time/distance for speed of lookup +// later. It will store the trajectories in a grid data structure allowing relatively fast lookups. +type ptgIK struct { + refDist float64 + alphaCnt uint + + maxTime float64 // secs of robot execution to simulate + diffT float64 // discretize trajectory simulation to this time granularity + + simPTG PrecomputePTG + + precomputeTraj [][]*TrajNode + + // Discretized x[y][]node maps for rapid NN lookups + trajNodeGrid map[int]map[int][]*TrajNode + searchRad float64 // Distance around a query point to search for precompute in the cached grid +} + +// NewPTGGridSim creates a new PTG by simulating a PrecomputePTG for some distance, then cacheing the results in a grid for fast lookup. +func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64) (PTG, error) { + if arcs == 0 { + arcs = defaultAlphaCnt + } + + ptg := &ptgGridSim{ + refDist: simDist, + alphaCnt: arcs, + maxTime: defaultMaxTime, + diffT: defaultDiffT, + searchRad: defaultSearchRadius, + + trajNodeGrid: map[int]map[int][]*TrajNode{}, + } + ptg.simPTG = simPTG + + precomp, err := ptg.simulateTrajectories(ptg.simPTG) + if err != nil { + return nil, err + } + ptg.precomputeTraj = precomp + + return ptg, nil +} + +func (ptg *ptgGridSim) CToTP(pose spatialmath.Pose) []*TrajNode { + + point := pose.Point() + x := point.X + y := point.Y + + nearbyNodes := []*TrajNode{} + + // First, try to do a quick grid-based lookup + // TODO: an octree should be faster + for tx := int(math.Round(x - ptg.searchRad)); tx < int(math.Round(x+ptg.searchRad)); tx++ { + if ptg.trajNodeGrid[tx] != nil { + for ty := int(math.Round(y - ptg.searchRad)); ty < int(math.Round(y+ptg.searchRad)); ty++ { + nearbyNodes = append(nearbyNodes, ptg.trajNodeGrid[tx][ty]...) + } + } + } + + if len(nearbyNodes) > 0 { + return nearbyNodes + } + + // Try to find a closest point to the paths: + bestDist := math.Inf(1) + var bestNode *TrajNode + + for k := 0; k < int(ptg.alphaCnt); k++ { + nMax := len(ptg.precomputeTraj[k]) - 1 + for n := 0; n <= nMax; n++ { + distToPoint := math.Pow(ptg.precomputeTraj[k][n].ptX-x, 2) + math.Pow(ptg.precomputeTraj[k][n].ptY-y, 2) + if distToPoint < bestDist { + bestDist = distToPoint + + bestNode = ptg.precomputeTraj[k][n] + } + } + } + + if bestNode != nil { + return []*TrajNode{bestNode} + } + + // Given a point (x,y), compute the "k_closest" whose extrapolation + // is closest to the point, and the associated "d_closest" distance, + // which can be normalized by "1/refDistance" to get TP-Space distances. + for k := 0; k < int(ptg.alphaCnt); k++ { + n := len(ptg.precomputeTraj[k]) - 1 + + distToPoint := math.Pow(ptg.precomputeTraj[k][n].Dist, 2) + + math.Pow(ptg.precomputeTraj[k][n].ptX-x, 2) + math.Pow(ptg.precomputeTraj[k][n].ptY-y, 2) + + if distToPoint < bestDist { + bestDist = distToPoint + bestNode = ptg.precomputeTraj[k][n] + } + } + + return []*TrajNode{bestNode} +} + +func (ptg *ptgGridSim) RefDistance() float64 { + return ptg.refDist +} + +func (ptg *ptgGridSim) Trajectory(k uint) []*TrajNode { + if int(k) >= len(ptg.precomputeTraj) { + return nil + } + return ptg.precomputeTraj[k] +} + +func (ptg *ptgGridSim) simulateTrajectories(simPtg PrecomputePTG) ([][]*TrajNode, error) { + xMin := 500.0 + xMax := -500.0 + yMin := 500.0 + yMax := -500.0 + + // C-space path structure + allTraj := make([][]*TrajNode, 0, ptg.alphaCnt) + + for k := uint(0); k < ptg.alphaCnt; k++ { + alpha := index2alpha(k, ptg.alphaCnt) + + // Initialize trajectory with an all-zero node + alphaTraj := []*TrajNode{{Pose: spatialmath.NewZeroPose()}} + + var err error + var w float64 + var v float64 + var x float64 + var y float64 + var phi float64 + var t float64 + var dist float64 + + // Last saved waypoints + accumulatedHeadingChange := 0. + + lastVs := [2]float64{0, 0} + lastWs := [2]float64{0, 0} + + // Step through each time point for this alpha + for t < ptg.maxTime && dist < ptg.refDist && accumulatedHeadingChange < defaultMaxHeadingChange { + v, w, err = simPtg.PTGVelocities(alpha, t, x, y, phi) + if err != nil { + return nil, err + } + lastVs[1] = lastVs[0] + lastWs[1] = lastWs[0] + lastVs[0] = v + lastWs[0] = w + + // finite difference eq + x += math.Cos(phi) * v * ptg.diffT + y += math.Sin(phi) * v * ptg.diffT + phi += w * ptg.diffT + accumulatedHeadingChange += w * ptg.diffT + + dist += v * ptg.diffT + t += ptg.diffT + + // Update velocities of last node because reasons + alphaTraj[len(alphaTraj)-1].LinVelMMPS = v + alphaTraj[len(alphaTraj)-1].AngVelRPS = w + + pose := xythetaToPose(x, y, phi) + alphaTraj = append(alphaTraj, &TrajNode{pose, t, dist, k, v, w, pose.Point().X, pose.Point().Y}) + + // For the grid! + xMin = math.Min(xMin, x) + xMax = math.Max(xMax, x) + yMin = math.Min(yMin, y) + yMax = math.Max(yMax, y) + } + + // Add final node + alphaTraj[len(alphaTraj)-1].LinVelMMPS = v + alphaTraj[len(alphaTraj)-1].AngVelRPS = w + pose := xythetaToPose(x, y, phi) + tNode := &TrajNode{pose, t, dist, k, v, w, pose.Point().X, pose.Point().Y} + + // Discretize into a grid for faster lookups later + if _, ok := ptg.trajNodeGrid[int(math.Round(x))]; !ok { + ptg.trajNodeGrid[int(math.Round(x))] = map[int][]*TrajNode{} + } + ptg.trajNodeGrid[int(math.Round(x))][int(math.Round(y))] = append(ptg.trajNodeGrid[int(math.Round(x))][int(math.Round(y))], tNode) + + alphaTraj = append(alphaTraj, tNode) + + allTraj = append(allTraj, alphaTraj) + } + + return allTraj, nil +} diff --git a/motionplan/tpspace/simPtgAlpha.go b/motionplan/tpspace/simPtgAlpha.go index 046a9ef7868..05839760870 100644 --- a/motionplan/tpspace/simPtgAlpha.go +++ b/motionplan/tpspace/simPtgAlpha.go @@ -2,6 +2,10 @@ package tpspace import ( "math" + "errors" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" ) // Pi / 4 (45 degrees), used as a default alpha constant @@ -37,34 +41,6 @@ func (ptg *simPTGAlpha) PTGVelocities(alpha, t, x, y, phi float64) (float64, flo return v, w, nil } -//~ func (ptg *simPTGAlpha) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { - //~ alpha := inputs[0].Value - //~ dist := inputs[1].Value - - //~ reverseDistance := math.Abs(alpha) * 0.5 - //~ fwdArcDistance := (reverseDistance + math.Pi/2) * ptg.r - //~ flip := math.Copysign(1., alpha) // left or right - //~ direction := math.Copysign(1., dist) // forwards or backwards - - //~ revPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {-1. * direction * math.Min(dist, reverseDistance)}}) - //~ if err != nil { - //~ return nil, err - //~ } - //~ if dist < reverseDistance { - //~ return revPose, nil - //~ } - //~ fwdPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {direction * (math.Min(dist, fwdArcDistance) - reverseDistance)}}) - //~ if err != nil { - //~ return nil, err - //~ } - //~ arcPose := spatialmath.Compose(revPose, fwdPose) - //~ if dist < reverseDistance + fwdArcDistance { - //~ return arcPose, nil - //~ } - - //~ finalPose, err := ptg.circle.Transform([]referenceframe.Input{{0}, {direction * (dist - (fwdArcDistance + reverseDistance))}}) - //~ if err != nil { - //~ return nil, err - //~ } - //~ return spatialmath.Compose(arcPose, finalPose), nil -//~ } +func (ptg *simPTGAlpha) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + return nil, errors.New("alpha PTG does not support Transform yet") +} From 0f8a7998ff9f9155fc29d7796a8ea6614863a91b Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Tue, 8 Aug 2023 12:30:38 -0700 Subject: [PATCH 05/38] Some intermediate work --- motionplan/ptgIK.go | 69 ++++++++++ motionplan/tpSpaceRRT.go | 6 +- motionplan/tpspace/ptg.go | 19 +-- motionplan/tpspace/ptgGridSim.go | 11 +- motionplan/tpspace/ptgIK.go | 209 ------------------------------- motionplan/tpspace/ptgIKFrame.go | 57 +++++++++ 6 files changed, 146 insertions(+), 225 deletions(-) create mode 100644 motionplan/ptgIK.go delete mode 100644 motionplan/tpspace/ptgIK.go create mode 100644 motionplan/tpspace/ptgIKFrame.go diff --git a/motionplan/ptgIK.go b/motionplan/ptgIK.go new file mode 100644 index 00000000000..b0ae116fb97 --- /dev/null +++ b/motionplan/ptgIK.go @@ -0,0 +1,69 @@ +package motionplan + + +import ( + "context" + "math/rand" + + "github.com/edaniels/golog" + + "go.viam.com/rdk/motionplan/tpspace" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/referenceframe" +) + +// ptgGridSim will take a PrecomputePTG, and simulate out a number of trajectories through some requested time/distance for speed of lookup +// later. It will store the trajectories in a grid data structure allowing relatively fast lookups. +type ptgIK struct { + refDist float64 + + ptgFrame referenceframe.Frame + fastGradDescent *NloptIK + randseed *rand.Rand +} + +// NewPTGIK creates a new PTG by simulating a PrecomputePTG for some distance, then cacheing the results in a grid for fast lookup. +func NewPTGIK(simPTG tpspace.PrecomputePTG, logger golog.Logger, refDist float64, randSeed int) (tpspace.PTG, error) { + ptgFrame, err := tpspace.NewPTGIKFrame(simPTG, refDist) + if err != nil { + return nil, err + } + + //nolint: gosec + rseed := rand.New(rand.NewSource(int64(randSeed))) + + nlopt, err := CreateNloptIKSolver(ptgFrame, logger, 1, defaultGoalThreshold) + if err != nil { + return nil, err + } + + ptg := &ptgIK{ + refDist: refDist, + ptgFrame: ptgFrame, + fastGradDescent: nlopt, + randseed: rseed, + } + + return ptg, nil +} + +func (ptg *ptgIK) CToTP(ctx context.Context, pose spatialmath.Pose) []*tpspace.TrajNode { + + solutionGen := make(chan []referenceframe.Input, 1) + // Spawn the IK solver to generate solutions until done + err := ptg.fastGradDescent.Solve(ctx, solutionGen, pose, pathMetric, ptg.randseed.Int()) + // We should have zero or one solutions + var solved []referenceframe.Input + select { + case solved = <-solutionGen: + default: + } + close(solutionGen) + if err != nil { + return nil + } +} + +func (ptg *ptgIK) RefDistance() float64 { + return ptg.refDist +} diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index c172f1058c9..2a27b22341c 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -242,10 +242,9 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( // Get cartesian distance from NN to rand relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) - relPosePt := relPose.Point() // Convert cartesian distance to tp-space using inverse curPtg, yielding TP-space coordinates goalK and goalD - nodes := curPtg.CToTP(relPosePt.X, relPosePt.Y) + nodes := curPtg.CToTP(relPose) bestNode, bestDist := mp.closestNode(relPose, nodes, mp.algOpts.dupeNodeBuffer) if bestNode == nil { return nil @@ -452,8 +451,7 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, return math.Inf(1) } relPose := spatialmath.Compose(spatialmath.PoseInverse(seg.StartPosition), seg.EndPosition) - relPosePt := relPose.Point() - nodes := ptg.CToTP(relPosePt.X, relPosePt.Y) + nodes := ptg.CToTP(relPose) closeNode, _ := mp.closestNode(relPose, nodes, min) if closeNode == nil { return math.Inf(1) diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index f87ea66ec18..1401283cdac 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -2,6 +2,7 @@ package tpspace import ( + "context" "math" "github.com/golang/geo/r3" @@ -16,14 +17,14 @@ const floatEpsilon = 0.0001 // If floats are closer than this consider them equa // PTG coordinates are specified in polar coordinates (alpha, d) // One of these is needed for each sort of motion that can be done. type PTG interface { - // CToTP Converts a pose to a (k, d) TP-space trajectory, returning the set of trajectory nodes - CToTP(spatialmath.Pose) []*TrajNode + // CToTP Converts a pose to a (k, d) TP-space trajectory, returning the set of trajectory nodes leading to that pose + CToTP(context.Context, spatialmath.Pose) []*TrajNode - // RefDistance returns the maximum distance that a single precomputed trajectory may travel + // RefDistance returns the maximum distance that a single trajectory may travel RefDistance() float64 - // Returns the set of trajectory nodes for alpha index K - Trajectory(uint) []*TrajNode + // Returns the set of trajectory nodes along the given trajectory, out to the requested distance + Trajectory(alpha, dist float64) []*TrajNode } // PTGProvider is something able to provide a set of PTGs associsated with it. For example, a frame which precomputes @@ -56,9 +57,6 @@ type TrajNode struct { } // discretized path to alpha. -// The inverse of this, which may be useful, looks like this: -// alpha = wrapTo2Pi(alpha) -// k := int(math.Round(0.5 * (float64(numPaths)*(1.0+alpha/math.Pi) - 1.0))). func index2alpha(k, numPaths uint) float64 { if k >= numPaths { return math.NaN() @@ -69,6 +67,11 @@ func index2alpha(k, numPaths uint) float64 { return math.Pi * (-1.0 + 2.0*(float64(k)+0.5)/float64(numPaths)) } +func alpha2index(alpha float64, numPaths uint) uint { + alpha = wrapTo2Pi(alpha) + return uint(math.Round(0.5 * (float64(numPaths)*(1.0+alpha/math.Pi) - 1.0))) +} + // Returns a given angle in the [0, 2pi) range. func wrapTo2Pi(theta float64) float64 { return theta - 2*math.Pi*math.Floor(theta/(2*math.Pi)) diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index d8919681b34..36bf89a2b7b 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -1,6 +1,7 @@ package tpspace import ( + "context" "math" "go.viam.com/rdk/spatialmath" @@ -60,7 +61,7 @@ func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64) (PTG, error return ptg, nil } -func (ptg *ptgGridSim) CToTP(pose spatialmath.Pose) []*TrajNode { +func (ptg *ptgGridSim) CToTP(ctx context.Context, pose spatialmath.Pose) []*TrajNode { point := pose.Point() x := point.X @@ -124,11 +125,13 @@ func (ptg *ptgGridSim) RefDistance() float64 { return ptg.refDist } -func (ptg *ptgGridSim) Trajectory(k uint) []*TrajNode { +func (ptg *ptgGridSim) Trajectory(alpha, dist float64) ([]*TrajNode, error) { + k := alpha2index(alpha) if int(k) >= len(ptg.precomputeTraj) { - return nil + return nil, fmt.Errorf("requested trajectory of index %d but this grid sim only has %d available", k, len(ptg.precomputeTraj)) } - return ptg.precomputeTraj[k] + fullTraj := ptg.precomputeTraj[k] + } func (ptg *ptgGridSim) simulateTrajectories(simPtg PrecomputePTG) ([][]*TrajNode, error) { diff --git a/motionplan/tpspace/ptgIK.go b/motionplan/tpspace/ptgIK.go deleted file mode 100644 index 705381e349e..00000000000 --- a/motionplan/tpspace/ptgIK.go +++ /dev/null @@ -1,209 +0,0 @@ -package tpspace - - -import ( - "math" - - "go.viam.com/rdk/spatialmath" - "go.viam.com/rdk/referenceframe" -) - - -// ptgGridSim will take a PrecomputePTG, and simulate out a number of trajectories through some requested time/distance for speed of lookup -// later. It will store the trajectories in a grid data structure allowing relatively fast lookups. -type ptgIK struct { - refDist float64 - alphaCnt uint - - maxTime float64 // secs of robot execution to simulate - diffT float64 // discretize trajectory simulation to this time granularity - - simPTG PrecomputePTG - - precomputeTraj [][]*TrajNode - - // Discretized x[y][]node maps for rapid NN lookups - trajNodeGrid map[int]map[int][]*TrajNode - searchRad float64 // Distance around a query point to search for precompute in the cached grid -} - -// NewPTGGridSim creates a new PTG by simulating a PrecomputePTG for some distance, then cacheing the results in a grid for fast lookup. -func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64) (PTG, error) { - if arcs == 0 { - arcs = defaultAlphaCnt - } - - ptg := &ptgGridSim{ - refDist: simDist, - alphaCnt: arcs, - maxTime: defaultMaxTime, - diffT: defaultDiffT, - searchRad: defaultSearchRadius, - - trajNodeGrid: map[int]map[int][]*TrajNode{}, - } - ptg.simPTG = simPTG - - precomp, err := ptg.simulateTrajectories(ptg.simPTG) - if err != nil { - return nil, err - } - ptg.precomputeTraj = precomp - - return ptg, nil -} - -func (ptg *ptgGridSim) CToTP(pose spatialmath.Pose) []*TrajNode { - - point := pose.Point() - x := point.X - y := point.Y - - nearbyNodes := []*TrajNode{} - - // First, try to do a quick grid-based lookup - // TODO: an octree should be faster - for tx := int(math.Round(x - ptg.searchRad)); tx < int(math.Round(x+ptg.searchRad)); tx++ { - if ptg.trajNodeGrid[tx] != nil { - for ty := int(math.Round(y - ptg.searchRad)); ty < int(math.Round(y+ptg.searchRad)); ty++ { - nearbyNodes = append(nearbyNodes, ptg.trajNodeGrid[tx][ty]...) - } - } - } - - if len(nearbyNodes) > 0 { - return nearbyNodes - } - - // Try to find a closest point to the paths: - bestDist := math.Inf(1) - var bestNode *TrajNode - - for k := 0; k < int(ptg.alphaCnt); k++ { - nMax := len(ptg.precomputeTraj[k]) - 1 - for n := 0; n <= nMax; n++ { - distToPoint := math.Pow(ptg.precomputeTraj[k][n].ptX-x, 2) + math.Pow(ptg.precomputeTraj[k][n].ptY-y, 2) - if distToPoint < bestDist { - bestDist = distToPoint - - bestNode = ptg.precomputeTraj[k][n] - } - } - } - - if bestNode != nil { - return []*TrajNode{bestNode} - } - - // Given a point (x,y), compute the "k_closest" whose extrapolation - // is closest to the point, and the associated "d_closest" distance, - // which can be normalized by "1/refDistance" to get TP-Space distances. - for k := 0; k < int(ptg.alphaCnt); k++ { - n := len(ptg.precomputeTraj[k]) - 1 - - distToPoint := math.Pow(ptg.precomputeTraj[k][n].Dist, 2) + - math.Pow(ptg.precomputeTraj[k][n].ptX-x, 2) + math.Pow(ptg.precomputeTraj[k][n].ptY-y, 2) - - if distToPoint < bestDist { - bestDist = distToPoint - bestNode = ptg.precomputeTraj[k][n] - } - } - - return []*TrajNode{bestNode} -} - -func (ptg *ptgGridSim) RefDistance() float64 { - return ptg.refDist -} - -func (ptg *ptgGridSim) Trajectory(k uint) []*TrajNode { - if int(k) >= len(ptg.precomputeTraj) { - return nil - } - return ptg.precomputeTraj[k] -} - -func (ptg *ptgGridSim) simulateTrajectories(simPtg PrecomputePTG) ([][]*TrajNode, error) { - xMin := 500.0 - xMax := -500.0 - yMin := 500.0 - yMax := -500.0 - - // C-space path structure - allTraj := make([][]*TrajNode, 0, ptg.alphaCnt) - - for k := uint(0); k < ptg.alphaCnt; k++ { - alpha := index2alpha(k, ptg.alphaCnt) - - // Initialize trajectory with an all-zero node - alphaTraj := []*TrajNode{{Pose: spatialmath.NewZeroPose()}} - - var err error - var w float64 - var v float64 - var x float64 - var y float64 - var phi float64 - var t float64 - var dist float64 - - // Last saved waypoints - accumulatedHeadingChange := 0. - - lastVs := [2]float64{0, 0} - lastWs := [2]float64{0, 0} - - // Step through each time point for this alpha - for t < ptg.maxTime && dist < ptg.refDist && accumulatedHeadingChange < defaultMaxHeadingChange { - v, w, err = simPtg.PTGVelocities(alpha, t, x, y, phi) - if err != nil { - return nil, err - } - lastVs[1] = lastVs[0] - lastWs[1] = lastWs[0] - lastVs[0] = v - lastWs[0] = w - - // finite difference eq - x += math.Cos(phi) * v * ptg.diffT - y += math.Sin(phi) * v * ptg.diffT - phi += w * ptg.diffT - accumulatedHeadingChange += w * ptg.diffT - - dist += v * ptg.diffT - t += ptg.diffT - - // Update velocities of last node because reasons - alphaTraj[len(alphaTraj)-1].LinVelMMPS = v - alphaTraj[len(alphaTraj)-1].AngVelRPS = w - - pose := xythetaToPose(x, y, phi) - alphaTraj = append(alphaTraj, &TrajNode{pose, t, dist, k, v, w, pose.Point().X, pose.Point().Y}) - - // For the grid! - xMin = math.Min(xMin, x) - xMax = math.Max(xMax, x) - yMin = math.Min(yMin, y) - yMax = math.Max(yMax, y) - } - - // Add final node - alphaTraj[len(alphaTraj)-1].LinVelMMPS = v - alphaTraj[len(alphaTraj)-1].AngVelRPS = w - pose := xythetaToPose(x, y, phi) - tNode := &TrajNode{pose, t, dist, k, v, w, pose.Point().X, pose.Point().Y} - - // Discretize into a grid for faster lookups later - if _, ok := ptg.trajNodeGrid[int(math.Round(x))]; !ok { - ptg.trajNodeGrid[int(math.Round(x))] = map[int][]*TrajNode{} - } - ptg.trajNodeGrid[int(math.Round(x))][int(math.Round(y))] = append(ptg.trajNodeGrid[int(math.Round(x))][int(math.Round(y))], tNode) - - alphaTraj = append(alphaTraj, tNode) - - allTraj = append(allTraj, alphaTraj) - } - - return allTraj, nil -} diff --git a/motionplan/tpspace/ptgIKFrame.go b/motionplan/tpspace/ptgIKFrame.go new file mode 100644 index 00000000000..412fe0e7701 --- /dev/null +++ b/motionplan/tpspace/ptgIKFrame.go @@ -0,0 +1,57 @@ +package tpspace + +import ( + "math" + + pb "go.viam.com/api/component/arm/v1" + + "go.viam.com/rdk/referenceframe" +) + +// ptgFrame wraps a tpspace.PrecomputePTG so that it fills the Frame interface and can be used by IK +type ptgIKFrame struct { + PrecomputePTG + limits []referenceframe.Limit +} + +func NewPTGIKFrame(ptg PrecomputePTG, dist float64) (referenceframe.Frame, error) { + pf := &ptgIKFrame{PrecomputePTG: ptg} + + pf.limits = []referenceframe.Limit{ + {Min: -math.Pi, Max: math.Pi}, + {Min: 0, Max: dist}, + } + return pf, nil +} + +func (pf *ptgIKFrame) DoF() []referenceframe.Limit { + return pf.limits +} + +func (pf *ptgIKFrame) Name() string { + return "" +} + +func (pf *ptgIKFrame) MarshalJSON() ([]byte, error) { + return nil, nil +} + +func (pf *ptgIKFrame) InputFromProtobuf(jp *pb.JointPositions) []referenceframe.Input { + n := make([]referenceframe.Input, len(jp.Values)) + for idx, d := range jp.Values { + n[idx] = referenceframe.Input{d} + } + return n +} + +func (pf *ptgIKFrame) ProtobufFromInput(input []referenceframe.Input) *pb.JointPositions { + n := make([]float64, len(input)) + for idx, a := range input { + n[idx] = a.Value + } + return &pb.JointPositions{Values: n} +} + +func (pf *ptgIKFrame) Geometries(inputs []referenceframe.Input) (*referenceframe.GeometriesInFrame, error) { + return nil, nil +} From 0c79ef397244916e2da2a6c939c15dd76d614dac Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Tue, 8 Aug 2023 12:31:18 -0700 Subject: [PATCH 06/38] add file --- motionplan/tpspace/ptgGridSimFrame.go | 157 ++++++++++++++++++++++++++ 1 file changed, 157 insertions(+) create mode 100644 motionplan/tpspace/ptgGridSimFrame.go diff --git a/motionplan/tpspace/ptgGridSimFrame.go b/motionplan/tpspace/ptgGridSimFrame.go new file mode 100644 index 00000000000..d314b10fa18 --- /dev/null +++ b/motionplan/tpspace/ptgGridSimFrame.go @@ -0,0 +1,157 @@ +package tpspace + +import ( + "fmt" + "math" + + pb "go.viam.com/api/component/arm/v1" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" +) + +const ( + defaultSimDistMM = 2000. +) + +const ( + ptgIndex int = iota + trajectoryIndexWithinPTG + distanceAlongTrajectoryIndex +) + +type ptgFactory func(float64, float64, float64) PrecomputePTG + +var defaultPTGs = []ptgFactory{ + NewCirclePTG, + NewCCPTG, + NewCCSPTG, + NewCSPTG, + NewAlphaPTG, +} + +type ptgGridSimFrame struct { + name string + limits []referenceframe.Limit + geometries []spatialmath.Geometry + ptgs []PTG +} + +// NewPTGFrameFromTurningRadius will create a new Frame which is also a PTGProvider. It will precompute the default set of +// trajectories out to a given distance, or a default distance if the given distance is <= 0. +func NewPTGFrameFromTurningRadius(name string, velocityMMps, turnRadMeters, simDist float64, geoms []spatialmath.Geometry) (referenceframe.Frame, error) { + if velocityMMps <= 0 { + return nil, fmt.Errorf("cannot create ptg frame, movement velocity %f must be >0", velocityMMps) + } + if turnRadMeters <= 0 { + return nil, fmt.Errorf("cannot create ptg frame, turning radius %f must be >0", turnRadMeters) + } + + if simDist <= 0 { + simDist = defaultSimDistMM + } + + // Get max angular velocity in radians per second + maxRPS := velocityMMps / (1000. * turnRadMeters) + pf := &ptgGridSimFrame{name: name} + err := pf.initPTGs(velocityMMps, maxRPS, simDist) + if err != nil { + return nil, err + } + + pf.geometries = geoms + + pf.limits = []referenceframe.Limit{ + {Min: 0, Max: float64(len(pf.ptgs) - 1)}, + {Min: 0, Max: float64(defaultAlphaCnt)}, + {Min: 0, Max: simDist}, + } + + return pf, nil +} + +func (pf *ptgGridSimFrame) DoF() []referenceframe.Limit { + return pf.limits +} + +func (pf *ptgGridSimFrame) Name() string { + return pf.name +} + +// TODO: Define some sort of config struct for a PTG frame. +func (pf *ptgGridSimFrame) MarshalJSON() ([]byte, error) { + return nil, nil +} + +// Inputs are: [0] index of PTG to use, [1] index of the trajectory within that PTG, and [2] distance to travel along that trajectory. +func (pf *ptgGridSimFrame) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + ptgIdx := int(math.Round(inputs[ptgIndex].Value)) + trajIdx := uint(math.Round(inputs[trajectoryIndexWithinPTG].Value)) + traj := pf.ptgs[ptgIdx].Trajectory(trajIdx) + lastPose := spatialmath.NewZeroPose() + for _, trajNode := range traj { + // Walk the trajectory until we pass the specified distance + if trajNode.Dist > inputs[distanceAlongTrajectoryIndex].Value { + break + } + lastPose = trajNode.Pose + } + + return lastPose, nil +} + +func (pf *ptgGridSimFrame) InputFromProtobuf(jp *pb.JointPositions) []referenceframe.Input { + n := make([]referenceframe.Input, len(jp.Values)) + for idx, d := range jp.Values { + n[idx] = referenceframe.Input{d} + } + return n +} + +func (pf *ptgGridSimFrame) ProtobufFromInput(input []referenceframe.Input) *pb.JointPositions { + n := make([]float64, len(input)) + for idx, a := range input { + n[idx] = a.Value + } + return &pb.JointPositions{Values: n} +} + +func (pf *ptgGridSimFrame) Geometries(inputs []referenceframe.Input) (*referenceframe.GeometriesInFrame, error) { + if len(pf.geometries) == 0 { + return referenceframe.NewGeometriesInFrame(pf.Name(), nil), nil + } + + transformedPose, err := pf.Transform(inputs) + if err != nil { + return nil, err + } + geoms := make([]spatialmath.Geometry, 0, len(pf.geometries)) + for _, geom := range pf.geometries { + geoms = append(geoms, geom.Transform(transformedPose)) + } + return referenceframe.NewGeometriesInFrame(pf.name, geoms), nil +} + +func (pf *ptgGridSimFrame) PTGs() []PTG { + return pf.ptgs +} + +func (pf *ptgGridSimFrame) initPTGs(maxMps, maxRPS, simDist float64) error { + ptgs := []PTG{} + for _, ptg := range defaultPTGs { + for _, k := range []float64{1., -1.} { + // Positive K calculates trajectories forwards, negative k calculates trajectories backwards + ptgGen := ptg(maxMps, maxRPS, k) + if ptgGen != nil { + // irreversible trajectories, e.g. alpha, will return nil for negative k + newptg, err := NewPTGGridSim(ptgGen, defaultAlphaCnt, simDist) + if err != nil { + return err + } + ptgs = append(ptgs, newptg) + } + } + } + pf.ptgs = ptgs + return nil +} From b4f05b583c9530cffe2ab3765afaf8fc0ff1ba5a Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Tue, 8 Aug 2023 16:22:24 -0700 Subject: [PATCH 07/38] Update ctxs and errors --- motionplan/ptgIK.go | 4 + motionplan/tpSpaceRRT.go | 71 +++++++----- motionplan/tpspace/ptg.go | 4 +- motionplan/tpspace/ptgFrame.go | 157 -------------------------- motionplan/tpspace/ptgGridSim.go | 20 +++- motionplan/tpspace/ptgGridSimFrame.go | 20 ++-- 6 files changed, 73 insertions(+), 203 deletions(-) delete mode 100644 motionplan/tpspace/ptgFrame.go diff --git a/motionplan/ptgIK.go b/motionplan/ptgIK.go index b0ae116fb97..12f4ea86502 100644 --- a/motionplan/ptgIK.go +++ b/motionplan/ptgIK.go @@ -67,3 +67,7 @@ func (ptg *ptgIK) CToTP(ctx context.Context, pose spatialmath.Pose) []*tpspace.T func (ptg *ptgIK) RefDistance() float64 { return ptg.refDist } + +func (ptg *ptgIK) Trajectory(alpha, dist float64) ([]*tpspace.TrajNode, error) { + return nil, nil +} diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index 2a27b22341c..b81dc37f9b4 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -201,7 +201,11 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( } randPosNode := &basicNode{q: nil, cost: 0, pose: randPos, corner: false} - successNode := mp.attemptExtension(ctx, nil, randPosNode, rrt) + successNode, err := mp.attemptExtension(ctx, nil, randPosNode, rrt) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return + } // If we tried the goal and have a close-enough XY location, check if the node is good enough to be a final goal if tryGoal && successNode != nil { if mp.planOpts.GoalThreshold > mp.planOpts.goalMetric(&State{Position: successNode.Pose(), Frame: mp.frame}) { @@ -224,7 +228,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( rrt *rrtParallelPlannerShared, nearest node, planOpts *plannerOptions, // Need to pass this in explicitly for smoothing -) *candidate { +) (*candidate, error) { nm := &neighborManager{nCPU: mp.planOpts.NumThreads} nm.parallelNeighbors = 10 @@ -236,7 +240,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( // Get nearest neighbor to rand config in tree using this PTG nearest = nm.nearestNeighbor(ctx, ptgDistOpt, randPosNode, rrt.maps.startMap) if nearest == nil { - return nil + return nil, nil } } @@ -244,15 +248,18 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) // Convert cartesian distance to tp-space using inverse curPtg, yielding TP-space coordinates goalK and goalD - nodes := curPtg.CToTP(relPose) + nodes := curPtg.CToTP(ctx, relPose) bestNode, bestDist := mp.closestNode(relPose, nodes, mp.algOpts.dupeNodeBuffer) if bestNode == nil { - return nil + return nil, nil } - goalK := bestNode.K + goalAlpha := bestNode.Alpha goalD := bestNode.Dist // Check collisions along this traj and get the longest distance viable - trajK := curPtg.Trajectory(goalK) + trajK, err := curPtg.Trajectory(goalAlpha, goalD) + if err != nil { + return nil, err + } var lastNode *tpspace.TrajNode var lastPose spatialmath.Pose @@ -272,7 +279,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( if sinceLastCollideCheck > planOpts.Resolution { ok, _ := planOpts.CheckStateConstraints(trajState) if !ok { - return nil + return nil, nil } sinceLastCollideCheck = 0. } @@ -283,7 +290,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( } // add the last node in trajectory successNode = &basicNode{ - q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), float64(goalK), lastNode.Dist}), + q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), goalAlpha, lastNode.Dist}), cost: nearest.Cost() + lastNode.Dist, pose: lastPose, corner: false, @@ -302,7 +309,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( cand = nil } } - return cand + return cand, nil } // attemptExtension will attempt to extend the rrt map towards the goal node, and will return the candidate added to the map that is @@ -312,32 +319,38 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( seedNode, goalNode node, rrt *rrtParallelPlannerShared, -) node { +) (node, error) { for { select { case <-ctx.Done(): - return nil + return nil, ctx.Err() default: } candidates := []*candidate{} for ptgNum, curPtg := range mp.tpFrame.PTGs() { // Find the best traj point for each traj family, and store for later comparison // TODO: run in parallel - cand := mp.getExtensionCandidate(ctx, goalNode, ptgNum, curPtg, rrt, seedNode, mp.planOpts) + cand, err := mp.getExtensionCandidate(ctx, goalNode, ptgNum, curPtg, rrt, seedNode, mp.planOpts) + if err != nil { + return nil, err + } if cand != nil { if cand.err == nil { candidates = append(candidates, cand) } } } - reseedCandidate := mp.extendMap(ctx, candidates, rrt) + reseedCandidate, err := mp.extendMap(ctx, candidates, rrt) + if err != nil { + return nil, err + } if reseedCandidate == nil { - return nil + return nil, nil } dist := mp.planOpts.DistanceFunc(&Segment{StartPosition: reseedCandidate.newNode.Pose(), EndPosition: goalNode.Pose()}) if dist < mp.planOpts.GoalThreshold || !reseedCandidate.lastInTraj { // Reached the goal position, or otherwise failed to fully extend to the end of a trajectory - return reseedCandidate.newNode + return reseedCandidate.newNode, nil } seedNode = reseedCandidate.newNode } @@ -348,9 +361,9 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( ctx context.Context, candidates []*candidate, rrt *rrtParallelPlannerShared, -) *candidate { +) (*candidate, error) { if len(candidates) == 0 { - return nil + return nil, nil } var addedNode node // If we found any valid nodes that we can extend to, find the very best one and add that to the tree @@ -362,24 +375,24 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( bestDist = cand.dist } } - treeNode := bestCand.treeNode + treeNode := bestCand.treeNode // The node already in the tree to which we are parenting newNode := bestCand.newNode ptgNum := int(newNode.Q()[0].Value) - randK := uint(newNode.Q()[1].Value) + randAlpha := newNode.Q()[1].Value + randDist := newNode.Q()[2].Value - trajK := mp.tpFrame.PTGs()[ptgNum].Trajectory(randK) + trajK, err := mp.tpFrame.PTGs()[ptgNum].Trajectory(randAlpha, randDist) + if err != nil { + return nil, err + } lastDist := 0. sinceLastNode := 0. for _, trajPt := range trajK { if ctx.Err() != nil { - return nil - } - if trajPt.Dist > newNode.Q()[2].Value { - // After we've passed goalD, no need to keep checking, just add to RRT tree - break + return nil, ctx.Err() } trajState := &State{Position: spatialmath.Compose(treeNode.Pose(), trajPt.Pose)} sinceLastNode += (trajPt.Dist - lastDist) @@ -388,7 +401,7 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( if mp.algOpts.addIntermediate && sinceLastNode > mp.algOpts.addNodeEvery { // add the last node in trajectory addedNode = &basicNode{ - q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), float64(randK), trajPt.Dist}), + q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), randAlpha, trajPt.Dist}), cost: treeNode.Cost() + trajPt.Dist, pose: trajState.Position, corner: false, @@ -399,7 +412,7 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( lastDist = trajPt.Dist } rrt.maps.startMap[newNode] = treeNode - return bestCand + return bestCand, nil } func (mp *tpSpaceRRTMotionPlanner) setupTPSpaceOptions() { @@ -451,7 +464,7 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, return math.Inf(1) } relPose := spatialmath.Compose(spatialmath.PoseInverse(seg.StartPosition), seg.EndPosition) - nodes := ptg.CToTP(relPose) + nodes := ptg.CToTP(context.Background(), relPose) closeNode, _ := mp.closestNode(relPose, nodes, min) if closeNode == nil { return math.Inf(1) diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index 1401283cdac..9ed689ff1fc 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -24,7 +24,7 @@ type PTG interface { RefDistance() float64 // Returns the set of trajectory nodes along the given trajectory, out to the requested distance - Trajectory(alpha, dist float64) []*TrajNode + Trajectory(alpha, dist float64) ([]*TrajNode, error) } // PTGProvider is something able to provide a set of PTGs associsated with it. For example, a frame which precomputes @@ -48,7 +48,7 @@ type TrajNode struct { Pose spatialmath.Pose // for 2d, we only use x, y, and OV theta Time float64 // elapsed time on trajectory Dist float64 // distance travelled down trajectory - K uint // alpha k-value at this node + Alpha float64 // alpha k-value at this node LinVelMMPS float64 // linvel in millimeters per second at this node AngVelRPS float64 // angvel in radians per second at this node diff --git a/motionplan/tpspace/ptgFrame.go b/motionplan/tpspace/ptgFrame.go deleted file mode 100644 index d314b10fa18..00000000000 --- a/motionplan/tpspace/ptgFrame.go +++ /dev/null @@ -1,157 +0,0 @@ -package tpspace - -import ( - "fmt" - "math" - - pb "go.viam.com/api/component/arm/v1" - - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/spatialmath" -) - -const ( - defaultSimDistMM = 2000. -) - -const ( - ptgIndex int = iota - trajectoryIndexWithinPTG - distanceAlongTrajectoryIndex -) - -type ptgFactory func(float64, float64, float64) PrecomputePTG - -var defaultPTGs = []ptgFactory{ - NewCirclePTG, - NewCCPTG, - NewCCSPTG, - NewCSPTG, - NewAlphaPTG, -} - -type ptgGridSimFrame struct { - name string - limits []referenceframe.Limit - geometries []spatialmath.Geometry - ptgs []PTG -} - -// NewPTGFrameFromTurningRadius will create a new Frame which is also a PTGProvider. It will precompute the default set of -// trajectories out to a given distance, or a default distance if the given distance is <= 0. -func NewPTGFrameFromTurningRadius(name string, velocityMMps, turnRadMeters, simDist float64, geoms []spatialmath.Geometry) (referenceframe.Frame, error) { - if velocityMMps <= 0 { - return nil, fmt.Errorf("cannot create ptg frame, movement velocity %f must be >0", velocityMMps) - } - if turnRadMeters <= 0 { - return nil, fmt.Errorf("cannot create ptg frame, turning radius %f must be >0", turnRadMeters) - } - - if simDist <= 0 { - simDist = defaultSimDistMM - } - - // Get max angular velocity in radians per second - maxRPS := velocityMMps / (1000. * turnRadMeters) - pf := &ptgGridSimFrame{name: name} - err := pf.initPTGs(velocityMMps, maxRPS, simDist) - if err != nil { - return nil, err - } - - pf.geometries = geoms - - pf.limits = []referenceframe.Limit{ - {Min: 0, Max: float64(len(pf.ptgs) - 1)}, - {Min: 0, Max: float64(defaultAlphaCnt)}, - {Min: 0, Max: simDist}, - } - - return pf, nil -} - -func (pf *ptgGridSimFrame) DoF() []referenceframe.Limit { - return pf.limits -} - -func (pf *ptgGridSimFrame) Name() string { - return pf.name -} - -// TODO: Define some sort of config struct for a PTG frame. -func (pf *ptgGridSimFrame) MarshalJSON() ([]byte, error) { - return nil, nil -} - -// Inputs are: [0] index of PTG to use, [1] index of the trajectory within that PTG, and [2] distance to travel along that trajectory. -func (pf *ptgGridSimFrame) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { - ptgIdx := int(math.Round(inputs[ptgIndex].Value)) - trajIdx := uint(math.Round(inputs[trajectoryIndexWithinPTG].Value)) - traj := pf.ptgs[ptgIdx].Trajectory(trajIdx) - lastPose := spatialmath.NewZeroPose() - for _, trajNode := range traj { - // Walk the trajectory until we pass the specified distance - if trajNode.Dist > inputs[distanceAlongTrajectoryIndex].Value { - break - } - lastPose = trajNode.Pose - } - - return lastPose, nil -} - -func (pf *ptgGridSimFrame) InputFromProtobuf(jp *pb.JointPositions) []referenceframe.Input { - n := make([]referenceframe.Input, len(jp.Values)) - for idx, d := range jp.Values { - n[idx] = referenceframe.Input{d} - } - return n -} - -func (pf *ptgGridSimFrame) ProtobufFromInput(input []referenceframe.Input) *pb.JointPositions { - n := make([]float64, len(input)) - for idx, a := range input { - n[idx] = a.Value - } - return &pb.JointPositions{Values: n} -} - -func (pf *ptgGridSimFrame) Geometries(inputs []referenceframe.Input) (*referenceframe.GeometriesInFrame, error) { - if len(pf.geometries) == 0 { - return referenceframe.NewGeometriesInFrame(pf.Name(), nil), nil - } - - transformedPose, err := pf.Transform(inputs) - if err != nil { - return nil, err - } - geoms := make([]spatialmath.Geometry, 0, len(pf.geometries)) - for _, geom := range pf.geometries { - geoms = append(geoms, geom.Transform(transformedPose)) - } - return referenceframe.NewGeometriesInFrame(pf.name, geoms), nil -} - -func (pf *ptgGridSimFrame) PTGs() []PTG { - return pf.ptgs -} - -func (pf *ptgGridSimFrame) initPTGs(maxMps, maxRPS, simDist float64) error { - ptgs := []PTG{} - for _, ptg := range defaultPTGs { - for _, k := range []float64{1., -1.} { - // Positive K calculates trajectories forwards, negative k calculates trajectories backwards - ptgGen := ptg(maxMps, maxRPS, k) - if ptgGen != nil { - // irreversible trajectories, e.g. alpha, will return nil for negative k - newptg, err := NewPTGGridSim(ptgGen, defaultAlphaCnt, simDist) - if err != nil { - return err - } - ptgs = append(ptgs, newptg) - } - } - } - pf.ptgs = ptgs - return nil -} diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index 36bf89a2b7b..a556f6ba1c4 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -2,6 +2,7 @@ package tpspace import ( "context" + "fmt" "math" "go.viam.com/rdk/spatialmath" @@ -126,12 +127,23 @@ func (ptg *ptgGridSim) RefDistance() float64 { } func (ptg *ptgGridSim) Trajectory(alpha, dist float64) ([]*TrajNode, error) { - k := alpha2index(alpha) + k := alpha2index(alpha, ptg.alphaCnt) if int(k) >= len(ptg.precomputeTraj) { return nil, fmt.Errorf("requested trajectory of index %d but this grid sim only has %d available", k, len(ptg.precomputeTraj)) } fullTraj := ptg.precomputeTraj[k] - + if fullTraj[len(fullTraj) - 1].Dist < dist { + return nil, fmt.Errorf("requested traj to dist %f but cannot request distances larger than %f", dist, fullTraj[len(fullTraj) - 1].Dist) + } + var traj []*TrajNode + for _, trajNode := range fullTraj { + // Walk the trajectory until we pass the specified distance + if trajNode.Dist > dist { + break + } + traj = append(traj, trajNode) + } + return traj, nil } func (ptg *ptgGridSim) simulateTrajectories(simPtg PrecomputePTG) ([][]*TrajNode, error) { @@ -189,7 +201,7 @@ func (ptg *ptgGridSim) simulateTrajectories(simPtg PrecomputePTG) ([][]*TrajNode alphaTraj[len(alphaTraj)-1].AngVelRPS = w pose := xythetaToPose(x, y, phi) - alphaTraj = append(alphaTraj, &TrajNode{pose, t, dist, k, v, w, pose.Point().X, pose.Point().Y}) + alphaTraj = append(alphaTraj, &TrajNode{pose, t, dist, alpha, v, w, pose.Point().X, pose.Point().Y}) // For the grid! xMin = math.Min(xMin, x) @@ -202,7 +214,7 @@ func (ptg *ptgGridSim) simulateTrajectories(simPtg PrecomputePTG) ([][]*TrajNode alphaTraj[len(alphaTraj)-1].LinVelMMPS = v alphaTraj[len(alphaTraj)-1].AngVelRPS = w pose := xythetaToPose(x, y, phi) - tNode := &TrajNode{pose, t, dist, k, v, w, pose.Point().X, pose.Point().Y} + tNode := &TrajNode{pose, t, dist, alpha, v, w, pose.Point().X, pose.Point().Y} // Discretize into a grid for faster lookups later if _, ok := ptg.trajNodeGrid[int(math.Round(x))]; !ok { diff --git a/motionplan/tpspace/ptgGridSimFrame.go b/motionplan/tpspace/ptgGridSimFrame.go index d314b10fa18..0a78bc33284 100644 --- a/motionplan/tpspace/ptgGridSimFrame.go +++ b/motionplan/tpspace/ptgGridSimFrame.go @@ -16,7 +16,7 @@ const ( const ( ptgIndex int = iota - trajectoryIndexWithinPTG + trajectoryAlphaWithinPTG distanceAlongTrajectoryIndex ) @@ -85,19 +85,17 @@ func (pf *ptgGridSimFrame) MarshalJSON() ([]byte, error) { // Inputs are: [0] index of PTG to use, [1] index of the trajectory within that PTG, and [2] distance to travel along that trajectory. func (pf *ptgGridSimFrame) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + alpha := inputs[trajectoryAlphaWithinPTG].Value + dist := inputs[distanceAlongTrajectoryIndex].Value + ptgIdx := int(math.Round(inputs[ptgIndex].Value)) - trajIdx := uint(math.Round(inputs[trajectoryIndexWithinPTG].Value)) - traj := pf.ptgs[ptgIdx].Trajectory(trajIdx) - lastPose := spatialmath.NewZeroPose() - for _, trajNode := range traj { - // Walk the trajectory until we pass the specified distance - if trajNode.Dist > inputs[distanceAlongTrajectoryIndex].Value { - break - } - lastPose = trajNode.Pose + + traj, err := pf.ptgs[ptgIdx].Trajectory(alpha, dist) + if err != nil { + return nil, err } - return lastPose, nil + return traj[len(traj)-1].Pose, nil } func (pf *ptgGridSimFrame) InputFromProtobuf(jp *pb.JointPositions) []referenceframe.Input { From da7752b3523432d42e77b979124d6ec932699510 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 9 Aug 2023 14:51:10 -0700 Subject: [PATCH 08/38] barely-working tp-space IK --- motionplan/ptgIK.go | 31 ++++-- motionplan/tpSpaceRRT.go | 47 +++----- motionplan/tpSpaceRRT_test.go | 56 ++++++++-- motionplan/tpspace/ptg.go | 75 ++++++++++++- motionplan/tpspace/ptgC.go | 14 +-- motionplan/tpspace/ptgCC.go | 17 ++- motionplan/tpspace/ptgCCS.go | 17 ++- motionplan/tpspace/ptgCS.go | 17 ++- motionplan/tpspace/ptgGridSim.go | 104 +++++------------ motionplan/tpspace/ptgGridSimFrame.go | 155 -------------------------- motionplan/tpspace/ptgGridSim_test.go | 8 -- motionplan/tpspace/ptgIKFrame.go | 2 +- 12 files changed, 217 insertions(+), 326 deletions(-) delete mode 100644 motionplan/tpspace/ptgGridSimFrame.go diff --git a/motionplan/ptgIK.go b/motionplan/ptgIK.go index 12f4ea86502..db86e4deeee 100644 --- a/motionplan/ptgIK.go +++ b/motionplan/ptgIK.go @@ -3,6 +3,8 @@ package motionplan import ( "context" + "math" + "fmt" "math/rand" "github.com/edaniels/golog" @@ -12,11 +14,16 @@ import ( "go.viam.com/rdk/referenceframe" ) +const ( + defaultMaxTime = 15. + defaultDiffT = 0.01 // seconds +) + // ptgGridSim will take a PrecomputePTG, and simulate out a number of trajectories through some requested time/distance for speed of lookup // later. It will store the trajectories in a grid data structure allowing relatively fast lookups. type ptgIK struct { refDist float64 - + simPTG tpspace.PrecomputePTG ptgFrame referenceframe.Frame fastGradDescent *NloptIK randseed *rand.Rand @@ -32,13 +39,14 @@ func NewPTGIK(simPTG tpspace.PrecomputePTG, logger golog.Logger, refDist float64 //nolint: gosec rseed := rand.New(rand.NewSource(int64(randSeed))) - nlopt, err := CreateNloptIKSolver(ptgFrame, logger, 1, defaultGoalThreshold) + nlopt, err := CreateNloptIKSolver(ptgFrame, logger, 1, defaultEpsilon*defaultEpsilon) if err != nil { return nil, err } ptg := &ptgIK{ refDist: refDist, + simPTG: simPTG, ptgFrame: ptgFrame, fastGradDescent: nlopt, randseed: rseed, @@ -47,11 +55,13 @@ func NewPTGIK(simPTG tpspace.PrecomputePTG, logger golog.Logger, refDist float64 return ptg, nil } -func (ptg *ptgIK) CToTP(ctx context.Context, pose spatialmath.Pose) []*tpspace.TrajNode { +func (ptg *ptgIK) CToTP(ctx context.Context, pose spatialmath.Pose) (*tpspace.TrajNode, error) { solutionGen := make(chan []referenceframe.Input, 1) - // Spawn the IK solver to generate solutions until done - err := ptg.fastGradDescent.Solve(ctx, solutionGen, pose, pathMetric, ptg.randseed.Int()) + + goalMetric := NewPositionOnlyMetric(pose) + // Spawn the IK solver to generate a solution + err := ptg.fastGradDescent.Solve(ctx, solutionGen, []referenceframe.Input{{math.Pi/4}, {ptg.refDist/2}}, goalMetric, ptg.randseed.Int()) // We should have zero or one solutions var solved []referenceframe.Input select { @@ -60,8 +70,15 @@ func (ptg *ptgIK) CToTP(ctx context.Context, pose spatialmath.Pose) []*tpspace.T } close(solutionGen) if err != nil { - return nil + return nil, err + } + fmt.Println("calling trajectory", solved) + // TODO: make this more efficient + traj, err := ptg.Trajectory(solved[0].Value, solved[1].Value) + if err != nil { + return nil, err } + return traj[len(traj) - 1], nil } func (ptg *ptgIK) RefDistance() float64 { @@ -69,5 +86,5 @@ func (ptg *ptgIK) RefDistance() float64 { } func (ptg *ptgIK) Trajectory(alpha, dist float64) ([]*tpspace.TrajNode, error) { - return nil, nil + return tpspace.ComputePTG(alpha, ptg.simPTG, defaultMaxTime, dist, defaultDiffT) } diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index b81dc37f9b4..608665b2421 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -248,11 +248,11 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) // Convert cartesian distance to tp-space using inverse curPtg, yielding TP-space coordinates goalK and goalD - nodes := curPtg.CToTP(ctx, relPose) - bestNode, bestDist := mp.closestNode(relPose, nodes, mp.algOpts.dupeNodeBuffer) - if bestNode == nil { - return nil, nil + bestNode, err := curPtg.CToTP(ctx, relPose) + if err != nil { + return nil, err } + bestDist := mp.planOpts.DistanceFunc(&Segment{StartPosition: relPose, EndPosition: bestNode.Pose}) goalAlpha := bestNode.Alpha goalD := bestNode.Dist // Check collisions along this traj and get the longest distance viable @@ -269,16 +269,13 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( isLastNode := true // Check each point along the trajectory to confirm constraints are met for _, trajPt := range trajK { - if trajPt.Dist > goalD { - // After we've passed randD, no need to keep checking, just add to RRT tree - isLastNode = false - break - } + sinceLastCollideCheck += (trajPt.Dist - lastDist) trajState := &State{Position: spatialmath.Compose(nearest.Pose(), trajPt.Pose), Frame: mp.frame} if sinceLastCollideCheck > planOpts.Resolution { ok, _ := planOpts.CheckStateConstraints(trajState) if !ok { + fmt.Println("not ok") return nil, nil } sinceLastCollideCheck = 0. @@ -436,42 +433,28 @@ func (mp *tpSpaceRRTMotionPlanner) setupTPSpaceOptions() { mp.algOpts = tpOpt } -// closestNode will look through a set of nodes and return the one that is closest to a given pose -// A minimum distance may be passed beyond which nodes are disregarded. -func (mp *tpSpaceRRTMotionPlanner) closestNode(pose spatialmath.Pose, nodes []*tpspace.TrajNode, min float64) (*tpspace.TrajNode, float64) { - var bestNode *tpspace.TrajNode - bestDist := math.Inf(1) - for _, tNode := range nodes { - if tNode.Dist < min { - continue - } - dist := mp.planOpts.DistanceFunc(&Segment{StartPosition: pose, EndPosition: tNode.Pose}) - if dist < bestDist { - bestNode = tNode - bestDist = dist - } - } - return bestNode, bestDist -} - // make2DTPSpaceDistanceOptions will create a plannerOptions object with a custom DistanceFunc constructed such that // distances can be computed in TP space using the given PTG. func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, min float64) *plannerOptions { opts := newBasicPlannerOptions() - segMet := func(seg *Segment) float64 { + segMetric := func(seg *Segment) float64 { if seg.StartPosition == nil || seg.EndPosition == nil { return math.Inf(1) } relPose := spatialmath.Compose(spatialmath.PoseInverse(seg.StartPosition), seg.EndPosition) - nodes := ptg.CToTP(context.Background(), relPose) - closeNode, _ := mp.closestNode(relPose, nodes, min) + closeNode, err := ptg.CToTP(context.Background(), relPose) + if err != nil { + return math.Inf(1) + } + closeDist := mp.planOpts.DistanceFunc(&Segment{StartPosition: relPose, EndPosition: closeNode.Pose}) + fmt.Println("close", closeNode.Pose.Point(), closeDist) if closeNode == nil { return math.Inf(1) } - return closeNode.Dist + return closeDist } - opts.DistanceFunc = segMet + opts.DistanceFunc = segMetric return opts } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 94be24a2ad3..a338675eb2a 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -3,6 +3,7 @@ package motionplan import ( + "fmt" "context" "math/rand" "testing" @@ -23,8 +24,9 @@ func TestPtgRrt(t *testing.T) { test.That(t, err, test.ShouldBeNil) geometries := []spatialmath.Geometry{roverGeom} - ackermanFrame, err := referenceframe.NewPTGFrameFromTurningRadius( + ackermanFrame, err := NewPTGFrameFromTurningRadius( "ackframe", + logger, 300., testTurnRad, 0, @@ -32,18 +34,23 @@ func TestPtgRrt(t *testing.T) { ) test.That(t, err, test.ShouldBeNil) - goalPos := spatialmath.NewPose(r3.Vector{X: 50, Y: 10, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 180}) + goalPos := spatialmath.NewPose(r3.Vector{X: 100, Y: 700, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 180}) opt := newBasicPlannerOptions() opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) opt.DistanceFunc = SquaredNormNoOrientSegmentMetric - opt.GoalThreshold = 10. + //~ opt.SetGoalMetric(NewSquaredNormMetric(goalPos)) + //~ opt.DistanceFunc = SquaredNormSegmentMetric + opt.GoalThreshold = 5. mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) tp, ok := mp.(*tpSpaceRRTMotionPlanner) test.That(t, ok, test.ShouldBeTrue) plan, err := tp.plan(context.Background(), goalPos, nil) + for _, wp := range plan { + fmt.Println(wp.Q()) + } test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThanOrEqualTo, 2) } @@ -53,8 +60,9 @@ func TestPtgWithObstacle(t *testing.T) { roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") test.That(t, err, test.ShouldBeNil) geometries := []spatialmath.Geometry{roverGeom} - ackermanFrame, err := referenceframe.NewPTGFrameFromTurningRadius( + ackermanFrame, err := NewPTGFrameFromTurningRadius( "ackframe", + logger, 300., testTurnRad, 0, @@ -70,9 +78,11 @@ func TestPtgWithObstacle(t *testing.T) { fs.AddFrame(ackermanFrame, fs.World()) opt := newBasicPlannerOptions() - opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) - opt.DistanceFunc = SquaredNormNoOrientSegmentMetric - opt.GoalThreshold = 30. + //~ opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) + //~ opt.DistanceFunc = SquaredNormNoOrientSegmentMetric + opt.SetGoalMetric(NewSquaredNormMetric(goalPos)) + opt.DistanceFunc = SquaredNormSegmentMetric + opt.GoalThreshold = 5. // obstacles obstacle1, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, -500, 0}), r3.Vector{180, 1800, 1}, "") test.That(t, err, test.ShouldBeNil) @@ -107,3 +117,35 @@ func TestPtgWithObstacle(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThan, 2) } + +func TestIKPtgRrt(t *testing.T) { + logger := golog.NewTestLogger(t) + roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") + test.That(t, err, test.ShouldBeNil) + geometries := []spatialmath.Geometry{roverGeom} + + ackermanFrame, err := NewPTGFrameFromTurningRadius( + "ackframe", + logger, + 300., + testTurnRad, + 0, + geometries, + ) + test.That(t, err, test.ShouldBeNil) + + goalPos := spatialmath.NewPose(r3.Vector{X: 50, Y: 10, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 180}) + + opt := newBasicPlannerOptions() + opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) + opt.DistanceFunc = SquaredNormNoOrientSegmentMetric + opt.GoalThreshold = 10. + mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) + test.That(t, err, test.ShouldBeNil) + tp, ok := mp.(*tpSpaceRRTMotionPlanner) + test.That(t, ok, test.ShouldBeTrue) + + plan, err := tp.plan(context.Background(), goalPos, nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, len(plan), test.ShouldBeGreaterThanOrEqualTo, 2) +} diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index 9ed689ff1fc..bf30dc412f6 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -17,8 +17,8 @@ const floatEpsilon = 0.0001 // If floats are closer than this consider them equa // PTG coordinates are specified in polar coordinates (alpha, d) // One of these is needed for each sort of motion that can be done. type PTG interface { - // CToTP Converts a pose to a (k, d) TP-space trajectory, returning the set of trajectory nodes leading to that pose - CToTP(context.Context, spatialmath.Pose) []*TrajNode + // CToTP Converts a pose to a (k, d) TP-space trajectory, returning the node closest to that pose + CToTP(context.Context, spatialmath.Pose) (*TrajNode, error) // RefDistance returns the maximum distance that a single trajectory may travel RefDistance() float64 @@ -37,7 +37,7 @@ type PTGProvider interface { // PrecomputePTG is a precomputable PTG. type PrecomputePTG interface { // PTGVelocities returns the linear and angular velocity at a specific point along a trajectory - PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) + PTGVelocities(alpha, dist, x, y, phi float64) (float64, float64, error) Transform([]referenceframe.Input) (spatialmath.Pose, error) } @@ -68,8 +68,9 @@ func index2alpha(k, numPaths uint) float64 { } func alpha2index(alpha float64, numPaths uint) uint { - alpha = wrapTo2Pi(alpha) - return uint(math.Round(0.5 * (float64(numPaths)*(1.0+alpha/math.Pi) - 1.0))) + alpha = wrapTo2Pi(alpha + math.Pi) - math.Pi + idx := uint(math.Round(0.5 * (float64(numPaths)*(1.0+alpha/math.Pi) - 1.0))) + return idx } // Returns a given angle in the [0, 2pi) range. @@ -80,3 +81,67 @@ func wrapTo2Pi(theta float64) float64 { func xythetaToPose(x, y, theta float64) spatialmath.Pose { return spatialmath.NewPose(r3.Vector{x, y, 0}, &spatialmath.OrientationVector{OZ: 1, Theta: theta}) } + +func ComputePTG( + alpha float64, + simPtg PrecomputePTG, + maxTime, refDist, diffT float64, + +) ([]*TrajNode, error) { + // Initialize trajectory with an all-zero node + alphaTraj := []*TrajNode{{Pose: spatialmath.NewZeroPose()}} + + var err error + var w float64 + var v float64 + var x float64 + var y float64 + var phi float64 + var t float64 + dist := math.Abs(v) * diffT + + // Last saved waypoints + accumulatedHeadingChange := 0. + + lastVs := [2]float64{0, 0} + lastWs := [2]float64{0, 0} + + // Step through each time point for this alpha + for t < maxTime && dist < refDist && accumulatedHeadingChange < defaultMaxHeadingChange { + + v, w, err = simPtg.PTGVelocities(alpha, dist, x, y, phi) + if err != nil { + return nil, err + } + lastVs[1] = lastVs[0] + lastWs[1] = lastWs[0] + lastVs[0] = v + lastWs[0] = w + + accumulatedHeadingChange += w * diffT + + t += diffT + + // Update velocities of last node because reasons + alphaTraj[len(alphaTraj)-1].LinVelMMPS = v + alphaTraj[len(alphaTraj)-1].AngVelRPS = w + + pose, err := simPtg.Transform([]referenceframe.Input{{alpha}, {dist}}) + if err != nil { + return nil, err + } + alphaTraj = append(alphaTraj, &TrajNode{pose, t, dist, alpha, v, w, pose.Point().X, pose.Point().Y}) + dist += math.Abs(v) * diffT + } + + // Add final node + alphaTraj[len(alphaTraj)-1].LinVelMMPS = v + alphaTraj[len(alphaTraj)-1].AngVelRPS = w + pose, err := simPtg.Transform([]referenceframe.Input{{alpha}, {refDist}}) + if err != nil { + return nil, err + } + tNode := &TrajNode{pose, t, refDist, alpha, v, w, pose.Point().X, pose.Point().Y} + alphaTraj = append(alphaTraj, tNode) + return alphaTraj, nil +} diff --git a/motionplan/tpspace/ptgC.go b/motionplan/tpspace/ptgC.go index 4c463ea7384..3f5a026d63e 100644 --- a/motionplan/tpspace/ptgC.go +++ b/motionplan/tpspace/ptgC.go @@ -14,26 +14,24 @@ import ( type ptgDiffDriveC struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius - k float64 // k = +1 for forwards, -1 for backwards } // NewCirclePTG creates a new PrecomputePTG of type ptgDiffDriveC. -func NewCirclePTG(maxMMPS, maxRPS, k float64) PrecomputePTG { +func NewCirclePTG(maxMMPS, maxRPS float64) PrecomputePTG { return &ptgDiffDriveC{ maxMMPS: maxMMPS, maxRPS: maxRPS, - k: k, } } // For this particular driver, turns alpha into a linear + angular velocity. Linear is just max * fwd/back. // Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased // because they will have zero linear velocity through their turns, not max. -func (ptg *ptgDiffDriveC) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { +func (ptg *ptgDiffDriveC) PTGVelocities(alpha, dist, x, y, phi float64) (float64, float64, error) { // (v,w) - v := ptg.maxMMPS * ptg.k - // Use a linear mapping: (Old was: w = tan( alpha/2 ) * W_MAX * sign(K)) - w := (alpha / math.Pi) * ptg.maxRPS * ptg.k + k := math.Copysign(1.0, dist) + v := ptg.maxMMPS * k + w := (alpha / math.Pi) * ptg.maxRPS * k return v, w, nil } @@ -50,7 +48,7 @@ func (ptg *ptgDiffDriveC) Transform(inputs []referenceframe.Input) (spatialmath. dist := inputs[1].Value // Check for OOB within FP error - if math.Pi - math.Abs(alpha) > floatEpsilon { + if math.Pi - math.Abs(alpha) > math.Pi + floatEpsilon { return nil, fmt.Errorf("ptgC input 0 is limited to [-pi, pi] but received %f", inputs[0]) } diff --git a/motionplan/tpspace/ptgCC.go b/motionplan/tpspace/ptgCC.go index 98b53f41050..d890cf88de4 100644 --- a/motionplan/tpspace/ptgCC.go +++ b/motionplan/tpspace/ptgCC.go @@ -13,20 +13,18 @@ import ( type ptgDiffDriveCC struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius - k float64 // k = +1 for forwards, -1 for backwards circle *ptgDiffDriveC } // NewCCPTG creates a new PrecomputePTG of type ptgDiffDriveCC. -func NewCCPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { +func NewCCPTG(maxMMPS, maxRPS float64) PrecomputePTG { - circle := NewCirclePTG(maxMMPS, maxRPS, k).(*ptgDiffDriveC) + circle := NewCirclePTG(maxMMPS, maxRPS).(*ptgDiffDriveC) return &ptgDiffDriveCC{ maxMMPS: maxMMPS, maxRPS: maxRPS, - k: k, circle: circle, } } @@ -34,7 +32,8 @@ func NewCCPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { // For this particular driver, turns alpha into a linear + angular velocity. Linear is just max * fwd/back. // Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased // because they will have zero linear velocity through their turns, not max. -func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { +func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, dist, x, y, phi float64) (float64, float64, error) { + k := math.Copysign(1.0, dist) r := ptg.maxMMPS / ptg.maxRPS u := math.Abs(alpha) * 0.5 @@ -42,11 +41,11 @@ func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, t, x, y, phi float64) (float64, v := 0. w := 0. - if t < u*r/ptg.maxMMPS { + if dist < u*r { // l- v = -ptg.maxMMPS w = ptg.maxRPS - } else if t < (u+math.Pi*0.5)*r/ptg.maxMMPS { + } else if dist < (u+math.Pi*0.5)*r { // l+ v = ptg.maxMMPS w = ptg.maxRPS @@ -57,8 +56,8 @@ func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, t, x, y, phi float64) (float64, w *= -1 } - v *= ptg.k - w *= ptg.k + v *= k + w *= k return v, w, nil } diff --git a/motionplan/tpspace/ptgCCS.go b/motionplan/tpspace/ptgCCS.go index 5657ff96d8b..00d682b8dc6 100644 --- a/motionplan/tpspace/ptgCCS.go +++ b/motionplan/tpspace/ptgCCS.go @@ -12,21 +12,19 @@ import ( type ptgDiffDriveCCS struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius - k float64 // k = +1 for forwards, -1 for backwards r float64 circle *ptgDiffDriveC } // NewCCSPTG creates a new PrecomputePTG of type ptgDiffDriveCCS. -func NewCCSPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { +func NewCCSPTG(maxMMPS, maxRPS float64) PrecomputePTG { r := maxMMPS / maxRPS - circle := NewCirclePTG(maxMMPS, maxRPS, k).(*ptgDiffDriveC) + circle := NewCirclePTG(maxMMPS, maxRPS).(*ptgDiffDriveC) return &ptgDiffDriveCCS{ maxMMPS: maxMMPS, maxRPS: maxRPS, - k: k, r: r, circle: circle, } @@ -35,17 +33,18 @@ func NewCCSPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { // For this particular driver, turns alpha into a linear + angular velocity. Linear is just max * fwd/back. // Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased // because they will have zero linear velocity through their turns, not max. -func (ptg *ptgDiffDriveCCS) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { +func (ptg *ptgDiffDriveCCS) PTGVelocities(alpha, dist, x, y, phi float64) (float64, float64, error) { u := math.Abs(alpha) * 0.5 + k := math.Copysign(1.0, dist) v := ptg.maxMMPS w := 0. - if t < u*ptg.r/ptg.maxMMPS { + if dist < u*ptg.r { // l- v = -ptg.maxMMPS w = ptg.maxRPS - } else if t < (u+math.Pi/2)*ptg.r/ptg.maxMMPS { + } else if dist < (u+math.Pi/2)*ptg.r { // l+ pi/2 v = ptg.maxMMPS w = ptg.maxRPS @@ -56,8 +55,8 @@ func (ptg *ptgDiffDriveCCS) PTGVelocities(alpha, t, x, y, phi float64) (float64, w *= -1 } - v *= ptg.k - w *= ptg.k + v *= k + w *= k return v, w, nil } diff --git a/motionplan/tpspace/ptgCS.go b/motionplan/tpspace/ptgCS.go index d9f3656e113..7abb69a15e3 100644 --- a/motionplan/tpspace/ptgCS.go +++ b/motionplan/tpspace/ptgCS.go @@ -16,7 +16,6 @@ const ( type ptgDiffDriveCS struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius - k float64 // k = +1 for forwards, -1 for backwards r float64 // turning radius turnStraight float64 @@ -24,14 +23,13 @@ type ptgDiffDriveCS struct { } // NewCSPTG creates a new PrecomputePTG of type ptgDiffDriveCS. -func NewCSPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { +func NewCSPTG(maxMMPS, maxRPS float64) PrecomputePTG { r := maxMMPS / maxRPS turnStraight := 1.2 * r - circle := NewCirclePTG(maxMMPS, maxRPS, k).(*ptgDiffDriveC) + circle := NewCirclePTG(maxMMPS, maxRPS).(*ptgDiffDriveC) return &ptgDiffDriveCS{ maxMMPS: maxMMPS, maxRPS: maxRPS, - k: k, r: r, turnStraight: turnStraight, circle: circle, @@ -41,14 +39,15 @@ func NewCSPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { // For this particular driver, turns alpha into a linear + angular velocity. Linear is just max * fwd/back. // Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased // because they will have zero linear velocity through their turns, not max. -func (ptg *ptgDiffDriveCS) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { +func (ptg *ptgDiffDriveCS) PTGVelocities(alpha, dist, x, y, phi float64) (float64, float64, error) { // Magic number; rotate this much before going straight - turnSecs := math.Sqrt(math.Abs(alpha)) * ptg.turnStraight / ptg.maxMMPS + turnDist := math.Sqrt(math.Abs(alpha)) * ptg.turnStraight + k := math.Copysign(1.0, dist) v := ptg.maxMMPS w := 0. - if t < turnSecs { + if dist < turnDist { // l+ v = ptg.maxMMPS w = ptg.maxRPS * math.Min(1.0, 1.0-math.Exp(-1*alpha*alpha)) @@ -59,8 +58,8 @@ func (ptg *ptgDiffDriveCS) PTGVelocities(alpha, t, x, y, phi float64) (float64, w *= -1 } - v *= ptg.k - w *= ptg.k + v *= k + w *= k return v, w, nil } diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index a556f6ba1c4..ed62f5d3039 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -62,7 +62,7 @@ func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64) (PTG, error return ptg, nil } -func (ptg *ptgGridSim) CToTP(ctx context.Context, pose spatialmath.Pose) []*TrajNode { +func (ptg *ptgGridSim) CToTP(ctx context.Context, pose spatialmath.Pose) (*TrajNode, error) { point := pose.Point() x := point.X @@ -80,14 +80,22 @@ func (ptg *ptgGridSim) CToTP(ctx context.Context, pose spatialmath.Pose) []*Traj } } - if len(nearbyNodes) > 0 { - return nearbyNodes - } - // Try to find a closest point to the paths: bestDist := math.Inf(1) var bestNode *TrajNode + if len(nearbyNodes) > 0 { + for _, nearbyNode := range nearbyNodes { + distToPoint := math.Pow(nearbyNode.ptX-x, 2) + math.Pow(nearbyNode.ptY-y, 2) + if distToPoint < bestDist { + bestDist = distToPoint + + bestNode = nearbyNode + } + } + return bestNode, nil + } + for k := 0; k < int(ptg.alphaCnt); k++ { nMax := len(ptg.precomputeTraj[k]) - 1 for n := 0; n <= nMax; n++ { @@ -101,7 +109,7 @@ func (ptg *ptgGridSim) CToTP(ctx context.Context, pose spatialmath.Pose) []*Traj } if bestNode != nil { - return []*TrajNode{bestNode} + return bestNode, nil } // Given a point (x,y), compute the "k_closest" whose extrapolation @@ -119,7 +127,7 @@ func (ptg *ptgGridSim) CToTP(ctx context.Context, pose spatialmath.Pose) []*Traj } } - return []*TrajNode{bestNode} + return bestNode, nil } func (ptg *ptgGridSim) RefDistance() float64 { @@ -147,82 +155,26 @@ func (ptg *ptgGridSim) Trajectory(alpha, dist float64) ([]*TrajNode, error) { } func (ptg *ptgGridSim) simulateTrajectories(simPtg PrecomputePTG) ([][]*TrajNode, error) { - xMin := 500.0 - xMax := -500.0 - yMin := 500.0 - yMax := -500.0 - // C-space path structure allTraj := make([][]*TrajNode, 0, ptg.alphaCnt) for k := uint(0); k < ptg.alphaCnt; k++ { alpha := index2alpha(k, ptg.alphaCnt) - - // Initialize trajectory with an all-zero node - alphaTraj := []*TrajNode{{Pose: spatialmath.NewZeroPose()}} - - var err error - var w float64 - var v float64 - var x float64 - var y float64 - var phi float64 - var t float64 - var dist float64 - - // Last saved waypoints - accumulatedHeadingChange := 0. - - lastVs := [2]float64{0, 0} - lastWs := [2]float64{0, 0} - - // Step through each time point for this alpha - for t < ptg.maxTime && dist < ptg.refDist && accumulatedHeadingChange < defaultMaxHeadingChange { - v, w, err = simPtg.PTGVelocities(alpha, t, x, y, phi) - if err != nil { - return nil, err - } - lastVs[1] = lastVs[0] - lastWs[1] = lastWs[0] - lastVs[0] = v - lastWs[0] = w - - // finite difference eq - x += math.Cos(phi) * v * ptg.diffT - y += math.Sin(phi) * v * ptg.diffT - phi += w * ptg.diffT - accumulatedHeadingChange += w * ptg.diffT - - dist += v * ptg.diffT - t += ptg.diffT - - // Update velocities of last node because reasons - alphaTraj[len(alphaTraj)-1].LinVelMMPS = v - alphaTraj[len(alphaTraj)-1].AngVelRPS = w - - pose := xythetaToPose(x, y, phi) - alphaTraj = append(alphaTraj, &TrajNode{pose, t, dist, alpha, v, w, pose.Point().X, pose.Point().Y}) - - // For the grid! - xMin = math.Min(xMin, x) - xMax = math.Max(xMax, x) - yMin = math.Min(yMin, y) - yMax = math.Max(yMax, y) + + alphaTraj, err := ComputePTG(alpha, simPtg, ptg.maxTime, ptg.refDist, ptg.diffT) + if err != nil { + return nil, err } - - // Add final node - alphaTraj[len(alphaTraj)-1].LinVelMMPS = v - alphaTraj[len(alphaTraj)-1].AngVelRPS = w - pose := xythetaToPose(x, y, phi) - tNode := &TrajNode{pose, t, dist, alpha, v, w, pose.Point().X, pose.Point().Y} - - // Discretize into a grid for faster lookups later - if _, ok := ptg.trajNodeGrid[int(math.Round(x))]; !ok { - ptg.trajNodeGrid[int(math.Round(x))] = map[int][]*TrajNode{} + + for _, tNode := range alphaTraj { + gridX := int(math.Round(tNode.ptX)) + gridY := int(math.Round(tNode.ptY)) + // Discretize into a grid for faster lookups later + if _, ok := ptg.trajNodeGrid[gridX]; !ok { + ptg.trajNodeGrid[gridX] = map[int][]*TrajNode{} + } + ptg.trajNodeGrid[gridX][gridY] = append(ptg.trajNodeGrid[gridX][gridY], tNode) } - ptg.trajNodeGrid[int(math.Round(x))][int(math.Round(y))] = append(ptg.trajNodeGrid[int(math.Round(x))][int(math.Round(y))], tNode) - - alphaTraj = append(alphaTraj, tNode) allTraj = append(allTraj, alphaTraj) } diff --git a/motionplan/tpspace/ptgGridSimFrame.go b/motionplan/tpspace/ptgGridSimFrame.go deleted file mode 100644 index 0a78bc33284..00000000000 --- a/motionplan/tpspace/ptgGridSimFrame.go +++ /dev/null @@ -1,155 +0,0 @@ -package tpspace - -import ( - "fmt" - "math" - - pb "go.viam.com/api/component/arm/v1" - - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/spatialmath" -) - -const ( - defaultSimDistMM = 2000. -) - -const ( - ptgIndex int = iota - trajectoryAlphaWithinPTG - distanceAlongTrajectoryIndex -) - -type ptgFactory func(float64, float64, float64) PrecomputePTG - -var defaultPTGs = []ptgFactory{ - NewCirclePTG, - NewCCPTG, - NewCCSPTG, - NewCSPTG, - NewAlphaPTG, -} - -type ptgGridSimFrame struct { - name string - limits []referenceframe.Limit - geometries []spatialmath.Geometry - ptgs []PTG -} - -// NewPTGFrameFromTurningRadius will create a new Frame which is also a PTGProvider. It will precompute the default set of -// trajectories out to a given distance, or a default distance if the given distance is <= 0. -func NewPTGFrameFromTurningRadius(name string, velocityMMps, turnRadMeters, simDist float64, geoms []spatialmath.Geometry) (referenceframe.Frame, error) { - if velocityMMps <= 0 { - return nil, fmt.Errorf("cannot create ptg frame, movement velocity %f must be >0", velocityMMps) - } - if turnRadMeters <= 0 { - return nil, fmt.Errorf("cannot create ptg frame, turning radius %f must be >0", turnRadMeters) - } - - if simDist <= 0 { - simDist = defaultSimDistMM - } - - // Get max angular velocity in radians per second - maxRPS := velocityMMps / (1000. * turnRadMeters) - pf := &ptgGridSimFrame{name: name} - err := pf.initPTGs(velocityMMps, maxRPS, simDist) - if err != nil { - return nil, err - } - - pf.geometries = geoms - - pf.limits = []referenceframe.Limit{ - {Min: 0, Max: float64(len(pf.ptgs) - 1)}, - {Min: 0, Max: float64(defaultAlphaCnt)}, - {Min: 0, Max: simDist}, - } - - return pf, nil -} - -func (pf *ptgGridSimFrame) DoF() []referenceframe.Limit { - return pf.limits -} - -func (pf *ptgGridSimFrame) Name() string { - return pf.name -} - -// TODO: Define some sort of config struct for a PTG frame. -func (pf *ptgGridSimFrame) MarshalJSON() ([]byte, error) { - return nil, nil -} - -// Inputs are: [0] index of PTG to use, [1] index of the trajectory within that PTG, and [2] distance to travel along that trajectory. -func (pf *ptgGridSimFrame) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { - alpha := inputs[trajectoryAlphaWithinPTG].Value - dist := inputs[distanceAlongTrajectoryIndex].Value - - ptgIdx := int(math.Round(inputs[ptgIndex].Value)) - - traj, err := pf.ptgs[ptgIdx].Trajectory(alpha, dist) - if err != nil { - return nil, err - } - - return traj[len(traj)-1].Pose, nil -} - -func (pf *ptgGridSimFrame) InputFromProtobuf(jp *pb.JointPositions) []referenceframe.Input { - n := make([]referenceframe.Input, len(jp.Values)) - for idx, d := range jp.Values { - n[idx] = referenceframe.Input{d} - } - return n -} - -func (pf *ptgGridSimFrame) ProtobufFromInput(input []referenceframe.Input) *pb.JointPositions { - n := make([]float64, len(input)) - for idx, a := range input { - n[idx] = a.Value - } - return &pb.JointPositions{Values: n} -} - -func (pf *ptgGridSimFrame) Geometries(inputs []referenceframe.Input) (*referenceframe.GeometriesInFrame, error) { - if len(pf.geometries) == 0 { - return referenceframe.NewGeometriesInFrame(pf.Name(), nil), nil - } - - transformedPose, err := pf.Transform(inputs) - if err != nil { - return nil, err - } - geoms := make([]spatialmath.Geometry, 0, len(pf.geometries)) - for _, geom := range pf.geometries { - geoms = append(geoms, geom.Transform(transformedPose)) - } - return referenceframe.NewGeometriesInFrame(pf.name, geoms), nil -} - -func (pf *ptgGridSimFrame) PTGs() []PTG { - return pf.ptgs -} - -func (pf *ptgGridSimFrame) initPTGs(maxMps, maxRPS, simDist float64) error { - ptgs := []PTG{} - for _, ptg := range defaultPTGs { - for _, k := range []float64{1., -1.} { - // Positive K calculates trajectories forwards, negative k calculates trajectories backwards - ptgGen := ptg(maxMps, maxRPS, k) - if ptgGen != nil { - // irreversible trajectories, e.g. alpha, will return nil for negative k - newptg, err := NewPTGGridSim(ptgGen, defaultAlphaCnt, simDist) - if err != nil { - return err - } - ptgs = append(ptgs, newptg) - } - } - } - pf.ptgs = ptgs - return nil -} diff --git a/motionplan/tpspace/ptgGridSim_test.go b/motionplan/tpspace/ptgGridSim_test.go index 4f22427460b..e6ed7bf0ff1 100644 --- a/motionplan/tpspace/ptgGridSim_test.go +++ b/motionplan/tpspace/ptgGridSim_test.go @@ -6,14 +6,6 @@ import ( "go.viam.com/test" ) -var defaultPTGs = []func(float64, float64, float64) PrecomputePTG{ - NewCirclePTG, - NewCCPTG, - NewCCSPTG, - NewCSPTG, - NewAlphaPTG, -} - var ( defaultMps = 0.3 turnRadMeters = 0.3 diff --git a/motionplan/tpspace/ptgIKFrame.go b/motionplan/tpspace/ptgIKFrame.go index 412fe0e7701..e425e2b0f95 100644 --- a/motionplan/tpspace/ptgIKFrame.go +++ b/motionplan/tpspace/ptgIKFrame.go @@ -19,7 +19,7 @@ func NewPTGIKFrame(ptg PrecomputePTG, dist float64) (referenceframe.Frame, error pf.limits = []referenceframe.Limit{ {Min: -math.Pi, Max: math.Pi}, - {Min: 0, Max: dist}, + {Min: -dist, Max: dist}, } return pf, nil } From 667232376d95dd625a5c787830bc0c0b60fa2227 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 9 Aug 2023 15:28:23 -0700 Subject: [PATCH 09/38] Add files --- motionplan/ptgGroupFrame.go | 172 +++++++++++++++++++++++++++++++++ motionplan/tpspace/ptg_test.go | 15 +++ 2 files changed, 187 insertions(+) create mode 100644 motionplan/ptgGroupFrame.go create mode 100644 motionplan/tpspace/ptg_test.go diff --git a/motionplan/ptgGroupFrame.go b/motionplan/ptgGroupFrame.go new file mode 100644 index 00000000000..0c5834b2d2e --- /dev/null +++ b/motionplan/ptgGroupFrame.go @@ -0,0 +1,172 @@ +package motionplan + +import ( + "fmt" + "math" + + pb "go.viam.com/api/component/arm/v1" + "github.com/edaniels/golog" + + "go.viam.com/rdk/motionplan/tpspace" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" +) + +const ( + defaultSimDistMM = 2000. +) + +const ( + ptgIndex int = iota + trajectoryAlphaWithinPTG + distanceAlongTrajectoryIndex +) + +type ptgFactory func(float64, float64) tpspace.PrecomputePTG + +var defaultPTGs = []ptgFactory{ + //~ tpspace.NewCirclePTG, + //~ tpspace.NewCCPTG, + //~ tpspace.NewCCSPTG, + tpspace.NewCSPTG, + //~ tpspace.NewAlphaPTG, +} + +type ptgGroupFrame struct { + name string + limits []referenceframe.Limit + geometries []spatialmath.Geometry + ptgs []tpspace.PTG +} + +// NewPTGFrameFromTurningRadius will create a new Frame which is also a PTGProvider. It will precompute the default set of +// trajectories out to a given distance, or a default distance if the given distance is <= 0. +func NewPTGFrameFromTurningRadius( + name string, + logger golog.Logger, + velocityMMps, turnRadMeters, refDist float64, + geoms []spatialmath.Geometry, +) (referenceframe.Frame, error) { + if velocityMMps <= 0 { + return nil, fmt.Errorf("cannot create ptg frame, movement velocity %f must be >0", velocityMMps) + } + if turnRadMeters <= 0 { + return nil, fmt.Errorf("cannot create ptg frame, turning radius %f must be >0", turnRadMeters) + } + + if refDist <= 0 { + refDist = defaultSimDistMM + } + + // Get max angular velocity in radians per second + maxRPS := velocityMMps / (1000. * turnRadMeters) + pf := &ptgGroupFrame{name: name} + err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist, true) + //~ err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist, true) + if err != nil { + return nil, err + } + + pf.geometries = geoms + + pf.limits = []referenceframe.Limit{ + {Min: 0, Max: float64(len(pf.ptgs) - 1)}, + {Min: -math.Pi, Max: math.Pi}, + {Min: 0, Max: refDist}, + } + + return pf, nil +} + +func (pf *ptgGroupFrame) DoF() []referenceframe.Limit { + return pf.limits +} + +func (pf *ptgGroupFrame) Name() string { + return pf.name +} + +// TODO: Define some sort of config struct for a PTG frame. +func (pf *ptgGroupFrame) MarshalJSON() ([]byte, error) { + return nil, nil +} + +// Inputs are: [0] index of PTG to use, [1] index of the trajectory within that PTG, and [2] distance to travel along that trajectory. +func (pf *ptgGroupFrame) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + alpha := inputs[trajectoryAlphaWithinPTG].Value + dist := inputs[distanceAlongTrajectoryIndex].Value + + ptgIdx := int(math.Round(inputs[ptgIndex].Value)) + + traj, err := pf.ptgs[ptgIdx].Trajectory(alpha, dist) + if err != nil { + return nil, err + } + + return traj[len(traj)-1].Pose, nil +} + +func (pf *ptgGroupFrame) InputFromProtobuf(jp *pb.JointPositions) []referenceframe.Input { + n := make([]referenceframe.Input, len(jp.Values)) + for idx, d := range jp.Values { + n[idx] = referenceframe.Input{d} + } + return n +} + +func (pf *ptgGroupFrame) ProtobufFromInput(input []referenceframe.Input) *pb.JointPositions { + n := make([]float64, len(input)) + for idx, a := range input { + n[idx] = a.Value + } + return &pb.JointPositions{Values: n} +} + +func (pf *ptgGroupFrame) Geometries(inputs []referenceframe.Input) (*referenceframe.GeometriesInFrame, error) { + if len(pf.geometries) == 0 { + return referenceframe.NewGeometriesInFrame(pf.Name(), nil), nil + } + + transformedPose, err := pf.Transform(inputs) + if err != nil { + return nil, err + } + geoms := make([]spatialmath.Geometry, 0, len(pf.geometries)) + for _, geom := range pf.geometries { + geoms = append(geoms, geom.Transform(transformedPose)) + } + return referenceframe.NewGeometriesInFrame(pf.name, geoms), nil +} + +func (pf *ptgGroupFrame) PTGs() []tpspace.PTG { + return pf.ptgs +} + +func (pf *ptgGroupFrame) initPTGs(logger golog.Logger, maxMps, maxRPS, simDist float64, simulate bool) error { + ptgs := []tpspace.PTG{} + for _, ptg := range defaultPTGs { + // Positive K calculates trajectories forwards, negative k calculates trajectories backwards + ptgGen := ptg(maxMps, maxRPS) + if ptgGen != nil { + if simulate { + //~ for _, k := range []float64{1., -1.} { + for _, k := range []float64{1.} { + // irreversible trajectories, e.g. alpha, will return nil for negative k + newptg, err := tpspace.NewPTGGridSim(ptgGen, 0, k*simDist) // 0 uses default alpha count + if err != nil { + return err + } + ptgs = append(ptgs, newptg) + } + } else { + newptg, err := NewPTGIK(ptgGen, logger, simDist, 1) + if err != nil { + return err + } + ptgs = append(ptgs, newptg) + } + } + } + pf.ptgs = ptgs + return nil +} diff --git a/motionplan/tpspace/ptg_test.go b/motionplan/tpspace/ptg_test.go new file mode 100644 index 00000000000..7b9ef65f631 --- /dev/null +++ b/motionplan/tpspace/ptg_test.go @@ -0,0 +1,15 @@ +package tpspace + +import ( + "testing" + + "go.viam.com/test" +) + +func TestAlphaIdx(t *testing.T) { + for i := uint(0); i < defaultAlphaCnt; i++ { + alpha := index2alpha(i, defaultAlphaCnt) + i2 := alpha2index(alpha, defaultAlphaCnt) + test.That(t, i, test.ShouldEqual, i2) + } +} From e17df2653e1500e52c61e3dc65979068ad22db72 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 9 Aug 2023 17:31:16 -0700 Subject: [PATCH 10/38] Kinda working, no obstacle avoidance yet --- motionplan/nloptInverseKinematics.go | 5 +-- motionplan/ptgGroupFrame.go | 10 +++--- motionplan/ptgIK.go | 2 -- motionplan/tpSpaceRRT.go | 2 -- motionplan/tpSpaceRRT_test.go | 43 +++++++++++++++++++--- motionplan/tpspace/ptg.go | 7 ++-- motionplan/tpspace/ptgC.go | 2 +- motionplan/tpspace/ptgCS.go | 19 +++++----- motionplan/tpspace/ptgGridSim_test.go | 52 ++++++++++++++++----------- motionplan/tpspace/ptg_test.go | 10 ++++++ 10 files changed, 103 insertions(+), 49 deletions(-) diff --git a/motionplan/nloptInverseKinematics.go b/motionplan/nloptInverseKinematics.go index b5f8a43d6f8..f4dd9edfb0d 100644 --- a/motionplan/nloptInverseKinematics.go +++ b/motionplan/nloptInverseKinematics.go @@ -179,14 +179,15 @@ func (ik *NloptIK) Solve(ctx context.Context, default: } iterations++ - solutionRaw, result, nloptErr := opt.Optimize(referenceframe.InputsToFloats(startingPos)) + solutionRaw, _, nloptErr := opt.Optimize(referenceframe.InputsToFloats(startingPos)) if nloptErr != nil { // This just *happens* sometimes due to weirdnesses in nonlinear randomized problems. // Ignore it, something else will find a solution err = multierr.Combine(err, nloptErr) } - if result < ik.epsilon*ik.epsilon { + //~ if result < ik.epsilon*ik.epsilon { + if solutionRaw != nil { select { case <-ctx.Done(): return err diff --git a/motionplan/ptgGroupFrame.go b/motionplan/ptgGroupFrame.go index 0c5834b2d2e..0afeebf2d53 100644 --- a/motionplan/ptgGroupFrame.go +++ b/motionplan/ptgGroupFrame.go @@ -13,7 +13,7 @@ import ( ) const ( - defaultSimDistMM = 2000. + defaultSimDistMM = 600. ) const ( @@ -25,9 +25,9 @@ const ( type ptgFactory func(float64, float64) tpspace.PrecomputePTG var defaultPTGs = []ptgFactory{ - //~ tpspace.NewCirclePTG, - //~ tpspace.NewCCPTG, - //~ tpspace.NewCCSPTG, + tpspace.NewCirclePTG, + tpspace.NewCCPTG, + tpspace.NewCCSPTG, tpspace.NewCSPTG, //~ tpspace.NewAlphaPTG, } @@ -61,8 +61,8 @@ func NewPTGFrameFromTurningRadius( // Get max angular velocity in radians per second maxRPS := velocityMMps / (1000. * turnRadMeters) pf := &ptgGroupFrame{name: name} + //~ err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist, false) err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist, true) - //~ err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist, true) if err != nil { return nil, err } diff --git a/motionplan/ptgIK.go b/motionplan/ptgIK.go index db86e4deeee..8d690499c32 100644 --- a/motionplan/ptgIK.go +++ b/motionplan/ptgIK.go @@ -4,7 +4,6 @@ package motionplan import ( "context" "math" - "fmt" "math/rand" "github.com/edaniels/golog" @@ -72,7 +71,6 @@ func (ptg *ptgIK) CToTP(ctx context.Context, pose spatialmath.Pose) (*tpspace.Tr if err != nil { return nil, err } - fmt.Println("calling trajectory", solved) // TODO: make this more efficient traj, err := ptg.Trajectory(solved[0].Value, solved[1].Value) if err != nil { diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index 608665b2421..aba8d1a131a 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -275,7 +275,6 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( if sinceLastCollideCheck > planOpts.Resolution { ok, _ := planOpts.CheckStateConstraints(trajState) if !ok { - fmt.Println("not ok") return nil, nil } sinceLastCollideCheck = 0. @@ -448,7 +447,6 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, return math.Inf(1) } closeDist := mp.planOpts.DistanceFunc(&Segment{StartPosition: relPose, EndPosition: closeNode.Pose}) - fmt.Println("close", closeNode.Pose.Point(), closeDist) if closeNode == nil { return math.Inf(1) } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index a338675eb2a..6bb7085ad17 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -7,11 +7,13 @@ import ( "context" "math/rand" "testing" + //~ "math" "github.com/edaniels/golog" "github.com/golang/geo/r3" "go.viam.com/test" + //~ "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -78,10 +80,10 @@ func TestPtgWithObstacle(t *testing.T) { fs.AddFrame(ackermanFrame, fs.World()) opt := newBasicPlannerOptions() - //~ opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) - //~ opt.DistanceFunc = SquaredNormNoOrientSegmentMetric - opt.SetGoalMetric(NewSquaredNormMetric(goalPos)) - opt.DistanceFunc = SquaredNormSegmentMetric + opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) + opt.DistanceFunc = SquaredNormNoOrientSegmentMetric + //~ opt.SetGoalMetric(NewSquaredNormMetric(goalPos)) + //~ opt.DistanceFunc = SquaredNormSegmentMetric opt.GoalThreshold = 5. // obstacles obstacle1, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, -500, 0}), r3.Vector{180, 1800, 1}, "") @@ -95,6 +97,16 @@ func TestPtgWithObstacle(t *testing.T) { geoms := []spatialmath.Geometry{obstacle1, obstacle2, obstacle3, obstacle4} + //~ fmt.Println("type,X,Y") + //~ for _, geom := range geoms { + //~ pts := geom.ToPoints(1.) + //~ for _, pt := range pts { + //~ if math.Abs(pt.Z) < 0.1 { + //~ fmt.Printf("OBS,%f,%f\n", pt.X, pt.Y) + //~ } + //~ } + //~ } + worldState, err := referenceframe.NewWorldState( []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, geoms)}, nil, @@ -116,8 +128,31 @@ func TestPtgWithObstacle(t *testing.T) { plan, err := tp.plan(ctx, goalPos, nil) test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThan, 2) + for _, wp := range plan { + fmt.Println(wp.Q()) + } + + //~ allPtgs := ackermanFrame.(tpspace.PTGProvider).PTGs() + //~ lastPose := spatialmath.NewZeroPose() + + //~ for _, mynode := range plan { + //~ trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) + //~ for i, pt := range trajPts { + //~ fmt.Println("pt", pt) + //~ intPose := spatialmath.Compose(lastPose, pt.Pose) + //~ if i == 0 { + //~ fmt.Printf("WP,%f,%f\n", intPose.Point().X, intPose.Point().Y) + //~ } + //~ fmt.Printf("FINALPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) + //~ if i == len(trajPts) - 1 { + //~ lastPose = spatialmath.Compose(lastPose, pt.Pose) + //~ break + //~ } + //~ } + //~ } } + func TestIKPtgRrt(t *testing.T) { logger := golog.NewTestLogger(t) roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index bf30dc412f6..e06a87477d5 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -98,7 +98,7 @@ func ComputePTG( var y float64 var phi float64 var t float64 - dist := math.Abs(v) * diffT + dist := math.Copysign(math.Abs(v) * diffT, refDist) // Last saved waypoints accumulatedHeadingChange := 0. @@ -107,8 +107,7 @@ func ComputePTG( lastWs := [2]float64{0, 0} // Step through each time point for this alpha - for t < maxTime && dist < refDist && accumulatedHeadingChange < defaultMaxHeadingChange { - + for t < maxTime && math.Abs(dist) < math.Abs(refDist) && accumulatedHeadingChange < defaultMaxHeadingChange { v, w, err = simPtg.PTGVelocities(alpha, dist, x, y, phi) if err != nil { return nil, err @@ -131,7 +130,7 @@ func ComputePTG( return nil, err } alphaTraj = append(alphaTraj, &TrajNode{pose, t, dist, alpha, v, w, pose.Point().X, pose.Point().Y}) - dist += math.Abs(v) * diffT + dist += math.Copysign(math.Abs(v) * diffT, refDist) } // Add final node diff --git a/motionplan/tpspace/ptgC.go b/motionplan/tpspace/ptgC.go index 3f5a026d63e..7176714229c 100644 --- a/motionplan/tpspace/ptgC.go +++ b/motionplan/tpspace/ptgC.go @@ -63,7 +63,7 @@ func (ptg *ptgDiffDriveC) Transform(inputs []referenceframe.Input) (spatialmath. angleRads := 0. if alpha != 0 { arcRadius := math.Pi * turnRad / math.Abs(alpha) // radius of arc - angleRads = dist / turnRad // number of radians to travel along arc + angleRads = dist / arcRadius // number of radians to travel along arc pt = r3.Vector{arcRadius * (1 - math.Cos(angleRads)), arcRadius * math.Sin(angleRads), 0} if alpha > 0 { // positive alpha = positive rotation = left turn = negative X diff --git a/motionplan/tpspace/ptgCS.go b/motionplan/tpspace/ptgCS.go index 7abb69a15e3..45375543d3d 100644 --- a/motionplan/tpspace/ptgCS.go +++ b/motionplan/tpspace/ptgCS.go @@ -19,20 +19,17 @@ type ptgDiffDriveCS struct { r float64 // turning radius turnStraight float64 - circle *ptgDiffDriveC } // NewCSPTG creates a new PrecomputePTG of type ptgDiffDriveCS. func NewCSPTG(maxMMPS, maxRPS float64) PrecomputePTG { r := maxMMPS / maxRPS turnStraight := 1.2 * r - circle := NewCirclePTG(maxMMPS, maxRPS).(*ptgDiffDriveC) return &ptgDiffDriveCS{ maxMMPS: maxMMPS, maxRPS: maxRPS, r: r, turnStraight: turnStraight, - circle: circle, } } @@ -67,18 +64,24 @@ func (ptg *ptgDiffDriveCS) Transform(inputs []referenceframe.Input) (spatialmath alpha := inputs[0].Value dist := inputs[1].Value + actualRPS := ptg.maxRPS * math.Min(1.0, 1.0-math.Exp(-1*alpha*alpha)) + circle := NewCirclePTG(ptg.maxMMPS, actualRPS).(*ptgDiffDriveC) + arcDistance := ptg.turnStraight * math.Sqrt(math.Abs(alpha)) flip := math.Copysign(1., alpha) // left or right direction := math.Copysign(1., dist) // forwards or backwards - - arcPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {direction * math.Min(dist, arcDistance)}}) - if err != nil { - return nil, err + var err error + arcPose := spatialmath.NewZeroPose() + if alpha != 0 { + arcPose, err = circle.Transform([]referenceframe.Input{{flip * math.Pi}, {direction * math.Min(dist, arcDistance)}}) + if err != nil { + return nil, err + } } if dist < arcDistance { return arcPose, nil } - fwdPose, err := ptg.circle.Transform([]referenceframe.Input{{0}, {direction * (dist - arcDistance)}}) + fwdPose, err := circle.Transform([]referenceframe.Input{{0}, {direction * (dist - arcDistance)}}) if err != nil { return nil, err } diff --git a/motionplan/tpspace/ptgGridSim_test.go b/motionplan/tpspace/ptgGridSim_test.go index e6ed7bf0ff1..dd59a7b9f0b 100644 --- a/motionplan/tpspace/ptgGridSim_test.go +++ b/motionplan/tpspace/ptgGridSim_test.go @@ -1,23 +1,33 @@ package tpspace -import ( - "testing" - - "go.viam.com/test" -) - -var ( - defaultMps = 0.3 - turnRadMeters = 0.3 -) - -func TestSim(t *testing.T) { - for _, ptg := range defaultPTGs { - radPS := defaultMps / turnRadMeters - - ptgGen := ptg(defaultMps, radPS, 1.) - test.That(t, ptgGen, test.ShouldNotBeNil) - _, err := NewPTGGridSim(ptgGen, defaultAlphaCnt, 1000.) - test.That(t, err, test.ShouldBeNil) - } -} +//~ import ( + //~ "testing" + + //~ "go.viam.com/test" +//~ ) + + +//~ type ptgFactory func(float64, float64) tpspace.PrecomputePTG + +//~ var defaultPTGs = []ptgFactory{ + //~ NewCirclePTG, + //~ NewCCPTG, + //~ NewCCSPTG, + //~ NewCSPTG, +//~ } + +//~ var ( + //~ defaultMps = 0.3 + //~ turnRadMeters = 0.3 +//~ ) + +//~ func TestSim(t *testing.T) { + //~ for _, ptg := range defaultPTGs { + //~ radPS := defaultMps / turnRadMeters + + //~ ptgGen := ptg(defaultMps, radPS, 1.) + //~ test.That(t, ptgGen, test.ShouldNotBeNil) + //~ _, err := NewPTGGridSim(ptgGen, defaultAlphaCnt, 1000.) + //~ test.That(t, err, test.ShouldBeNil) + //~ } +//~ } diff --git a/motionplan/tpspace/ptg_test.go b/motionplan/tpspace/ptg_test.go index 7b9ef65f631..88bdc509163 100644 --- a/motionplan/tpspace/ptg_test.go +++ b/motionplan/tpspace/ptg_test.go @@ -2,6 +2,7 @@ package tpspace import ( "testing" + "fmt" "go.viam.com/test" ) @@ -13,3 +14,12 @@ func TestAlphaIdx(t *testing.T) { test.That(t, i, test.ShouldEqual, i2) } } + +func TestRevPTGs(t *testing.T) { + cs := NewCirclePTG(0.3, 0.3) + //~ cs := NewCSPTG(0.3, 0.3) + traj, err := ComputePTG(0, cs, 15., -2000, 0.05) + test.That(t, err, test.ShouldBeNil) + fmt.Println(traj) + +} From 827f26552f479f0d73c74d6f1e1e9bab1e160f7b Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Thu, 10 Aug 2023 13:44:59 -0700 Subject: [PATCH 11/38] Decently-working IK solving, unidirectional --- motionplan/nloptInverseKinematics.go | 16 ++--- motionplan/ptgGroupFrame.go | 10 +-- motionplan/ptgIK.go | 24 +++++--- motionplan/tpSpaceRRT.go | 53 ++++++++-------- motionplan/tpSpaceRRT_test.go | 75 ++++++++++++----------- motionplan/tpspace/ptg.go | 15 ++--- motionplan/tpspace/ptgC.go | 3 +- motionplan/tpspace/ptgCC.go | 10 +-- motionplan/tpspace/ptgCCS.go | 16 +++-- motionplan/tpspace/ptgGridSim.go | 88 +++++++++++++++------------ motionplan/tpspace/ptgGridSim_test.go | 60 ++++++++++-------- motionplan/tpspace/ptgIKFrame.go | 3 +- motionplan/tpspace/ptg_test.go | 2 +- 13 files changed, 211 insertions(+), 164 deletions(-) diff --git a/motionplan/nloptInverseKinematics.go b/motionplan/nloptInverseKinematics.go index f4dd9edfb0d..e547d95dd79 100644 --- a/motionplan/nloptInverseKinematics.go +++ b/motionplan/nloptInverseKinematics.go @@ -66,9 +66,9 @@ func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int // Solve runs the actual solver and sends any solutions found to the given channel. func (ik *NloptIK) Solve(ctx context.Context, - c chan<- []referenceframe.Input, + solutionChan chan<- []referenceframe.Input, seed []referenceframe.Input, - m StateMetric, + solveMetric StateMetric, rseed int, ) error { //nolint: gosec @@ -109,7 +109,7 @@ func (ik *NloptIK) Solve(ctx context.Context, } mInput.Configuration = inputs mInput.Position = eePos - dist := m(mInput) + dist := solveMetric(mInput) if len(gradient) > 0 { for i := range gradient { @@ -125,7 +125,7 @@ func (ik *NloptIK) Solve(ctx context.Context, } mInput.Configuration = inputs mInput.Position = eePos - dist2 := m(mInput) + dist2 := solveMetric(mInput) gradient[i] = (dist2 - dist) / ik.jump } @@ -179,20 +179,20 @@ func (ik *NloptIK) Solve(ctx context.Context, default: } iterations++ - solutionRaw, _, nloptErr := opt.Optimize(referenceframe.InputsToFloats(startingPos)) + solutionRaw, result, nloptErr := opt.Optimize(referenceframe.InputsToFloats(startingPos)) if nloptErr != nil { // This just *happens* sometimes due to weirdnesses in nonlinear randomized problems. // Ignore it, something else will find a solution err = multierr.Combine(err, nloptErr) } - //~ if result < ik.epsilon*ik.epsilon { - if solutionRaw != nil { + if result < ik.epsilon*ik.epsilon || solutionRaw != nil { select { case <-ctx.Done(): return err - case c <- referenceframe.FloatsToInputs(solutionRaw): + default: } + solutionChan <- referenceframe.FloatsToInputs(solutionRaw) solutionsFound++ } tries++ diff --git a/motionplan/ptgGroupFrame.go b/motionplan/ptgGroupFrame.go index 0afeebf2d53..e9aa643ed06 100644 --- a/motionplan/ptgGroupFrame.go +++ b/motionplan/ptgGroupFrame.go @@ -13,7 +13,7 @@ import ( ) const ( - defaultSimDistMM = 600. + defaultSimDistMM = 4000. ) const ( @@ -61,8 +61,8 @@ func NewPTGFrameFromTurningRadius( // Get max angular velocity in radians per second maxRPS := velocityMMps / (1000. * turnRadMeters) pf := &ptgGroupFrame{name: name} - //~ err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist, false) - err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist, true) + err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist, false) + //~ err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist, true) if err != nil { return nil, err } @@ -152,14 +152,14 @@ func (pf *ptgGroupFrame) initPTGs(logger golog.Logger, maxMps, maxRPS, simDist f //~ for _, k := range []float64{1., -1.} { for _, k := range []float64{1.} { // irreversible trajectories, e.g. alpha, will return nil for negative k - newptg, err := tpspace.NewPTGGridSim(ptgGen, 0, k*simDist) // 0 uses default alpha count + newptg, err := tpspace.NewPTGGridSim(ptgGen, 0, k*simDist, false) // 0 uses default alpha count if err != nil { return err } ptgs = append(ptgs, newptg) } } else { - newptg, err := NewPTGIK(ptgGen, logger, simDist, 1) + newptg, err := NewPTGIK(ptgGen, logger, simDist, 2) if err != nil { return err } diff --git a/motionplan/ptgIK.go b/motionplan/ptgIK.go index 8d690499c32..9d5457a3838 100644 --- a/motionplan/ptgIK.go +++ b/motionplan/ptgIK.go @@ -14,7 +14,6 @@ import ( ) const ( - defaultMaxTime = 15. defaultDiffT = 0.01 // seconds ) @@ -26,6 +25,8 @@ type ptgIK struct { ptgFrame referenceframe.Frame fastGradDescent *NloptIK randseed *rand.Rand + + gridSim tpspace.PTG } // NewPTGIK creates a new PTG by simulating a PrecomputePTG for some distance, then cacheing the results in a grid for fast lookup. @@ -42,6 +43,12 @@ func NewPTGIK(simPTG tpspace.PrecomputePTG, logger golog.Logger, refDist float64 if err != nil { return nil, err } + + // create an ends-only grid sim for quick end-of-trajectory calculations + gridSim, err := tpspace.NewPTGGridSim(simPTG, 0, refDist/2, true) + if err != nil { + return nil, err + } ptg := &ptgIK{ refDist: refDist, @@ -49,6 +56,7 @@ func NewPTGIK(simPTG tpspace.PrecomputePTG, logger golog.Logger, refDist float64 ptgFrame: ptgFrame, fastGradDescent: nlopt, randseed: rseed, + gridSim: gridSim, } return ptg, nil @@ -57,10 +65,10 @@ func NewPTGIK(simPTG tpspace.PrecomputePTG, logger golog.Logger, refDist float64 func (ptg *ptgIK) CToTP(ctx context.Context, pose spatialmath.Pose) (*tpspace.TrajNode, error) { solutionGen := make(chan []referenceframe.Input, 1) - - goalMetric := NewPositionOnlyMetric(pose) + seedInput := []referenceframe.Input{{math.Pi/3}, {ptg.refDist/3}} + goalMetric := NewSquaredNormMetric(pose) // Spawn the IK solver to generate a solution - err := ptg.fastGradDescent.Solve(ctx, solutionGen, []referenceframe.Input{{math.Pi/4}, {ptg.refDist/2}}, goalMetric, ptg.randseed.Int()) + err := ptg.fastGradDescent.Solve(ctx, solutionGen, seedInput, goalMetric, ptg.randseed.Int()) // We should have zero or one solutions var solved []referenceframe.Input select { @@ -68,8 +76,10 @@ func (ptg *ptgIK) CToTP(ctx context.Context, pose spatialmath.Pose) (*tpspace.Tr default: } close(solutionGen) - if err != nil { - return nil, err + if err != nil || solved == nil { + //~ solved = seedInput + return ptg.gridSim.CToTP(ctx, pose) + //~ return nil, nil } // TODO: make this more efficient traj, err := ptg.Trajectory(solved[0].Value, solved[1].Value) @@ -84,5 +94,5 @@ func (ptg *ptgIK) RefDistance() float64 { } func (ptg *ptgIK) Trajectory(alpha, dist float64) ([]*tpspace.TrajNode, error) { - return tpspace.ComputePTG(alpha, ptg.simPTG, defaultMaxTime, dist, defaultDiffT) + return tpspace.ComputePTG(alpha, ptg.simPTG, dist, defaultDiffT) } diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index aba8d1a131a..550c2372061 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -178,6 +178,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( var randPos spatialmath.Pose for iter := 0; iter < mp.algOpts.PlanIter; iter++ { + fmt.Println("iter", iter) if ctx.Err() != nil { mp.logger.Debugf("TP Space RRT timed out after %d iterations", iter) rrt.solutionChan <- &rrtPlanReturn{planerr: fmt.Errorf("TP Space RRT timeout %w", ctx.Err()), maps: rrt.maps} @@ -352,7 +353,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( } } -// extendMap grows the rrt map to the best candidate node if it is valid to do so, returning the added candidate. +// extendMap grows the rrt map to the best candidate node, returning the added candidate. func (mp *tpSpaceRRTMotionPlanner) extendMap( ctx context.Context, candidates []*candidate, @@ -386,27 +387,30 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( lastDist := 0. sinceLastNode := 0. - for _, trajPt := range trajK { - if ctx.Err() != nil { - return nil, ctx.Err() - } - trajState := &State{Position: spatialmath.Compose(treeNode.Pose(), trajPt.Pose)} - sinceLastNode += (trajPt.Dist - lastDist) - - // Optionally add sub-nodes along the way. Will make the final path a bit better - if mp.algOpts.addIntermediate && sinceLastNode > mp.algOpts.addNodeEvery { - // add the last node in trajectory - addedNode = &basicNode{ - q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), randAlpha, trajPt.Dist}), - cost: treeNode.Cost() + trajPt.Dist, - pose: trajState.Position, - corner: false, + //~ if mp.algOpts.addIntermediate { + for _, trajPt := range trajK { + if ctx.Err() != nil { + return nil, ctx.Err() + } + trajState := &State{Position: spatialmath.Compose(treeNode.Pose(), trajPt.Pose)} + //~ fmt.Printf("FINALPATH,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + sinceLastNode += (trajPt.Dist - lastDist) + + // Optionally add sub-nodes along the way. Will make the final path a bit better + if sinceLastNode > mp.algOpts.addNodeEvery { + // add the last node in trajectory + addedNode = &basicNode{ + q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), randAlpha, trajPt.Dist}), + cost: treeNode.Cost() + trajPt.Dist, + pose: trajState.Position, + corner: false, + } + rrt.maps.startMap[addedNode] = treeNode + sinceLastNode = 0. } - rrt.maps.startMap[addedNode] = treeNode - sinceLastNode = 0. + lastDist = trajPt.Dist } - lastDist = trajPt.Dist - } + //~ } rrt.maps.startMap[newNode] = treeNode return bestCand, nil } @@ -443,14 +447,11 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, } relPose := spatialmath.Compose(spatialmath.PoseInverse(seg.StartPosition), seg.EndPosition) closeNode, err := ptg.CToTP(context.Background(), relPose) - if err != nil { - return math.Inf(1) - } - closeDist := mp.planOpts.DistanceFunc(&Segment{StartPosition: relPose, EndPosition: closeNode.Pose}) - if closeNode == nil { + if err != nil || closeNode == nil { return math.Inf(1) } - return closeDist + // Note that we want to return distances in TP space here + return closeNode.Dist } opts.DistanceFunc = segMetric return opts diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 6bb7085ad17..bbcff7b93fc 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -7,17 +7,19 @@ import ( "context" "math/rand" "testing" - //~ "math" + "math" "github.com/edaniels/golog" "github.com/golang/geo/r3" "go.viam.com/test" - //~ "go.viam.com/rdk/motionplan/tpspace" + "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) +var printPath = false + const testTurnRad = 0.3 func TestPtgRrt(t *testing.T) { @@ -80,15 +82,16 @@ func TestPtgWithObstacle(t *testing.T) { fs.AddFrame(ackermanFrame, fs.World()) opt := newBasicPlannerOptions() - opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) - opt.DistanceFunc = SquaredNormNoOrientSegmentMetric - //~ opt.SetGoalMetric(NewSquaredNormMetric(goalPos)) - //~ opt.DistanceFunc = SquaredNormSegmentMetric - opt.GoalThreshold = 5. + //~ opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) + //~ opt.DistanceFunc = SquaredNormNoOrientSegmentMetric + opt.SetGoalMetric(NewSquaredNormMetric(goalPos)) + opt.DistanceFunc = SquaredNormSegmentMetric + //~ opt.GoalThreshold = 0.001 + opt.GoalThreshold = 5 // obstacles obstacle1, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, -500, 0}), r3.Vector{180, 1800, 1}, "") test.That(t, err, test.ShouldBeNil) - obstacle2, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, 2000, 0}), r3.Vector{180, 1800, 1}, "") + obstacle2, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, 1800, 0}), r3.Vector{180, 1800, 1}, "") test.That(t, err, test.ShouldBeNil) obstacle3, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, -1400, 0}), r3.Vector{50000, 30, 1}, "") test.That(t, err, test.ShouldBeNil) @@ -97,15 +100,17 @@ func TestPtgWithObstacle(t *testing.T) { geoms := []spatialmath.Geometry{obstacle1, obstacle2, obstacle3, obstacle4} - //~ fmt.Println("type,X,Y") - //~ for _, geom := range geoms { - //~ pts := geom.ToPoints(1.) - //~ for _, pt := range pts { - //~ if math.Abs(pt.Z) < 0.1 { - //~ fmt.Printf("OBS,%f,%f\n", pt.X, pt.Y) - //~ } - //~ } - //~ } + if printPath { + fmt.Println("type,X,Y") + for _, geom := range geoms { + pts := geom.ToPoints(1.) + for _, pt := range pts { + if math.Abs(pt.Z) < 0.1 { + fmt.Printf("OBS,%f,%f\n", pt.X, pt.Y) + } + } + } + } worldState, err := referenceframe.NewWorldState( []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, geoms)}, @@ -132,24 +137,26 @@ func TestPtgWithObstacle(t *testing.T) { fmt.Println(wp.Q()) } - //~ allPtgs := ackermanFrame.(tpspace.PTGProvider).PTGs() - //~ lastPose := spatialmath.NewZeroPose() + allPtgs := ackermanFrame.(tpspace.PTGProvider).PTGs() + lastPose := spatialmath.NewZeroPose() - //~ for _, mynode := range plan { - //~ trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) - //~ for i, pt := range trajPts { - //~ fmt.Println("pt", pt) - //~ intPose := spatialmath.Compose(lastPose, pt.Pose) - //~ if i == 0 { - //~ fmt.Printf("WP,%f,%f\n", intPose.Point().X, intPose.Point().Y) - //~ } - //~ fmt.Printf("FINALPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) - //~ if i == len(trajPts) - 1 { - //~ lastPose = spatialmath.Compose(lastPose, pt.Pose) - //~ break - //~ } - //~ } - //~ } + if printPath { + for _, mynode := range plan { + trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) + for i, pt := range trajPts { + fmt.Println("pt", pt) + intPose := spatialmath.Compose(lastPose, pt.Pose) + if i == 0 { + fmt.Printf("WP,%f,%f\n", intPose.Point().X, intPose.Point().Y) + } + fmt.Printf("FINALPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) + if i == len(trajPts) - 1 { + lastPose = spatialmath.Compose(lastPose, pt.Pose) + break + } + } + } + } } diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index e06a87477d5..4140cf43dd4 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -84,8 +84,8 @@ func xythetaToPose(x, y, theta float64) spatialmath.Pose { func ComputePTG( alpha float64, - simPtg PrecomputePTG, - maxTime, refDist, diffT float64, + simPTG PrecomputePTG, + refDist, diffT float64, ) ([]*TrajNode, error) { // Initialize trajectory with an all-zero node @@ -101,14 +101,13 @@ func ComputePTG( dist := math.Copysign(math.Abs(v) * diffT, refDist) // Last saved waypoints - accumulatedHeadingChange := 0. lastVs := [2]float64{0, 0} lastWs := [2]float64{0, 0} // Step through each time point for this alpha - for t < maxTime && math.Abs(dist) < math.Abs(refDist) && accumulatedHeadingChange < defaultMaxHeadingChange { - v, w, err = simPtg.PTGVelocities(alpha, dist, x, y, phi) + for math.Abs(dist) < math.Abs(refDist) { + v, w, err = simPTG.PTGVelocities(alpha, dist, x, y, phi) if err != nil { return nil, err } @@ -117,15 +116,13 @@ func ComputePTG( lastVs[0] = v lastWs[0] = w - accumulatedHeadingChange += w * diffT - t += diffT // Update velocities of last node because reasons alphaTraj[len(alphaTraj)-1].LinVelMMPS = v alphaTraj[len(alphaTraj)-1].AngVelRPS = w - pose, err := simPtg.Transform([]referenceframe.Input{{alpha}, {dist}}) + pose, err := simPTG.Transform([]referenceframe.Input{{alpha}, {dist}}) if err != nil { return nil, err } @@ -136,7 +133,7 @@ func ComputePTG( // Add final node alphaTraj[len(alphaTraj)-1].LinVelMMPS = v alphaTraj[len(alphaTraj)-1].AngVelRPS = w - pose, err := simPtg.Transform([]referenceframe.Input{{alpha}, {refDist}}) + pose, err := simPTG.Transform([]referenceframe.Input{{alpha}, {refDist}}) if err != nil { return nil, err } diff --git a/motionplan/tpspace/ptgC.go b/motionplan/tpspace/ptgC.go index 7176714229c..6915e101c6f 100644 --- a/motionplan/tpspace/ptgC.go +++ b/motionplan/tpspace/ptgC.go @@ -60,6 +60,7 @@ func (ptg *ptgDiffDriveC) Transform(inputs []referenceframe.Input) (spatialmath. } pt := r3.Vector{0, dist, 0} // Straight line, +Y is "forwards" + //~ pt := r3.Vector{dist,0, 0} // Straight line, +Y is "forwards" angleRads := 0. if alpha != 0 { arcRadius := math.Pi * turnRad / math.Abs(alpha) // radius of arc @@ -71,7 +72,7 @@ func (ptg *ptgDiffDriveC) Transform(inputs []referenceframe.Input) (spatialmath. angleRads *= -1 } } - pose := spatialmath.NewPose(pt, &spatialmath.OrientationVector{OZ: 1, Theta: angleRads}) + pose := spatialmath.NewPose(pt, &spatialmath.OrientationVector{OZ: 1, Theta: -angleRads}) return pose, nil } diff --git a/motionplan/tpspace/ptgCC.go b/motionplan/tpspace/ptgCC.go index d890cf88de4..cb4cf7ca54f 100644 --- a/motionplan/tpspace/ptgCC.go +++ b/motionplan/tpspace/ptgCC.go @@ -1,6 +1,7 @@ package tpspace import ( + //~ "fmt" "math" "go.viam.com/rdk/referenceframe" @@ -45,7 +46,7 @@ func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, dist, x, y, phi float64) (float6 // l- v = -ptg.maxMMPS w = ptg.maxRPS - } else if dist < (u+math.Pi*0.5)*r { + } else { // l+ v = ptg.maxMMPS w = ptg.maxRPS @@ -63,14 +64,15 @@ func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, dist, x, y, phi float64) (float6 } func (ptg *ptgDiffDriveCC) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + //~ fmt.Println("CC") alpha := inputs[0].Value dist := inputs[1].Value - - reverseDistance := math.Abs(alpha) * 0.5 + r := ptg.maxMMPS / ptg.maxRPS + reverseDistance := math.Abs(alpha) * 0.5 * r flip := math.Copysign(1., alpha) // left or right direction := math.Copysign(1., dist) // forwards or backwards - revPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {-1. * direction * math.Min(dist, reverseDistance)}}) + revPose, err := ptg.circle.Transform([]referenceframe.Input{{-1 * flip * math.Pi}, {-1. * direction * math.Min(dist, reverseDistance)}}) if err != nil { return nil, err } diff --git a/motionplan/tpspace/ptgCCS.go b/motionplan/tpspace/ptgCCS.go index 00d682b8dc6..5a87d305041 100644 --- a/motionplan/tpspace/ptgCCS.go +++ b/motionplan/tpspace/ptgCCS.go @@ -2,6 +2,7 @@ package tpspace import ( "math" + //~ "fmt" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" @@ -62,22 +63,29 @@ func (ptg *ptgDiffDriveCCS) PTGVelocities(alpha, dist, x, y, phi float64) (float } func (ptg *ptgDiffDriveCCS) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + //~ fmt.Println("CCS") alpha := inputs[0].Value dist := inputs[1].Value - reverseDistance := math.Abs(alpha) * 0.5 - fwdArcDistance := (reverseDistance + math.Pi/2) * ptg.r + arcConstant := math.Abs(alpha) * 0.5 + reverseDistance := arcConstant * ptg.r + fwdArcDistance := (arcConstant + math.Pi/2) * ptg.r flip := math.Copysign(1., alpha) // left or right direction := math.Copysign(1., dist) // forwards or backwards - revPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {-1. * direction * math.Min(dist, reverseDistance)}}) + revPose, err := ptg.circle.Transform([]referenceframe.Input{{-1 * flip * math.Pi}, {-1. * direction * math.Min(dist, reverseDistance)}}) if err != nil { return nil, err } if dist < reverseDistance { return revPose, nil } - fwdPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {direction * (math.Min(dist, fwdArcDistance) - reverseDistance)}}) + fwdPose, err := ptg.circle.Transform( + []referenceframe.Input{ + {flip * math.Pi}, + {direction * (math.Min(dist, fwdArcDistance) - reverseDistance)}, + }, + ) if err != nil { return nil, err } diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index ed62f5d3039..5df4a071ced 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -31,13 +31,17 @@ type ptgGridSim struct { precomputeTraj [][]*TrajNode + // If true, then CToTP calls will *only* check the furthest end of each precomputed trajectory. + // This is useful when used in conjunction with IK + endsOnly bool + // Discretized x[y][]node maps for rapid NN lookups trajNodeGrid map[int]map[int][]*TrajNode searchRad float64 // Distance around a query point to search for precompute in the cached grid } // NewPTGGridSim creates a new PTG by simulating a PrecomputePTG for some distance, then cacheing the results in a grid for fast lookup. -func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64) (PTG, error) { +func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64, endsOnly bool) (PTG, error) { if arcs == 0 { arcs = defaultAlphaCnt } @@ -48,6 +52,7 @@ func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64) (PTG, error maxTime: defaultMaxTime, diffT: defaultDiffT, searchRad: defaultSearchRadius, + endsOnly: endsOnly, trajNodeGrid: map[int]map[int][]*TrajNode{}, } @@ -69,47 +74,48 @@ func (ptg *ptgGridSim) CToTP(ctx context.Context, pose spatialmath.Pose) (*TrajN y := point.Y nearbyNodes := []*TrajNode{} - - // First, try to do a quick grid-based lookup - // TODO: an octree should be faster - for tx := int(math.Round(x - ptg.searchRad)); tx < int(math.Round(x+ptg.searchRad)); tx++ { - if ptg.trajNodeGrid[tx] != nil { - for ty := int(math.Round(y - ptg.searchRad)); ty < int(math.Round(y+ptg.searchRad)); ty++ { - nearbyNodes = append(nearbyNodes, ptg.trajNodeGrid[tx][ty]...) - } - } - } - // Try to find a closest point to the paths: bestDist := math.Inf(1) var bestNode *TrajNode - if len(nearbyNodes) > 0 { - for _, nearbyNode := range nearbyNodes { - distToPoint := math.Pow(nearbyNode.ptX-x, 2) + math.Pow(nearbyNode.ptY-y, 2) - if distToPoint < bestDist { - bestDist = distToPoint + if !ptg.endsOnly { + // First, try to do a quick grid-based lookup + // TODO: an octree should be faster + for tx := int(math.Round(x - ptg.searchRad)); tx < int(math.Round(x+ptg.searchRad)); tx++ { + if ptg.trajNodeGrid[tx] != nil { + for ty := int(math.Round(y - ptg.searchRad)); ty < int(math.Round(y+ptg.searchRad)); ty++ { + nearbyNodes = append(nearbyNodes, ptg.trajNodeGrid[tx][ty]...) + } + } + } + + if len(nearbyNodes) > 0 { + for _, nearbyNode := range nearbyNodes { + distToPoint := math.Pow(nearbyNode.ptX-x, 2) + math.Pow(nearbyNode.ptY-y, 2) + if distToPoint < bestDist { + bestDist = distToPoint - bestNode = nearbyNode + bestNode = nearbyNode + } } + return bestNode, nil } - return bestNode, nil - } - for k := 0; k < int(ptg.alphaCnt); k++ { - nMax := len(ptg.precomputeTraj[k]) - 1 - for n := 0; n <= nMax; n++ { - distToPoint := math.Pow(ptg.precomputeTraj[k][n].ptX-x, 2) + math.Pow(ptg.precomputeTraj[k][n].ptY-y, 2) - if distToPoint < bestDist { - bestDist = distToPoint + for k := 0; k < int(ptg.alphaCnt); k++ { + nMax := len(ptg.precomputeTraj[k]) - 1 + for n := 0; n <= nMax; n++ { + distToPoint := math.Pow(ptg.precomputeTraj[k][n].ptX-x, 2) + math.Pow(ptg.precomputeTraj[k][n].ptY-y, 2) + if distToPoint < bestDist { + bestDist = distToPoint - bestNode = ptg.precomputeTraj[k][n] + bestNode = ptg.precomputeTraj[k][n] + } } } - } - if bestNode != nil { - return bestNode, nil + if bestNode != nil { + return bestNode, nil + } } // Given a point (x,y), compute the "k_closest" whose extrapolation @@ -141,7 +147,7 @@ func (ptg *ptgGridSim) Trajectory(alpha, dist float64) ([]*TrajNode, error) { } fullTraj := ptg.precomputeTraj[k] if fullTraj[len(fullTraj) - 1].Dist < dist { - return nil, fmt.Errorf("requested traj to dist %f but cannot request distances larger than %f", dist, fullTraj[len(fullTraj) - 1].Dist) + return nil, fmt.Errorf("requested traj to dist %f but only simulated trajectories to distance %f", dist, fullTraj[len(fullTraj) - 1].Dist) } var traj []*TrajNode for _, trajNode := range fullTraj { @@ -154,26 +160,28 @@ func (ptg *ptgGridSim) Trajectory(alpha, dist float64) ([]*TrajNode, error) { return traj, nil } -func (ptg *ptgGridSim) simulateTrajectories(simPtg PrecomputePTG) ([][]*TrajNode, error) { +func (ptg *ptgGridSim) simulateTrajectories(simPTG PrecomputePTG) ([][]*TrajNode, error) { // C-space path structure allTraj := make([][]*TrajNode, 0, ptg.alphaCnt) for k := uint(0); k < ptg.alphaCnt; k++ { alpha := index2alpha(k, ptg.alphaCnt) - alphaTraj, err := ComputePTG(alpha, simPtg, ptg.maxTime, ptg.refDist, ptg.diffT) + alphaTraj, err := ComputePTG(alpha, simPTG, ptg.refDist, ptg.diffT) if err != nil { return nil, err } - for _, tNode := range alphaTraj { - gridX := int(math.Round(tNode.ptX)) - gridY := int(math.Round(tNode.ptY)) - // Discretize into a grid for faster lookups later - if _, ok := ptg.trajNodeGrid[gridX]; !ok { - ptg.trajNodeGrid[gridX] = map[int][]*TrajNode{} + if !ptg.endsOnly { + for _, tNode := range alphaTraj { + gridX := int(math.Round(tNode.ptX)) + gridY := int(math.Round(tNode.ptY)) + // Discretize into a grid for faster lookups later + if _, ok := ptg.trajNodeGrid[gridX]; !ok { + ptg.trajNodeGrid[gridX] = map[int][]*TrajNode{} + } + ptg.trajNodeGrid[gridX][gridY] = append(ptg.trajNodeGrid[gridX][gridY], tNode) } - ptg.trajNodeGrid[gridX][gridY] = append(ptg.trajNodeGrid[gridX][gridY], tNode) } allTraj = append(allTraj, alphaTraj) diff --git a/motionplan/tpspace/ptgGridSim_test.go b/motionplan/tpspace/ptgGridSim_test.go index dd59a7b9f0b..39cba671879 100644 --- a/motionplan/tpspace/ptgGridSim_test.go +++ b/motionplan/tpspace/ptgGridSim_test.go @@ -1,33 +1,45 @@ package tpspace -//~ import ( - //~ "testing" +import ( + "testing" + "fmt" - //~ "go.viam.com/test" -//~ ) + "go.viam.com/test" +) -//~ type ptgFactory func(float64, float64) tpspace.PrecomputePTG +type ptgFactory func(float64, float64) PrecomputePTG -//~ var defaultPTGs = []ptgFactory{ - //~ NewCirclePTG, - //~ NewCCPTG, - //~ NewCCSPTG, - //~ NewCSPTG, -//~ } +var defaultPTGs = []ptgFactory{ + NewCirclePTG, + NewCCPTG, + NewCCSPTG, + NewCSPTG, +} -//~ var ( - //~ defaultMps = 0.3 - //~ turnRadMeters = 0.3 -//~ ) +var ( + defaultMMps = 800. + turnRadMeters = 1. +) -//~ func TestSim(t *testing.T) { +func TestSim(t *testing.T) { + simDist := 6000. + alphaCnt := uint(121) + fmt.Println("type,X,Y") //~ for _, ptg := range defaultPTGs { - //~ radPS := defaultMps / turnRadMeters - - //~ ptgGen := ptg(defaultMps, radPS, 1.) - //~ test.That(t, ptgGen, test.ShouldNotBeNil) - //~ _, err := NewPTGGridSim(ptgGen, defaultAlphaCnt, 1000.) - //~ test.That(t, err, test.ShouldBeNil) - //~ } -//~ } + ptg := NewCCSPTG + radPS := defaultMMps / (turnRadMeters * 1000) + + ptgGen := ptg(defaultMMps, radPS) + test.That(t, ptgGen, test.ShouldNotBeNil) + grid, err := NewPTGGridSim(ptgGen, alphaCnt, simDist) + test.That(t, err, test.ShouldBeNil) + + for i := uint(0); i < alphaCnt; i++ { + //~ i := uint(60) + traj, _ := grid.Trajectory(index2alpha(i, alphaCnt), simDist) + for _, intPose := range traj{ + fmt.Printf("FINALPATH,%f,%f\n", intPose.Pose.Point().X, intPose.Pose.Point().Y) + } + } +} diff --git a/motionplan/tpspace/ptgIKFrame.go b/motionplan/tpspace/ptgIKFrame.go index e425e2b0f95..0db8b84b447 100644 --- a/motionplan/tpspace/ptgIKFrame.go +++ b/motionplan/tpspace/ptgIKFrame.go @@ -19,7 +19,8 @@ func NewPTGIKFrame(ptg PrecomputePTG, dist float64) (referenceframe.Frame, error pf.limits = []referenceframe.Limit{ {Min: -math.Pi, Max: math.Pi}, - {Min: -dist, Max: dist}, + {Min: 0, Max: dist}, + //~ {Min: -dist, Max: dist}, } return pf, nil } diff --git a/motionplan/tpspace/ptg_test.go b/motionplan/tpspace/ptg_test.go index 88bdc509163..d3e72e33443 100644 --- a/motionplan/tpspace/ptg_test.go +++ b/motionplan/tpspace/ptg_test.go @@ -18,7 +18,7 @@ func TestAlphaIdx(t *testing.T) { func TestRevPTGs(t *testing.T) { cs := NewCirclePTG(0.3, 0.3) //~ cs := NewCSPTG(0.3, 0.3) - traj, err := ComputePTG(0, cs, 15., -2000, 0.05) + traj, err := ComputePTG(0, cs, -2000, 0.05) test.That(t, err, test.ShouldBeNil) fmt.Println(traj) From 0270fc78c68b1f62acb0538245c1f78e225bf587 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Fri, 11 Aug 2023 09:10:04 -0700 Subject: [PATCH 12/38] Bidirectional solving working, but tree reconstruction afterwards not yet there --- motionplan/ptgGroupFrame.go | 2 +- motionplan/ptgIK.go | 12 +- motionplan/tpSpaceRRT.go | 229 ++++++++++++++++++-------- motionplan/tpSpaceRRT_test.go | 22 ++- motionplan/tpspace/ptgGridSim_test.go | 18 +- motionplan/tpspace/ptg_test.go | 6 +- 6 files changed, 196 insertions(+), 93 deletions(-) diff --git a/motionplan/ptgGroupFrame.go b/motionplan/ptgGroupFrame.go index e9aa643ed06..93c5b47e6cc 100644 --- a/motionplan/ptgGroupFrame.go +++ b/motionplan/ptgGroupFrame.go @@ -13,7 +13,7 @@ import ( ) const ( - defaultSimDistMM = 4000. + defaultSimDistMM = 2000. ) const ( diff --git a/motionplan/ptgIK.go b/motionplan/ptgIK.go index 9d5457a3838..d0868fda6a1 100644 --- a/motionplan/ptgIK.go +++ b/motionplan/ptgIK.go @@ -15,6 +15,7 @@ import ( const ( defaultDiffT = 0.01 // seconds + nloptSeed = 42 ) // ptgGridSim will take a PrecomputePTG, and simulate out a number of trajectories through some requested time/distance for speed of lookup @@ -45,7 +46,7 @@ func NewPTGIK(simPTG tpspace.PrecomputePTG, logger golog.Logger, refDist float64 } // create an ends-only grid sim for quick end-of-trajectory calculations - gridSim, err := tpspace.NewPTGGridSim(simPTG, 0, refDist/2, true) + gridSim, err := tpspace.NewPTGGridSim(simPTG, 0, refDist, true) if err != nil { return nil, err } @@ -64,11 +65,13 @@ func NewPTGIK(simPTG tpspace.PrecomputePTG, logger golog.Logger, refDist float64 func (ptg *ptgIK) CToTP(ctx context.Context, pose spatialmath.Pose) (*tpspace.TrajNode, error) { + + solutionGen := make(chan []referenceframe.Input, 1) - seedInput := []referenceframe.Input{{math.Pi/3}, {ptg.refDist/3}} + seedInput := []referenceframe.Input{{math.Pi/3}, {ptg.refDist/3}} // random value to seed the IK solver goalMetric := NewSquaredNormMetric(pose) // Spawn the IK solver to generate a solution - err := ptg.fastGradDescent.Solve(ctx, solutionGen, seedInput, goalMetric, ptg.randseed.Int()) + err := ptg.fastGradDescent.Solve(ctx, solutionGen, seedInput, goalMetric, nloptSeed) // We should have zero or one solutions var solved []referenceframe.Input select { @@ -77,9 +80,8 @@ func (ptg *ptgIK) CToTP(ctx context.Context, pose spatialmath.Pose) (*tpspace.Tr } close(solutionGen) if err != nil || solved == nil { - //~ solved = seedInput + return nil, nil return ptg.gridSim.CToTP(ctx, pose) - //~ return nil, nil } // TODO: make this more efficient traj, err := ptg.Trajectory(solved[0].Value, solved[1].Value) diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index 550c2372061..6f4929cd44f 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -26,10 +26,10 @@ const ( // from a single set of inputs. // whether to add intermediate waypoints. - defaultAddInt = false + defaultAddInt = true // Add a subnode every this many mm along a valid trajectory. Large values run faster, small gives better paths // Meaningless if the above is false. - defaultAddNodeEvery = 50. + defaultAddNodeEvery = 250. // When getting the closest node to a pose, only look for nodes at least this far away. defaultDuplicateNodeBuffer = 50. @@ -39,6 +39,8 @@ const ( // Default distance in mm to get within for tp-space trajectories. defaultTPSpaceGoalDist = 10. + + defaultMaxReseeds = 10 ) type tpspaceOptions struct { @@ -55,9 +57,15 @@ type tpspaceOptions struct { // Don't add new RRT tree nodes if there is an existing node within this distance dupeNodeBuffer float64 + + // If the squared norm between two poses is less than this, consider them equal + poseSolveDist float64 + + maxReseeds int // Cached functions for calculating TP-space distances for each PTG distOptions map[tpspace.PTG]*plannerOptions + invertDistOptions map[tpspace.PTG]*plannerOptions } // candidate is putative node which could be added to an RRT tree. It includes a distance score, the new node and its future parent. @@ -174,10 +182,13 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( } dist := math.Sqrt(mp.planOpts.DistanceFunc(&Segment{StartPosition: startPose, EndPosition: goalPose})) - midPt := goalPose.Point().Sub(startPose.Point()) + fmt.Println("dist", dist) + fmt.Println(startPose.Point(), goalPose.Point()) + midPt := goalPose.Point().Sub(startPose.Point()) // used for initial seed var randPos spatialmath.Pose - for iter := 0; iter < mp.algOpts.PlanIter; iter++ { + //~ for iter := 0; iter < mp.algOpts.PlanIter; iter++ { + for iter := 0; iter < 125; iter++ { fmt.Println("iter", iter) if ctx.Err() != nil { mp.logger.Debugf("TP Space RRT timed out after %d iterations", iter) @@ -185,33 +196,57 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( return } // Get random cartesian configuration - tryGoal := true - // Check if we can reach the goal every N iters - if iter%mp.algOpts.goalCheck != 0 { - rDist := dist * (mp.algOpts.autoBB + float64(iter)/10.) - tryGoal = false - randPosX := float64(mp.randseed.Intn(int(rDist))) - randPosY := float64(mp.randseed.Intn(int(rDist))) - randPosTheta := math.Pi * (mp.randseed.Float64() - 0.5) - randPos = spatialmath.NewPose( - r3.Vector{midPt.X + (randPosX - rDist/2.), midPt.Y + (randPosY - rDist/2.), 0}, - &spatialmath.OrientationVector{OZ: 1, Theta: randPosTheta}, - ) - } else { - randPos = goalPose - } + rDist := dist * (mp.algOpts.autoBB + float64(iter)/10.) + randPosX := float64(mp.randseed.Intn(int(rDist))) + randPosY := float64(mp.randseed.Intn(int(rDist))) + randPosTheta := math.Pi * (mp.randseed.Float64() - 0.5) + randPos = spatialmath.NewPose( + r3.Vector{midPt.X + (randPosX - rDist/2.), midPt.Y + (randPosY - rDist/2.), 0}, + &spatialmath.OrientationVector{OZ: 1, Theta: randPosTheta}, + ) randPosNode := &basicNode{q: nil, cost: 0, pose: randPos, corner: false} + _ = randPosNode + + //~ goalNode := &basicNode{pose: goalPose} - successNode, err := mp.attemptExtension(ctx, nil, randPosNode, rrt) + seedMapNode, err := mp.attemptExtension(ctx, nil, randPosNode, rrt.maps.startMap, false) if err != nil { rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} return } - // If we tried the goal and have a close-enough XY location, check if the node is good enough to be a final goal - if tryGoal && successNode != nil { - if mp.planOpts.GoalThreshold > mp.planOpts.goalMetric(&State{Position: successNode.Pose(), Frame: mp.frame}) { + //~ seedMapNode := &basicNode{pose: spatialmath.NewZeroPose()} + goalMapNode, err := mp.attemptExtension(ctx, nil, randPosNode, rrt.maps.goalMap, true) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return + } + + if seedMapNode != nil && goalMapNode != nil { + + seedMapNode, err = mp.attemptExtension(ctx, nil, goalMapNode, rrt.maps.startMap, false) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return + } + if seedMapNode == nil { + continue + } + goalMapNode, err := mp.attemptExtension(ctx, nil, seedMapNode, rrt.maps.goalMap, true) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return + } + if goalMapNode == nil { + continue + } + fmt.Println("start tree", seedMapNode.Pose().Point(), seedMapNode.Pose().Orientation().OrientationVectorDegrees()) + fmt.Println("goal tree", goalMapNode.Pose().Point(), goalMapNode.Pose().Orientation().OrientationVectorDegrees()) + reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartPosition: seedMapNode.Pose(), EndPosition: goalMapNode.Pose()}) + // If we tried the goal and have a close-enough XY location, check if the node is good enough to be a final goal + if reachedDelta <= mp.algOpts.poseSolveDist { // If we've reached the goal, break out - path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: successNode}) + path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: seedMapNode, b: goalMapNode}) + //~ path = path[1:] // The rrt.solutionChan <- &rrtPlanReturn{steps: path, maps: rrt.maps} return } @@ -226,9 +261,10 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( randPosNode node, ptgNum int, curPtg tpspace.PTG, - rrt *rrtParallelPlannerShared, + rrt rrtMap, nearest node, planOpts *plannerOptions, // Need to pass this in explicitly for smoothing + invert bool, ) (*candidate, error) { nm := &neighborManager{nCPU: mp.planOpts.NumThreads} nm.parallelNeighbors = 10 @@ -236,43 +272,61 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( var successNode node // Get the distance function that will find the nearest RRT map node in TP-space of *this* PTG ptgDistOpt := mp.algOpts.distOptions[curPtg] + if invert { + ptgDistOpt = mp.algOpts.invertDistOptions[curPtg] + } if nearest == nil { // Get nearest neighbor to rand config in tree using this PTG - nearest = nm.nearestNeighbor(ctx, ptgDistOpt, randPosNode, rrt.maps.startMap) + nearest = nm.nearestNeighbor(ctx, ptgDistOpt, randPosNode, rrt) if nearest == nil { return nil, nil } } + //~ fmt.Println("nearest", nearest.Pose().Point(), nearest.Pose().Orientation().OrientationVectorDegrees()) // Get cartesian distance from NN to rand relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) - // Convert cartesian distance to tp-space using inverse curPtg, yielding TP-space coordinates goalK and goalD bestNode, err := curPtg.CToTP(ctx, relPose) - if err != nil { + if err != nil || bestNode == nil { return nil, err } bestDist := mp.planOpts.DistanceFunc(&Segment{StartPosition: relPose, EndPosition: bestNode.Pose}) goalAlpha := bestNode.Alpha goalD := bestNode.Dist + //~ fmt.Println("alpha dist", goalAlpha, goalD) // Check collisions along this traj and get the longest distance viable trajK, err := curPtg.Trajectory(goalAlpha, goalD) if err != nil { return nil, err } - var lastNode *tpspace.TrajNode - var lastPose spatialmath.Pose + finalTrajNode := trajK[len(trajK) - 1] + + arcStartPose := nearest.Pose() + if invert { + arcStartPose = spatialmath.Compose(arcStartPose, spatialmath.PoseInverse(finalTrajNode.Pose)) + } + //~ fmt.Println("arc tform", finalTrajNode.Pose.Point()) + //~ fmt.Println("arc tform invert", spatialmath.PoseInverse(finalTrajNode.Pose).Point()) + //~ fmt.Println("arcstart", arcStartPose.Point()) sinceLastCollideCheck := 0. lastDist := 0. - - isLastNode := true + var nodePose spatialmath.Pose // Check each point along the trajectory to confirm constraints are met - for _, trajPt := range trajK { + for i := 0; i < len(trajK); i++ { + trajPt := trajK[i] + if invert { + // Start at known-good map point and extend + // For the goal tree this means iterating backwards + trajPt = trajK[(len(trajK)-1)-i] + } - sinceLastCollideCheck += (trajPt.Dist - lastDist) - trajState := &State{Position: spatialmath.Compose(nearest.Pose(), trajPt.Pose), Frame: mp.frame} + sinceLastCollideCheck += math.Abs(trajPt.Dist - lastDist) + trajState := &State{Position: spatialmath.Compose(arcStartPose, trajPt.Pose), Frame: mp.frame} + //~ fmt.Println("trajstate", trajState.Position.Point()) + nodePose = trajState.Position if sinceLastCollideCheck > planOpts.Resolution { ok, _ := planOpts.CheckStateConstraints(trajState) if !ok { @@ -281,15 +335,16 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( sinceLastCollideCheck = 0. } - lastPose = trajState.Position - lastNode = trajPt - lastDist = lastNode.Dist + lastDist = trajPt.Dist } + + isLastNode := finalTrajNode.Dist == curPtg.RefDistance() + // add the last node in trajectory successNode = &basicNode{ - q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), goalAlpha, lastNode.Dist}), - cost: nearest.Cost() + lastNode.Dist, - pose: lastPose, + q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), goalAlpha, finalTrajNode.Dist}), + cost: nearest.Cost() + finalTrajNode.Dist, + pose: nodePose, corner: false, } @@ -297,15 +352,15 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( // check if this successNode is too close to nodes already in the tree, and if so, do not add. // Get nearest neighbor to new node that's already in the tree - nearest = nm.nearestNeighbor(ctx, planOpts, successNode, rrt.maps.startMap) - if nearest != nil { - dist := planOpts.DistanceFunc(&Segment{StartPosition: successNode.Pose(), EndPosition: nearest.Pose()}) - // Ensure successNode is sufficiently far from the nearest node already existing in the tree - // If too close, don't add a new node - if dist < defaultIdenticalNodeDistance { - cand = nil - } - } + //~ nearest = nm.nearestNeighbor(ctx, planOpts, successNode, rrt) + //~ if nearest != nil { + //~ dist := planOpts.DistanceFunc(&Segment{StartPosition: successNode.Pose(), EndPosition: nearest.Pose()}) + //~ // Ensure successNode is sufficiently far from the nearest node already existing in the tree + //~ // If too close, don't add a new node + //~ if dist < defaultIdenticalNodeDistance { + //~ cand = nil + //~ } + //~ } return cand, nil } @@ -315,9 +370,11 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( ctx context.Context, seedNode, goalNode node, - rrt *rrtParallelPlannerShared, + rrt rrtMap, + invert bool, ) (node, error) { - for { + var reseedCandidate *candidate + for i := 0; i < mp.algOpts.maxReseeds; i++ { select { case <-ctx.Done(): return nil, ctx.Err() @@ -327,7 +384,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( for ptgNum, curPtg := range mp.tpFrame.PTGs() { // Find the best traj point for each traj family, and store for later comparison // TODO: run in parallel - cand, err := mp.getExtensionCandidate(ctx, goalNode, ptgNum, curPtg, rrt, seedNode, mp.planOpts) + cand, err := mp.getExtensionCandidate(ctx, goalNode, ptgNum, curPtg, rrt, seedNode, mp.planOpts, invert) if err != nil { return nil, err } @@ -337,7 +394,8 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( } } } - reseedCandidate, err := mp.extendMap(ctx, candidates, rrt) + var err error + reseedCandidate, err = mp.extendMap(ctx, candidates, rrt, invert) if err != nil { return nil, err } @@ -349,15 +407,20 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( // Reached the goal position, or otherwise failed to fully extend to the end of a trajectory return reseedCandidate.newNode, nil } + if dist < reseedCandidate.newNode.Q()[2].Value && i < mp.algOpts.maxReseeds - 2{ + i = mp.algOpts.maxReseeds - 2 + } seedNode = reseedCandidate.newNode } + return reseedCandidate.newNode, nil } // extendMap grows the rrt map to the best candidate node, returning the added candidate. func (mp *tpSpaceRRTMotionPlanner) extendMap( ctx context.Context, candidates []*candidate, - rrt *rrtParallelPlannerShared, + rrt rrtMap, + invert bool, ) (*candidate, error) { if len(candidates) == 0 { return nil, nil @@ -373,27 +436,43 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( } } treeNode := bestCand.treeNode // The node already in the tree to which we are parenting - newNode := bestCand.newNode + newNode := bestCand.newNode // The node we are adding because it was the best extending PTG + //~ fmt.Println("treenode", treeNode.Pose().Point()) + //~ fmt.Println("newnode", newNode.Pose().Point()) ptgNum := int(newNode.Q()[0].Value) randAlpha := newNode.Q()[1].Value randDist := newNode.Q()[2].Value + fmt.Println("traj", randAlpha, randDist) trajK, err := mp.tpFrame.PTGs()[ptgNum].Trajectory(randAlpha, randDist) if err != nil { return nil, err } + arcStartPose := treeNode.Pose() + if invert { + arcStartPose = spatialmath.Compose(arcStartPose, spatialmath.PoseInverse(trajK[len(trajK) - 1].Pose)) + } lastDist := 0. sinceLastNode := 0. - //~ if mp.algOpts.addIntermediate { - for _, trajPt := range trajK { + var trajState *State + if mp.algOpts.addIntermediate { + for i := 0; i < len(trajK); i++ { + trajPt := trajK[i] + if invert { + trajPt = trajK[(len(trajK)-1)-i] + } if ctx.Err() != nil { return nil, ctx.Err() } - trajState := &State{Position: spatialmath.Compose(treeNode.Pose(), trajPt.Pose)} - //~ fmt.Printf("FINALPATH,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + trajState = &State{Position: spatialmath.Compose(arcStartPose, trajPt.Pose)} + //~ if !invert { + //~ fmt.Printf("FINALPATH,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + //~ }else{ + //~ fmt.Printf("FINALPATH2,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + //~ } sinceLastNode += (trajPt.Dist - lastDist) // Optionally add sub-nodes along the way. Will make the final path a bit better @@ -405,13 +484,14 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( pose: trajState.Position, corner: false, } - rrt.maps.startMap[addedNode] = treeNode + rrt[addedNode] = treeNode sinceLastNode = 0. } lastDist = trajPt.Dist } - //~ } - rrt.maps.startMap[newNode] = treeNode + //~ fmt.Printf("WP,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + } + rrt[newNode] = treeNode return bestCand, nil } @@ -425,12 +505,17 @@ func (mp *tpSpaceRRTMotionPlanner) setupTPSpaceOptions() { addNodeEvery: defaultAddNodeEvery, dupeNodeBuffer: defaultDuplicateNodeBuffer, + poseSolveDist: defaultIdenticalNodeDistance, + + maxReseeds: defaultMaxReseeds, distOptions: map[tpspace.PTG]*plannerOptions{}, + invertDistOptions: map[tpspace.PTG]*plannerOptions{}, } for _, curPtg := range mp.tpFrame.PTGs() { - tpOpt.distOptions[curPtg] = mp.make2DTPSpaceDistanceOptions(curPtg, tpOpt.dupeNodeBuffer) + tpOpt.distOptions[curPtg] = mp.make2DTPSpaceDistanceOptions(curPtg, tpOpt.dupeNodeBuffer, false) + tpOpt.invertDistOptions[curPtg] = mp.make2DTPSpaceDistanceOptions(curPtg, tpOpt.dupeNodeBuffer, true) } mp.algOpts = tpOpt @@ -438,20 +523,30 @@ func (mp *tpSpaceRRTMotionPlanner) setupTPSpaceOptions() { // make2DTPSpaceDistanceOptions will create a plannerOptions object with a custom DistanceFunc constructed such that // distances can be computed in TP space using the given PTG. -func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, min float64) *plannerOptions { +func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, min float64, invert bool) *plannerOptions { opts := newBasicPlannerOptions() segMetric := func(seg *Segment) float64 { if seg.StartPosition == nil || seg.EndPosition == nil { return math.Inf(1) } - relPose := spatialmath.Compose(spatialmath.PoseInverse(seg.StartPosition), seg.EndPosition) + //~ fmt.Println("calc dist", seg.StartPosition.Point(), seg.EndPosition.Point()) + var relPose spatialmath.Pose + if invert { + relPose = spatialmath.Compose(spatialmath.PoseInverse(seg.EndPosition), seg.StartPosition) + } else { + relPose = spatialmath.Compose(spatialmath.PoseInverse(seg.StartPosition), seg.EndPosition) + } closeNode, err := ptg.CToTP(context.Background(), relPose) if err != nil || closeNode == nil { + //~ fmt.Println("inf") return math.Inf(1) } + dist := SquaredNormSegmentMetric(&Segment{StartPosition: closeNode.Pose, EndPosition: relPose}) + //~ fmt.Println("dist", dist) // Note that we want to return distances in TP space here - return closeNode.Dist + //~ return closeNode.Dist + return dist } opts.DistanceFunc = segMetric return opts diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index bbcff7b93fc..da730f44065 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -18,11 +18,12 @@ import ( "go.viam.com/rdk/spatialmath" ) -var printPath = false +var printPath = true const testTurnRad = 0.3 func TestPtgRrt(t *testing.T) { + fmt.Println("type,X,Y") logger := golog.NewTestLogger(t) roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") test.That(t, err, test.ShouldBeNil) @@ -38,19 +39,21 @@ func TestPtgRrt(t *testing.T) { ) test.That(t, err, test.ShouldBeNil) - goalPos := spatialmath.NewPose(r3.Vector{X: 100, Y: 700, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 180}) + //~ goalPos := spatialmath.NewPose(r3.Vector{X: 100, Y: 700, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 180}) + goalPos := spatialmath.NewPose(r3.Vector{X: 100, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 0}) opt := newBasicPlannerOptions() - opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) - opt.DistanceFunc = SquaredNormNoOrientSegmentMetric - //~ opt.SetGoalMetric(NewSquaredNormMetric(goalPos)) - //~ opt.DistanceFunc = SquaredNormSegmentMetric + //~ opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) + //~ opt.DistanceFunc = SquaredNormNoOrientSegmentMetric + opt.SetGoalMetric(NewSquaredNormMetric(goalPos)) + opt.DistanceFunc = SquaredNormSegmentMetric opt.GoalThreshold = 5. mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) tp, ok := mp.(*tpSpaceRRTMotionPlanner) test.That(t, ok, test.ShouldBeTrue) - + fmt.Printf("SG,%f,%f\n", 0., 0.) + fmt.Printf("SG,%f,%f\n", goalPos.Point().X, goalPos.Point().Y) plan, err := tp.plan(context.Background(), goalPos, nil) for _, wp := range plan { fmt.Println(wp.Q()) @@ -129,8 +132,10 @@ func TestPtgWithObstacle(t *testing.T) { mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) tp, _ := mp.(*tpSpaceRRTMotionPlanner) - + fmt.Printf("SG,%f,%f\n", 0., 0.) + fmt.Printf("SG,%f,%f\n", goalPos.Point().X, goalPos.Point().Y) plan, err := tp.plan(ctx, goalPos, nil) + test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThan, 2) for _, wp := range plan { @@ -144,7 +149,6 @@ func TestPtgWithObstacle(t *testing.T) { for _, mynode := range plan { trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) for i, pt := range trajPts { - fmt.Println("pt", pt) intPose := spatialmath.Compose(lastPose, pt.Pose) if i == 0 { fmt.Printf("WP,%f,%f\n", intPose.Point().X, intPose.Point().Y) diff --git a/motionplan/tpspace/ptgGridSim_test.go b/motionplan/tpspace/ptgGridSim_test.go index 39cba671879..a9a624c68ca 100644 --- a/motionplan/tpspace/ptgGridSim_test.go +++ b/motionplan/tpspace/ptgGridSim_test.go @@ -18,28 +18,30 @@ var defaultPTGs = []ptgFactory{ } var ( - defaultMMps = 800. - turnRadMeters = 1. + defaultMMps = 300. + turnRadMeters = 0.3 ) func TestSim(t *testing.T) { - simDist := 6000. + simDist := 150. alphaCnt := uint(121) fmt.Println("type,X,Y") //~ for _, ptg := range defaultPTGs { - ptg := NewCCSPTG + ptg := NewCSPTG radPS := defaultMMps / (turnRadMeters * 1000) ptgGen := ptg(defaultMMps, radPS) test.That(t, ptgGen, test.ShouldNotBeNil) - grid, err := NewPTGGridSim(ptgGen, alphaCnt, simDist) + grid, err := NewPTGGridSim(ptgGen, alphaCnt, simDist, false) test.That(t, err, test.ShouldBeNil) - for i := uint(0); i < alphaCnt; i++ { + //~ for i := uint(0); i < alphaCnt; i++ { //~ i := uint(60) - traj, _ := grid.Trajectory(index2alpha(i, alphaCnt), simDist) + alpha := -3.115629077940291 + //~ traj, _ := grid.Trajectory(index2alpha(i, alphaCnt), simDist) + traj, _ := grid.Trajectory(alpha, simDist) for _, intPose := range traj{ fmt.Printf("FINALPATH,%f,%f\n", intPose.Pose.Point().X, intPose.Pose.Point().Y) } - } + //~ } } diff --git a/motionplan/tpspace/ptg_test.go b/motionplan/tpspace/ptg_test.go index d3e72e33443..74c99ac4fe2 100644 --- a/motionplan/tpspace/ptg_test.go +++ b/motionplan/tpspace/ptg_test.go @@ -2,7 +2,7 @@ package tpspace import ( "testing" - "fmt" + //~ "fmt" "go.viam.com/test" ) @@ -18,8 +18,8 @@ func TestAlphaIdx(t *testing.T) { func TestRevPTGs(t *testing.T) { cs := NewCirclePTG(0.3, 0.3) //~ cs := NewCSPTG(0.3, 0.3) - traj, err := ComputePTG(0, cs, -2000, 0.05) + _, err := ComputePTG(0, cs, -2000, 0.05) test.That(t, err, test.ShouldBeNil) - fmt.Println(traj) + //~ fmt.Println(traj) } From 4445a9c29c2c048df053f75530f5ebbe101fa678 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Fri, 11 Aug 2023 09:14:08 -0700 Subject: [PATCH 13/38] deprecate x y phi from ptgs --- motionplan/tpspace/ptg.go | 7 +-- motionplan/tpspace/ptgC.go | 2 +- motionplan/tpspace/ptgCC.go | 2 +- motionplan/tpspace/ptgCCS.go | 2 +- motionplan/tpspace/ptgCS.go | 2 +- motionplan/tpspace/simPtgAlpha.go | 86 +++++++++++++++---------------- 6 files changed, 49 insertions(+), 52 deletions(-) diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index 4140cf43dd4..41d8573d0cb 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -37,7 +37,7 @@ type PTGProvider interface { // PrecomputePTG is a precomputable PTG. type PrecomputePTG interface { // PTGVelocities returns the linear and angular velocity at a specific point along a trajectory - PTGVelocities(alpha, dist, x, y, phi float64) (float64, float64, error) + PTGVelocities(alpha, dist float64) (float64, float64, error) Transform([]referenceframe.Input) (spatialmath.Pose, error) } @@ -94,9 +94,6 @@ func ComputePTG( var err error var w float64 var v float64 - var x float64 - var y float64 - var phi float64 var t float64 dist := math.Copysign(math.Abs(v) * diffT, refDist) @@ -107,7 +104,7 @@ func ComputePTG( // Step through each time point for this alpha for math.Abs(dist) < math.Abs(refDist) { - v, w, err = simPTG.PTGVelocities(alpha, dist, x, y, phi) + v, w, err = simPTG.PTGVelocities(alpha, dist) if err != nil { return nil, err } diff --git a/motionplan/tpspace/ptgC.go b/motionplan/tpspace/ptgC.go index 6915e101c6f..72da4651260 100644 --- a/motionplan/tpspace/ptgC.go +++ b/motionplan/tpspace/ptgC.go @@ -27,7 +27,7 @@ func NewCirclePTG(maxMMPS, maxRPS float64) PrecomputePTG { // For this particular driver, turns alpha into a linear + angular velocity. Linear is just max * fwd/back. // Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased // because they will have zero linear velocity through their turns, not max. -func (ptg *ptgDiffDriveC) PTGVelocities(alpha, dist, x, y, phi float64) (float64, float64, error) { +func (ptg *ptgDiffDriveC) PTGVelocities(alpha, dist float64) (float64, float64, error) { // (v,w) k := math.Copysign(1.0, dist) v := ptg.maxMMPS * k diff --git a/motionplan/tpspace/ptgCC.go b/motionplan/tpspace/ptgCC.go index cb4cf7ca54f..6866723c892 100644 --- a/motionplan/tpspace/ptgCC.go +++ b/motionplan/tpspace/ptgCC.go @@ -33,7 +33,7 @@ func NewCCPTG(maxMMPS, maxRPS float64) PrecomputePTG { // For this particular driver, turns alpha into a linear + angular velocity. Linear is just max * fwd/back. // Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased // because they will have zero linear velocity through their turns, not max. -func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, dist, x, y, phi float64) (float64, float64, error) { +func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, dist float64) (float64, float64, error) { k := math.Copysign(1.0, dist) r := ptg.maxMMPS / ptg.maxRPS diff --git a/motionplan/tpspace/ptgCCS.go b/motionplan/tpspace/ptgCCS.go index 5a87d305041..b930d7f1bf1 100644 --- a/motionplan/tpspace/ptgCCS.go +++ b/motionplan/tpspace/ptgCCS.go @@ -34,7 +34,7 @@ func NewCCSPTG(maxMMPS, maxRPS float64) PrecomputePTG { // For this particular driver, turns alpha into a linear + angular velocity. Linear is just max * fwd/back. // Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased // because they will have zero linear velocity through their turns, not max. -func (ptg *ptgDiffDriveCCS) PTGVelocities(alpha, dist, x, y, phi float64) (float64, float64, error) { +func (ptg *ptgDiffDriveCCS) PTGVelocities(alpha, dist float64) (float64, float64, error) { u := math.Abs(alpha) * 0.5 k := math.Copysign(1.0, dist) diff --git a/motionplan/tpspace/ptgCS.go b/motionplan/tpspace/ptgCS.go index 45375543d3d..7cc4ce3f61d 100644 --- a/motionplan/tpspace/ptgCS.go +++ b/motionplan/tpspace/ptgCS.go @@ -36,7 +36,7 @@ func NewCSPTG(maxMMPS, maxRPS float64) PrecomputePTG { // For this particular driver, turns alpha into a linear + angular velocity. Linear is just max * fwd/back. // Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased // because they will have zero linear velocity through their turns, not max. -func (ptg *ptgDiffDriveCS) PTGVelocities(alpha, dist, x, y, phi float64) (float64, float64, error) { +func (ptg *ptgDiffDriveCS) PTGVelocities(alpha, dist float64) (float64, float64, error) { // Magic number; rotate this much before going straight turnDist := math.Sqrt(math.Abs(alpha)) * ptg.turnStraight k := math.Copysign(1.0, dist) diff --git a/motionplan/tpspace/simPtgAlpha.go b/motionplan/tpspace/simPtgAlpha.go index 05839760870..99324d383a3 100644 --- a/motionplan/tpspace/simPtgAlpha.go +++ b/motionplan/tpspace/simPtgAlpha.go @@ -1,46 +1,46 @@ package tpspace -import ( - "math" - "errors" +//~ import ( + //~ "math" + //~ "errors" - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/spatialmath" -) - -// Pi / 4 (45 degrees), used as a default alpha constant -// This controls how tightly our parabolas arc -// 57 degrees is also sometimes used by the reference. -const quarterPi = 0.78539816339 - -// simPtgAlpha defines a PTG family which follows a parabolic path. -type simPTGAlpha struct { - maxMMPS float64 // millimeters per second velocity to target - maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius -} - -// NewAlphaPTG creates a new PrecomputePTG of type simPtgAlpha. -// K is unused for alpha PTGs *for now* but we may add in the future. -func NewAlphaPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { - return &simPTGAlpha{ - maxMMPS: maxMMPS, - maxRPS: maxRPS, - } -} - -func (ptg *simPTGAlpha) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { - // In order to know what to set our angvel at, we need to know how far into the path we are - atA := wrapTo2Pi(alpha - phi) - if atA > math.Pi { - atA -= 2 * math.Pi - } - - v := ptg.maxMMPS * math.Exp(-1.*math.Pow(atA/quarterPi, 2)) - w := ptg.maxRPS * (-0.5 + (1. / (1. + math.Exp(-atA/quarterPi)))) - - return v, w, nil -} - -func (ptg *simPTGAlpha) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { - return nil, errors.New("alpha PTG does not support Transform yet") -} + //~ "go.viam.com/rdk/referenceframe" + //~ "go.viam.com/rdk/spatialmath" +//~ ) + +//~ // Pi / 4 (45 degrees), used as a default alpha constant +//~ // This controls how tightly our parabolas arc +//~ // 57 degrees is also sometimes used by the reference. +//~ const quarterPi = 0.78539816339 + +//~ // simPtgAlpha defines a PTG family which follows a parabolic path. +//~ type simPTGAlpha struct { + //~ maxMMPS float64 // millimeters per second velocity to target + //~ maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius +//~ } + +//~ // NewAlphaPTG creates a new PrecomputePTG of type simPtgAlpha. +//~ // K is unused for alpha PTGs *for now* but we may add in the future. +//~ func NewAlphaPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { + //~ return &simPTGAlpha{ + //~ maxMMPS: maxMMPS, + //~ maxRPS: maxRPS, + //~ } +//~ } + +//~ func (ptg *simPTGAlpha) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { + //~ // In order to know what to set our angvel at, we need to know how far into the path we are + //~ atA := wrapTo2Pi(alpha - phi) + //~ if atA > math.Pi { + //~ atA -= 2 * math.Pi + //~ } + + //~ v := ptg.maxMMPS * math.Exp(-1.*math.Pow(atA/quarterPi, 2)) + //~ w := ptg.maxRPS * (-0.5 + (1. / (1. + math.Exp(-atA/quarterPi)))) + + //~ return v, w, nil +//~ } + +//~ func (ptg *simPTGAlpha) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + //~ return nil, errors.New("alpha PTG does not support Transform yet") +//~ } From 62a5ba6b7059e0f556855e337ac75b547b3ea107 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Sat, 12 Aug 2023 15:21:43 -0700 Subject: [PATCH 14/38] Bidirectional tp-space IK solving working reasonably quickly and well. Smoothing not there yet. --- motionplan/cBiRRT.go | 10 +- motionplan/combinedInverseKinematics.go | 6 +- motionplan/inverseKinematics.go | 8 +- motionplan/kinematic_test.go | 4 +- motionplan/metrics.go | 2 +- motionplan/motionPlanner.go | 5 +- motionplan/nloptInverseKinematics.go | 18 +- motionplan/nloptInverseKinematics_test.go | 2 +- motionplan/ptgGroupFrame.go | 2 +- motionplan/ptgIK.go | 90 +++++++-- motionplan/rrt.go | 2 +- motionplan/tpSpaceRRT.go | 221 ++++++++++++++++------ motionplan/tpSpaceRRT_test.go | 42 +++- motionplan/tpspace/ptg.go | 42 ++-- motionplan/tpspace/ptgGridSim.go | 50 ++--- motionplan/tpspace/ptgGridSim_test.go | 18 +- motionplan/tpspace/ptg_test.go | 12 +- 17 files changed, 381 insertions(+), 153 deletions(-) diff --git a/motionplan/cBiRRT.go b/motionplan/cBiRRT.go index 3befcdccea5..5c20dd467eb 100644 --- a/motionplan/cBiRRT.go +++ b/motionplan/cBiRRT.go @@ -104,7 +104,7 @@ func newCBiRRTMotionPlanner( return nil, err } // nlopt should try only once - nlopt, err := CreateNloptIKSolver(frame, logger, 1, opt.GoalThreshold) + nlopt, err := CreateNloptIKSolver(frame, logger, 1, opt.GoalThreshold, false) if err != nil { return nil, err } @@ -413,11 +413,11 @@ func (mp *cBiRRTMotionPlanner) constrainNear( if ok { return target } - solutionGen := make(chan []referenceframe.Input, 1) + solutionGen := make(chan *IKSolution, 1) // Spawn the IK solver to generate solutions until done err = mp.fastGradDescent.Solve(ctx, solutionGen, target, mp.planOpts.pathMetric, randseed.Int()) // We should have zero or one solutions - var solved []referenceframe.Input + var solved *IKSolution select { case solved = <-solutionGen: default: @@ -428,11 +428,11 @@ func (mp *cBiRRTMotionPlanner) constrainNear( } ok, failpos := mp.planOpts.CheckSegmentAndStateValidity( - &Segment{StartConfiguration: seedInputs, EndConfiguration: solved, Frame: mp.frame}, + &Segment{StartConfiguration: seedInputs, EndConfiguration: solved.Configuration, Frame: mp.frame}, mp.planOpts.Resolution, ) if ok { - return solved + return solved.Configuration } if failpos != nil { dist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: target, EndConfiguration: failpos.EndConfiguration}) diff --git a/motionplan/combinedInverseKinematics.go b/motionplan/combinedInverseKinematics.go index 6b528802961..89d529df223 100644 --- a/motionplan/combinedInverseKinematics.go +++ b/motionplan/combinedInverseKinematics.go @@ -30,7 +30,7 @@ func CreateCombinedIKSolver(model referenceframe.Frame, logger golog.Logger, nCP nCPU = 1 } for i := 1; i <= nCPU; i++ { - nlopt, err := CreateNloptIKSolver(model, logger, -1, goalThreshold) + nlopt, err := CreateNloptIKSolver(model, logger, -1, goalThreshold, false) nlopt.id = i if err != nil { return nil, err @@ -43,7 +43,7 @@ func CreateCombinedIKSolver(model referenceframe.Frame, logger golog.Logger, nCP func runSolver(ctx context.Context, solver InverseKinematics, - c chan<- []referenceframe.Input, + c chan<-*IKSolution, seed []referenceframe.Input, m StateMetric, rseed int, @@ -54,7 +54,7 @@ func runSolver(ctx context.Context, // Solve will initiate solving for the given position in all child solvers, seeding with the specified initial joint // positions. If unable to solve, the returned error will be non-nil. func (ik *CombinedIK) Solve(ctx context.Context, - c chan<- []referenceframe.Input, + c chan<-*IKSolution, seed []referenceframe.Input, m StateMetric, rseed int, diff --git a/motionplan/inverseKinematics.go b/motionplan/inverseKinematics.go index d759acd3a2f..866da9f0826 100644 --- a/motionplan/inverseKinematics.go +++ b/motionplan/inverseKinematics.go @@ -10,7 +10,13 @@ import ( // solutions to the provided channel until cancelled or otherwise completes. type InverseKinematics interface { // Solve receives a context, the goal arm position, and current joint angles. - Solve(context.Context, chan<- []referenceframe.Input, []referenceframe.Input, StateMetric, int) error + Solve(context.Context, chan<-*IKSolution, []referenceframe.Input, StateMetric, int) error +} + +type IKSolution struct { + Configuration []referenceframe.Input + Score float64 + Partial bool } func limitsToArrays(limits []referenceframe.Limit) ([]float64, []float64) { diff --git a/motionplan/kinematic_test.go b/motionplan/kinematic_test.go index 726655755b2..00b14a2055c 100644 --- a/motionplan/kinematic_test.go +++ b/motionplan/kinematic_test.go @@ -354,7 +354,7 @@ func TestCombinedCPUs(t *testing.T) { } func solveTest(ctx context.Context, solver InverseKinematics, goal spatial.Pose, seed []frame.Input) ([][]frame.Input, error) { - solutionGen := make(chan []frame.Input) + solutionGen := make(chan *IKSolution) ikErr := make(chan error) ctxWithCancel, cancel := context.WithCancel(ctx) defer cancel() @@ -378,7 +378,7 @@ IK: select { case step := <-solutionGen: - solutions = append(solutions, step) + solutions = append(solutions, step.Configuration) // Skip the return check below until we have nothing left to read from solutionGen continue IK default: diff --git a/motionplan/metrics.go b/motionplan/metrics.go index 6dbcb62636a..aa4cedfbc54 100644 --- a/motionplan/metrics.go +++ b/motionplan/metrics.go @@ -8,7 +8,7 @@ import ( "go.viam.com/rdk/utils" ) -const orientationDistanceScaling = 10. +const orientationDistanceScaling = 50. // StateMetric are functions which, given a State, produces some score. Lower is better. // This is used for gradient descent to converge upon a goal pose, for example. diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 2c9a14ea4da..e75ab7596da 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -255,7 +255,7 @@ func (mp *planner) getSolutions(ctx context.Context, seed []frame.Input) ([]node ctxWithCancel, cancel := context.WithCancel(ctx) defer cancel() - solutionGen := make(chan []frame.Input) + solutionGen := make(chan *IKSolution) ikErr := make(chan error, 1) // Spawn the IK solver to generate solutions until done utils.PanicCapturingGo(func() { @@ -279,7 +279,8 @@ IK: } select { - case step := <-solutionGen: + case stepSolution := <-solutionGen: + step := stepSolution.Configuration // Ensure the end state is a valid one statePass, failName := mp.planOpts.CheckStateConstraints(&State{ Configuration: step, diff --git a/motionplan/nloptInverseKinematics.go b/motionplan/nloptInverseKinematics.go index e547d95dd79..ca501016ea7 100644 --- a/motionplan/nloptInverseKinematics.go +++ b/motionplan/nloptInverseKinematics.go @@ -38,12 +38,13 @@ type NloptIK struct { solveEpsilon float64 logger golog.Logger jump float64 + partial bool } // CreateNloptIKSolver creates an nloptIK object that can perform gradient descent on metrics for Frames. The parameters are the Frame on // which Transform() will be called, a logger, and the number of iterations to run. If the iteration count is less than 1, it will be set // to the default of 5000. -func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int, solveEpsilon float64) (*NloptIK, error) { +func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int, solveEpsilon float64, partial bool) (*NloptIK, error) { ik := &NloptIK{logger: logger} ik.model = mdl @@ -60,13 +61,14 @@ func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int ik.lowerBound, ik.upperBound = limitsToArrays(mdl.DoF()) // How much to adjust joints to determine slope ik.jump = 0.00000001 + ik.partial = partial return ik, nil } // Solve runs the actual solver and sends any solutions found to the given channel. func (ik *NloptIK) Solve(ctx context.Context, - solutionChan chan<- []referenceframe.Input, + solutionChan chan<-*IKSolution, seed []referenceframe.Input, solveMetric StateMetric, rseed int, @@ -186,13 +188,21 @@ func (ik *NloptIK) Solve(ctx context.Context, err = multierr.Combine(err, nloptErr) } - if result < ik.epsilon*ik.epsilon || solutionRaw != nil { + //~ if solutionRaw != nil { + //~ fmt.Println("best nlopt", result) + //~ } + + if result < ik.epsilon*ik.epsilon || (solutionRaw != nil && ik.partial) { select { case <-ctx.Done(): return err default: } - solutionChan <- referenceframe.FloatsToInputs(solutionRaw) + solutionChan <- &IKSolution{ + Configuration: referenceframe.FloatsToInputs(solutionRaw), + Score: result, + Partial: result >= ik.epsilon*ik.epsilon, + } solutionsFound++ } tries++ diff --git a/motionplan/nloptInverseKinematics_test.go b/motionplan/nloptInverseKinematics_test.go index 9716e4e4d0f..25095f95354 100644 --- a/motionplan/nloptInverseKinematics_test.go +++ b/motionplan/nloptInverseKinematics_test.go @@ -18,7 +18,7 @@ func TestCreateNloptIKSolver(t *testing.T) { logger := golog.NewTestLogger(t) m, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") test.That(t, err, test.ShouldBeNil) - ik, err := CreateNloptIKSolver(m, logger, -1, defaultGoalThreshold) + ik, err := CreateNloptIKSolver(m, logger, -1, defaultGoalThreshold, false) test.That(t, err, test.ShouldBeNil) ik.id = 1 diff --git a/motionplan/ptgGroupFrame.go b/motionplan/ptgGroupFrame.go index 93c5b47e6cc..4f208dbdee5 100644 --- a/motionplan/ptgGroupFrame.go +++ b/motionplan/ptgGroupFrame.go @@ -13,7 +13,7 @@ import ( ) const ( - defaultSimDistMM = 2000. + defaultSimDistMM = 600. ) const ( diff --git a/motionplan/ptgIK.go b/motionplan/ptgIK.go index d0868fda6a1..31ce97e77f7 100644 --- a/motionplan/ptgIK.go +++ b/motionplan/ptgIK.go @@ -4,7 +4,8 @@ package motionplan import ( "context" "math" - "math/rand" + "sync" + //~ "fmt" "github.com/edaniels/golog" @@ -15,7 +16,9 @@ import ( const ( defaultDiffT = 0.01 // seconds - nloptSeed = 42 + nloptSeed = 42 // This should be fine to kepe constant + + defaultZeroDist = 1e-3 // Sometimes nlopt will minimize trajectories to zero. Ensure min traj dist is at least this ) // ptgGridSim will take a PrecomputePTG, and simulate out a number of trajectories through some requested time/distance for speed of lookup @@ -25,9 +28,11 @@ type ptgIK struct { simPTG tpspace.PrecomputePTG ptgFrame referenceframe.Frame fastGradDescent *NloptIK - randseed *rand.Rand gridSim tpspace.PTG + + mu sync.RWMutex + trajCache map[float64][]*tpspace.TrajNode } // NewPTGIK creates a new PTG by simulating a PrecomputePTG for some distance, then cacheing the results in a grid for fast lookup. @@ -37,10 +42,7 @@ func NewPTGIK(simPTG tpspace.PrecomputePTG, logger golog.Logger, refDist float64 return nil, err } - //nolint: gosec - rseed := rand.New(rand.NewSource(int64(randSeed))) - - nlopt, err := CreateNloptIKSolver(ptgFrame, logger, 1, defaultEpsilon*defaultEpsilon) + nlopt, err := CreateNloptIKSolver(ptgFrame, logger, 1, defaultEpsilon*defaultEpsilon, true) if err != nil { return nil, err } @@ -56,35 +58,50 @@ func NewPTGIK(simPTG tpspace.PrecomputePTG, logger golog.Logger, refDist float64 simPTG: simPTG, ptgFrame: ptgFrame, fastGradDescent: nlopt, - randseed: rseed, gridSim: gridSim, + trajCache: map[float64][]*tpspace.TrajNode{}, } return ptg, nil } -func (ptg *ptgIK) CToTP(ctx context.Context, pose spatialmath.Pose) (*tpspace.TrajNode, error) { - +func (ptg *ptgIK) CToTP(ctx context.Context, distFunc func(spatialmath.Pose) float64) (*tpspace.TrajNode, error) { + //~ if pose.Point().Norm() > ptg.refDist { + //~ fmt.Println("simming", pose.Point()) + //~ return ptg.gridSim.CToTP(ctx, pose) + //~ } - - solutionGen := make(chan []referenceframe.Input, 1) - seedInput := []referenceframe.Input{{math.Pi/3}, {ptg.refDist/3}} // random value to seed the IK solver - goalMetric := NewSquaredNormMetric(pose) + solutionGen := make(chan *IKSolution, 1) + seedInput := []referenceframe.Input{{math.Pi/2}, {ptg.refDist/2}} // random value to seed the IK solver + //~ goalMetric := NewSquaredNormMetric(pose) + goalMetric := func(state *State) float64 { + return distFunc(state.Position) + } // Spawn the IK solver to generate a solution err := ptg.fastGradDescent.Solve(ctx, solutionGen, seedInput, goalMetric, nloptSeed) // We should have zero or one solutions - var solved []referenceframe.Input + var solved *IKSolution select { case solved = <-solutionGen: default: } close(solutionGen) - if err != nil || solved == nil { - return nil, nil - return ptg.gridSim.CToTP(ctx, pose) + if err != nil || solved == nil || (solved != nil && solved.Configuration[1].Value < defaultZeroDist) { + return ptg.gridSim.CToTP(ctx, distFunc) } - // TODO: make this more efficient - traj, err := ptg.Trajectory(solved[0].Value, solved[1].Value) + + if solved.Partial { + gridNode, err := ptg.gridSim.CToTP(ctx, distFunc) + if err == nil { + // Check if the grid has a better solution + //~ fmt.Println("comparing score. nlopt:", solved.Score, "grid", distFunc(gridNode.Pose)) + if distFunc(gridNode.Pose) < solved.Score { + return gridNode, nil + } + } + } + + traj, err := ptg.Trajectory(solved.Configuration[0].Value, solved.Configuration[1].Value) if err != nil { return nil, err } @@ -96,5 +113,36 @@ func (ptg *ptgIK) RefDistance() float64 { } func (ptg *ptgIK) Trajectory(alpha, dist float64) ([]*tpspace.TrajNode, error) { - return tpspace.ComputePTG(alpha, ptg.simPTG, dist, defaultDiffT) + ptg.mu.RLock() + precomp := ptg.trajCache[alpha] + ptg.mu.RUnlock() + if precomp != nil { + if precomp[len(precomp)-1].Dist >= dist { + subTraj := []*tpspace.TrajNode{} + for _, wp := range precomp { + if wp.Dist < dist { + subTraj = append(subTraj, wp) + } else { + break + } + } + time := 0. + if len(subTraj) > 0 { + time = subTraj[len(subTraj)-1].Time + } + lastNode, err := tpspace.ComputeNextPTGNode(ptg.simPTG, alpha, dist, time) + if err != nil { + return nil, err + } + subTraj = append(subTraj, lastNode) + } + } + traj, err := tpspace.ComputePTG(ptg.simPTG, alpha, dist, defaultDiffT) + if err != nil { + return nil, err + } + ptg.mu.Lock() + ptg.trajCache[alpha] = traj + ptg.mu.Unlock() + return traj, nil } diff --git a/motionplan/rrt.go b/motionplan/rrt.go index d8065be03c3..91f61321691 100644 --- a/motionplan/rrt.go +++ b/motionplan/rrt.go @@ -211,7 +211,7 @@ func extractPath(startMap, goalMap map[node]node, pair *nodePair) []node { if goalReached != nil { // skip goalReached node and go directly to its parent in order to not repeat this node - goalReached = goalMap[goalReached] + //~ goalReached = goalMap[goalReached] // extract the path to the goal for goalReached != nil { diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index 6f4929cd44f..89ce7d858e9 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -20,7 +20,7 @@ import ( const ( defaultGoalCheck = 5 // Check if the goal is reachable every this many iterations - defaultAutoBB = 1. // Automatic bounding box on driveable area as a multiple of start-goal distance + defaultAutoBB = 0.3 // Automatic bounding box on driveable area as a multiple of start-goal distance // Note: while fully holonomic planners can use the limits of the frame as implicit boundaries, with non-holonomic motion // this is not the case, and the total workspace available to the planned frame is not directly related to the motion available // from a single set of inputs. @@ -29,18 +29,15 @@ const ( defaultAddInt = true // Add a subnode every this many mm along a valid trajectory. Large values run faster, small gives better paths // Meaningless if the above is false. - defaultAddNodeEvery = 250. - - // When getting the closest node to a pose, only look for nodes at least this far away. - defaultDuplicateNodeBuffer = 50. + defaultAddNodeEvery = 75. // Don't add new RRT tree nodes if there is an existing node within this distance. - defaultIdenticalNodeDistance = 5. + defaultIdenticalNodeDistance = 0.05 // Default distance in mm to get within for tp-space trajectories. defaultTPSpaceGoalDist = 10. - defaultMaxReseeds = 10 + defaultMaxReseeds = 5 ) type tpspaceOptions struct { @@ -55,13 +52,9 @@ type tpspaceOptions struct { // Meaningless if the above is false. addNodeEvery float64 - // Don't add new RRT tree nodes if there is an existing node within this distance - dupeNodeBuffer float64 // If the squared norm between two poses is less than this, consider them equal poseSolveDist float64 - - maxReseeds int // Cached functions for calculating TP-space distances for each PTG distOptions map[tpspace.PTG]*plannerOptions @@ -123,7 +116,7 @@ func (mp *tpSpaceRRTMotionPlanner) plan(ctx context.Context, seedPos := spatialmath.NewZeroPose() startNode := &basicNode{q: make([]referenceframe.Input, len(mp.frame.DoF())), cost: 0, pose: seedPos, corner: false} - goalNode := &basicNode{q: nil, cost: 0, pose: goal, corner: false} + goalNode := &basicNode{q: make([]referenceframe.Input, len(mp.frame.DoF())), cost: 0, pose: goal, corner: false} utils.PanicCapturingGo(func() { mp.planRunner(ctx, seed, &rrtParallelPlannerShared{ @@ -180,15 +173,18 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( break } } + + bidirectional := true dist := math.Sqrt(mp.planOpts.DistanceFunc(&Segment{StartPosition: startPose, EndPosition: goalPose})) fmt.Println("dist", dist) fmt.Println(startPose.Point(), goalPose.Point()) - midPt := goalPose.Point().Sub(startPose.Point()) // used for initial seed + //~ midPt := goalPose.Point().Sub(startPose.Point()).Mul(0.5) // used for initial seed + midPt := spatialmath.Interpolate(startPose, goalPose, 0.5) // used for initial seed var randPos spatialmath.Pose - //~ for iter := 0; iter < mp.algOpts.PlanIter; iter++ { - for iter := 0; iter < 125; iter++ { + for iter := 0; iter < mp.algOpts.PlanIter; iter++ { + //~ for iter := 0; iter < 1; iter++ { fmt.Println("iter", iter) if ctx.Err() != nil { mp.logger.Debugf("TP Space RRT timed out after %d iterations", iter) @@ -201,26 +197,41 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( randPosY := float64(mp.randseed.Intn(int(rDist))) randPosTheta := math.Pi * (mp.randseed.Float64() - 0.5) randPos = spatialmath.NewPose( - r3.Vector{midPt.X + (randPosX - rDist/2.), midPt.Y + (randPosY - rDist/2.), 0}, + r3.Vector{midPt.Point().X + (randPosX - rDist/2.), midPt.Point().Y + (randPosY - rDist/2.), 0}, &spatialmath.OrientationVector{OZ: 1, Theta: randPosTheta}, ) + if iter == 0 { + randPos = midPt + } + if !bidirectional && iter % 5 == 0 { + randPos = goalPose + } randPosNode := &basicNode{q: nil, cost: 0, pose: randPos, corner: false} _ = randPosNode //~ goalNode := &basicNode{pose: goalPose} - seedMapNode, err := mp.attemptExtension(ctx, nil, randPosNode, rrt.maps.startMap, false) + var seedMapNode node + var goalMapNode node + var err error + + fmt.Println("extending towards", spatialmath.PoseToProtobuf(randPos)) + // TODO: parallel + seedMapNode, err = mp.attemptExtension(ctx, nil, randPosNode, rrt.maps.startMap, false) if err != nil { rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} return } - //~ seedMapNode := &basicNode{pose: spatialmath.NewZeroPose()} - goalMapNode, err := mp.attemptExtension(ctx, nil, randPosNode, rrt.maps.goalMap, true) + fmt.Println("____done start") + + goalMapNode, err = mp.attemptExtension(ctx, nil, randPosNode, rrt.maps.goalMap, true) if err != nil { rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} return } + fmt.Println("____done goal") + //~ if seedMapNode != nil && goalMapNode != nil && false{ if seedMapNode != nil && goalMapNode != nil { seedMapNode, err = mp.attemptExtension(ctx, nil, goalMapNode, rrt.maps.startMap, false) @@ -228,6 +239,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} return } + fmt.Println("____done start ext") if seedMapNode == nil { continue } @@ -236,6 +248,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} return } + fmt.Println("____done goal ext") if goalMapNode == nil { continue } @@ -245,6 +258,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( // If we tried the goal and have a close-enough XY location, check if the node is good enough to be a final goal if reachedDelta <= mp.algOpts.poseSolveDist { // If we've reached the goal, break out + fmt.Println("__connected!\n", seedMapNode, "\n", goalMapNode) path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: seedMapNode, b: goalMapNode}) //~ path = path[1:] // The rrt.solutionChan <- &rrtPlanReturn{steps: path, maps: rrt.maps} @@ -255,7 +269,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( rrt.solutionChan <- &rrtPlanReturn{maps: rrt.maps, planerr: errors.New("tpspace RRT unable to create valid path")} } -// getExtensionCandidate will return either nil, or the best node on a PTG to reach the desired random node and its RRT tree parent. +// getExtensionCandidate will return either nil, or the best node on a valid PTG to reach the desired random node and its RRT tree parent. func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( ctx context.Context, randPosNode node, @@ -263,7 +277,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( curPtg tpspace.PTG, rrt rrtMap, nearest node, - planOpts *plannerOptions, // Need to pass this in explicitly for smoothing + planOpts *plannerOptions, invert bool, ) (*candidate, error) { nm := &neighborManager{nCPU: mp.planOpts.NumThreads} @@ -286,15 +300,32 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( //~ fmt.Println("nearest", nearest.Pose().Point(), nearest.Pose().Orientation().OrientationVectorDegrees()) // Get cartesian distance from NN to rand - relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) + var targetFunc func(spatialmath.Pose) float64 + if invert { + //~ relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) + sqMet := NewSquaredNormMetric(randPosNode.Pose()) + targetFunc = func(pose spatialmath.Pose) float64 { + return sqMet(&State{Position: spatialmath.Compose(nearest.Pose(), spatialmath.PoseInverse(pose))}) + } + + } else { + relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) + sqMet := NewSquaredNormMetric(relPose) + targetFunc = func(pose spatialmath.Pose) float64 { + return sqMet(&State{Position: pose}) + } + } + // Convert cartesian distance to tp-space using inverse curPtg, yielding TP-space coordinates goalK and goalD - bestNode, err := curPtg.CToTP(ctx, relPose) + bestNode, err := curPtg.CToTP(ctx, targetFunc) if err != nil || bestNode == nil { return nil, err } - bestDist := mp.planOpts.DistanceFunc(&Segment{StartPosition: relPose, EndPosition: bestNode.Pose}) + bestDist := targetFunc(bestNode.Pose) goalAlpha := bestNode.Alpha goalD := bestNode.Dist + + //~ fmt.Println("best was", bestDist, bestNode.Pose.Point()) //~ fmt.Println("alpha dist", goalAlpha, goalD) // Check collisions along this traj and get the longest distance viable trajK, err := curPtg.Trajectory(goalAlpha, goalD) @@ -338,7 +369,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( lastDist = trajPt.Dist } - isLastNode := finalTrajNode.Dist == curPtg.RefDistance() + isLastNode := math.Abs(finalTrajNode.Dist - curPtg.RefDistance()) < 1e-3 // add the last node in trajectory successNode = &basicNode{ @@ -358,6 +389,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( //~ // Ensure successNode is sufficiently far from the nearest node already existing in the tree //~ // If too close, don't add a new node //~ if dist < defaultIdenticalNodeDistance { + //~ fmt.Println("too close!!!") //~ cand = nil //~ } //~ } @@ -374,7 +406,8 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( invert bool, ) (node, error) { var reseedCandidate *candidate - for i := 0; i < mp.algOpts.maxReseeds; i++ { + maxReseeds := 1 // Will be updated as necessary + for i := 0; i <= maxReseeds; i++ { select { case <-ctx.Done(): return nil, ctx.Err() @@ -397,18 +430,25 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( var err error reseedCandidate, err = mp.extendMap(ctx, candidates, rrt, invert) if err != nil { + //~ fmt.Println("ret err") return nil, err } if reseedCandidate == nil { + //~ fmt.Println("ret nil") return nil, nil } dist := mp.planOpts.DistanceFunc(&Segment{StartPosition: reseedCandidate.newNode.Pose(), EndPosition: goalNode.Pose()}) if dist < mp.planOpts.GoalThreshold || !reseedCandidate.lastInTraj { // Reached the goal position, or otherwise failed to fully extend to the end of a trajectory + //~ fmt.Println("ret success") return reseedCandidate.newNode, nil } - if dist < reseedCandidate.newNode.Q()[2].Value && i < mp.algOpts.maxReseeds - 2{ - i = mp.algOpts.maxReseeds - 2 + if i == 0 { + dist = math.Sqrt(dist) + // TP-space distance is NOT the same thing as cartesian distance, but they track sufficiently well that this is valid to do. + maxReseeds = int(math.Min(float64(defaultMaxReseeds), math.Ceil(dist / reseedCandidate.newNode.Q()[2].Value) + 2)) + //~ fmt.Println("updating max reseed to", maxReseeds) + } seedNode = reseedCandidate.newNode } @@ -444,7 +484,7 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( randAlpha := newNode.Q()[1].Value randDist := newNode.Q()[2].Value - fmt.Println("traj", randAlpha, randDist) + fmt.Println("__ADDINGtraj", ptgNum, randAlpha, randDist) trajK, err := mp.tpFrame.PTGs()[ptgNum].Trajectory(randAlpha, randDist) if err != nil { return nil, err @@ -468,15 +508,16 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( return nil, ctx.Err() } trajState = &State{Position: spatialmath.Compose(arcStartPose, trajPt.Pose)} - //~ if !invert { - //~ fmt.Printf("FINALPATH,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) - //~ }else{ - //~ fmt.Printf("FINALPATH2,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) - //~ } + if !invert { + fmt.Printf("FINALPATH1,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + }else{ + fmt.Printf("FINALPATH2,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + } sinceLastNode += (trajPt.Dist - lastDist) // Optionally add sub-nodes along the way. Will make the final path a bit better - if sinceLastNode > mp.algOpts.addNodeEvery { + // Intermediate nodes currently disabled on the goal map because they do not invert nicely + if sinceLastNode > mp.algOpts.addNodeEvery && !invert { // add the last node in trajectory addedNode = &basicNode{ q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), randAlpha, trajPt.Dist}), @@ -489,7 +530,7 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( } lastDist = trajPt.Dist } - //~ fmt.Printf("WP,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + fmt.Printf("WPI,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) } rrt[newNode] = treeNode return bestCand, nil @@ -504,18 +545,15 @@ func (mp *tpSpaceRRTMotionPlanner) setupTPSpaceOptions() { addIntermediate: defaultAddInt, addNodeEvery: defaultAddNodeEvery, - dupeNodeBuffer: defaultDuplicateNodeBuffer, poseSolveDist: defaultIdenticalNodeDistance, - maxReseeds: defaultMaxReseeds, - distOptions: map[tpspace.PTG]*plannerOptions{}, invertDistOptions: map[tpspace.PTG]*plannerOptions{}, } for _, curPtg := range mp.tpFrame.PTGs() { - tpOpt.distOptions[curPtg] = mp.make2DTPSpaceDistanceOptions(curPtg, tpOpt.dupeNodeBuffer, false) - tpOpt.invertDistOptions[curPtg] = mp.make2DTPSpaceDistanceOptions(curPtg, tpOpt.dupeNodeBuffer, true) + tpOpt.distOptions[curPtg] = mp.make2DTPSpaceDistanceOptions(curPtg, false) + tpOpt.invertDistOptions[curPtg] = mp.make2DTPSpaceDistanceOptions(curPtg, true) } mp.algOpts = tpOpt @@ -523,36 +561,113 @@ func (mp *tpSpaceRRTMotionPlanner) setupTPSpaceOptions() { // make2DTPSpaceDistanceOptions will create a plannerOptions object with a custom DistanceFunc constructed such that // distances can be computed in TP space using the given PTG. -func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, min float64, invert bool) *plannerOptions { +func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, invert bool) *plannerOptions { opts := newBasicPlannerOptions() segMetric := func(seg *Segment) float64 { + // When running NearestNeighbor: + // StartPosition is the seed/query + // EndPosition is the pose already in the RRT tree if seg.StartPosition == nil || seg.EndPosition == nil { + //~ fmt.Println("ret inf") return math.Inf(1) } - //~ fmt.Println("calc dist", seg.StartPosition.Point(), seg.EndPosition.Point()) - var relPose spatialmath.Pose + var targetFunc func(spatialmath.Pose) float64 if invert { - relPose = spatialmath.Compose(spatialmath.PoseInverse(seg.EndPosition), seg.StartPosition) + sqMet := NewSquaredNormMetric(seg.StartPosition) + targetFunc = func(pose spatialmath.Pose) float64 { + return sqMet(&State{Position: spatialmath.Compose(seg.EndPosition, spatialmath.PoseInverse(pose))}) + } } else { - relPose = spatialmath.Compose(spatialmath.PoseInverse(seg.StartPosition), seg.EndPosition) + + relPose := spatialmath.Compose(spatialmath.PoseInverse(seg.EndPosition), seg.StartPosition) + sqMet := NewSquaredNormMetric(relPose) + targetFunc = func(pose spatialmath.Pose) float64 { + return sqMet(&State{Position: pose}) + } } - closeNode, err := ptg.CToTP(context.Background(), relPose) + closeNode, err := ptg.CToTP(context.Background(), targetFunc) if err != nil || closeNode == nil { //~ fmt.Println("inf") return math.Inf(1) } - dist := SquaredNormSegmentMetric(&Segment{StartPosition: closeNode.Pose, EndPosition: relPose}) - //~ fmt.Println("dist", dist) - // Note that we want to return distances in TP space here - //~ return closeNode.Dist - return dist + return targetFunc(closeNode.Pose) } opts.DistanceFunc = segMetric return opts } func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) []node { - mp.logger.Info("smoothing not yet supported for tp-space") + mp.logger.Info("smoothing not yet working well for tp-space") + return path + toIter := int(math.Min(float64(len(path)*len(path)), float64(mp.planOpts.SmoothIter))) * 5 + //~ toIter = 1 + for i := 0; i < toIter; i++ { + select { + case <-ctx.Done(): + return path + default: + } + // get start node of first edge. Cannot be either the last or second-to-last node. + // Intn will return an int in the half-open interval half-open interval [0,n) + firstEdge := mp.randseed.Intn(len(path) - 2) + secondEdge := firstEdge + 1 + mp.randseed.Intn((len(path)-2)-firstEdge) + mp.logger.Debugf("checking shortcut between nodes %d and %d", firstEdge, secondEdge+1) + fmt.Printf("checking shortcut between nodes %d and %d \n", firstEdge, secondEdge+1) + + startMap := map[node]node{} + var parent node + parentPose := spatialmath.NewZeroPose() + for j := 0; j <= firstEdge; j++ { + pathNode := path[j] + startMap[pathNode] = parent + for _, adj := range []float64{0.25, 0.5, 0.75} { + fullQ := pathNode.Q() + newQ := []referenceframe.Input{fullQ[0], fullQ[1], referenceframe.Input{fullQ[2].Value * adj}} + trajK, err := mp.tpFrame.PTGs()[int(math.Round(newQ[0].Value))].Trajectory(newQ[1].Value, newQ[2].Value) + if err != nil { + continue + } + + intNode := &basicNode{ + q: newQ, + cost: pathNode.Cost() - (pathNode.Q()[2].Value * (1 - adj)), + pose: spatialmath.Compose(parentPose, trajK[len(trajK)-1].Pose), + corner: false, + } + startMap[intNode] = parent + } + parent = pathNode + parentPose = parent.Pose() + } + + reached, err := mp.attemptExtension(ctx, nil, path[secondEdge+1], startMap, false) + if err != nil || reached == nil { + continue + } + + reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartPosition: reached.Pose(), EndPosition: path[secondEdge+1].Pose()}) + // If we tried the goal and have a close-enough XY location, check if the node is good enough to be a final goal + if reachedDelta > mp.algOpts.poseSolveDist { + continue + } + + newInputSteps := extractPath(startMap, nil, &nodePair{a: reached, b: nil}) + fmt.Println("newpath no extra", newInputSteps) + for i, wp := range newInputSteps { + fmt.Println(i, wp.Cost(), wp.Q()) + } + newInputSteps = append(newInputSteps, path[secondEdge+1:]...) + fmt.Println("oldpath", path) + for i, wp := range path { + fmt.Println(i, wp.Cost(), wp.Q()) + } + fmt.Println("newpath", newInputSteps) + path = newInputSteps + for i, wp := range path { + fmt.Println(i, wp.Cost(), wp.Q()) + } + //~ break + } return path } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index da730f44065..bd692689d09 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -39,8 +39,9 @@ func TestPtgRrt(t *testing.T) { ) test.That(t, err, test.ShouldBeNil) - //~ goalPos := spatialmath.NewPose(r3.Vector{X: 100, Y: 700, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 180}) - goalPos := spatialmath.NewPose(r3.Vector{X: 100, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 0}) + goalPos := spatialmath.NewPose(r3.Vector{X: 200, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 80}) + //~ goalPos := spatialmath.NewPose(r3.Vector{X: 200, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 0}) + //~ goalPos := spatialmath.NewPose(r3.Vector{X: 0, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 0}) opt := newBasicPlannerOptions() //~ opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) @@ -60,6 +61,26 @@ func TestPtgRrt(t *testing.T) { } test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThanOrEqualTo, 2) + + allPtgs := ackermanFrame.(tpspace.PTGProvider).PTGs() + lastPose := spatialmath.NewZeroPose() + + if printPath { + for _, mynode := range plan { + trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) + for i, pt := range trajPts { + intPose := spatialmath.Compose(lastPose, pt.Pose) + if i == 0 { + fmt.Printf("WP,%f,%f\n", intPose.Point().X, intPose.Point().Y) + } + fmt.Printf("FINALPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) + if i == len(trajPts) - 1 { + lastPose = spatialmath.Compose(lastPose, pt.Pose) + break + } + } + } + } } func TestPtgWithObstacle(t *testing.T) { @@ -161,6 +182,23 @@ func TestPtgWithObstacle(t *testing.T) { } } } + + plan = tp.smoothPath(ctx, plan) + lastPose = spatialmath.NewZeroPose() + for _, mynode := range plan { + trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) + for i, pt := range trajPts { + intPose := spatialmath.Compose(lastPose, pt.Pose) + if i == 0 { + fmt.Printf("SMOOTHWP,%f,%f\n", intPose.Point().X, intPose.Point().Y) + } + fmt.Printf("SMOOTHPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) + if pt.Dist >= mynode.Q()[2].Value { + lastPose = spatialmath.Compose(lastPose, pt.Pose) + break + } + } + } } diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index 41d8573d0cb..d036bf9536f 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -17,8 +17,10 @@ const floatEpsilon = 0.0001 // If floats are closer than this consider them equa // PTG coordinates are specified in polar coordinates (alpha, d) // One of these is needed for each sort of motion that can be done. type PTG interface { - // CToTP Converts a pose to a (k, d) TP-space trajectory, returning the node closest to that pose - CToTP(context.Context, spatialmath.Pose) (*TrajNode, error) + //~ // CToTP Converts a pose to a (k, d) TP-space trajectory, returning the node closest to that pose + //~ CToTP(context.Context, spatialmath.Pose) (*TrajNode, error) + // CToTP will return the (alpha, dist) TP-space coordinates whose corresponding relative pose minimizes the given function + CToTP(context.Context, func(spatialmath.Pose) float64) (*TrajNode, error) // RefDistance returns the maximum distance that a single trajectory may travel RefDistance() float64 @@ -83,18 +85,14 @@ func xythetaToPose(x, y, theta float64) spatialmath.Pose { } func ComputePTG( - alpha float64, simPTG PrecomputePTG, - refDist, diffT float64, - + alpha, refDist, diffT float64, ) ([]*TrajNode, error) { // Initialize trajectory with an all-zero node alphaTraj := []*TrajNode{{Pose: spatialmath.NewZeroPose()}} var err error - var w float64 - var v float64 - var t float64 + var t, v, w float64 dist := math.Copysign(math.Abs(v) * diffT, refDist) // Last saved waypoints @@ -104,26 +102,27 @@ func ComputePTG( // Step through each time point for this alpha for math.Abs(dist) < math.Abs(refDist) { - v, w, err = simPTG.PTGVelocities(alpha, dist) + + t += diffT + nextNode, err := ComputeNextPTGNode(simPTG, alpha, dist, t) if err != nil { return nil, err } + v = nextNode.LinVelMMPS + w = nextNode.AngVelRPS + lastVs[1] = lastVs[0] lastWs[1] = lastWs[0] lastVs[0] = v lastWs[0] = w - t += diffT + // Update velocities of last node because reasons alphaTraj[len(alphaTraj)-1].LinVelMMPS = v alphaTraj[len(alphaTraj)-1].AngVelRPS = w - pose, err := simPTG.Transform([]referenceframe.Input{{alpha}, {dist}}) - if err != nil { - return nil, err - } - alphaTraj = append(alphaTraj, &TrajNode{pose, t, dist, alpha, v, w, pose.Point().X, pose.Point().Y}) + alphaTraj = append(alphaTraj, nextNode) dist += math.Copysign(math.Abs(v) * diffT, refDist) } @@ -138,3 +137,16 @@ func ComputePTG( alphaTraj = append(alphaTraj, tNode) return alphaTraj, nil } + +func ComputeNextPTGNode( + simPTG PrecomputePTG, + alpha, dist, atT float64, +) (*TrajNode, error) { + v, w, err := simPTG.PTGVelocities(alpha, dist) + + pose, err := simPTG.Transform([]referenceframe.Input{{alpha}, {dist}}) + if err != nil { + return nil, err + } + return &TrajNode{pose, atT, dist, alpha, v, w, pose.Point().X, pose.Point().Y}, nil +} diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index 5df4a071ced..6042513a0c7 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -16,6 +16,7 @@ const ( defaultSearchRadius = 10. defaultMaxHeadingChange = 1.95 * math.Pi + orientationDistanceScaling = 10. ) // ptgGridSim will take a PrecomputePTG, and simulate out a number of trajectories through some requested time/distance for speed of lookup @@ -67,44 +68,18 @@ func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64, endsOnly bo return ptg, nil } -func (ptg *ptgGridSim) CToTP(ctx context.Context, pose spatialmath.Pose) (*TrajNode, error) { +func (ptg *ptgGridSim) CToTP(ctx context.Context, distFunc func(spatialmath.Pose) float64) (*TrajNode, error) { - point := pose.Point() - x := point.X - y := point.Y - - nearbyNodes := []*TrajNode{} // Try to find a closest point to the paths: bestDist := math.Inf(1) var bestNode *TrajNode if !ptg.endsOnly { - // First, try to do a quick grid-based lookup - // TODO: an octree should be faster - for tx := int(math.Round(x - ptg.searchRad)); tx < int(math.Round(x+ptg.searchRad)); tx++ { - if ptg.trajNodeGrid[tx] != nil { - for ty := int(math.Round(y - ptg.searchRad)); ty < int(math.Round(y+ptg.searchRad)); ty++ { - nearbyNodes = append(nearbyNodes, ptg.trajNodeGrid[tx][ty]...) - } - } - } - - if len(nearbyNodes) > 0 { - for _, nearbyNode := range nearbyNodes { - distToPoint := math.Pow(nearbyNode.ptX-x, 2) + math.Pow(nearbyNode.ptY-y, 2) - if distToPoint < bestDist { - bestDist = distToPoint - - bestNode = nearbyNode - } - } - return bestNode, nil - } for k := 0; k < int(ptg.alphaCnt); k++ { nMax := len(ptg.precomputeTraj[k]) - 1 for n := 0; n <= nMax; n++ { - distToPoint := math.Pow(ptg.precomputeTraj[k][n].ptX-x, 2) + math.Pow(ptg.precomputeTraj[k][n].ptY-y, 2) + distToPoint := distFunc(ptg.precomputeTraj[k][n].Pose) if distToPoint < bestDist { bestDist = distToPoint @@ -123,9 +98,11 @@ func (ptg *ptgGridSim) CToTP(ctx context.Context, pose spatialmath.Pose) (*TrajN // which can be normalized by "1/refDistance" to get TP-Space distances. for k := 0; k < int(ptg.alphaCnt); k++ { n := len(ptg.precomputeTraj[k]) - 1 - - distToPoint := math.Pow(ptg.precomputeTraj[k][n].Dist, 2) + - math.Pow(ptg.precomputeTraj[k][n].ptX-x, 2) + math.Pow(ptg.precomputeTraj[k][n].ptY-y, 2) + //~ fmt.Println("checking", ptg.precomputeTraj[k][n].ptX, ptg.precomputeTraj[k][n].ptY) + distToPoint := distFunc(ptg.precomputeTraj[k][n].Pose) + //~ distToPoint := math.Sqrt(math.Pow(ptg.precomputeTraj[k][n].Dist, 2) + + //~ math.Pow(ptg.precomputeTraj[k][n].ptX-x, 2) + math.Pow(ptg.precomputeTraj[k][n].ptY-y, 2)) + //~ fmt.Println("checked dist", distToPoint) if distToPoint < bestDist { bestDist = distToPoint @@ -167,7 +144,7 @@ func (ptg *ptgGridSim) simulateTrajectories(simPTG PrecomputePTG) ([][]*TrajNode for k := uint(0); k < ptg.alphaCnt; k++ { alpha := index2alpha(k, ptg.alphaCnt) - alphaTraj, err := ComputePTG(alpha, simPTG, ptg.refDist, ptg.diffT) + alphaTraj, err := ComputePTG(simPTG, alpha, ptg.refDist, ptg.diffT) if err != nil { return nil, err } @@ -189,3 +166,12 @@ func (ptg *ptgGridSim) simulateTrajectories(simPTG PrecomputePTG) ([][]*TrajNode return allTraj, nil } + + +func goalPoseDiscFunc(goal spatialmath.Pose) func(spatialmath.Pose) float64 { + return func (query spatialmath.Pose) float64 { + delta := spatialmath.PoseDelta(goal, query) + // Increase weight for orientation since it's a small number + return delta.Point().Norm2() + spatialmath.QuatToR3AA(delta.Orientation().Quaternion()).Mul(orientationDistanceScaling).Norm2() + } +} diff --git a/motionplan/tpspace/ptgGridSim_test.go b/motionplan/tpspace/ptgGridSim_test.go index a9a624c68ca..7a990d1ac09 100644 --- a/motionplan/tpspace/ptgGridSim_test.go +++ b/motionplan/tpspace/ptgGridSim_test.go @@ -5,6 +5,8 @@ import ( "fmt" "go.viam.com/test" + "go.viam.com/rdk/spatialmath" + "github.com/golang/geo/r3" ) @@ -23,7 +25,7 @@ var ( ) func TestSim(t *testing.T) { - simDist := 150. + simDist := 600. alphaCnt := uint(121) fmt.Println("type,X,Y") //~ for _, ptg := range defaultPTGs { @@ -37,7 +39,7 @@ func TestSim(t *testing.T) { //~ for i := uint(0); i < alphaCnt; i++ { //~ i := uint(60) - alpha := -3.115629077940291 + alpha := -0.41541721039203877 //~ traj, _ := grid.Trajectory(index2alpha(i, alphaCnt), simDist) traj, _ := grid.Trajectory(alpha, simDist) for _, intPose := range traj{ @@ -45,3 +47,15 @@ func TestSim(t *testing.T) { } //~ } } + +func TestPose(t *testing.T) { + goalPose := spatialmath.NewPoseFromPoint(r3.Vector{50,1000,0}) + trajPose := spatialmath.NewPose(r3.Vector{-100,10,0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 90}) + startPose := spatialmath.Compose(goalPose, spatialmath.PoseInverse(trajPose)) + // resultPose x:39.999999999999886 y:1100 o_z:1 theta:-89.99999999999999 + + //~ startPose := spatialmath.Compose(spatialmath.PoseInverse(trajPose), goalPose) + // resultPose x:990 y:50.000000000000114 o_z:1 theta:-89.99999999999999 + + fmt.Println("resultPose", spatialmath.PoseToProtobuf(startPose)) +} diff --git a/motionplan/tpspace/ptg_test.go b/motionplan/tpspace/ptg_test.go index 74c99ac4fe2..6ea2a0e218d 100644 --- a/motionplan/tpspace/ptg_test.go +++ b/motionplan/tpspace/ptg_test.go @@ -15,11 +15,9 @@ func TestAlphaIdx(t *testing.T) { } } -func TestRevPTGs(t *testing.T) { - cs := NewCirclePTG(0.3, 0.3) - //~ cs := NewCSPTG(0.3, 0.3) - _, err := ComputePTG(0, cs, -2000, 0.05) - test.That(t, err, test.ShouldBeNil) +//~ func TestRevPTGs(t *testing.T) { + //~ cs := NewCirclePTG(0.3, 0.3) + //~ traj, err := ComputePTG(cs, 0, -2000, 0.05) + //~ test.That(t, err, test.ShouldBeNil) //~ fmt.Println(traj) - -} +//~ } From 248f7b4a7e7a3bd61a3d05d8c88398966581fdb8 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Tue, 15 Aug 2023 08:54:06 -0700 Subject: [PATCH 15/38] Fiddling with parallelization etc --- motionplan/cBiRRT.go | 2 +- motionplan/metrics.go | 2 +- motionplan/planManager.go | 3 - motionplan/ptgGroupFrame.go | 44 ++++- motionplan/rrt.go | 23 ++- motionplan/tpSpaceRRT.go | 298 ++++++++++++++++--------------- motionplan/tpSpaceRRT_test.go | 13 +- motionplan/tpspace/ptgGridSim.go | 25 +-- motionplan/tpspace/ptgIKFrame.go | 2 +- 9 files changed, 228 insertions(+), 184 deletions(-) diff --git a/motionplan/cBiRRT.go b/motionplan/cBiRRT.go index 5c20dd467eb..5bd24b1034a 100644 --- a/motionplan/cBiRRT.go +++ b/motionplan/cBiRRT.go @@ -257,7 +257,7 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( if reachedDelta <= mp.algOpts.JointSolveDist { mp.logger.Debugf("CBiRRT found solution after %d iterations", i) cancel() - path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{map1reached, map2reached}) + path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{map1reached, map2reached}, true) rrt.solutionChan <- &rrtPlanReturn{steps: path, maps: rrt.maps} return } diff --git a/motionplan/metrics.go b/motionplan/metrics.go index aa4cedfbc54..4898974197f 100644 --- a/motionplan/metrics.go +++ b/motionplan/metrics.go @@ -8,7 +8,7 @@ import ( "go.viam.com/rdk/utils" ) -const orientationDistanceScaling = 50. +const orientationDistanceScaling = 30. // StateMetric are functions which, given a State, produces some score. Lower is better. // This is used for gradient descent to converge upon a goal pose, for example. diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 3d9a152cee5..0e1bcdbf51b 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -508,9 +508,6 @@ func (pm *planManager) plannerSetupFromMoveRequest( // Distances are computed in cartesian space rather than configuration space opt.DistanceFunc = SquaredNormSegmentMetric - // TODO: instead of using a default this should be set from the TP frame as a function of the resolution of - // the simulated trajectories - opt.GoalThreshold = defaultTPSpaceGoalDist planAlg = "tpspace" } diff --git a/motionplan/ptgGroupFrame.go b/motionplan/ptgGroupFrame.go index 4f208dbdee5..d48cb41932c 100644 --- a/motionplan/ptgGroupFrame.go +++ b/motionplan/ptgGroupFrame.go @@ -1,6 +1,7 @@ package motionplan import ( + "errors" "fmt" "math" @@ -13,7 +14,7 @@ import ( ) const ( - defaultSimDistMM = 600. + defaultSimDistMM = 400. ) const ( @@ -37,6 +38,9 @@ type ptgGroupFrame struct { limits []referenceframe.Limit geometries []spatialmath.Geometry ptgs []tpspace.PTG + velocityMMps float64 + turnRadMeters float64 + logger golog.Logger } // NewPTGFrameFromTurningRadius will create a new Frame which is also a PTGProvider. It will precompute the default set of @@ -56,18 +60,20 @@ func NewPTGFrameFromTurningRadius( if refDist <= 0 { refDist = defaultSimDistMM + //~ refDist = 1000. * turnRadMeters * math.Pi * 0.8 } // Get max angular velocity in radians per second maxRPS := velocityMMps / (1000. * turnRadMeters) pf := &ptgGroupFrame{name: name} err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist, false) - //~ err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist, true) if err != nil { return nil, err } pf.geometries = geoms + pf.velocityMMps = velocityMMps + pf.turnRadMeters = turnRadMeters pf.limits = []referenceframe.Limit{ {Min: 0, Max: float64(len(pf.ptgs) - 1)}, @@ -78,6 +84,38 @@ func NewPTGFrameFromTurningRadius( return pf, nil } +// newPTGFrameFromPTGFrame will create a new Frame from a preexisting ptgGroupFrame, allowing the adjustment of `refDist` while keeping +// other params the same. This may be expanded to allow altering turning radius, geometries, etc +func newPTGFrameFromPTGFrame(frame referenceframe.Frame, refDist float64) (referenceframe.Frame, error) { + + ptgFrame, ok := frame.(*ptgGroupFrame) + if !ok { + return nil, errors.New("cannot create ptg framem given frame is not a ptgGroupFrame") + } + + if refDist <= 0 { + refDist = 1000. * ptgFrame.turnRadMeters * math.Pi * 0.9 + } + + // Get max angular velocity in radians per second + maxRPS := ptgFrame.velocityMMps / (1000. * ptgFrame.turnRadMeters) + pf := &ptgGroupFrame{name: ptgFrame.name} + err := pf.initPTGs(ptgFrame.logger, ptgFrame.velocityMMps, maxRPS, refDist, false) + if err != nil { + return nil, err + } + + pf.geometries = ptgFrame.geometries + + pf.limits = []referenceframe.Limit{ + {Min: 0, Max: float64(len(pf.ptgs) - 1)}, + {Min: -math.Pi, Max: math.Pi}, + {Min: -refDist, Max: refDist}, + } + + return pf, nil +} + func (pf *ptgGroupFrame) DoF() []referenceframe.Limit { return pf.limits } @@ -145,11 +183,9 @@ func (pf *ptgGroupFrame) PTGs() []tpspace.PTG { func (pf *ptgGroupFrame) initPTGs(logger golog.Logger, maxMps, maxRPS, simDist float64, simulate bool) error { ptgs := []tpspace.PTG{} for _, ptg := range defaultPTGs { - // Positive K calculates trajectories forwards, negative k calculates trajectories backwards ptgGen := ptg(maxMps, maxRPS) if ptgGen != nil { if simulate { - //~ for _, k := range []float64{1., -1.} { for _, k := range []float64{1.} { // irreversible trajectories, e.g. alpha, will return nil for negative k newptg, err := tpspace.NewPTGGridSim(ptgGen, 0, k*simDist, false) // 0 uses default alpha count diff --git a/motionplan/rrt.go b/motionplan/rrt.go index 91f61321691..76780615d8e 100644 --- a/motionplan/rrt.go +++ b/motionplan/rrt.go @@ -117,7 +117,7 @@ func shortestPath(maps *rrtMaps, nodePairs []*nodePair) *rrtPlanReturn { minIdx = i } } - return &rrtPlanReturn{steps: extractPath(maps.startMap, maps.goalMap, nodePairs[minIdx]), maps: maps} + return &rrtPlanReturn{steps: extractPath(maps.startMap, maps.goalMap, nodePairs[minIdx], true), maps: maps} } // node interface is used to wrap a configuration for planning purposes. @@ -188,7 +188,7 @@ func (np *nodePair) sumCosts() float64 { return aCost + bCost } -func extractPath(startMap, goalMap map[node]node, pair *nodePair) []node { +func extractPath(startMap, goalMap map[node]node, pair *nodePair, matched bool) []node { // need to figure out which of the two nodes is in the start map var startReached, goalReached node if _, ok := startMap[pair.a]; ok { @@ -210,14 +210,29 @@ func extractPath(startMap, goalMap map[node]node, pair *nodePair) []node { } if goalReached != nil { - // skip goalReached node and go directly to its parent in order to not repeat this node - //~ goalReached = goalMap[goalReached] + if matched { + // skip goalReached node and go directly to its parent in order to not repeat this node + goalReached = goalMap[goalReached] + } // extract the path to the goal for goalReached != nil { + // special rewriting poses for inverted tree + if lastNode, ok :=path[len(path)-1].(*basicNode); ok { + lastNode.pose = goalReached.Pose() + } + path = append(path, goalReached) goalReached = goalMap[goalReached] } } return path } + +func sumCosts(path []node) float64 { + cost := 0. + for _, wp := range path { + cost += wp.Cost() + } + return cost +} diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index 89ce7d858e9..91e42b18c70 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -29,15 +29,13 @@ const ( defaultAddInt = true // Add a subnode every this many mm along a valid trajectory. Large values run faster, small gives better paths // Meaningless if the above is false. - defaultAddNodeEvery = 75. + defaultAddNodeEvery = 100. // Don't add new RRT tree nodes if there is an existing node within this distance. - defaultIdenticalNodeDistance = 0.05 + // Consider nodes on trees to be connected if they are within this distance. + defaultIdenticalNodeDistance = 1. - // Default distance in mm to get within for tp-space trajectories. - defaultTPSpaceGoalDist = 10. - - defaultMaxReseeds = 5 + defaultMaxReseeds = 50 ) type tpspaceOptions struct { @@ -70,6 +68,11 @@ type candidate struct { lastInTraj bool } +type nodeAndError struct { + node + error +} + // tpSpaceRRTMotionPlanner. type tpSpaceRRTMotionPlanner struct { *planner @@ -92,7 +95,6 @@ func newTPSpaceMotionPlanner( return nil, err } - // either the passed in frame, tpFrame, ok := mp.frame.(tpspace.PTGProvider) if !ok { return nil, fmt.Errorf("frame %v must be a PTGProvider", mp.frame) @@ -176,15 +178,17 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( bidirectional := true + m1chan := make(chan *nodeAndError, 1) + m2chan := make(chan *nodeAndError, 1) + defer close(m1chan) + defer close(m2chan) + dist := math.Sqrt(mp.planOpts.DistanceFunc(&Segment{StartPosition: startPose, EndPosition: goalPose})) - fmt.Println("dist", dist) - fmt.Println(startPose.Point(), goalPose.Point()) - //~ midPt := goalPose.Point().Sub(startPose.Point()).Mul(0.5) // used for initial seed midPt := spatialmath.Interpolate(startPose, goalPose, 0.5) // used for initial seed var randPos spatialmath.Pose - for iter := 0; iter < mp.algOpts.PlanIter; iter++ { - //~ for iter := 0; iter < 1; iter++ { + //~ for iter := 0; iter < mp.algOpts.PlanIter; iter++ { + for iter := 0; iter < 1; iter++ { fmt.Println("iter", iter) if ctx.Err() != nil { mp.logger.Debugf("TP Space RRT timed out after %d iterations", iter) @@ -206,61 +210,57 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( if !bidirectional && iter % 5 == 0 { randPos = goalPose } - randPosNode := &basicNode{q: nil, cost: 0, pose: randPos, corner: false} - _ = randPosNode + randPosNode := &basicNode{pose: randPos} - //~ goalNode := &basicNode{pose: goalPose} - - var seedMapNode node - var goalMapNode node - var err error - - fmt.Println("extending towards", spatialmath.PoseToProtobuf(randPos)) - // TODO: parallel - seedMapNode, err = mp.attemptExtension(ctx, nil, randPosNode, rrt.maps.startMap, false) + utils.PanicCapturingGo(func() { + m1chan <- mp.attemptExtension(ctx, nil, randPosNode, rrt.maps.startMap, false) + }) + utils.PanicCapturingGo(func() { + m2chan <- mp.attemptExtension(ctx, nil, randPosNode, rrt.maps.goalMap, true) + + }) + seedMapReached := <-m1chan + goalMapReached := <-m2chan + + seedMapNode := seedMapReached.node + goalMapNode := goalMapReached.node + err := errors.Join(seedMapReached.error, goalMapReached.error) + if err != nil { rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} return } - fmt.Println("____done start") - goalMapNode, err = mp.attemptExtension(ctx, nil, randPosNode, rrt.maps.goalMap, true) + if err != nil { rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} return } - fmt.Println("____done goal") - //~ if seedMapNode != nil && goalMapNode != nil && false{ if seedMapNode != nil && goalMapNode != nil { - seedMapNode, err = mp.attemptExtension(ctx, nil, goalMapNode, rrt.maps.startMap, false) - if err != nil { - rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + seedReached := mp.attemptExtension(ctx, nil, goalMapNode, rrt.maps.startMap, false) + if seedReached.error != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: seedReached.error, maps: rrt.maps} return } - fmt.Println("____done start ext") - if seedMapNode == nil { + if seedReached.node == nil { continue } - goalMapNode, err := mp.attemptExtension(ctx, nil, seedMapNode, rrt.maps.goalMap, true) - if err != nil { - rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + goalReached := mp.attemptExtension(ctx, nil, seedReached.node, rrt.maps.goalMap, true) + if goalReached.error != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: goalReached.error, maps: rrt.maps} return } - fmt.Println("____done goal ext") - if goalMapNode == nil { + if goalReached.node == nil { continue } - fmt.Println("start tree", seedMapNode.Pose().Point(), seedMapNode.Pose().Orientation().OrientationVectorDegrees()) - fmt.Println("goal tree", goalMapNode.Pose().Point(), goalMapNode.Pose().Orientation().OrientationVectorDegrees()) - reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartPosition: seedMapNode.Pose(), EndPosition: goalMapNode.Pose()}) - // If we tried the goal and have a close-enough XY location, check if the node is good enough to be a final goal + reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartPosition: seedReached.node.Pose(), EndPosition: goalReached.node.Pose()}) + fmt.Println("reached seed", spatialmath.PoseToProtobuf(seedReached.node.Pose())) + fmt.Println("reached goal", spatialmath.PoseToProtobuf(goalReached.node.Pose())) if reachedDelta <= mp.algOpts.poseSolveDist { - // If we've reached the goal, break out - fmt.Println("__connected!\n", seedMapNode, "\n", goalMapNode) - path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: seedMapNode, b: goalMapNode}) - //~ path = path[1:] // The + // If we've reached the goal, extract the path from the RRT trees and return + path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: seedReached.node, b: goalReached.node}, false) rrt.solutionChan <- &rrtPlanReturn{steps: path, maps: rrt.maps} return } @@ -280,7 +280,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( planOpts *plannerOptions, invert bool, ) (*candidate, error) { - nm := &neighborManager{nCPU: mp.planOpts.NumThreads} + nm := &neighborManager{nCPU: mp.planOpts.NumThreads/len(mp.tpFrame.PTGs())} nm.parallelNeighbors = 10 var successNode node @@ -297,12 +297,10 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( return nil, nil } } - //~ fmt.Println("nearest", nearest.Pose().Point(), nearest.Pose().Orientation().OrientationVectorDegrees()) // Get cartesian distance from NN to rand var targetFunc func(spatialmath.Pose) float64 if invert { - //~ relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) sqMet := NewSquaredNormMetric(randPosNode.Pose()) targetFunc = func(pose spatialmath.Pose) float64 { return sqMet(&State{Position: spatialmath.Compose(nearest.Pose(), spatialmath.PoseInverse(pose))}) @@ -325,8 +323,6 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( goalAlpha := bestNode.Alpha goalD := bestNode.Dist - //~ fmt.Println("best was", bestDist, bestNode.Pose.Point()) - //~ fmt.Println("alpha dist", goalAlpha, goalD) // Check collisions along this traj and get the longest distance viable trajK, err := curPtg.Trajectory(goalAlpha, goalD) if err != nil { @@ -338,9 +334,6 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( if invert { arcStartPose = spatialmath.Compose(arcStartPose, spatialmath.PoseInverse(finalTrajNode.Pose)) } - //~ fmt.Println("arc tform", finalTrajNode.Pose.Point()) - //~ fmt.Println("arc tform invert", spatialmath.PoseInverse(finalTrajNode.Pose).Point()) - //~ fmt.Println("arcstart", arcStartPose.Point()) sinceLastCollideCheck := 0. lastDist := 0. @@ -356,8 +349,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( sinceLastCollideCheck += math.Abs(trajPt.Dist - lastDist) trajState := &State{Position: spatialmath.Compose(arcStartPose, trajPt.Pose), Frame: mp.frame} - //~ fmt.Println("trajstate", trajState.Position.Point()) - nodePose = trajState.Position + nodePose = trajState.Position // This will get rewritten later for inverted trees if sinceLastCollideCheck > planOpts.Resolution { ok, _ := planOpts.CheckStateConstraints(trajState) if !ok { @@ -374,25 +366,13 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( // add the last node in trajectory successNode = &basicNode{ q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), goalAlpha, finalTrajNode.Dist}), - cost: nearest.Cost() + finalTrajNode.Dist, + cost: finalTrajNode.Dist, pose: nodePose, corner: false, } cand := &candidate{dist: bestDist, treeNode: nearest, newNode: successNode, lastInTraj: isLastNode} - // check if this successNode is too close to nodes already in the tree, and if so, do not add. - // Get nearest neighbor to new node that's already in the tree - //~ nearest = nm.nearestNeighbor(ctx, planOpts, successNode, rrt) - //~ if nearest != nil { - //~ dist := planOpts.DistanceFunc(&Segment{StartPosition: successNode.Pose(), EndPosition: nearest.Pose()}) - //~ // Ensure successNode is sufficiently far from the nearest node already existing in the tree - //~ // If too close, don't add a new node - //~ if dist < defaultIdenticalNodeDistance { - //~ fmt.Println("too close!!!") - //~ cand = nil - //~ } - //~ } return cand, nil } @@ -404,25 +384,46 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( goalNode node, rrt rrtMap, invert bool, -) (node, error) { +) *nodeAndError { var reseedCandidate *candidate maxReseeds := 1 // Will be updated as necessary for i := 0; i <= maxReseeds; i++ { select { case <-ctx.Done(): - return nil, ctx.Err() + return &nodeAndError{nil, ctx.Err()} default: } candidates := []*candidate{} + + candChan := make(chan *candidate, len(mp.tpFrame.PTGs())) + defer close(candChan) for ptgNum, curPtg := range mp.tpFrame.PTGs() { // Find the best traj point for each traj family, and store for later comparison - // TODO: run in parallel - cand, err := mp.getExtensionCandidate(ctx, goalNode, ptgNum, curPtg, rrt, seedNode, mp.planOpts, invert) - if err != nil { - return nil, err - } - if cand != nil { - if cand.err == nil { + ptgNumPar, curPtgPar := ptgNum, curPtg + utils.PanicCapturingGo(func() { + + cand, err := mp.getExtensionCandidate(ctx, goalNode, ptgNumPar, curPtgPar, rrt, seedNode, mp.planOpts, invert) + + if err != nil { + candChan <- nil + return + } + if cand != nil { + if cand.err == nil { + candChan <- cand + return + } + } + candChan <- nil + }) + } + + for i := 0; i < len(mp.tpFrame.PTGs()); i++ { + select { + case <-ctx.Done(): + return &nodeAndError{nil, ctx.Err()} + case cand := <-candChan: + if cand != nil { candidates = append(candidates, cand) } } @@ -430,29 +431,25 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( var err error reseedCandidate, err = mp.extendMap(ctx, candidates, rrt, invert) if err != nil { - //~ fmt.Println("ret err") - return nil, err + return &nodeAndError{nil, err} } if reseedCandidate == nil { - //~ fmt.Println("ret nil") - return nil, nil + return &nodeAndError{nil, nil} } dist := mp.planOpts.DistanceFunc(&Segment{StartPosition: reseedCandidate.newNode.Pose(), EndPosition: goalNode.Pose()}) - if dist < mp.planOpts.GoalThreshold || !reseedCandidate.lastInTraj { + if dist < mp.algOpts.poseSolveDist || !reseedCandidate.lastInTraj { // Reached the goal position, or otherwise failed to fully extend to the end of a trajectory - //~ fmt.Println("ret success") - return reseedCandidate.newNode, nil + return &nodeAndError{reseedCandidate.newNode, nil} } if i == 0 { dist = math.Sqrt(dist) // TP-space distance is NOT the same thing as cartesian distance, but they track sufficiently well that this is valid to do. maxReseeds = int(math.Min(float64(defaultMaxReseeds), math.Ceil(dist / reseedCandidate.newNode.Q()[2].Value) + 2)) - //~ fmt.Println("updating max reseed to", maxReseeds) } seedNode = reseedCandidate.newNode } - return reseedCandidate.newNode, nil + return &nodeAndError{reseedCandidate.newNode, nil} } // extendMap grows the rrt map to the best candidate node, returning the added candidate. @@ -477,14 +474,12 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( } treeNode := bestCand.treeNode // The node already in the tree to which we are parenting newNode := bestCand.newNode // The node we are adding because it was the best extending PTG - //~ fmt.Println("treenode", treeNode.Pose().Point()) - //~ fmt.Println("newnode", newNode.Pose().Point()) ptgNum := int(newNode.Q()[0].Value) randAlpha := newNode.Q()[1].Value randDist := newNode.Q()[2].Value - fmt.Println("__ADDINGtraj", ptgNum, randAlpha, randDist) + //~ fmt.Println("__ADDINGtraj", ptgNum, randAlpha, randDist) trajK, err := mp.tpFrame.PTGs()[ptgNum].Trajectory(randAlpha, randDist) if err != nil { return nil, err @@ -521,7 +516,7 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( // add the last node in trajectory addedNode = &basicNode{ q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), randAlpha, trajPt.Dist}), - cost: treeNode.Cost() + trajPt.Dist, + cost: trajPt.Dist, pose: trajState.Position, corner: false, } @@ -569,7 +564,6 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, // StartPosition is the seed/query // EndPosition is the pose already in the RRT tree if seg.StartPosition == nil || seg.EndPosition == nil { - //~ fmt.Println("ret inf") return math.Inf(1) } var targetFunc func(spatialmath.Pose) float64 @@ -588,7 +582,6 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, } closeNode, err := ptg.CToTP(context.Background(), targetFunc) if err != nil || closeNode == nil { - //~ fmt.Println("inf") return math.Inf(1) } return targetFunc(closeNode.Pose) @@ -598,10 +591,8 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, } func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) []node { - mp.logger.Info("smoothing not yet working well for tp-space") - return path - toIter := int(math.Min(float64(len(path)*len(path)), float64(mp.planOpts.SmoothIter))) * 5 - //~ toIter = 1 + toIter := int(math.Min(float64(len(path)*len(path)), float64(mp.planOpts.SmoothIter)))*5 + currCost := sumCosts(path) for i := 0; i < toIter; i++ { select { case <-ctx.Done(): @@ -611,63 +602,80 @@ func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) // get start node of first edge. Cannot be either the last or second-to-last node. // Intn will return an int in the half-open interval half-open interval [0,n) firstEdge := mp.randseed.Intn(len(path) - 2) - secondEdge := firstEdge + 1 + mp.randseed.Intn((len(path)-2)-firstEdge) - mp.logger.Debugf("checking shortcut between nodes %d and %d", firstEdge, secondEdge+1) - fmt.Printf("checking shortcut between nodes %d and %d \n", firstEdge, secondEdge+1) + secondEdge := firstEdge + 2 + mp.randseed.Intn((len(path)-2)-firstEdge) + mp.logger.Debugf("checking shortcut between nodes %d and %d", firstEdge, secondEdge) + //~ fmt.Printf("checking shortcut between nodes %d and %d \n", firstEdge, secondEdge) - startMap := map[node]node{} - var parent node - parentPose := spatialmath.NewZeroPose() - for j := 0; j <= firstEdge; j++ { - pathNode := path[j] - startMap[pathNode] = parent - for _, adj := range []float64{0.25, 0.5, 0.75} { - fullQ := pathNode.Q() - newQ := []referenceframe.Input{fullQ[0], fullQ[1], referenceframe.Input{fullQ[2].Value * adj}} - trajK, err := mp.tpFrame.PTGs()[int(math.Round(newQ[0].Value))].Trajectory(newQ[1].Value, newQ[2].Value) - if err != nil { - continue - } - - intNode := &basicNode{ - q: newQ, - cost: pathNode.Cost() - (pathNode.Q()[2].Value * (1 - adj)), - pose: spatialmath.Compose(parentPose, trajK[len(trajK)-1].Pose), - corner: false, - } - startMap[intNode] = parent - } - parent = pathNode - parentPose = parent.Pose() + newInputSteps, err := mp.attemptSmooth(ctx, path, firstEdge, secondEdge) + if err != nil || newInputSteps == nil { + continue } - - reached, err := mp.attemptExtension(ctx, nil, path[secondEdge+1], startMap, false) - if err != nil || reached == nil { + newCost := sumCosts(newInputSteps) + if newCost >= currCost { continue } - - reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartPosition: reached.Pose(), EndPosition: path[secondEdge+1].Pose()}) - // If we tried the goal and have a close-enough XY location, check if the node is good enough to be a final goal - if reachedDelta > mp.algOpts.poseSolveDist { + // Re-connect to the final goal + newInputSteps = append(newInputSteps, path[len(path)-1]) + goalInputSteps, err := mp.attemptSmooth(ctx, newInputSteps, len(newInputSteps)-3, len(newInputSteps)-1) + if err != nil || goalInputSteps == nil { continue } + goalInputSteps = append(goalInputSteps, path[len(path)-1]) + + + path = goalInputSteps + currCost = sumCosts(path) + } + return path +} - newInputSteps := extractPath(startMap, nil, &nodePair{a: reached, b: nil}) - fmt.Println("newpath no extra", newInputSteps) - for i, wp := range newInputSteps { - fmt.Println(i, wp.Cost(), wp.Q()) +// firstEdge and secondEdge must not be adjacent +func (mp *tpSpaceRRTMotionPlanner) attemptSmooth(ctx context.Context, path []node, firstEdge, secondEdge int) ([]node, error) { + startMap := map[node]node{} + var parent node + parentPose := spatialmath.NewZeroPose() + + for j := 0; j <= firstEdge; j++ { + pathNode := path[j] + startMap[pathNode] = parent + for _, adj := range []float64{0.25, 0.5, 0.75} { + fullQ := pathNode.Q() + newQ := []referenceframe.Input{fullQ[0], fullQ[1], referenceframe.Input{fullQ[2].Value * adj}} + trajK, err := mp.tpFrame.PTGs()[int(math.Round(newQ[0].Value))].Trajectory(newQ[1].Value, newQ[2].Value) + if err != nil { + continue + } + + intNode := &basicNode{ + q: newQ, + cost: pathNode.Cost() - (pathNode.Q()[2].Value * (1 - adj)), + pose: spatialmath.Compose(parentPose, trajK[len(trajK)-1].Pose), + corner: false, + } + startMap[intNode] = parent } + parent = pathNode + parentPose = parent.Pose() + } + + reached := mp.attemptExtension(ctx, nil, path[secondEdge], startMap, false) + if reached.error != nil || reached.node == nil { + return nil, errors.New("could not extend to smoothing destination") + } + + reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartPosition: reached.Pose(), EndPosition: path[secondEdge].Pose()}) + // If we tried the goal and have a close-enough XY location, check if the node is good enough to be a final goal + if reachedDelta > mp.algOpts.poseSolveDist { + return nil, errors.New("could not precisely reach smoothing destination") + } + //~ fmt.Println("matched poses", reached.Pose().Point(), path[secondEdge].Pose().Point()) + //~ fmt.Println("reached", reached.node) + + newInputSteps := extractPath(startMap, nil, &nodePair{a: reached.node, b: nil}, false) + + if secondEdge < len(path) - 1 { + //~ fmt.Println("appending") newInputSteps = append(newInputSteps, path[secondEdge+1:]...) - fmt.Println("oldpath", path) - for i, wp := range path { - fmt.Println(i, wp.Cost(), wp.Q()) - } - fmt.Println("newpath", newInputSteps) - path = newInputSteps - for i, wp := range path { - fmt.Println(i, wp.Cost(), wp.Q()) - } - //~ break } - return path + return newInputSteps, nil } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index bd692689d09..b1c4029a224 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -8,6 +8,7 @@ import ( "math/rand" "testing" "math" + "time" "github.com/edaniels/golog" "github.com/golang/geo/r3" @@ -155,7 +156,9 @@ func TestPtgWithObstacle(t *testing.T) { tp, _ := mp.(*tpSpaceRRTMotionPlanner) fmt.Printf("SG,%f,%f\n", 0., 0.) fmt.Printf("SG,%f,%f\n", goalPos.Point().X, goalPos.Point().Y) + start := time.Now() plan, err := tp.plan(ctx, goalPos, nil) + fmt.Println("planning took", time.Since(start)) test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThan, 2) @@ -176,14 +179,18 @@ func TestPtgWithObstacle(t *testing.T) { } fmt.Printf("FINALPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) if i == len(trajPts) - 1 { - lastPose = spatialmath.Compose(lastPose, pt.Pose) + lastPose = intPose break } } } } - + start = time.Now() plan = tp.smoothPath(ctx, plan) + fmt.Println("smoothing took", time.Since(start)) + for _, wp := range plan { + fmt.Println(wp.Q()) + } lastPose = spatialmath.NewZeroPose() for _, mynode := range plan { trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) @@ -194,7 +201,7 @@ func TestPtgWithObstacle(t *testing.T) { } fmt.Printf("SMOOTHPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) if pt.Dist >= mynode.Q()[2].Value { - lastPose = spatialmath.Compose(lastPose, pt.Pose) + lastPose = intPose break } } diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index 6042513a0c7..b56fddf1e1c 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -2,7 +2,7 @@ package tpspace import ( "context" - "fmt" + //~ "fmt" "math" "go.viam.com/rdk/spatialmath" @@ -11,9 +11,8 @@ import ( const ( defaultMaxTime = 15. defaultDiffT = 0.005 - defaultAlphaCnt uint = 121 + defaultAlphaCnt uint = 91 - defaultSearchRadius = 10. defaultMaxHeadingChange = 1.95 * math.Pi orientationDistanceScaling = 10. @@ -38,7 +37,6 @@ type ptgGridSim struct { // Discretized x[y][]node maps for rapid NN lookups trajNodeGrid map[int]map[int][]*TrajNode - searchRad float64 // Distance around a query point to search for precompute in the cached grid } // NewPTGGridSim creates a new PTG by simulating a PrecomputePTG for some distance, then cacheing the results in a grid for fast lookup. @@ -52,7 +50,6 @@ func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64, endsOnly bo alphaCnt: arcs, maxTime: defaultMaxTime, diffT: defaultDiffT, - searchRad: defaultSearchRadius, endsOnly: endsOnly, trajNodeGrid: map[int]map[int][]*TrajNode{}, @@ -118,23 +115,7 @@ func (ptg *ptgGridSim) RefDistance() float64 { } func (ptg *ptgGridSim) Trajectory(alpha, dist float64) ([]*TrajNode, error) { - k := alpha2index(alpha, ptg.alphaCnt) - if int(k) >= len(ptg.precomputeTraj) { - return nil, fmt.Errorf("requested trajectory of index %d but this grid sim only has %d available", k, len(ptg.precomputeTraj)) - } - fullTraj := ptg.precomputeTraj[k] - if fullTraj[len(fullTraj) - 1].Dist < dist { - return nil, fmt.Errorf("requested traj to dist %f but only simulated trajectories to distance %f", dist, fullTraj[len(fullTraj) - 1].Dist) - } - var traj []*TrajNode - for _, trajNode := range fullTraj { - // Walk the trajectory until we pass the specified distance - if trajNode.Dist > dist { - break - } - traj = append(traj, trajNode) - } - return traj, nil + return nil, nil } func (ptg *ptgGridSim) simulateTrajectories(simPTG PrecomputePTG) ([][]*TrajNode, error) { diff --git a/motionplan/tpspace/ptgIKFrame.go b/motionplan/tpspace/ptgIKFrame.go index 0db8b84b447..ee42153bf19 100644 --- a/motionplan/tpspace/ptgIKFrame.go +++ b/motionplan/tpspace/ptgIKFrame.go @@ -19,7 +19,7 @@ func NewPTGIKFrame(ptg PrecomputePTG, dist float64) (referenceframe.Frame, error pf.limits = []referenceframe.Limit{ {Min: -math.Pi, Max: math.Pi}, - {Min: 0, Max: dist}, + {Min: -dist, Max: dist}, //~ {Min: -dist, Max: dist}, } return pf, nil From ab8deaa853efa8293f40593f60d71249446cfd35 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Tue, 15 Aug 2023 15:13:10 -0700 Subject: [PATCH 16/38] Fixed smoothing, but for real this time --- motionplan/metrics.go | 12 +- motionplan/ptgGroupFrame.go | 11 +- motionplan/rrt.go | 17 ++- motionplan/tpSpaceRRT.go | 154 ++++++++++++++++++++++---- motionplan/tpSpaceRRT_test.go | 11 +- motionplan/tpspace/ptgGridSim.go | 2 +- motionplan/tpspace/ptgGridSim_test.go | 16 +-- motionplan/tpspace/ptgSideS.go | 108 ++++++++++++++++++ 8 files changed, 287 insertions(+), 44 deletions(-) create mode 100644 motionplan/tpspace/ptgSideS.go diff --git a/motionplan/metrics.go b/motionplan/metrics.go index 4898974197f..8627de2baca 100644 --- a/motionplan/metrics.go +++ b/motionplan/metrics.go @@ -8,7 +8,7 @@ import ( "go.viam.com/rdk/utils" ) -const orientationDistanceScaling = 30. +const orientationDistanceScaling = 10. // StateMetric are functions which, given a State, produces some score. Lower is better. // This is used for gradient descent to converge upon a goal pose, for example. @@ -120,6 +120,16 @@ func SquaredNormSegmentMetric(segment *Segment) float64 { return delta.Point().Norm2() + spatial.QuatToR3AA(delta.Orientation().Quaternion()).Mul(orientationDistanceScaling).Norm2() } +// NewSquaredNormSegmentMetricWithScaling returns a metric which will return the cartesian distance between the two positions. +// It allows the caller to choose the scaling level of orientation. +func NewSquaredNormSegmentMetricWithScaling(scaleFactor float64) SegmentMetric { + return func(segment *Segment) float64 { + delta := spatial.PoseDelta(segment.StartPosition, segment.EndPosition) + // Increase weight for orientation since it's a small number + return delta.Point().Norm2() + spatial.QuatToR3AA(delta.Orientation().Quaternion()).Mul(scaleFactor).Norm2() + } +} + // SquaredNormNoOrientSegmentMetric is a metric which will return the cartesian distance between the two positions. func SquaredNormNoOrientSegmentMetric(segment *Segment) float64 { delta := spatial.PoseDelta(segment.StartPosition, segment.EndPosition) diff --git a/motionplan/ptgGroupFrame.go b/motionplan/ptgGroupFrame.go index d48cb41932c..87448b8dff1 100644 --- a/motionplan/ptgGroupFrame.go +++ b/motionplan/ptgGroupFrame.go @@ -14,7 +14,7 @@ import ( ) const ( - defaultSimDistMM = 400. + defaultSimDistMM =600. ) const ( @@ -30,7 +30,8 @@ var defaultPTGs = []ptgFactory{ tpspace.NewCCPTG, tpspace.NewCCSPTG, tpspace.NewCSPTG, - //~ tpspace.NewAlphaPTG, + tpspace.NewSideSPTG, + tpspace.NewSideSOverturnPTG, } type ptgGroupFrame struct { @@ -59,8 +60,8 @@ func NewPTGFrameFromTurningRadius( } if refDist <= 0 { - refDist = defaultSimDistMM - //~ refDist = 1000. * turnRadMeters * math.Pi * 0.8 + // Default to a distance of just over one half of a circle turning at max radius + refDist = 1000. * turnRadMeters * math.Pi * 0.9 } // Get max angular velocity in radians per second @@ -110,7 +111,7 @@ func newPTGFrameFromPTGFrame(frame referenceframe.Frame, refDist float64) (refer pf.limits = []referenceframe.Limit{ {Min: 0, Max: float64(len(pf.ptgs) - 1)}, {Min: -math.Pi, Max: math.Pi}, - {Min: -refDist, Max: refDist}, + {Min: 0, Max: refDist}, } return pf, nil diff --git a/motionplan/rrt.go b/motionplan/rrt.go index 76780615d8e..78e9dd4bb82 100644 --- a/motionplan/rrt.go +++ b/motionplan/rrt.go @@ -3,6 +3,7 @@ package motionplan import ( "context" "math" + //~ "fmt" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" @@ -215,13 +216,23 @@ func extractPath(startMap, goalMap map[node]node, pair *nodePair, matched bool) goalReached = goalMap[goalReached] } + //~ var lastNode *basicNode // extract the path to the goal for goalReached != nil { + //~ fmt.Println("goal reached pose", goalReached.Q(), goalReached.Pose().Point()) // special rewriting poses for inverted tree - if lastNode, ok :=path[len(path)-1].(*basicNode); ok { - lastNode.pose = goalReached.Pose() - } + //~ if lastNode != nil { + //~ lastNode.pose = goalReached.Pose() + //~ } + //~ thisNode := &basicNode{ + //~ q: goalReached.Q(), + //~ cost: goalReached.Cost(), + //~ pose: goalReached.Pose(), + //~ corner: goalReached.Corner(), + //~ } + //~ lastNode = thisNode + //~ path = append(path, thisNode) path = append(path, goalReached) goalReached = goalMap[goalReached] } diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index 91e42b18c70..615edd0b0f4 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -36,6 +36,13 @@ const ( defaultIdenticalNodeDistance = 1. defaultMaxReseeds = 50 + + defaultSmoothScaleFactor = 1. + + // Make an attempt to solve the tree every this many iterations + // For a unidirectional solve, this means attempting to reach the goal rather than a random point + // For a bidirectional solve, this means trying to connect the two trees directly + defaultAttemptSolveEvery = 15 ) type tpspaceOptions struct { @@ -53,6 +60,10 @@ type tpspaceOptions struct { // If the squared norm between two poses is less than this, consider them equal poseSolveDist float64 + + smoothScaleFactor float64 + + attemptSolveEvery int // Cached functions for calculating TP-space distances for each PTG distOptions map[tpspace.PTG]*plannerOptions @@ -135,6 +146,10 @@ func (mp *tpSpaceRRTMotionPlanner) plan(ctx context.Context, return nil, ctx.Err() case plan := <-solutionChan: if plan != nil { + fmt.Println("path") + for i, wp := range plan.steps { + fmt.Println(i, wp.Q(), wp.Pose().Point()) + } return plan.steps, plan.err() } return nil, errors.New("nil tp-space plan returned, unable to complete plan") @@ -187,8 +202,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( midPt := spatialmath.Interpolate(startPose, goalPose, 0.5) // used for initial seed var randPos spatialmath.Pose - //~ for iter := 0; iter < mp.algOpts.PlanIter; iter++ { - for iter := 0; iter < 1; iter++ { + for iter := 0; iter < mp.algOpts.PlanIter; iter++ { fmt.Println("iter", iter) if ctx.Err() != nil { mp.logger.Debugf("TP Space RRT timed out after %d iterations", iter) @@ -207,7 +221,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( if iter == 0 { randPos = midPt } - if !bidirectional && iter % 5 == 0 { + if !bidirectional && iter % mp.algOpts.attemptSolveEvery == 0 { randPos = goalPose } randPosNode := &basicNode{pose: randPos} @@ -215,9 +229,9 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( utils.PanicCapturingGo(func() { m1chan <- mp.attemptExtension(ctx, nil, randPosNode, rrt.maps.startMap, false) }) + fmt.Println("seed done") utils.PanicCapturingGo(func() { m2chan <- mp.attemptExtension(ctx, nil, randPosNode, rrt.maps.goalMap, true) - }) seedMapReached := <-m1chan goalMapReached := <-m2chan @@ -237,6 +251,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( return } + fmt.Println("reseeding") if seedMapNode != nil && goalMapNode != nil { seedReached := mp.attemptExtension(ctx, nil, goalMapNode, rrt.maps.startMap, false) @@ -265,6 +280,45 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( return } } + if iter % mp.algOpts.attemptSolveEvery == 0 { + paths := [][]node{} + for goalMapNode, _ := range rrt.maps.goalMap { + seedReached := mp.attemptExtension(ctx, nil, goalMapNode, rrt.maps.startMap, false) + if seedReached.error != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: seedReached.error, maps: rrt.maps} + return + } + if seedReached.node == nil { + continue + } + reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartPosition: seedReached.node.Pose(), EndPosition: goalMapNode.Pose()}) + if reachedDelta <= mp.algOpts.poseSolveDist { + fmt.Println("reached seed", spatialmath.PoseToProtobuf(seedReached.node.Pose())) + fmt.Println("reached goal", spatialmath.PoseToProtobuf(goalMapNode.Pose())) + // If we've reached the goal, extract the path from the RRT trees and return + path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: seedReached.node, b: goalMapNode}, false) + paths = append(paths, path) + } + } + if len(paths) > 0 { + var bestPath []node + bestCost := math.Inf(1) + for _, goodPath := range paths { + currCost := sumCosts(goodPath) + if currCost < bestCost { + bestCost = currCost + bestPath = goodPath + } + } + correctedPath, err := mp.rectifyPath(bestPath) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return + } + rrt.solutionChan <- &rrtPlanReturn{steps: correctedPath, maps: rrt.maps} + return + } + } } rrt.solutionChan <- &rrtPlanReturn{maps: rrt.maps, planerr: errors.New("tpspace RRT unable to create valid path")} } @@ -361,7 +415,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( lastDist = trajPt.Dist } - isLastNode := math.Abs(finalTrajNode.Dist - curPtg.RefDistance()) < 1e-3 + isLastNode := math.Abs(finalTrajNode.Dist - curPtg.RefDistance()) < 0.1 // add the last node in trajectory successNode = &basicNode{ @@ -437,7 +491,8 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( return &nodeAndError{nil, nil} } dist := mp.planOpts.DistanceFunc(&Segment{StartPosition: reseedCandidate.newNode.Pose(), EndPosition: goalNode.Pose()}) - if dist < mp.algOpts.poseSolveDist || !reseedCandidate.lastInTraj { + //~ if dist < mp.algOpts.poseSolveDist || !reseedCandidate.lastInTraj { + if dist < mp.algOpts.poseSolveDist { // Reached the goal position, or otherwise failed to fully extend to the end of a trajectory return &nodeAndError{reseedCandidate.newNode, nil} } @@ -539,6 +594,8 @@ func (mp *tpSpaceRRTMotionPlanner) setupTPSpaceOptions() { addIntermediate: defaultAddInt, addNodeEvery: defaultAddNodeEvery, + attemptSolveEvery: defaultAttemptSolveEvery, + smoothScaleFactor: defaultSmoothScaleFactor, poseSolveDist: defaultIdenticalNodeDistance, @@ -590,9 +647,46 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, return opts } +func (mp *tpSpaceRRTMotionPlanner) rectifyPath(path []node) ([]node, error) { + correctedPath := []node{} + runningPose := spatialmath.NewZeroPose() + for _, wp := range path { + wpPose, err := mp.frame.Transform(wp.Q()) + if err != nil { + return nil, err + } + runningPose = spatialmath.Compose(runningPose, wpPose) + + thisNode := &basicNode{ + q: wp.Q(), + cost: wp.Cost(), + pose: runningPose, + corner: wp.Corner(), + } + correctedPath = append(correctedPath, thisNode) + } + return correctedPath, nil +} + func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) []node { - toIter := int(math.Min(float64(len(path)*len(path)), float64(mp.planOpts.SmoothIter)))*5 + toIter := int(math.Min(float64(len(path)*len(path)), float64(mp.planOpts.SmoothIter))) currCost := sumCosts(path) + + maxCost := math.Inf(-1) + for _, wp := range path { + if wp.Cost() > maxCost { + maxCost = wp.Cost() + } + } + newFrame, err := newPTGFrameFromPTGFrame(mp.frame, maxCost * mp.algOpts.smoothScaleFactor) + if err != nil { + return path + } + smoothPlannerMP, err := newTPSpaceMotionPlanner(newFrame, mp.randseed, mp.logger, mp.planOpts) + if err != nil { + return path + } + smoothPlanner := smoothPlannerMP.(*tpSpaceRRTMotionPlanner) for i := 0; i < toIter; i++ { select { case <-ctx.Done(): @@ -604,9 +698,9 @@ func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) firstEdge := mp.randseed.Intn(len(path) - 2) secondEdge := firstEdge + 2 + mp.randseed.Intn((len(path)-2)-firstEdge) mp.logger.Debugf("checking shortcut between nodes %d and %d", firstEdge, secondEdge) - //~ fmt.Printf("checking shortcut between nodes %d and %d \n", firstEdge, secondEdge) + fmt.Printf("checking shortcut between nodes %d and %d \n", firstEdge, secondEdge) - newInputSteps, err := mp.attemptSmooth(ctx, path, firstEdge, secondEdge) + newInputSteps, err := mp.attemptSmooth(ctx, path, firstEdge, secondEdge, smoothPlanner) if err != nil || newInputSteps == nil { continue } @@ -615,22 +709,43 @@ func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) continue } // Re-connect to the final goal - newInputSteps = append(newInputSteps, path[len(path)-1]) - goalInputSteps, err := mp.attemptSmooth(ctx, newInputSteps, len(newInputSteps)-3, len(newInputSteps)-1) + if newInputSteps[len(newInputSteps)-1] != path[len(path)-1] { + newInputSteps = append(newInputSteps, path[len(path)-1]) + } + fmt.Println("success, checking goal") + + goalInputSteps, err := mp.attemptSmooth(ctx, newInputSteps, len(newInputSteps)-3, len(newInputSteps)-1, smoothPlanner) if err != nil || goalInputSteps == nil { + fmt.Println("failed to reconnect goal") continue } goalInputSteps = append(goalInputSteps, path[len(path)-1]) - - + fmt.Println("path") + for i, wp := range path { + fmt.Println(i, wp.Q(), wp.Pose().Point()) + } + fmt.Println("new") + for i, wp := range newInputSteps { + fmt.Println(i, wp.Q(), wp.Pose().Point()) + } + fmt.Println("successful smooth") + for i, wp := range goalInputSteps { + fmt.Println(i, wp.Q(), wp.Pose().Point()) + } path = goalInputSteps + //~ path = newInputSteps currCost = sumCosts(path) } return path } // firstEdge and secondEdge must not be adjacent -func (mp *tpSpaceRRTMotionPlanner) attemptSmooth(ctx context.Context, path []node, firstEdge, secondEdge int) ([]node, error) { +func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( + ctx context.Context, + path []node, + firstEdge, secondEdge int, + smoother *tpSpaceRRTMotionPlanner, +) ([]node, error) { startMap := map[node]node{} var parent node parentPose := spatialmath.NewZeroPose() @@ -641,7 +756,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth(ctx context.Context, path []nod for _, adj := range []float64{0.25, 0.5, 0.75} { fullQ := pathNode.Q() newQ := []referenceframe.Input{fullQ[0], fullQ[1], referenceframe.Input{fullQ[2].Value * adj}} - trajK, err := mp.tpFrame.PTGs()[int(math.Round(newQ[0].Value))].Trajectory(newQ[1].Value, newQ[2].Value) + trajK, err := smoother.tpFrame.PTGs()[int(math.Round(newQ[0].Value))].Trajectory(newQ[1].Value, newQ[2].Value) if err != nil { continue } @@ -657,8 +772,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth(ctx context.Context, path []nod parent = pathNode parentPose = parent.Pose() } - - reached := mp.attemptExtension(ctx, nil, path[secondEdge], startMap, false) + reached := smoother.attemptExtension(ctx, nil, path[secondEdge], startMap, false) if reached.error != nil || reached.node == nil { return nil, errors.New("could not extend to smoothing destination") } @@ -668,8 +782,8 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth(ctx context.Context, path []nod if reachedDelta > mp.algOpts.poseSolveDist { return nil, errors.New("could not precisely reach smoothing destination") } - //~ fmt.Println("matched poses", reached.Pose().Point(), path[secondEdge].Pose().Point()) - //~ fmt.Println("reached", reached.node) + fmt.Println("matched poses", reached.Pose().Point(), path[secondEdge].Pose().Point()) + fmt.Println("reached", reached.node) newInputSteps := extractPath(startMap, nil, &nodePair{a: reached.node, b: nil}, false) @@ -677,5 +791,5 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth(ctx context.Context, path []nod //~ fmt.Println("appending") newInputSteps = append(newInputSteps, path[secondEdge+1:]...) } - return newInputSteps, nil + return mp.rectifyPath(newInputSteps) } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index b1c4029a224..c18aaaa56fd 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -40,7 +40,7 @@ func TestPtgRrt(t *testing.T) { ) test.That(t, err, test.ShouldBeNil) - goalPos := spatialmath.NewPose(r3.Vector{X: 200, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 80}) + goalPos := spatialmath.NewPose(r3.Vector{X: 200, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 90}) //~ goalPos := spatialmath.NewPose(r3.Vector{X: 200, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 0}) //~ goalPos := spatialmath.NewPose(r3.Vector{X: 0, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 0}) @@ -101,7 +101,7 @@ func TestPtgWithObstacle(t *testing.T) { ctx := context.Background() - goalPos := spatialmath.NewPoseFromPoint(r3.Vector{X: 5000, Y: 0, Z: 0}) + goalPos := spatialmath.NewPoseFromPoint(r3.Vector{X: 6500, Y: 0, Z: 0}) fs := referenceframe.NewEmptyFrameSystem("test") fs.AddFrame(ackermanFrame, fs.World()) @@ -109,14 +109,13 @@ func TestPtgWithObstacle(t *testing.T) { opt := newBasicPlannerOptions() //~ opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) //~ opt.DistanceFunc = SquaredNormNoOrientSegmentMetric - opt.SetGoalMetric(NewSquaredNormMetric(goalPos)) - opt.DistanceFunc = SquaredNormSegmentMetric + opt.DistanceFunc = NewSquaredNormSegmentMetricWithScaling(40.) //~ opt.GoalThreshold = 0.001 opt.GoalThreshold = 5 // obstacles - obstacle1, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, -500, 0}), r3.Vector{180, 1800, 1}, "") + obstacle1, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{3300, -500, 0}), r3.Vector{180, 1800, 1}, "") test.That(t, err, test.ShouldBeNil) - obstacle2, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, 1800, 0}), r3.Vector{180, 1800, 1}, "") + obstacle2, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{3300, 1800, 0}), r3.Vector{180, 1800, 1}, "") test.That(t, err, test.ShouldBeNil) obstacle3, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2500, -1400, 0}), r3.Vector{50000, 30, 1}, "") test.That(t, err, test.ShouldBeNil) diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index b56fddf1e1c..577e753fb0a 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -115,7 +115,7 @@ func (ptg *ptgGridSim) RefDistance() float64 { } func (ptg *ptgGridSim) Trajectory(alpha, dist float64) ([]*TrajNode, error) { - return nil, nil + return ComputePTG(ptg.simPTG, alpha, dist, defaultDiffT) } func (ptg *ptgGridSim) simulateTrajectories(simPTG PrecomputePTG) ([][]*TrajNode, error) { diff --git a/motionplan/tpspace/ptgGridSim_test.go b/motionplan/tpspace/ptgGridSim_test.go index 7a990d1ac09..e9719172b7d 100644 --- a/motionplan/tpspace/ptgGridSim_test.go +++ b/motionplan/tpspace/ptgGridSim_test.go @@ -25,11 +25,11 @@ var ( ) func TestSim(t *testing.T) { - simDist := 600. - alphaCnt := uint(121) + simDist :=2500. + alphaCnt := uint(61) fmt.Println("type,X,Y") //~ for _, ptg := range defaultPTGs { - ptg := NewCSPTG + ptg := NewSideSOverturnPTG radPS := defaultMMps / (turnRadMeters * 1000) ptgGen := ptg(defaultMMps, radPS) @@ -37,15 +37,15 @@ func TestSim(t *testing.T) { grid, err := NewPTGGridSim(ptgGen, alphaCnt, simDist, false) test.That(t, err, test.ShouldBeNil) - //~ for i := uint(0); i < alphaCnt; i++ { + for i := uint(0); i < alphaCnt; i++ { //~ i := uint(60) - alpha := -0.41541721039203877 - //~ traj, _ := grid.Trajectory(index2alpha(i, alphaCnt), simDist) - traj, _ := grid.Trajectory(alpha, simDist) + //~ alpha := -0.41541721039203877 + traj, _ := grid.Trajectory(index2alpha(i, alphaCnt), simDist) + //~ traj, _ := grid.Trajectory(alpha, simDist) for _, intPose := range traj{ fmt.Printf("FINALPATH,%f,%f\n", intPose.Pose.Point().X, intPose.Pose.Point().Y) } - //~ } + } } func TestPose(t *testing.T) { diff --git a/motionplan/tpspace/ptgSideS.go b/motionplan/tpspace/ptgSideS.go new file mode 100644 index 00000000000..0d2f5e53719 --- /dev/null +++ b/motionplan/tpspace/ptgSideS.go @@ -0,0 +1,108 @@ +package tpspace + +import ( + "math" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" +) + +const defaultCountersteer = 1.5 + +// ptgDiffDriveSideS defines a PTG family which makes a forwards turn, then a counter turn the other direction, and goes straight. +// This has the effect of translating to one side or the other without orientation change +type ptgDiffDriveSideS struct { + maxMMPS float64 // millimeters per second velocity to target + maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius + r float64 + countersteer float64 + circle *ptgDiffDriveC +} + +// NewSideSPTG creates a new PrecomputePTG of type ptgDiffDriveSideS. +func NewSideSPTG(maxMMPS, maxRPS float64) PrecomputePTG { + + r := maxMMPS / maxRPS + circle := NewCirclePTG(maxMMPS, maxRPS).(*ptgDiffDriveC) + + return &ptgDiffDriveSideS{ + maxMMPS: maxMMPS, + maxRPS: maxRPS, + r: r, + countersteer: 1.0, + circle: circle, + } +} + +// NewSideSOverturnPTG creates a new PrecomputePTG of type ptgDiffDriveSideS which overturns. +// It turns X amount in one direction, then countersteers X*countersteerFactor in the other direction +func NewSideSOverturnPTG(maxMMPS, maxRPS float64) PrecomputePTG { + + r := maxMMPS / maxRPS + circle := NewCirclePTG(maxMMPS, maxRPS).(*ptgDiffDriveC) + + return &ptgDiffDriveSideS{ + maxMMPS: maxMMPS, + maxRPS: maxRPS, + r: r, + countersteer: defaultCountersteer, + circle: circle, + } +} + +// For this particular driver, turns alpha into a linear + angular velocity. Linear is just max * fwd/back. +// Note that this will NOT work as-is for 0-radius turning. Robots capable of turning in place will need to be special-cased +// because they will have zero linear velocity through their turns, not max. +func (ptg *ptgDiffDriveSideS) PTGVelocities(alpha, dist float64) (float64, float64, error) { + arcLength := math.Abs(alpha) * 0.5 * ptg.r + v := ptg.maxMMPS + w := 0. + flip := math.Copysign(1., alpha) // left or right + + if dist < arcLength { + // l- + v = ptg.maxMMPS + w = ptg.maxRPS * flip + } else if dist < arcLength + arcLength * ptg.countersteer { + v = ptg.maxMMPS + w = ptg.maxRPS * -1 * flip + } + + return v, w, nil +} + +func (ptg *ptgDiffDriveSideS) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + alpha := inputs[0].Value + dist := inputs[1].Value + + flip := math.Copysign(1., alpha) // left or right + direction := math.Copysign(1., dist) // forwards or backwards + arcLength := math.Abs(alpha) * 0.5 * ptg.r // + + revPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {direction * math.Min(dist, arcLength)}}) + if err != nil { + return nil, err + } + if dist < arcLength { + return revPose, nil + } + fwdPose, err := ptg.circle.Transform( + []referenceframe.Input{ + {-1 * flip * math.Pi}, + {direction * (math.Min(dist, arcLength + arcLength * ptg.countersteer) - arcLength)}, + }, + ) + if err != nil { + return nil, err + } + arcPose := spatialmath.Compose(revPose, fwdPose) + if dist < arcLength + arcLength * ptg.countersteer { + return arcPose, nil + } + + finalPose, err := ptg.circle.Transform([]referenceframe.Input{{0}, {direction * (dist - (arcLength + arcLength * ptg.countersteer))}}) + if err != nil { + return nil, err + } + return spatialmath.Compose(arcPose, finalPose), nil +} From 9aa1af8be3622d37db99bfac71d3b6c9dd0334f3 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Tue, 15 Aug 2023 15:18:04 -0700 Subject: [PATCH 17/38] Reintroduce filtering on node nearness to avoid 15-point turns --- motionplan/tpSpaceRRT.go | 15 +++++++++++++-- motionplan/tpSpaceRRT_test.go | 2 +- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index 615edd0b0f4..55e0c77851d 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -33,7 +33,7 @@ const ( // Don't add new RRT tree nodes if there is an existing node within this distance. // Consider nodes on trees to be connected if they are within this distance. - defaultIdenticalNodeDistance = 1. + defaultIdenticalNodeDistance = 3. defaultMaxReseeds = 50 @@ -426,7 +426,18 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( } cand := &candidate{dist: bestDist, treeNode: nearest, newNode: successNode, lastInTraj: isLastNode} - + // check if this successNode is too close to nodes already in the tree, and if so, do not add. + // Get nearest neighbor to new node that's already in the tree + nearest = nm.nearestNeighbor(ctx, planOpts, successNode, rrt) + if nearest != nil { + dist := planOpts.DistanceFunc(&Segment{StartPosition: successNode.Pose(), EndPosition: nearest.Pose()}) + // Ensure successNode is sufficiently far from the nearest node already existing in the tree + // If too close, don't add a new node + if dist < defaultIdenticalNodeDistance { + fmt.Println("too close!!!") + cand = nil + } + } return cand, nil } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index c18aaaa56fd..10a214d6b10 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -109,7 +109,7 @@ func TestPtgWithObstacle(t *testing.T) { opt := newBasicPlannerOptions() //~ opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) //~ opt.DistanceFunc = SquaredNormNoOrientSegmentMetric - opt.DistanceFunc = NewSquaredNormSegmentMetricWithScaling(40.) + opt.DistanceFunc = NewSquaredNormSegmentMetricWithScaling(30.) //~ opt.GoalThreshold = 0.001 opt.GoalThreshold = 5 // obstacles From 59381c663fc0b3f6556505762917ef42df7f6bfe Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 16 Aug 2023 10:02:31 -0700 Subject: [PATCH 18/38] Linting and working --- .../base/kinematicbase/ptgKinematics.go | 18 +- .../base/kinematicbase/ptgKinematics_test.go | 2 +- motionplan/combinedInverseKinematics.go | 4 +- motionplan/errors.go | 6 + motionplan/inverseKinematics.go | 4 +- motionplan/nloptInverseKinematics.go | 14 +- motionplan/planManager.go | 7 +- motionplan/ptgGroupFrame.go | 27 +-- motionplan/ptgIK.go | 55 ++--- motionplan/rrt.go | 23 +- motionplan/tpSpaceRRT.go | 199 ++++++++--------- motionplan/tpSpaceRRT_test.go | 207 ++++++++++++------ motionplan/tpspace/ptg.go | 42 ++-- motionplan/tpspace/ptgC.go | 24 +- motionplan/tpspace/ptgCC.go | 16 +- motionplan/tpspace/ptgCCS.go | 24 +- motionplan/tpspace/ptgCS.go | 20 +- motionplan/tpspace/ptgGridSim.go | 36 +-- motionplan/tpspace/ptgGridSim_test.go | 39 +--- motionplan/tpspace/ptgIKFrame.go | 12 +- motionplan/tpspace/ptgSideS.go | 56 +++-- motionplan/tpspace/ptg_test.go | 8 - motionplan/tpspace/simPtgAlpha.go | 74 +++---- 23 files changed, 443 insertions(+), 474 deletions(-) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 1531357daa6..eb119e39fd9 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -14,6 +14,7 @@ import ( utils "go.viam.com/utils" "go.viam.com/rdk/components/base" + "go.viam.com/rdk/motionplan" "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/referenceframe" rdkutils "go.viam.com/rdk/utils" @@ -74,11 +75,12 @@ func wrapWithPTGKinematics( return nil, err } - frame, err := referenceframe.NewPTGFrameFromTurningRadius( + frame, err := motionplan.NewPTGFrameFromTurningRadius( b.Name().ShortName(), + logger, baseMillimetersPerSecond, baseTurningRadius, - 0, // pass 0 to use the default + 0, // pass 0 to use the default refDist geometries, ) if err != nil { @@ -122,15 +124,13 @@ func (ptgk *ptgBaseKinematics) GoToInputs(ctx context.Context, inputs []referenc ptgk.logger.Debugf("GoToInputs going to %v", inputs) selectedPTG := ptgk.ptgs[int(math.Round(inputs[ptgIndex].Value))] - selectedTraj := selectedPTG.Trajectory(uint(math.Round(inputs[trajectoryIndexWithinPTG].Value))) + selectedTraj, err := selectedPTG.Trajectory(inputs[trajectoryIndexWithinPTG].Value, inputs[distanceAlongTrajectoryIndex].Value) + if err != nil { + return errors.Join(err, ptgk.Base.Stop(ctx, nil)) + } lastTime := 0. - for i, trajNode := range selectedTraj { - if trajNode.Dist > inputs[distanceAlongTrajectoryIndex].Value { - ptgk.logger.Debugf("finished executing trajectory after %d steps", i) - // We have reached the desired distance along the given trajectory - break - } + for _, trajNode := range selectedTraj { timestep := time.Duration(trajNode.Time-lastTime) * time.Second lastTime = trajNode.Time linVel := r3.Vector{0, trajNode.LinVelMMPS, 0} diff --git a/components/base/kinematicbase/ptgKinematics_test.go b/components/base/kinematicbase/ptgKinematics_test.go index fe46b8de106..124c7be0cb9 100644 --- a/components/base/kinematicbase/ptgKinematics_test.go +++ b/components/base/kinematicbase/ptgKinematics_test.go @@ -86,7 +86,7 @@ func TestPTGKinematicsWithGeom(t *testing.T) { test.That(t, err, test.ShouldBeNil) fs.AddFrame(f, fs.World()) inputMap := referenceframe.StartPositions(fs) - + obstacle, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{1000, 0, 0}), r3.Vector{1, 1, 1}, "") test.That(t, err, test.ShouldBeNil) diff --git a/motionplan/combinedInverseKinematics.go b/motionplan/combinedInverseKinematics.go index 89d529df223..fcaa0a4cc06 100644 --- a/motionplan/combinedInverseKinematics.go +++ b/motionplan/combinedInverseKinematics.go @@ -43,7 +43,7 @@ func CreateCombinedIKSolver(model referenceframe.Frame, logger golog.Logger, nCP func runSolver(ctx context.Context, solver InverseKinematics, - c chan<-*IKSolution, + c chan<- *IKSolution, seed []referenceframe.Input, m StateMetric, rseed int, @@ -54,7 +54,7 @@ func runSolver(ctx context.Context, // Solve will initiate solving for the given position in all child solvers, seeding with the specified initial joint // positions. If unable to solve, the returned error will be non-nil. func (ik *CombinedIK) Solve(ctx context.Context, - c chan<-*IKSolution, + c chan<- *IKSolution, seed []referenceframe.Input, m StateMetric, rseed int, diff --git a/motionplan/errors.go b/motionplan/errors.go index 682c9daa020..db9cbe5f1c8 100644 --- a/motionplan/errors.go +++ b/motionplan/errors.go @@ -13,6 +13,12 @@ var ( errNoPlannerOptions = errors.New("PlannerOptions are required but have not been specified") errIKConstraint = "all IK solutions failed constraints. Failures: " + + errNoNeighbors = errors.New("no neighbors found") + + errInvalidCandidate = errors.New("candidate did not meet constraints") + + errNoCandidates = errors.New("no candidates passed in, skipping") ) func genIKConstraintErr(failures map[string]int, constraintFailCnt int) error { diff --git a/motionplan/inverseKinematics.go b/motionplan/inverseKinematics.go index 866da9f0826..9bc36538c69 100644 --- a/motionplan/inverseKinematics.go +++ b/motionplan/inverseKinematics.go @@ -10,9 +10,11 @@ import ( // solutions to the provided channel until cancelled or otherwise completes. type InverseKinematics interface { // Solve receives a context, the goal arm position, and current joint angles. - Solve(context.Context, chan<-*IKSolution, []referenceframe.Input, StateMetric, int) error + Solve(context.Context, chan<- *IKSolution, []referenceframe.Input, StateMetric, int) error } +// IKSolution is the struct returned from an IK solver. It contains the solution configuration, the score of the solution, and a flag +// indicating whether that configuration and score met the solution criteria requested by the caller. type IKSolution struct { Configuration []referenceframe.Input Score float64 diff --git a/motionplan/nloptInverseKinematics.go b/motionplan/nloptInverseKinematics.go index ca501016ea7..3b1108aac70 100644 --- a/motionplan/nloptInverseKinematics.go +++ b/motionplan/nloptInverseKinematics.go @@ -38,7 +38,7 @@ type NloptIK struct { solveEpsilon float64 logger golog.Logger jump float64 - partial bool + partial bool } // CreateNloptIKSolver creates an nloptIK object that can perform gradient descent on metrics for Frames. The parameters are the Frame on @@ -68,7 +68,7 @@ func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int // Solve runs the actual solver and sends any solutions found to the given channel. func (ik *NloptIK) Solve(ctx context.Context, - solutionChan chan<-*IKSolution, + solutionChan chan<- *IKSolution, seed []referenceframe.Input, solveMetric StateMetric, rseed int, @@ -188,9 +188,9 @@ func (ik *NloptIK) Solve(ctx context.Context, err = multierr.Combine(err, nloptErr) } - //~ if solutionRaw != nil { - //~ fmt.Println("best nlopt", result) - //~ } + // ~ if solutionRaw != nil { + // ~ fmt.Println("best nlopt", result) + // ~ } if result < ik.epsilon*ik.epsilon || (solutionRaw != nil && ik.partial) { select { @@ -200,8 +200,8 @@ func (ik *NloptIK) Solve(ctx context.Context, } solutionChan <- &IKSolution{ Configuration: referenceframe.FloatsToInputs(solutionRaw), - Score: result, - Partial: result >= ik.epsilon*ik.epsilon, + Score: result, + Partial: result >= ik.epsilon*ik.epsilon, } solutionsFound++ } diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 0e1bcdbf51b..ec4eba244ea 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -19,8 +19,9 @@ import ( ) const ( - defaultOptimalityMultiple = 2.0 - defaultFallbackTimeout = 1.5 + defaultOptimalityMultiple = 2.0 + defaultFallbackTimeout = 1.5 + defaultTPspaceOrientationScale = 30. ) // planManager is intended to be the single entry point to motion planners, wrapping all others, dealing with fallbacks, etc. @@ -506,7 +507,7 @@ func (pm *planManager) plannerSetupFromMoveRequest( // overwrite default with TP space opt.PlannerConstructor = newTPSpaceMotionPlanner // Distances are computed in cartesian space rather than configuration space - opt.DistanceFunc = SquaredNormSegmentMetric + opt.DistanceFunc = NewSquaredNormSegmentMetricWithScaling(defaultTPspaceOrientationScale) planAlg = "tpspace" } diff --git a/motionplan/ptgGroupFrame.go b/motionplan/ptgGroupFrame.go index 87448b8dff1..6477f481864 100644 --- a/motionplan/ptgGroupFrame.go +++ b/motionplan/ptgGroupFrame.go @@ -5,18 +5,14 @@ import ( "fmt" "math" - pb "go.viam.com/api/component/arm/v1" "github.com/edaniels/golog" + pb "go.viam.com/api/component/arm/v1" "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) -const ( - defaultSimDistMM =600. -) - const ( ptgIndex int = iota trajectoryAlphaWithinPTG @@ -35,13 +31,13 @@ var defaultPTGs = []ptgFactory{ } type ptgGroupFrame struct { - name string - limits []referenceframe.Limit - geometries []spatialmath.Geometry - ptgs []tpspace.PTG - velocityMMps float64 + name string + limits []referenceframe.Limit + geometries []spatialmath.Geometry + ptgs []tpspace.PTG + velocityMMps float64 turnRadMeters float64 - logger golog.Logger + logger golog.Logger } // NewPTGFrameFromTurningRadius will create a new Frame which is also a PTGProvider. It will precompute the default set of @@ -86,16 +82,15 @@ func NewPTGFrameFromTurningRadius( } // newPTGFrameFromPTGFrame will create a new Frame from a preexisting ptgGroupFrame, allowing the adjustment of `refDist` while keeping -// other params the same. This may be expanded to allow altering turning radius, geometries, etc +// other params the same. This may be expanded to allow altering turning radius, geometries, etc. func newPTGFrameFromPTGFrame(frame referenceframe.Frame, refDist float64) (referenceframe.Frame, error) { - ptgFrame, ok := frame.(*ptgGroupFrame) if !ok { return nil, errors.New("cannot create ptg framem given frame is not a ptgGroupFrame") } if refDist <= 0 { - refDist = 1000. * ptgFrame.turnRadMeters * math.Pi * 0.9 + refDist = 1000. * ptgFrame.turnRadMeters * math.Pi * 1.5 } // Get max angular velocity in radians per second @@ -134,9 +129,9 @@ func (pf *ptgGroupFrame) MarshalJSON() ([]byte, error) { func (pf *ptgGroupFrame) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { alpha := inputs[trajectoryAlphaWithinPTG].Value dist := inputs[distanceAlongTrajectoryIndex].Value - + ptgIdx := int(math.Round(inputs[ptgIndex].Value)) - + traj, err := pf.ptgs[ptgIdx].Trajectory(alpha, dist) if err != nil { return nil, err diff --git a/motionplan/ptgIK.go b/motionplan/ptgIK.go index 31ce97e77f7..70210d8b27a 100644 --- a/motionplan/ptgIK.go +++ b/motionplan/ptgIK.go @@ -1,37 +1,35 @@ package motionplan - import ( "context" "math" "sync" - //~ "fmt" "github.com/edaniels/golog" "go.viam.com/rdk/motionplan/tpspace" - "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" ) const ( defaultDiffT = 0.01 // seconds - nloptSeed = 42 // This should be fine to kepe constant - + nloptSeed = 42 // This should be fine to kepe constant + defaultZeroDist = 1e-3 // Sometimes nlopt will minimize trajectories to zero. Ensure min traj dist is at least this ) // ptgGridSim will take a PrecomputePTG, and simulate out a number of trajectories through some requested time/distance for speed of lookup // later. It will store the trajectories in a grid data structure allowing relatively fast lookups. type ptgIK struct { - refDist float64 - simPTG tpspace.PrecomputePTG - ptgFrame referenceframe.Frame + refDist float64 + simPTG tpspace.PrecomputePTG + ptgFrame referenceframe.Frame fastGradDescent *NloptIK - + gridSim tpspace.PTG - - mu sync.RWMutex + + mu sync.RWMutex trajCache map[float64][]*tpspace.TrajNode } @@ -41,12 +39,12 @@ func NewPTGIK(simPTG tpspace.PrecomputePTG, logger golog.Logger, refDist float64 if err != nil { return nil, err } - + nlopt, err := CreateNloptIKSolver(ptgFrame, logger, 1, defaultEpsilon*defaultEpsilon, true) if err != nil { return nil, err } - + // create an ends-only grid sim for quick end-of-trajectory calculations gridSim, err := tpspace.NewPTGGridSim(simPTG, 0, refDist, true) if err != nil { @@ -54,26 +52,20 @@ func NewPTGIK(simPTG tpspace.PrecomputePTG, logger golog.Logger, refDist float64 } ptg := &ptgIK{ - refDist: refDist, - simPTG: simPTG, - ptgFrame: ptgFrame, + refDist: refDist, + simPTG: simPTG, + ptgFrame: ptgFrame, fastGradDescent: nlopt, - gridSim: gridSim, - trajCache: map[float64][]*tpspace.TrajNode{}, + gridSim: gridSim, + trajCache: map[float64][]*tpspace.TrajNode{}, } return ptg, nil } func (ptg *ptgIK) CToTP(ctx context.Context, distFunc func(spatialmath.Pose) float64) (*tpspace.TrajNode, error) { - //~ if pose.Point().Norm() > ptg.refDist { - //~ fmt.Println("simming", pose.Point()) - //~ return ptg.gridSim.CToTP(ctx, pose) - //~ } - solutionGen := make(chan *IKSolution, 1) - seedInput := []referenceframe.Input{{math.Pi/2}, {ptg.refDist/2}} // random value to seed the IK solver - //~ goalMetric := NewSquaredNormMetric(pose) + seedInput := []referenceframe.Input{{math.Pi / 2}, {ptg.refDist / 2}} // random value to seed the IK solver goalMetric := func(state *State) float64 { return distFunc(state.Position) } @@ -86,26 +78,25 @@ func (ptg *ptgIK) CToTP(ctx context.Context, distFunc func(spatialmath.Pose) flo default: } close(solutionGen) - if err != nil || solved == nil || (solved != nil && solved.Configuration[1].Value < defaultZeroDist) { + if err != nil || solved == nil || solved.Configuration[1].Value < defaultZeroDist { return ptg.gridSim.CToTP(ctx, distFunc) } - + if solved.Partial { gridNode, err := ptg.gridSim.CToTP(ctx, distFunc) if err == nil { // Check if the grid has a better solution - //~ fmt.Println("comparing score. nlopt:", solved.Score, "grid", distFunc(gridNode.Pose)) if distFunc(gridNode.Pose) < solved.Score { return gridNode, nil } } } - + traj, err := ptg.Trajectory(solved.Configuration[0].Value, solved.Configuration[1].Value) if err != nil { return nil, err } - return traj[len(traj) - 1], nil + return traj[len(traj)-1], nil } func (ptg *ptgIK) RefDistance() float64 { @@ -118,6 +109,7 @@ func (ptg *ptgIK) Trajectory(alpha, dist float64) ([]*tpspace.TrajNode, error) { ptg.mu.RUnlock() if precomp != nil { if precomp[len(precomp)-1].Dist >= dist { + // Cacheing here provides a ~33% speedup to a solve call subTraj := []*tpspace.TrajNode{} for _, wp := range precomp { if wp.Dist < dist { @@ -130,11 +122,12 @@ func (ptg *ptgIK) Trajectory(alpha, dist float64) ([]*tpspace.TrajNode, error) { if len(subTraj) > 0 { time = subTraj[len(subTraj)-1].Time } - lastNode, err := tpspace.ComputeNextPTGNode(ptg.simPTG, alpha, dist, time) + lastNode, err := tpspace.ComputePTGNode(ptg.simPTG, alpha, dist, time) if err != nil { return nil, err } subTraj = append(subTraj, lastNode) + return subTraj, nil } } traj, err := tpspace.ComputePTG(ptg.simPTG, alpha, dist, defaultDiffT) diff --git a/motionplan/rrt.go b/motionplan/rrt.go index 78e9dd4bb82..f143004115e 100644 --- a/motionplan/rrt.go +++ b/motionplan/rrt.go @@ -3,7 +3,6 @@ package motionplan import ( "context" "math" - //~ "fmt" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" @@ -216,20 +215,20 @@ func extractPath(startMap, goalMap map[node]node, pair *nodePair, matched bool) goalReached = goalMap[goalReached] } - //~ var lastNode *basicNode + // ~ var lastNode *basicNode // extract the path to the goal for goalReached != nil { - //~ fmt.Println("goal reached pose", goalReached.Q(), goalReached.Pose().Point()) + // ~ fmt.Println("goal reached pose", goalReached.Q(), goalReached.Pose().Point()) // special rewriting poses for inverted tree - //~ if lastNode != nil { - //~ lastNode.pose = goalReached.Pose() - //~ } - - //~ thisNode := &basicNode{ - //~ q: goalReached.Q(), - //~ cost: goalReached.Cost(), - //~ pose: goalReached.Pose(), - //~ corner: goalReached.Corner(), + // ~ if lastNode != nil { + // ~ lastNode.pose = goalReached.Pose() + // ~ } + + // ~ thisNode := &basicNode{ + // ~ q: goalReached.Q(), + // ~ cost: goalReached.Cost(), + // ~ pose: goalReached.Pose(), + // ~ corner: goalReached.Corner(), //~ } //~ lastNode = thisNode //~ path = append(path, thisNode) diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index 55e0c77851d..2b76dbc7f59 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -19,7 +19,7 @@ import ( ) const ( - defaultGoalCheck = 5 // Check if the goal is reachable every this many iterations + defaultGoalCheck = 5 // Check if the goal is reachable every this many iterations defaultAutoBB = 0.3 // Automatic bounding box on driveable area as a multiple of start-goal distance // Note: while fully holonomic planners can use the limits of the frame as implicit boundaries, with non-holonomic motion // this is not the case, and the total workspace available to the planned frame is not directly related to the motion available @@ -33,15 +33,15 @@ const ( // Don't add new RRT tree nodes if there is an existing node within this distance. // Consider nodes on trees to be connected if they are within this distance. - defaultIdenticalNodeDistance = 3. + defaultIdenticalNodeDistance = 5. defaultMaxReseeds = 50 - - defaultSmoothScaleFactor = 1. - + + defaultSmoothScaleFactor = 0.5 + // Make an attempt to solve the tree every this many iterations // For a unidirectional solve, this means attempting to reach the goal rather than a random point - // For a bidirectional solve, this means trying to connect the two trees directly + // For a bidirectional solve, this means trying to connect the two trees directly. defaultAttemptSolveEvery = 15 ) @@ -57,16 +57,22 @@ type tpspaceOptions struct { // Meaningless if the above is false. addNodeEvery float64 - // If the squared norm between two poses is less than this, consider them equal poseSolveDist float64 - + + // When smoothing, adjust the trajectory path length to be this proportion of the length used for solving smoothScaleFactor float64 + // Make an attempt to solve the tree every this many iterations + // For a unidirectional solve, this means attempting to reach the goal rather than a random point + // For a bidirectional solve, this means trying to connect the two trees directly attemptSolveEvery int + // Print very fine-grained debug info. Useful for observing the inner RRT tree structure directly + pathdebug bool + // Cached functions for calculating TP-space distances for each PTG - distOptions map[tpspace.PTG]*plannerOptions + distOptions map[tpspace.PTG]*plannerOptions invertDistOptions map[tpspace.PTG]*plannerOptions } @@ -146,10 +152,6 @@ func (mp *tpSpaceRRTMotionPlanner) plan(ctx context.Context, return nil, ctx.Err() case plan := <-solutionChan: if plan != nil { - fmt.Println("path") - for i, wp := range plan.steps { - fmt.Println(i, wp.Q(), wp.Pose().Point()) - } return plan.steps, plan.err() } return nil, errors.New("nil tp-space plan returned, unable to complete plan") @@ -190,7 +192,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( break } } - + bidirectional := true m1chan := make(chan *nodeAndError, 1) @@ -203,7 +205,6 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( var randPos spatialmath.Pose for iter := 0; iter < mp.algOpts.PlanIter; iter++ { - fmt.Println("iter", iter) if ctx.Err() != nil { mp.logger.Debugf("TP Space RRT timed out after %d iterations", iter) rrt.solutionChan <- &rrtPlanReturn{planerr: fmt.Errorf("TP Space RRT timeout %w", ctx.Err()), maps: rrt.maps} @@ -221,40 +222,30 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( if iter == 0 { randPos = midPt } - if !bidirectional && iter % mp.algOpts.attemptSolveEvery == 0 { + if !bidirectional && iter%mp.algOpts.attemptSolveEvery == 0 { randPos = goalPose } randPosNode := &basicNode{pose: randPos} utils.PanicCapturingGo(func() { - m1chan <- mp.attemptExtension(ctx, nil, randPosNode, rrt.maps.startMap, false) + m1chan <- mp.attemptExtension(ctx, randPosNode, rrt.maps.startMap, false) }) - fmt.Println("seed done") utils.PanicCapturingGo(func() { - m2chan <- mp.attemptExtension(ctx, nil, randPosNode, rrt.maps.goalMap, true) + m2chan <- mp.attemptExtension(ctx, randPosNode, rrt.maps.goalMap, true) }) seedMapReached := <-m1chan goalMapReached := <-m2chan - + seedMapNode := seedMapReached.node goalMapNode := goalMapReached.node err := errors.Join(seedMapReached.error, goalMapReached.error) - - if err != nil { - rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} - return - } - - if err != nil { rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} return } - - fmt.Println("reseeding") + if seedMapNode != nil && goalMapNode != nil { - - seedReached := mp.attemptExtension(ctx, nil, goalMapNode, rrt.maps.startMap, false) + seedReached := mp.attemptExtension(ctx, goalMapNode, rrt.maps.startMap, false) if seedReached.error != nil { rrt.solutionChan <- &rrtPlanReturn{planerr: seedReached.error, maps: rrt.maps} return @@ -262,7 +253,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( if seedReached.node == nil { continue } - goalReached := mp.attemptExtension(ctx, nil, seedReached.node, rrt.maps.goalMap, true) + goalReached := mp.attemptExtension(ctx, seedReached.node, rrt.maps.goalMap, true) if goalReached.error != nil { rrt.solutionChan <- &rrtPlanReturn{planerr: goalReached.error, maps: rrt.maps} return @@ -271,8 +262,6 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( continue } reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartPosition: seedReached.node.Pose(), EndPosition: goalReached.node.Pose()}) - fmt.Println("reached seed", spatialmath.PoseToProtobuf(seedReached.node.Pose())) - fmt.Println("reached goal", spatialmath.PoseToProtobuf(goalReached.node.Pose())) if reachedDelta <= mp.algOpts.poseSolveDist { // If we've reached the goal, extract the path from the RRT trees and return path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: seedReached.node, b: goalReached.node}, false) @@ -280,10 +269,10 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( return } } - if iter % mp.algOpts.attemptSolveEvery == 0 { + if iter%mp.algOpts.attemptSolveEvery == 0 { paths := [][]node{} - for goalMapNode, _ := range rrt.maps.goalMap { - seedReached := mp.attemptExtension(ctx, nil, goalMapNode, rrt.maps.startMap, false) + for goalMapNode := range rrt.maps.goalMap { + seedReached := mp.attemptExtension(ctx, goalMapNode, rrt.maps.startMap, false) if seedReached.error != nil { rrt.solutionChan <- &rrtPlanReturn{planerr: seedReached.error, maps: rrt.maps} return @@ -293,8 +282,6 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( } reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartPosition: seedReached.node.Pose(), EndPosition: goalMapNode.Pose()}) if reachedDelta <= mp.algOpts.poseSolveDist { - fmt.Println("reached seed", spatialmath.PoseToProtobuf(seedReached.node.Pose())) - fmt.Println("reached goal", spatialmath.PoseToProtobuf(goalMapNode.Pose())) // If we've reached the goal, extract the path from the RRT trees and return path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: seedReached.node, b: goalMapNode}, false) paths = append(paths, path) @@ -334,7 +321,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( planOpts *plannerOptions, invert bool, ) (*candidate, error) { - nm := &neighborManager{nCPU: mp.planOpts.NumThreads/len(mp.tpFrame.PTGs())} + nm := &neighborManager{nCPU: mp.planOpts.NumThreads / len(mp.tpFrame.PTGs())} nm.parallelNeighbors = 10 var successNode node @@ -348,7 +335,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( // Get nearest neighbor to rand config in tree using this PTG nearest = nm.nearestNeighbor(ctx, ptgDistOpt, randPosNode, rrt) if nearest == nil { - return nil, nil + return nil, errNoNeighbors } } @@ -359,7 +346,6 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( targetFunc = func(pose spatialmath.Pose) float64 { return sqMet(&State{Position: spatialmath.Compose(nearest.Pose(), spatialmath.PoseInverse(pose))}) } - } else { relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) sqMet := NewSquaredNormMetric(relPose) @@ -367,7 +353,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( return sqMet(&State{Position: pose}) } } - + // Convert cartesian distance to tp-space using inverse curPtg, yielding TP-space coordinates goalK and goalD bestNode, err := curPtg.CToTP(ctx, targetFunc) if err != nil || bestNode == nil { @@ -376,13 +362,13 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( bestDist := targetFunc(bestNode.Pose) goalAlpha := bestNode.Alpha goalD := bestNode.Dist - + // Check collisions along this traj and get the longest distance viable trajK, err := curPtg.Trajectory(goalAlpha, goalD) if err != nil { return nil, err } - finalTrajNode := trajK[len(trajK) - 1] + finalTrajNode := trajK[len(trajK)-1] arcStartPose := nearest.Pose() if invert { @@ -407,16 +393,16 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( if sinceLastCollideCheck > planOpts.Resolution { ok, _ := planOpts.CheckStateConstraints(trajState) if !ok { - return nil, nil + return nil, errInvalidCandidate } sinceLastCollideCheck = 0. } lastDist = trajPt.Dist } - - isLastNode := math.Abs(finalTrajNode.Dist - curPtg.RefDistance()) < 0.1 - + + isLastNode := math.Abs(finalTrajNode.Dist-curPtg.RefDistance()) < 0.1 + // add the last node in trajectory successNode = &basicNode{ q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), goalAlpha, finalTrajNode.Dist}), @@ -434,7 +420,6 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( // Ensure successNode is sufficiently far from the nearest node already existing in the tree // If too close, don't add a new node if dist < defaultIdenticalNodeDistance { - fmt.Println("too close!!!") cand = nil } } @@ -445,13 +430,16 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( // closest to that goal. func (mp *tpSpaceRRTMotionPlanner) attemptExtension( ctx context.Context, - seedNode, goalNode node, rrt rrtMap, invert bool, ) *nodeAndError { var reseedCandidate *candidate + var seedNode node maxReseeds := 1 // Will be updated as necessary + lastIteration := false + candChan := make(chan *candidate, len(mp.tpFrame.PTGs())) + defer close(candChan) for i := 0; i <= maxReseeds; i++ { select { case <-ctx.Done(): @@ -459,17 +447,13 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( default: } candidates := []*candidate{} - - candChan := make(chan *candidate, len(mp.tpFrame.PTGs())) - defer close(candChan) + for ptgNum, curPtg := range mp.tpFrame.PTGs() { // Find the best traj point for each traj family, and store for later comparison ptgNumPar, curPtgPar := ptgNum, curPtg utils.PanicCapturingGo(func() { - cand, err := mp.getExtensionCandidate(ctx, goalNode, ptgNumPar, curPtgPar, rrt, seedNode, mp.planOpts, invert) - - if err != nil { + if err != nil && !errors.Is(err, errNoNeighbors) && !errors.Is(err, errInvalidCandidate) { candChan <- nil return } @@ -482,7 +466,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( candChan <- nil }) } - + for i := 0; i < len(mp.tpFrame.PTGs()); i++ { select { case <-ctx.Done(): @@ -495,24 +479,28 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( } var err error reseedCandidate, err = mp.extendMap(ctx, candidates, rrt, invert) - if err != nil { + if err != nil && !errors.Is(err, errNoCandidates) { return &nodeAndError{nil, err} } if reseedCandidate == nil { return &nodeAndError{nil, nil} } dist := mp.planOpts.DistanceFunc(&Segment{StartPosition: reseedCandidate.newNode.Pose(), EndPosition: goalNode.Pose()}) - //~ if dist < mp.algOpts.poseSolveDist || !reseedCandidate.lastInTraj { - if dist < mp.algOpts.poseSolveDist { + if dist < mp.algOpts.poseSolveDist || lastIteration { // Reached the goal position, or otherwise failed to fully extend to the end of a trajectory return &nodeAndError{reseedCandidate.newNode, nil} } if i == 0 { dist = math.Sqrt(dist) // TP-space distance is NOT the same thing as cartesian distance, but they track sufficiently well that this is valid to do. - maxReseeds = int(math.Min(float64(defaultMaxReseeds), math.Ceil(dist / reseedCandidate.newNode.Q()[2].Value) + 2)) - + maxReseeds = int(math.Min(float64(defaultMaxReseeds), math.Ceil(dist/reseedCandidate.newNode.Q()[2].Value)+2)) + } + // If our most recent traj was not a full-length extension, try to extend one more time and then return our best node. + // This helps prevent the planner from doing a 15-point turn to adjust orientation, which is very difficult to accurately execute. + if !reseedCandidate.lastInTraj { + lastIteration = true } + seedNode = reseedCandidate.newNode } return &nodeAndError{reseedCandidate.newNode, nil} @@ -526,7 +514,7 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( invert bool, ) (*candidate, error) { if len(candidates) == 0 { - return nil, nil + return nil, errNoCandidates } var addedNode node // If we found any valid nodes that we can extend to, find the very best one and add that to the tree @@ -539,13 +527,13 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( } } treeNode := bestCand.treeNode // The node already in the tree to which we are parenting - newNode := bestCand.newNode // The node we are adding because it was the best extending PTG + newNode := bestCand.newNode // The node we are adding because it was the best extending PTG ptgNum := int(newNode.Q()[0].Value) randAlpha := newNode.Q()[1].Value randDist := newNode.Q()[2].Value - //~ fmt.Println("__ADDINGtraj", ptgNum, randAlpha, randDist) + // ~ fmt.Println("__ADDINGtraj", ptgNum, randAlpha, randDist) trajK, err := mp.tpFrame.PTGs()[ptgNum].Trajectory(randAlpha, randDist) if err != nil { return nil, err @@ -553,7 +541,7 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( arcStartPose := treeNode.Pose() if invert { - arcStartPose = spatialmath.Compose(arcStartPose, spatialmath.PoseInverse(trajK[len(trajK) - 1].Pose)) + arcStartPose = spatialmath.Compose(arcStartPose, spatialmath.PoseInverse(trajK[len(trajK)-1].Pose)) } lastDist := 0. sinceLastNode := 0. @@ -569,10 +557,12 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( return nil, ctx.Err() } trajState = &State{Position: spatialmath.Compose(arcStartPose, trajPt.Pose)} - if !invert { - fmt.Printf("FINALPATH1,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) - }else{ - fmt.Printf("FINALPATH2,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + if mp.algOpts.pathdebug { + if !invert { + mp.logger.Debugf("$FWDTREE,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + } else { + mp.logger.Debugf("$REVTREE,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + } } sinceLastNode += (trajPt.Dist - lastDist) @@ -591,7 +581,9 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( } lastDist = trajPt.Dist } - fmt.Printf("WPI,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + if mp.algOpts.pathdebug { + mp.logger.Debugf("$WPI,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) + } } rrt[newNode] = treeNode return bestCand, nil @@ -603,14 +595,14 @@ func (mp *tpSpaceRRTMotionPlanner) setupTPSpaceOptions() { goalCheck: defaultGoalCheck, autoBB: defaultAutoBB, - addIntermediate: defaultAddInt, - addNodeEvery: defaultAddNodeEvery, + addIntermediate: defaultAddInt, + addNodeEvery: defaultAddNodeEvery, attemptSolveEvery: defaultAttemptSolveEvery, smoothScaleFactor: defaultSmoothScaleFactor, poseSolveDist: defaultIdenticalNodeDistance, - - distOptions: map[tpspace.PTG]*plannerOptions{}, + + distOptions: map[tpspace.PTG]*plannerOptions{}, invertDistOptions: map[tpspace.PTG]*plannerOptions{}, } @@ -641,7 +633,6 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, return sqMet(&State{Position: spatialmath.Compose(seg.EndPosition, spatialmath.PoseInverse(pose))}) } } else { - relPose := spatialmath.Compose(spatialmath.PoseInverse(seg.EndPosition), seg.StartPosition) sqMet := NewSquaredNormMetric(relPose) targetFunc = func(pose spatialmath.Pose) float64 { @@ -658,8 +649,13 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, return opts } +// rectifyPath is needed because of how trees are currently stored. As trees grow from the start or goal, the Pose stored in the node +// is the distal pose away from the root of the tree, which in the case of the goal tree is in fact the 0-distance point of the traj. +// When this becomes a single path, poses should reflect the transformation at the end of each traj. Here we go through and recompute +// each pose in order to ensure correctness. +// TODO: if trees are stored as segments rather than nodes, then this becomes simpler/unnecessary. Related to RSDK-4139. func (mp *tpSpaceRRTMotionPlanner) rectifyPath(path []node) ([]node, error) { - correctedPath := []node{} + correctedPath := []node{} runningPose := spatialmath.NewZeroPose() for _, wp := range path { wpPose, err := mp.frame.Transform(wp.Q()) @@ -667,7 +663,7 @@ func (mp *tpSpaceRRTMotionPlanner) rectifyPath(path []node) ([]node, error) { return nil, err } runningPose = spatialmath.Compose(runningPose, wpPose) - + thisNode := &basicNode{ q: wp.Q(), cost: wp.Cost(), @@ -680,16 +676,16 @@ func (mp *tpSpaceRRTMotionPlanner) rectifyPath(path []node) ([]node, error) { } func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) []node { - toIter := int(math.Min(float64(len(path)*len(path)), float64(mp.planOpts.SmoothIter))) + toIter := int(math.Min(float64(len(path)*len(path))/2, float64(mp.planOpts.SmoothIter))) currCost := sumCosts(path) - + maxCost := math.Inf(-1) for _, wp := range path { if wp.Cost() > maxCost { maxCost = wp.Cost() } } - newFrame, err := newPTGFrameFromPTGFrame(mp.frame, maxCost * mp.algOpts.smoothScaleFactor) + newFrame, err := newPTGFrameFromPTGFrame(mp.frame, maxCost*mp.algOpts.smoothScaleFactor) if err != nil { return path } @@ -708,9 +704,7 @@ func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) // Intn will return an int in the half-open interval half-open interval [0,n) firstEdge := mp.randseed.Intn(len(path) - 2) secondEdge := firstEdge + 2 + mp.randseed.Intn((len(path)-2)-firstEdge) - mp.logger.Debugf("checking shortcut between nodes %d and %d", firstEdge, secondEdge) - fmt.Printf("checking shortcut between nodes %d and %d \n", firstEdge, secondEdge) - + newInputSteps, err := mp.attemptSmooth(ctx, path, firstEdge, secondEdge, smoothPlanner) if err != nil || newInputSteps == nil { continue @@ -723,34 +717,19 @@ func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) if newInputSteps[len(newInputSteps)-1] != path[len(path)-1] { newInputSteps = append(newInputSteps, path[len(path)-1]) } - fmt.Println("success, checking goal") goalInputSteps, err := mp.attemptSmooth(ctx, newInputSteps, len(newInputSteps)-3, len(newInputSteps)-1, smoothPlanner) if err != nil || goalInputSteps == nil { - fmt.Println("failed to reconnect goal") continue } goalInputSteps = append(goalInputSteps, path[len(path)-1]) - fmt.Println("path") - for i, wp := range path { - fmt.Println(i, wp.Q(), wp.Pose().Point()) - } - fmt.Println("new") - for i, wp := range newInputSteps { - fmt.Println(i, wp.Q(), wp.Pose().Point()) - } - fmt.Println("successful smooth") - for i, wp := range goalInputSteps { - fmt.Println(i, wp.Q(), wp.Pose().Point()) - } path = goalInputSteps - //~ path = newInputSteps currCost = sumCosts(path) } return path } -// firstEdge and secondEdge must not be adjacent +// firstEdge and secondEdge must not be adjacent. func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( ctx context.Context, path []node, @@ -760,18 +739,18 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( startMap := map[node]node{} var parent node parentPose := spatialmath.NewZeroPose() - + for j := 0; j <= firstEdge; j++ { pathNode := path[j] startMap[pathNode] = parent for _, adj := range []float64{0.25, 0.5, 0.75} { fullQ := pathNode.Q() - newQ := []referenceframe.Input{fullQ[0], fullQ[1], referenceframe.Input{fullQ[2].Value * adj}} + newQ := []referenceframe.Input{fullQ[0], fullQ[1], {fullQ[2].Value * adj}} trajK, err := smoother.tpFrame.PTGs()[int(math.Round(newQ[0].Value))].Trajectory(newQ[1].Value, newQ[2].Value) if err != nil { continue } - + intNode := &basicNode{ q: newQ, cost: pathNode.Cost() - (pathNode.Q()[2].Value * (1 - adj)), @@ -783,23 +762,21 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( parent = pathNode parentPose = parent.Pose() } - reached := smoother.attemptExtension(ctx, nil, path[secondEdge], startMap, false) + // TODO: everything below this point can become an invocation of `smoother.planRunner` + reached := smoother.attemptExtension(ctx, path[secondEdge], startMap, false) if reached.error != nil || reached.node == nil { return nil, errors.New("could not extend to smoothing destination") } - + reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartPosition: reached.Pose(), EndPosition: path[secondEdge].Pose()}) // If we tried the goal and have a close-enough XY location, check if the node is good enough to be a final goal if reachedDelta > mp.algOpts.poseSolveDist { return nil, errors.New("could not precisely reach smoothing destination") } - fmt.Println("matched poses", reached.Pose().Point(), path[secondEdge].Pose().Point()) - fmt.Println("reached", reached.node) newInputSteps := extractPath(startMap, nil, &nodePair{a: reached.node, b: nil}, false) - - if secondEdge < len(path) - 1 { - //~ fmt.Println("appending") + + if secondEdge < len(path)-1 { newInputSteps = append(newInputSteps, path[secondEdge+1:]...) } return mp.rectifyPath(newInputSteps) diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 10a214d6b10..fadd9fb63c0 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -3,12 +3,10 @@ package motionplan import ( - "fmt" "context" + "math" "math/rand" "testing" - "math" - "time" "github.com/edaniels/golog" "github.com/golang/geo/r3" @@ -19,17 +17,19 @@ import ( "go.viam.com/rdk/spatialmath" ) -var printPath = true +var printPath = false const testTurnRad = 0.3 func TestPtgRrt(t *testing.T) { - fmt.Println("type,X,Y") + t.Parallel() logger := golog.NewTestLogger(t) roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") test.That(t, err, test.ShouldBeNil) geometries := []spatialmath.Geometry{roverGeom} + ctx := context.Background() + ackermanFrame, err := NewPTGFrameFromTurningRadius( "ackframe", logger, @@ -41,42 +41,55 @@ func TestPtgRrt(t *testing.T) { test.That(t, err, test.ShouldBeNil) goalPos := spatialmath.NewPose(r3.Vector{X: 200, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 90}) - //~ goalPos := spatialmath.NewPose(r3.Vector{X: 200, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 0}) - //~ goalPos := spatialmath.NewPose(r3.Vector{X: 0, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 0}) opt := newBasicPlannerOptions() - //~ opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) - //~ opt.DistanceFunc = SquaredNormNoOrientSegmentMetric - opt.SetGoalMetric(NewSquaredNormMetric(goalPos)) - opt.DistanceFunc = SquaredNormSegmentMetric - opt.GoalThreshold = 5. + opt.DistanceFunc = NewSquaredNormSegmentMetricWithScaling(30.) mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) tp, ok := mp.(*tpSpaceRRTMotionPlanner) - test.That(t, ok, test.ShouldBeTrue) - fmt.Printf("SG,%f,%f\n", 0., 0.) - fmt.Printf("SG,%f,%f\n", goalPos.Point().X, goalPos.Point().Y) - plan, err := tp.plan(context.Background(), goalPos, nil) - for _, wp := range plan { - fmt.Println(wp.Q()) + tp.algOpts.pathdebug = printPath + if tp.algOpts.pathdebug { + tp.logger.Debug("$type,X,Y") + tp.logger.Debugf("$SG,%f,%f\n", 0., 0.) + tp.logger.Debugf("$SG,%f,%f\n", goalPos.Point().X, goalPos.Point().Y) } + test.That(t, ok, test.ShouldBeTrue) + plan, err := tp.plan(ctx, goalPos, nil) test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThanOrEqualTo, 2) - + allPtgs := ackermanFrame.(tpspace.PTGProvider).PTGs() lastPose := spatialmath.NewZeroPose() - - if printPath { + + if tp.algOpts.pathdebug { for _, mynode := range plan { trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) for i, pt := range trajPts { intPose := spatialmath.Compose(lastPose, pt.Pose) if i == 0 { - fmt.Printf("WP,%f,%f\n", intPose.Point().X, intPose.Point().Y) + tp.logger.Debugf("$WP,%f,%f\n", intPose.Point().X, intPose.Point().Y) } - fmt.Printf("FINALPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) - if i == len(trajPts) - 1 { - lastPose = spatialmath.Compose(lastPose, pt.Pose) + tp.logger.Debugf("$FINALPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) + if i == len(trajPts)-1 { + lastPose = intPose + break + } + } + } + } + plan = tp.smoothPath(ctx, plan) + if tp.algOpts.pathdebug { + lastPose = spatialmath.NewZeroPose() + for _, mynode := range plan { + trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) + for i, pt := range trajPts { + intPose := spatialmath.Compose(lastPose, pt.Pose) + if i == 0 { + tp.logger.Debugf("$SMOOTHWP,%f,%f\n", intPose.Point().X, intPose.Point().Y) + } + tp.logger.Debugf("$SMOOTHPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) + if pt.Dist >= mynode.Q()[2].Value { + lastPose = intPose break } } @@ -85,6 +98,7 @@ func TestPtgRrt(t *testing.T) { } func TestPtgWithObstacle(t *testing.T) { + t.Parallel() logger := golog.NewTestLogger(t) roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") test.That(t, err, test.ShouldBeNil) @@ -107,10 +121,7 @@ func TestPtgWithObstacle(t *testing.T) { fs.AddFrame(ackermanFrame, fs.World()) opt := newBasicPlannerOptions() - //~ opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) - //~ opt.DistanceFunc = SquaredNormNoOrientSegmentMetric opt.DistanceFunc = NewSquaredNormSegmentMetricWithScaling(30.) - //~ opt.GoalThreshold = 0.001 opt.GoalThreshold = 5 // obstacles obstacle1, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{3300, -500, 0}), r3.Vector{180, 1800, 1}, "") @@ -124,18 +135,6 @@ func TestPtgWithObstacle(t *testing.T) { geoms := []spatialmath.Geometry{obstacle1, obstacle2, obstacle3, obstacle4} - if printPath { - fmt.Println("type,X,Y") - for _, geom := range geoms { - pts := geom.ToPoints(1.) - for _, pt := range pts { - if math.Abs(pt.Z) < 0.1 { - fmt.Printf("OBS,%f,%f\n", pt.X, pt.Y) - } - } - } - } - worldState, err := referenceframe.NewWorldState( []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, geoms)}, nil, @@ -153,62 +152,66 @@ func TestPtgWithObstacle(t *testing.T) { mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) tp, _ := mp.(*tpSpaceRRTMotionPlanner) - fmt.Printf("SG,%f,%f\n", 0., 0.) - fmt.Printf("SG,%f,%f\n", goalPos.Point().X, goalPos.Point().Y) - start := time.Now() + tp.algOpts.pathdebug = printPath + if tp.algOpts.pathdebug { + tp.logger.Debug("$type,X,Y") + for _, geom := range geoms { + pts := geom.ToPoints(1.) + for _, pt := range pts { + if math.Abs(pt.Z) < 0.1 { + tp.logger.Debugf("$OBS,%f,%f\n", pt.X, pt.Y) + } + } + } + tp.logger.Debugf("$SG,%f,%f\n", 0., 0.) + tp.logger.Debugf("$SG,%f,%f\n", goalPos.Point().X, goalPos.Point().Y) + } plan, err := tp.plan(ctx, goalPos, nil) - fmt.Println("planning took", time.Since(start)) - + test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThan, 2) - for _, wp := range plan { - fmt.Println(wp.Q()) - } - + allPtgs := ackermanFrame.(tpspace.PTGProvider).PTGs() lastPose := spatialmath.NewZeroPose() - - if printPath { + + if tp.algOpts.pathdebug { for _, mynode := range plan { trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) for i, pt := range trajPts { intPose := spatialmath.Compose(lastPose, pt.Pose) if i == 0 { - fmt.Printf("WP,%f,%f\n", intPose.Point().X, intPose.Point().Y) + tp.logger.Debugf("$WP,%f,%f\n", intPose.Point().X, intPose.Point().Y) } - fmt.Printf("FINALPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) - if i == len(trajPts) - 1 { + tp.logger.Debugf("$FINALPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) + if i == len(trajPts)-1 { lastPose = intPose break } } } } - start = time.Now() plan = tp.smoothPath(ctx, plan) - fmt.Println("smoothing took", time.Since(start)) - for _, wp := range plan { - fmt.Println(wp.Q()) - } - lastPose = spatialmath.NewZeroPose() - for _, mynode := range plan { - trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) - for i, pt := range trajPts { - intPose := spatialmath.Compose(lastPose, pt.Pose) - if i == 0 { - fmt.Printf("SMOOTHWP,%f,%f\n", intPose.Point().X, intPose.Point().Y) - } - fmt.Printf("SMOOTHPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) - if pt.Dist >= mynode.Q()[2].Value { - lastPose = intPose - break + if tp.algOpts.pathdebug { + lastPose = spatialmath.NewZeroPose() + for _, mynode := range plan { + trajPts, _ := allPtgs[int(mynode.Q()[0].Value)].Trajectory(mynode.Q()[1].Value, mynode.Q()[2].Value) + for i, pt := range trajPts { + intPose := spatialmath.Compose(lastPose, pt.Pose) + if i == 0 { + tp.logger.Debugf("$SMOOTHWP,%f,%f\n", intPose.Point().X, intPose.Point().Y) + } + tp.logger.Debugf("$SMOOTHPATH,%f,%f\n", intPose.Point().X, intPose.Point().Y) + if pt.Dist >= mynode.Q()[2].Value { + lastPose = intPose + break + } } } } } - func TestIKPtgRrt(t *testing.T) { + t.Parallel() logger := golog.NewTestLogger(t) roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") test.That(t, err, test.ShouldBeNil) @@ -239,3 +242,63 @@ func TestIKPtgRrt(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThanOrEqualTo, 2) } + +func TestTPsmoothing(t *testing.T) { + // TODO: this doesn't smooth properly yet. This should be made to smooth better. + t.Parallel() + logger := golog.NewTestLogger(t) + roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") + test.That(t, err, test.ShouldBeNil) + geometries := []spatialmath.Geometry{roverGeom} + + ctx := context.Background() + + ackermanFrame, err := NewPTGFrameFromTurningRadius( + "ackframe", + logger, + 300., + testTurnRad, + 0, + geometries, + ) + test.That(t, err, test.ShouldBeNil) + + opt := newBasicPlannerOptions() + opt.DistanceFunc = NewSquaredNormSegmentMetricWithScaling(30.) + mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) + test.That(t, err, test.ShouldBeNil) + tp, _ := mp.(*tpSpaceRRTMotionPlanner) + + // plan which is known to be able to use some smoothing + planInputs := [][]referenceframe.Input{ + {{0}, {0}, {0}}, + {{3}, {-0.20713797715976653}, {848.2300164692441}}, + {{5}, {0.0314906475636095}, {848.2300108402619}}, + {{5}, {0.0016660735709435135}, {848.2300146893297}}, + {{0}, {0.00021343061342569985}, {408}}, + {{5}, {1.9088870836327245}, {737.7547597081078}}, + {{2}, {-1.3118738553451883}, {848.2300164692441}}, + {{0}, {-3.1070696573964987}, {848.2300164692441}}, + {{0}, {-2.5547017183037877}, {306}}, + {{4}, {-2.31209484211255}, {408}}, + {{0}, {1.1943809502464207}, {571.4368241014894}}, + {{0}, {0.724950779684863}, {848.2300164692441}}, + {{0}, {-1.2295409308605127}, {848.2294213788913}}, + {{5}, {2.677652944060827}, {848.230013198154}}, + {{0}, {2.7618396954635545}, {848.2300164692441}}, + {{0}, {0}, {0}}, + } + plan := []node{} + for _, inp := range planInputs { + thisNode := &basicNode{ + q: inp, + cost: inp[2].Value, + } + plan = append(plan, thisNode) + } + plan, err = tp.rectifyPath(plan) + test.That(t, err, test.ShouldBeNil) + + plan = tp.smoothPath(ctx, plan) + test.That(t, plan, test.ShouldNotBeNil) +} diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index d036bf9536f..7c024428198 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -5,8 +5,6 @@ import ( "context" "math" - "github.com/golang/geo/r3" - "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -17,8 +15,8 @@ const floatEpsilon = 0.0001 // If floats are closer than this consider them equa // PTG coordinates are specified in polar coordinates (alpha, d) // One of these is needed for each sort of motion that can be done. type PTG interface { - //~ // CToTP Converts a pose to a (k, d) TP-space trajectory, returning the node closest to that pose - //~ CToTP(context.Context, spatialmath.Pose) (*TrajNode, error) + // ~ // CToTP Converts a pose to a (k, d) TP-space trajectory, returning the node closest to that pose + // ~ CToTP(context.Context, spatialmath.Pose) (*TrajNode, error) // CToTP will return the (alpha, dist) TP-space coordinates whose corresponding relative pose minimizes the given function CToTP(context.Context, func(spatialmath.Pose) float64) (*TrajNode, error) @@ -70,7 +68,7 @@ func index2alpha(k, numPaths uint) float64 { } func alpha2index(alpha float64, numPaths uint) uint { - alpha = wrapTo2Pi(alpha + math.Pi) - math.Pi + alpha = wrapTo2Pi(alpha+math.Pi) - math.Pi idx := uint(math.Round(0.5 * (float64(numPaths)*(1.0+alpha/math.Pi) - 1.0))) return idx } @@ -80,10 +78,7 @@ func wrapTo2Pi(theta float64) float64 { return theta - 2*math.Pi*math.Floor(theta/(2*math.Pi)) } -func xythetaToPose(x, y, theta float64) spatialmath.Pose { - return spatialmath.NewPose(r3.Vector{x, y, 0}, &spatialmath.OrientationVector{OZ: 1, Theta: theta}) -} - +// ComputePTG will compute all nodes of simPTG at the requested alpha, out to the requested distance, at the specified diffT resolution. func ComputePTG( simPTG PrecomputePTG, alpha, refDist, diffT float64, @@ -93,37 +88,24 @@ func ComputePTG( var err error var t, v, w float64 - dist := math.Copysign(math.Abs(v) * diffT, refDist) - - // Last saved waypoints - - lastVs := [2]float64{0, 0} - lastWs := [2]float64{0, 0} + dist := math.Copysign(math.Abs(v)*diffT, refDist) // Step through each time point for this alpha for math.Abs(dist) < math.Abs(refDist) { - t += diffT - nextNode, err := ComputeNextPTGNode(simPTG, alpha, dist, t) + nextNode, err := ComputePTGNode(simPTG, alpha, dist, t) if err != nil { return nil, err } v = nextNode.LinVelMMPS w = nextNode.AngVelRPS - - lastVs[1] = lastVs[0] - lastWs[1] = lastWs[0] - lastVs[0] = v - lastWs[0] = w - - - // Update velocities of last node because reasons + // Update velocities of last node because the computed velocities at this node are what should be set after passing the last node alphaTraj[len(alphaTraj)-1].LinVelMMPS = v alphaTraj[len(alphaTraj)-1].AngVelRPS = w alphaTraj = append(alphaTraj, nextNode) - dist += math.Copysign(math.Abs(v) * diffT, refDist) + dist += math.Copysign(math.Abs(v)*diffT, refDist) } // Add final node @@ -138,12 +120,18 @@ func ComputePTG( return alphaTraj, nil } -func ComputeNextPTGNode( +// ComputePTGNode will return the TrajNode of the requested PTG, at the specified alpha and dist. The provided time is used +// to fill in the time field. +func ComputePTGNode( simPTG PrecomputePTG, alpha, dist, atT float64, ) (*TrajNode, error) { v, w, err := simPTG.PTGVelocities(alpha, dist) + if err != nil { + return nil, err + } + // ptgIK caches these, so this should be fast. If cacheing is removed or a different simPTG used, this could be slow. pose, err := simPTG.Transform([]referenceframe.Input{{alpha}, {dist}}) if err != nil { return nil, err diff --git a/motionplan/tpspace/ptgC.go b/motionplan/tpspace/ptgC.go index 72da4651260..fd9ca35be56 100644 --- a/motionplan/tpspace/ptgC.go +++ b/motionplan/tpspace/ptgC.go @@ -3,11 +3,11 @@ package tpspace import ( "fmt" "math" - + + "github.com/golang/geo/r3" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" - - "github.com/golang/geo/r3" ) // ptgDiffDriveC defines a PTG family composed of circular trajectories with an alpha-dependent radius. @@ -40,31 +40,31 @@ func (ptg *ptgDiffDriveC) PTGVelocities(alpha, dist float64) (float64, float64, // of (input/pi)*minradius. A negative value denotes turning left. The second input is the distance traveled along this arc. func (ptg *ptgDiffDriveC) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { turnRad := ptg.maxMMPS / ptg.maxRPS - + if len(inputs) != 2 { return nil, fmt.Errorf("ptgC takes 2 inputs, but received %d", len(inputs)) } alpha := inputs[0].Value dist := inputs[1].Value - + // Check for OOB within FP error - if math.Pi - math.Abs(alpha) > math.Pi + floatEpsilon { + if math.Pi-math.Abs(alpha) > math.Pi+floatEpsilon { return nil, fmt.Errorf("ptgC input 0 is limited to [-pi, pi] but received %f", inputs[0]) } - + if alpha > math.Pi { alpha = math.Pi } - if alpha < -1 * math.Pi { + if alpha < -1*math.Pi { alpha = -1 * math.Pi } - + pt := r3.Vector{0, dist, 0} // Straight line, +Y is "forwards" - //~ pt := r3.Vector{dist,0, 0} // Straight line, +Y is "forwards" + // ~ pt := r3.Vector{dist,0, 0} // Straight line, +Y is "forwards" angleRads := 0. if alpha != 0 { arcRadius := math.Pi * turnRad / math.Abs(alpha) // radius of arc - angleRads = dist / arcRadius // number of radians to travel along arc + angleRads = dist / arcRadius // number of radians to travel along arc pt = r3.Vector{arcRadius * (1 - math.Cos(angleRads)), arcRadius * math.Sin(angleRads), 0} if alpha > 0 { // positive alpha = positive rotation = left turn = negative X @@ -73,6 +73,6 @@ func (ptg *ptgDiffDriveC) Transform(inputs []referenceframe.Input) (spatialmath. } } pose := spatialmath.NewPose(pt, &spatialmath.OrientationVector{OZ: 1, Theta: -angleRads}) - + return pose, nil } diff --git a/motionplan/tpspace/ptgCC.go b/motionplan/tpspace/ptgCC.go index 6866723c892..f8f2de6fe7b 100644 --- a/motionplan/tpspace/ptgCC.go +++ b/motionplan/tpspace/ptgCC.go @@ -1,9 +1,8 @@ package tpspace import ( - //~ "fmt" "math" - + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -14,19 +13,18 @@ import ( type ptgDiffDriveCC struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius - + circle *ptgDiffDriveC } // NewCCPTG creates a new PrecomputePTG of type ptgDiffDriveCC. func NewCCPTG(maxMMPS, maxRPS float64) PrecomputePTG { - circle := NewCirclePTG(maxMMPS, maxRPS).(*ptgDiffDriveC) - + return &ptgDiffDriveCC{ maxMMPS: maxMMPS, maxRPS: maxRPS, - circle: circle, + circle: circle, } } @@ -64,14 +62,14 @@ func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, dist float64) (float64, float64, } func (ptg *ptgDiffDriveCC) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { - //~ fmt.Println("CC") + // ~ fmt.Println("CC") alpha := inputs[0].Value dist := inputs[1].Value r := ptg.maxMMPS / ptg.maxRPS reverseDistance := math.Abs(alpha) * 0.5 * r - flip := math.Copysign(1., alpha) // left or right + flip := math.Copysign(1., alpha) // left or right direction := math.Copysign(1., dist) // forwards or backwards - + revPose, err := ptg.circle.Transform([]referenceframe.Input{{-1 * flip * math.Pi}, {-1. * direction * math.Min(dist, reverseDistance)}}) if err != nil { return nil, err diff --git a/motionplan/tpspace/ptgCCS.go b/motionplan/tpspace/ptgCCS.go index b930d7f1bf1..c5bf70362fe 100644 --- a/motionplan/tpspace/ptgCCS.go +++ b/motionplan/tpspace/ptgCCS.go @@ -2,8 +2,7 @@ package tpspace import ( "math" - //~ "fmt" - + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -14,20 +13,19 @@ type ptgDiffDriveCCS struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius r float64 - circle *ptgDiffDriveC + circle *ptgDiffDriveC } // NewCCSPTG creates a new PrecomputePTG of type ptgDiffDriveCCS. func NewCCSPTG(maxMMPS, maxRPS float64) PrecomputePTG { - r := maxMMPS / maxRPS circle := NewCirclePTG(maxMMPS, maxRPS).(*ptgDiffDriveC) - + return &ptgDiffDriveCCS{ maxMMPS: maxMMPS, maxRPS: maxRPS, - r: r, - circle: circle, + r: r, + circle: circle, } } @@ -63,16 +61,16 @@ func (ptg *ptgDiffDriveCCS) PTGVelocities(alpha, dist float64) (float64, float64 } func (ptg *ptgDiffDriveCCS) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { - //~ fmt.Println("CCS") + // ~ fmt.Println("CCS") alpha := inputs[0].Value dist := inputs[1].Value - + arcConstant := math.Abs(alpha) * 0.5 reverseDistance := arcConstant * ptg.r fwdArcDistance := (arcConstant + math.Pi/2) * ptg.r - flip := math.Copysign(1., alpha) // left or right + flip := math.Copysign(1., alpha) // left or right direction := math.Copysign(1., dist) // forwards or backwards - + revPose, err := ptg.circle.Transform([]referenceframe.Input{{-1 * flip * math.Pi}, {-1. * direction * math.Min(dist, reverseDistance)}}) if err != nil { return nil, err @@ -90,10 +88,10 @@ func (ptg *ptgDiffDriveCCS) Transform(inputs []referenceframe.Input) (spatialmat return nil, err } arcPose := spatialmath.Compose(revPose, fwdPose) - if dist < reverseDistance + fwdArcDistance { + if dist < reverseDistance+fwdArcDistance { return arcPose, nil } - + finalPose, err := ptg.circle.Transform([]referenceframe.Input{{0}, {direction * (dist - (fwdArcDistance + reverseDistance))}}) if err != nil { return nil, err diff --git a/motionplan/tpspace/ptgCS.go b/motionplan/tpspace/ptgCS.go index 7cc4ce3f61d..0e9eaa9da37 100644 --- a/motionplan/tpspace/ptgCS.go +++ b/motionplan/tpspace/ptgCS.go @@ -2,7 +2,7 @@ package tpspace import ( "math" - + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -16,19 +16,19 @@ const ( type ptgDiffDriveCS struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius - - r float64 // turning radius + + r float64 // turning radius turnStraight float64 } // NewCSPTG creates a new PrecomputePTG of type ptgDiffDriveCS. func NewCSPTG(maxMMPS, maxRPS float64) PrecomputePTG { r := maxMMPS / maxRPS - turnStraight := 1.2 * r + turnStraight := turnStraightConst * r return &ptgDiffDriveCS{ - maxMMPS: maxMMPS, - maxRPS: maxRPS, - r: r, + maxMMPS: maxMMPS, + maxRPS: maxRPS, + r: r, turnStraight: turnStraight, } } @@ -63,12 +63,12 @@ func (ptg *ptgDiffDriveCS) PTGVelocities(alpha, dist float64) (float64, float64, func (ptg *ptgDiffDriveCS) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { alpha := inputs[0].Value dist := inputs[1].Value - + actualRPS := ptg.maxRPS * math.Min(1.0, 1.0-math.Exp(-1*alpha*alpha)) circle := NewCirclePTG(ptg.maxMMPS, actualRPS).(*ptgDiffDriveC) - + arcDistance := ptg.turnStraight * math.Sqrt(math.Abs(alpha)) - flip := math.Copysign(1., alpha) // left or right + flip := math.Copysign(1., alpha) // left or right direction := math.Copysign(1., dist) // forwards or backwards var err error arcPose := spatialmath.NewZeroPose() diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index 577e753fb0a..b2dc264aa76 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -2,7 +2,6 @@ package tpspace import ( "context" - //~ "fmt" "math" "go.viam.com/rdk/spatialmath" @@ -12,10 +11,6 @@ const ( defaultMaxTime = 15. defaultDiffT = 0.005 defaultAlphaCnt uint = 91 - - - defaultMaxHeadingChange = 1.95 * math.Pi - orientationDistanceScaling = 10. ) // ptgGridSim will take a PrecomputePTG, and simulate out a number of trajectories through some requested time/distance for speed of lookup @@ -34,7 +29,7 @@ type ptgGridSim struct { // If true, then CToTP calls will *only* check the furthest end of each precomputed trajectory. // This is useful when used in conjunction with IK endsOnly bool - + // Discretized x[y][]node maps for rapid NN lookups trajNodeGrid map[int]map[int][]*TrajNode } @@ -46,11 +41,11 @@ func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64, endsOnly bo } ptg := &ptgGridSim{ - refDist: simDist, - alphaCnt: arcs, - maxTime: defaultMaxTime, - diffT: defaultDiffT, - endsOnly: endsOnly, + refDist: simDist, + alphaCnt: arcs, + maxTime: defaultMaxTime, + diffT: defaultDiffT, + endsOnly: endsOnly, trajNodeGrid: map[int]map[int][]*TrajNode{}, } @@ -66,13 +61,11 @@ func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64, endsOnly bo } func (ptg *ptgGridSim) CToTP(ctx context.Context, distFunc func(spatialmath.Pose) float64) (*TrajNode, error) { - // Try to find a closest point to the paths: bestDist := math.Inf(1) var bestNode *TrajNode if !ptg.endsOnly { - for k := 0; k < int(ptg.alphaCnt); k++ { nMax := len(ptg.precomputeTraj[k]) - 1 for n := 0; n <= nMax; n++ { @@ -95,11 +88,7 @@ func (ptg *ptgGridSim) CToTP(ctx context.Context, distFunc func(spatialmath.Pose // which can be normalized by "1/refDistance" to get TP-Space distances. for k := 0; k < int(ptg.alphaCnt); k++ { n := len(ptg.precomputeTraj[k]) - 1 - //~ fmt.Println("checking", ptg.precomputeTraj[k][n].ptX, ptg.precomputeTraj[k][n].ptY) distToPoint := distFunc(ptg.precomputeTraj[k][n].Pose) - //~ distToPoint := math.Sqrt(math.Pow(ptg.precomputeTraj[k][n].Dist, 2) + - //~ math.Pow(ptg.precomputeTraj[k][n].ptX-x, 2) + math.Pow(ptg.precomputeTraj[k][n].ptY-y, 2)) - //~ fmt.Println("checked dist", distToPoint) if distToPoint < bestDist { bestDist = distToPoint @@ -124,12 +113,12 @@ func (ptg *ptgGridSim) simulateTrajectories(simPTG PrecomputePTG) ([][]*TrajNode for k := uint(0); k < ptg.alphaCnt; k++ { alpha := index2alpha(k, ptg.alphaCnt) - + alphaTraj, err := ComputePTG(simPTG, alpha, ptg.refDist, ptg.diffT) if err != nil { return nil, err } - + if !ptg.endsOnly { for _, tNode := range alphaTraj { gridX := int(math.Round(tNode.ptX)) @@ -147,12 +136,3 @@ func (ptg *ptgGridSim) simulateTrajectories(simPTG PrecomputePTG) ([][]*TrajNode return allTraj, nil } - - -func goalPoseDiscFunc(goal spatialmath.Pose) func(spatialmath.Pose) float64 { - return func (query spatialmath.Pose) float64 { - delta := spatialmath.PoseDelta(goal, query) - // Increase weight for orientation since it's a small number - return delta.Point().Norm2() + spatialmath.QuatToR3AA(delta.Orientation().Quaternion()).Mul(orientationDistanceScaling).Norm2() - } -} diff --git a/motionplan/tpspace/ptgGridSim_test.go b/motionplan/tpspace/ptgGridSim_test.go index e9719172b7d..2f783a7f981 100644 --- a/motionplan/tpspace/ptgGridSim_test.go +++ b/motionplan/tpspace/ptgGridSim_test.go @@ -2,14 +2,10 @@ package tpspace import ( "testing" - "fmt" "go.viam.com/test" - "go.viam.com/rdk/spatialmath" - "github.com/golang/geo/r3" ) - type ptgFactory func(float64, float64) PrecomputePTG var defaultPTGs = []ptgFactory{ @@ -20,42 +16,25 @@ var defaultPTGs = []ptgFactory{ } var ( - defaultMMps = 300. + defaultMMps = 300. turnRadMeters = 0.3 ) func TestSim(t *testing.T) { - simDist :=2500. - alphaCnt := uint(61) - fmt.Println("type,X,Y") - //~ for _, ptg := range defaultPTGs { - ptg := NewSideSOverturnPTG + simDist := 2500. + alphaCnt := uint(121) + for _, ptg := range defaultPTGs { radPS := defaultMMps / (turnRadMeters * 1000) ptgGen := ptg(defaultMMps, radPS) test.That(t, ptgGen, test.ShouldNotBeNil) grid, err := NewPTGGridSim(ptgGen, alphaCnt, simDist, false) test.That(t, err, test.ShouldBeNil) - + for i := uint(0); i < alphaCnt; i++ { - //~ i := uint(60) - //~ alpha := -0.41541721039203877 - traj, _ := grid.Trajectory(index2alpha(i, alphaCnt), simDist) - //~ traj, _ := grid.Trajectory(alpha, simDist) - for _, intPose := range traj{ - fmt.Printf("FINALPATH,%f,%f\n", intPose.Pose.Point().X, intPose.Pose.Point().Y) - } + traj, err := grid.Trajectory(index2alpha(i, alphaCnt), simDist) + test.That(t, err, test.ShouldBeNil) + test.That(t, traj, test.ShouldNotBeNil) } -} - -func TestPose(t *testing.T) { - goalPose := spatialmath.NewPoseFromPoint(r3.Vector{50,1000,0}) - trajPose := spatialmath.NewPose(r3.Vector{-100,10,0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 90}) - startPose := spatialmath.Compose(goalPose, spatialmath.PoseInverse(trajPose)) - // resultPose x:39.999999999999886 y:1100 o_z:1 theta:-89.99999999999999 - - //~ startPose := spatialmath.Compose(spatialmath.PoseInverse(trajPose), goalPose) - // resultPose x:990 y:50.000000000000114 o_z:1 theta:-89.99999999999999 - - fmt.Println("resultPose", spatialmath.PoseToProtobuf(startPose)) + } } diff --git a/motionplan/tpspace/ptgIKFrame.go b/motionplan/tpspace/ptgIKFrame.go index ee42153bf19..7408cc86295 100644 --- a/motionplan/tpspace/ptgIKFrame.go +++ b/motionplan/tpspace/ptgIKFrame.go @@ -1,6 +1,7 @@ package tpspace import ( + "errors" "math" pb "go.viam.com/api/component/arm/v1" @@ -8,19 +9,20 @@ import ( "go.viam.com/rdk/referenceframe" ) -// ptgFrame wraps a tpspace.PrecomputePTG so that it fills the Frame interface and can be used by IK +// ptgFrame wraps a tpspace.PrecomputePTG so that it fills the Frame interface and can be used by IK. type ptgIKFrame struct { PrecomputePTG - limits []referenceframe.Limit + limits []referenceframe.Limit } +// NewPTGIKFrame will create a new frame intended to be passed to an Inverse Kinematics solver, allowing IK to solve for parameters +// for the passed in PTG. func NewPTGIKFrame(ptg PrecomputePTG, dist float64) (referenceframe.Frame, error) { pf := &ptgIKFrame{PrecomputePTG: ptg} pf.limits = []referenceframe.Limit{ {Min: -math.Pi, Max: math.Pi}, {Min: -dist, Max: dist}, - //~ {Min: -dist, Max: dist}, } return pf, nil } @@ -34,7 +36,7 @@ func (pf *ptgIKFrame) Name() string { } func (pf *ptgIKFrame) MarshalJSON() ([]byte, error) { - return nil, nil + return nil, errors.New("marshal json not implemented for ptg IK frame") } func (pf *ptgIKFrame) InputFromProtobuf(jp *pb.JointPositions) []referenceframe.Input { @@ -54,5 +56,5 @@ func (pf *ptgIKFrame) ProtobufFromInput(input []referenceframe.Input) *pb.JointP } func (pf *ptgIKFrame) Geometries(inputs []referenceframe.Input) (*referenceframe.GeometriesInFrame, error) { - return nil, nil + return nil, errors.New("geometries not implemented for ptg IK frame") } diff --git a/motionplan/tpspace/ptgSideS.go b/motionplan/tpspace/ptgSideS.go index 0d2f5e53719..8de4e58249d 100644 --- a/motionplan/tpspace/ptgSideS.go +++ b/motionplan/tpspace/ptgSideS.go @@ -2,7 +2,7 @@ package tpspace import ( "math" - + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -10,43 +10,41 @@ import ( const defaultCountersteer = 1.5 // ptgDiffDriveSideS defines a PTG family which makes a forwards turn, then a counter turn the other direction, and goes straight. -// This has the effect of translating to one side or the other without orientation change +// This has the effect of translating to one side or the other without orientation change. type ptgDiffDriveSideS struct { - maxMMPS float64 // millimeters per second velocity to target - maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius - r float64 + maxMMPS float64 // millimeters per second velocity to target + maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius + r float64 countersteer float64 - circle *ptgDiffDriveC + circle *ptgDiffDriveC } // NewSideSPTG creates a new PrecomputePTG of type ptgDiffDriveSideS. func NewSideSPTG(maxMMPS, maxRPS float64) PrecomputePTG { - r := maxMMPS / maxRPS circle := NewCirclePTG(maxMMPS, maxRPS).(*ptgDiffDriveC) - + return &ptgDiffDriveSideS{ - maxMMPS: maxMMPS, - maxRPS: maxRPS, - r: r, + maxMMPS: maxMMPS, + maxRPS: maxRPS, + r: r, countersteer: 1.0, - circle: circle, + circle: circle, } } // NewSideSOverturnPTG creates a new PrecomputePTG of type ptgDiffDriveSideS which overturns. -// It turns X amount in one direction, then countersteers X*countersteerFactor in the other direction +// It turns X amount in one direction, then countersteers X*countersteerFactor in the other direction. func NewSideSOverturnPTG(maxMMPS, maxRPS float64) PrecomputePTG { - r := maxMMPS / maxRPS circle := NewCirclePTG(maxMMPS, maxRPS).(*ptgDiffDriveC) - + return &ptgDiffDriveSideS{ - maxMMPS: maxMMPS, - maxRPS: maxRPS, - r: r, + maxMMPS: maxMMPS, + maxRPS: maxRPS, + r: r, countersteer: defaultCountersteer, - circle: circle, + circle: circle, } } @@ -63,7 +61,7 @@ func (ptg *ptgDiffDriveSideS) PTGVelocities(alpha, dist float64) (float64, float // l- v = ptg.maxMMPS w = ptg.maxRPS * flip - } else if dist < arcLength + arcLength * ptg.countersteer { + } else if dist < arcLength+arcLength*ptg.countersteer { v = ptg.maxMMPS w = ptg.maxRPS * -1 * flip } @@ -74,11 +72,11 @@ func (ptg *ptgDiffDriveSideS) PTGVelocities(alpha, dist float64) (float64, float func (ptg *ptgDiffDriveSideS) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { alpha := inputs[0].Value dist := inputs[1].Value - - flip := math.Copysign(1., alpha) // left or right - direction := math.Copysign(1., dist) // forwards or backwards - arcLength := math.Abs(alpha) * 0.5 * ptg.r // - + + flip := math.Copysign(1., alpha) // left or right + direction := math.Copysign(1., dist) // forwards or backwards + arcLength := math.Abs(alpha) * 0.5 * ptg.r // + revPose, err := ptg.circle.Transform([]referenceframe.Input{{flip * math.Pi}, {direction * math.Min(dist, arcLength)}}) if err != nil { return nil, err @@ -89,18 +87,18 @@ func (ptg *ptgDiffDriveSideS) Transform(inputs []referenceframe.Input) (spatialm fwdPose, err := ptg.circle.Transform( []referenceframe.Input{ {-1 * flip * math.Pi}, - {direction * (math.Min(dist, arcLength + arcLength * ptg.countersteer) - arcLength)}, + {direction * (math.Min(dist, arcLength+arcLength*ptg.countersteer) - arcLength)}, }, ) if err != nil { return nil, err } arcPose := spatialmath.Compose(revPose, fwdPose) - if dist < arcLength + arcLength * ptg.countersteer { + if dist < arcLength+arcLength*ptg.countersteer { return arcPose, nil } - - finalPose, err := ptg.circle.Transform([]referenceframe.Input{{0}, {direction * (dist - (arcLength + arcLength * ptg.countersteer))}}) + + finalPose, err := ptg.circle.Transform([]referenceframe.Input{{0}, {direction * (dist - (arcLength + arcLength*ptg.countersteer))}}) if err != nil { return nil, err } diff --git a/motionplan/tpspace/ptg_test.go b/motionplan/tpspace/ptg_test.go index 6ea2a0e218d..7b9ef65f631 100644 --- a/motionplan/tpspace/ptg_test.go +++ b/motionplan/tpspace/ptg_test.go @@ -2,7 +2,6 @@ package tpspace import ( "testing" - //~ "fmt" "go.viam.com/test" ) @@ -14,10 +13,3 @@ func TestAlphaIdx(t *testing.T) { test.That(t, i, test.ShouldEqual, i2) } } - -//~ func TestRevPTGs(t *testing.T) { - //~ cs := NewCirclePTG(0.3, 0.3) - //~ traj, err := ComputePTG(cs, 0, -2000, 0.05) - //~ test.That(t, err, test.ShouldBeNil) - //~ fmt.Println(traj) -//~ } diff --git a/motionplan/tpspace/simPtgAlpha.go b/motionplan/tpspace/simPtgAlpha.go index 99324d383a3..a869c8f3608 100644 --- a/motionplan/tpspace/simPtgAlpha.go +++ b/motionplan/tpspace/simPtgAlpha.go @@ -1,46 +1,44 @@ package tpspace -//~ import ( - //~ "math" - //~ "errors" - - //~ "go.viam.com/rdk/referenceframe" - //~ "go.viam.com/rdk/spatialmath" -//~ ) - -//~ // Pi / 4 (45 degrees), used as a default alpha constant -//~ // This controls how tightly our parabolas arc -//~ // 57 degrees is also sometimes used by the reference. -//~ const quarterPi = 0.78539816339 - -//~ // simPtgAlpha defines a PTG family which follows a parabolic path. -//~ type simPTGAlpha struct { - //~ maxMMPS float64 // millimeters per second velocity to target - //~ maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius +// ~ import ( +// ~ "math" +// ~ "errors" + +// ~ "go.viam.com/rdk/referenceframe" +// ~ "go.viam.com/rdk/spatialmath" +// ~ ) + +// ~ // Pi / 4 (45 degrees), used as a default alpha constant +// ~ // This controls how tightly our parabolas arc +// ~ // 57 degrees is also sometimes used by the reference. +// ~ const quarterPi = 0.78539816339 + +// ~ // simPtgAlpha defines a PTG family which follows a parabolic path. +// ~ type simPTGAlpha struct { +// ~ maxMMPS float64 // millimeters per second velocity to target +// ~ maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius +// ~ } + +// ~ // NewAlphaPTG creates a new PrecomputePTG of type simPtgAlpha. +// ~ // K is unused for alpha PTGs *for now* but we may add in the future. +// ~ func NewAlphaPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { +// ~ return &simPTGAlpha{ +// ~ maxMMPS: maxMMPS, +//~ maxRPS: maxRPS, //~ } - -//~ // NewAlphaPTG creates a new PrecomputePTG of type simPtgAlpha. -//~ // K is unused for alpha PTGs *for now* but we may add in the future. -//~ func NewAlphaPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { - //~ return &simPTGAlpha{ - //~ maxMMPS: maxMMPS, - //~ maxRPS: maxRPS, - //~ } //~ } -//~ func (ptg *simPTGAlpha) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { - //~ // In order to know what to set our angvel at, we need to know how far into the path we are - //~ atA := wrapTo2Pi(alpha - phi) - //~ if atA > math.Pi { - //~ atA -= 2 * math.Pi - //~ } +// ~ func (ptg *simPTGAlpha) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { +// ~ // In order to know what to set our angvel at, we need to know how far into the path we are +// ~ atA := wrapTo2Pi(alpha - phi) +// ~ if atA > math.Pi { +// ~ atA -= 2 * math.Pi +//~ } - //~ v := ptg.maxMMPS * math.Exp(-1.*math.Pow(atA/quarterPi, 2)) - //~ w := ptg.maxRPS * (-0.5 + (1. / (1. + math.Exp(-atA/quarterPi)))) +// ~ w := ptg.maxRPS * (-0.5 + (1. / (1. + math.Exp(-atA/quarterPi)))) - //~ return v, w, nil -//~ } +// ~ } -//~ func (ptg *simPTGAlpha) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { - //~ return nil, errors.New("alpha PTG does not support Transform yet") -//~ } +// ~ func (ptg *simPTGAlpha) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { +// ~ return nil, errors.New("alpha PTG does not support Transform yet") +// ~ } From c797032e86ba2dd8a7ddd71623e68b5e1cb13613 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 16 Aug 2023 10:14:22 -0700 Subject: [PATCH 19/38] Remove some comments and old file --- motionplan/nloptInverseKinematics.go | 4 --- motionplan/tpspace/ptg.go | 2 -- motionplan/tpspace/ptgC.go | 1 - motionplan/tpspace/ptgCC.go | 1 - motionplan/tpspace/simPtgAlpha.go | 44 ---------------------------- 5 files changed, 52 deletions(-) delete mode 100644 motionplan/tpspace/simPtgAlpha.go diff --git a/motionplan/nloptInverseKinematics.go b/motionplan/nloptInverseKinematics.go index 3b1108aac70..c75101cdadf 100644 --- a/motionplan/nloptInverseKinematics.go +++ b/motionplan/nloptInverseKinematics.go @@ -188,10 +188,6 @@ func (ik *NloptIK) Solve(ctx context.Context, err = multierr.Combine(err, nloptErr) } - // ~ if solutionRaw != nil { - // ~ fmt.Println("best nlopt", result) - // ~ } - if result < ik.epsilon*ik.epsilon || (solutionRaw != nil && ik.partial) { select { case <-ctx.Done(): diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index 7c024428198..00d13421c57 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -15,8 +15,6 @@ const floatEpsilon = 0.0001 // If floats are closer than this consider them equa // PTG coordinates are specified in polar coordinates (alpha, d) // One of these is needed for each sort of motion that can be done. type PTG interface { - // ~ // CToTP Converts a pose to a (k, d) TP-space trajectory, returning the node closest to that pose - // ~ CToTP(context.Context, spatialmath.Pose) (*TrajNode, error) // CToTP will return the (alpha, dist) TP-space coordinates whose corresponding relative pose minimizes the given function CToTP(context.Context, func(spatialmath.Pose) float64) (*TrajNode, error) diff --git a/motionplan/tpspace/ptgC.go b/motionplan/tpspace/ptgC.go index fd9ca35be56..acf7b100c5f 100644 --- a/motionplan/tpspace/ptgC.go +++ b/motionplan/tpspace/ptgC.go @@ -60,7 +60,6 @@ func (ptg *ptgDiffDriveC) Transform(inputs []referenceframe.Input) (spatialmath. } pt := r3.Vector{0, dist, 0} // Straight line, +Y is "forwards" - // ~ pt := r3.Vector{dist,0, 0} // Straight line, +Y is "forwards" angleRads := 0. if alpha != 0 { arcRadius := math.Pi * turnRad / math.Abs(alpha) // radius of arc diff --git a/motionplan/tpspace/ptgCC.go b/motionplan/tpspace/ptgCC.go index f8f2de6fe7b..a437e0ed337 100644 --- a/motionplan/tpspace/ptgCC.go +++ b/motionplan/tpspace/ptgCC.go @@ -62,7 +62,6 @@ func (ptg *ptgDiffDriveCC) PTGVelocities(alpha, dist float64) (float64, float64, } func (ptg *ptgDiffDriveCC) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { - // ~ fmt.Println("CC") alpha := inputs[0].Value dist := inputs[1].Value r := ptg.maxMMPS / ptg.maxRPS diff --git a/motionplan/tpspace/simPtgAlpha.go b/motionplan/tpspace/simPtgAlpha.go deleted file mode 100644 index a869c8f3608..00000000000 --- a/motionplan/tpspace/simPtgAlpha.go +++ /dev/null @@ -1,44 +0,0 @@ -package tpspace - -// ~ import ( -// ~ "math" -// ~ "errors" - -// ~ "go.viam.com/rdk/referenceframe" -// ~ "go.viam.com/rdk/spatialmath" -// ~ ) - -// ~ // Pi / 4 (45 degrees), used as a default alpha constant -// ~ // This controls how tightly our parabolas arc -// ~ // 57 degrees is also sometimes used by the reference. -// ~ const quarterPi = 0.78539816339 - -// ~ // simPtgAlpha defines a PTG family which follows a parabolic path. -// ~ type simPTGAlpha struct { -// ~ maxMMPS float64 // millimeters per second velocity to target -// ~ maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius -// ~ } - -// ~ // NewAlphaPTG creates a new PrecomputePTG of type simPtgAlpha. -// ~ // K is unused for alpha PTGs *for now* but we may add in the future. -// ~ func NewAlphaPTG(maxMMPS, maxRPS, k float64) PrecomputePTG { -// ~ return &simPTGAlpha{ -// ~ maxMMPS: maxMMPS, -//~ maxRPS: maxRPS, -//~ } -//~ } - -// ~ func (ptg *simPTGAlpha) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { -// ~ // In order to know what to set our angvel at, we need to know how far into the path we are -// ~ atA := wrapTo2Pi(alpha - phi) -// ~ if atA > math.Pi { -// ~ atA -= 2 * math.Pi -//~ } - -// ~ w := ptg.maxRPS * (-0.5 + (1. / (1. + math.Exp(-atA/quarterPi)))) - -// ~ } - -// ~ func (ptg *simPTGAlpha) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { -// ~ return nil, errors.New("alpha PTG does not support Transform yet") -// ~ } From a9a1150b579a66149e94010deb694c8def60990b Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 16 Aug 2023 14:18:05 -0700 Subject: [PATCH 20/38] Fix merge conflict --- components/base/kinematicbase/ptgKinematics.go | 9 --------- 1 file changed, 9 deletions(-) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 6bc69eaf0ec..083afa74d98 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -130,18 +130,9 @@ func (ptgk *ptgBaseKinematics) GoToInputs(ctx context.Context, inputs []referenc } lastTime := 0. -<<<<<<< HEAD for _, trajNode := range selectedTraj { timestep := time.Duration(trajNode.Time-lastTime) * time.Second -======= - for i, trajNode := range selectedTraj { - if trajNode.Dist > inputs[distanceAlongTrajectoryIndex].Value { - ptgk.logger.Debugf("finished executing trajectory after %d steps", i) - // We have reached the desired distance along the given trajectory - break - } timestep := time.Duration((trajNode.Time-lastTime)*1000*1000) * time.Microsecond ->>>>>>> main lastTime = trajNode.Time linVel := r3.Vector{0, trajNode.LinVelMMPS, 0} angVel := r3.Vector{0, 0, rdkutils.RadToDeg(trajNode.AngVelRPS)} From 1939e7816ad8e11aa29da71a15927200066e4441 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 16 Aug 2023 14:19:58 -0700 Subject: [PATCH 21/38] lint --- components/base/kinematicbase/ptgKinematics.go | 1 - motionplan/rrt.go | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 083afa74d98..adb4d35886f 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -131,7 +131,6 @@ func (ptgk *ptgBaseKinematics) GoToInputs(ctx context.Context, inputs []referenc lastTime := 0. for _, trajNode := range selectedTraj { - timestep := time.Duration(trajNode.Time-lastTime) * time.Second timestep := time.Duration((trajNode.Time-lastTime)*1000*1000) * time.Microsecond lastTime = trajNode.Time linVel := r3.Vector{0, trajNode.LinVelMMPS, 0} diff --git a/motionplan/rrt.go b/motionplan/rrt.go index 67fc0ff6e2e..3f333605b5d 100644 --- a/motionplan/rrt.go +++ b/motionplan/rrt.go @@ -245,7 +245,7 @@ func extractPath(startMap, goalMap map[node]node, pair *nodePair, matched bool) // ~ cost: goalReached.Cost(), // ~ pose: goalReached.Pose(), // ~ corner: goalReached.Corner(), - //~ } + // ~ } //~ lastNode = thisNode //~ path = append(path, thisNode) path = append(path, goalReached) From 92b1b29ff2ff158261c2c7d0049ca044155941c7 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 16 Aug 2023 14:39:22 -0700 Subject: [PATCH 22/38] Assorted cleanup --- .../base/kinematicbase/ptgKinematics.go | 2 ++ motionplan/metrics.go | 8 +++--- motionplan/nloptInverseKinematics.go | 5 +++- motionplan/ptgGroupFrame.go | 25 ++++++------------- motionplan/ptgIK.go | 2 +- motionplan/rrt.go | 15 ----------- motionplan/tpSpaceRRT.go | 4 ++- motionplan/tpspace/ptgGridSim.go | 18 ------------- 8 files changed, 20 insertions(+), 59 deletions(-) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index adb4d35886f..e3b46b58025 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -131,6 +131,8 @@ func (ptgk *ptgBaseKinematics) GoToInputs(ctx context.Context, inputs []referenc lastTime := 0. for _, trajNode := range selectedTraj { + // TODO: Most trajectories update their velocities infrequently, or sometimes never. + // This function could be improved by looking ahead through the trajectory and minimizing the amount of SetVelocity calls. timestep := time.Duration((trajNode.Time-lastTime)*1000*1000) * time.Microsecond lastTime = trajNode.Time linVel := r3.Vector{0, trajNode.LinVelMMPS, 0} diff --git a/motionplan/metrics.go b/motionplan/metrics.go index 8627de2baca..eb9453c0ac1 100644 --- a/motionplan/metrics.go +++ b/motionplan/metrics.go @@ -113,11 +113,9 @@ func L2InputMetric(segment *Segment) float64 { return referenceframe.InputsL2Distance(segment.StartConfiguration, segment.EndConfiguration) } -// SquaredNormSegmentMetric is a metric which will return the cartesian distance between the two positions. -func SquaredNormSegmentMetric(segment *Segment) float64 { - delta := spatial.PoseDelta(segment.StartPosition, segment.EndPosition) - // Increase weight for orientation since it's a small number - return delta.Point().Norm2() + spatial.QuatToR3AA(delta.Orientation().Quaternion()).Mul(orientationDistanceScaling).Norm2() +// NewSquaredNormSegmentMetric returns a metric which will return the cartesian distance between the two positions. +func NewSquaredNormSegmentMetric() SegmentMetric { + return NewSquaredNormSegmentMetricWithScaling(orientationDistanceScaling) } // NewSquaredNormSegmentMetricWithScaling returns a metric which will return the cartesian distance between the two positions. diff --git a/motionplan/nloptInverseKinematics.go b/motionplan/nloptInverseKinematics.go index c75101cdadf..6a0512b0c87 100644 --- a/motionplan/nloptInverseKinematics.go +++ b/motionplan/nloptInverseKinematics.go @@ -38,7 +38,10 @@ type NloptIK struct { solveEpsilon float64 logger golog.Logger jump float64 - partial bool + + // Nlopt will try to minimize a configuration for whatever is passed in. If partial is true, then the solver will emit partial + // solutions where it was not able to meet the goal criteria but still was able to improve upon the seed. + partial bool } // CreateNloptIKSolver creates an nloptIK object that can perform gradient descent on metrics for Frames. The parameters are the Frame on diff --git a/motionplan/ptgGroupFrame.go b/motionplan/ptgGroupFrame.go index 6477f481864..02e7655a46d 100644 --- a/motionplan/ptgGroupFrame.go +++ b/motionplan/ptgGroupFrame.go @@ -63,7 +63,7 @@ func NewPTGFrameFromTurningRadius( // Get max angular velocity in radians per second maxRPS := velocityMMps / (1000. * turnRadMeters) pf := &ptgGroupFrame{name: name} - err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist, false) + err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist) if err != nil { return nil, err } @@ -96,7 +96,7 @@ func newPTGFrameFromPTGFrame(frame referenceframe.Frame, refDist float64) (refer // Get max angular velocity in radians per second maxRPS := ptgFrame.velocityMMps / (1000. * ptgFrame.turnRadMeters) pf := &ptgGroupFrame{name: ptgFrame.name} - err := pf.initPTGs(ptgFrame.logger, ptgFrame.velocityMMps, maxRPS, refDist, false) + err := pf.initPTGs(ptgFrame.logger, ptgFrame.velocityMMps, maxRPS, refDist) if err != nil { return nil, err } @@ -176,27 +176,16 @@ func (pf *ptgGroupFrame) PTGs() []tpspace.PTG { return pf.ptgs } -func (pf *ptgGroupFrame) initPTGs(logger golog.Logger, maxMps, maxRPS, simDist float64, simulate bool) error { +func (pf *ptgGroupFrame) initPTGs(logger golog.Logger, maxMps, maxRPS, simDist float64) error { ptgs := []tpspace.PTG{} for _, ptg := range defaultPTGs { ptgGen := ptg(maxMps, maxRPS) if ptgGen != nil { - if simulate { - for _, k := range []float64{1.} { - // irreversible trajectories, e.g. alpha, will return nil for negative k - newptg, err := tpspace.NewPTGGridSim(ptgGen, 0, k*simDist, false) // 0 uses default alpha count - if err != nil { - return err - } - ptgs = append(ptgs, newptg) - } - } else { - newptg, err := NewPTGIK(ptgGen, logger, simDist, 2) - if err != nil { - return err - } - ptgs = append(ptgs, newptg) + newptg, err := NewPTGIK(ptgGen, logger, simDist, 2) + if err != nil { + return err } + ptgs = append(ptgs, newptg) } } pf.ptgs = ptgs diff --git a/motionplan/ptgIK.go b/motionplan/ptgIK.go index 70210d8b27a..33fd208912a 100644 --- a/motionplan/ptgIK.go +++ b/motionplan/ptgIK.go @@ -13,7 +13,7 @@ import ( ) const ( - defaultDiffT = 0.01 // seconds + defaultDiffT = 0.01 // seconds. Return trajectories updating velocities at this resolution. nloptSeed = 42 // This should be fine to kepe constant defaultZeroDist = 1e-3 // Sometimes nlopt will minimize trajectories to zero. Ensure min traj dist is at least this diff --git a/motionplan/rrt.go b/motionplan/rrt.go index 3f333605b5d..45fb1bf9ad0 100644 --- a/motionplan/rrt.go +++ b/motionplan/rrt.go @@ -231,23 +231,8 @@ func extractPath(startMap, goalMap map[node]node, pair *nodePair, matched bool) goalReached = goalMap[goalReached] } - // ~ var lastNode *basicNode // extract the path to the goal for goalReached != nil { - // ~ fmt.Println("goal reached pose", goalReached.Q(), goalReached.Pose().Point()) - // special rewriting poses for inverted tree - // ~ if lastNode != nil { - // ~ lastNode.pose = goalReached.Pose() - // ~ } - - // ~ thisNode := &basicNode{ - // ~ q: goalReached.Q(), - // ~ cost: goalReached.Cost(), - // ~ pose: goalReached.Pose(), - // ~ corner: goalReached.Corner(), - // ~ } - //~ lastNode = thisNode - //~ path = append(path, thisNode) path = append(path, goalReached) goalReached = goalMap[goalReached] } diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index a32e311f710..a2cee315004 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -35,6 +35,7 @@ const ( // Consider nodes on trees to be connected if they are within this distance. defaultIdenticalNodeDistance = 5. + // When extending the RRT tree towards some point, do not extend more than this many times in a single RRT invocation. defaultMaxReseeds = 50 defaultSmoothScaleFactor = 0.5 @@ -337,6 +338,8 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( return nil, errNoNeighbors } } + // TODO: We could potentially improve solving by first getting the rough distance to the randPosNode to any point in the rrt tree, + // then dynamically expanding or contracting the limits of IK to be some fraction of that distance. // Get cartesian distance from NN to rand var targetFunc func(spatialmath.Pose) float64 @@ -532,7 +535,6 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( randAlpha := newNode.Q()[1].Value randDist := newNode.Q()[2].Value - // ~ fmt.Println("__ADDINGtraj", ptgNum, randAlpha, randDist) trajK, err := mp.tpFrame.PTGs()[ptgNum].Trajectory(randAlpha, randDist) if err != nil { return nil, err diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index b2dc264aa76..c55833f6ffe 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -29,9 +29,6 @@ type ptgGridSim struct { // If true, then CToTP calls will *only* check the furthest end of each precomputed trajectory. // This is useful when used in conjunction with IK endsOnly bool - - // Discretized x[y][]node maps for rapid NN lookups - trajNodeGrid map[int]map[int][]*TrajNode } // NewPTGGridSim creates a new PTG by simulating a PrecomputePTG for some distance, then cacheing the results in a grid for fast lookup. @@ -46,8 +43,6 @@ func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64, endsOnly bo maxTime: defaultMaxTime, diffT: defaultDiffT, endsOnly: endsOnly, - - trajNodeGrid: map[int]map[int][]*TrajNode{}, } ptg.simPTG = simPTG @@ -118,19 +113,6 @@ func (ptg *ptgGridSim) simulateTrajectories(simPTG PrecomputePTG) ([][]*TrajNode if err != nil { return nil, err } - - if !ptg.endsOnly { - for _, tNode := range alphaTraj { - gridX := int(math.Round(tNode.ptX)) - gridY := int(math.Round(tNode.ptY)) - // Discretize into a grid for faster lookups later - if _, ok := ptg.trajNodeGrid[gridX]; !ok { - ptg.trajNodeGrid[gridX] = map[int][]*TrajNode{} - } - ptg.trajNodeGrid[gridX][gridY] = append(ptg.trajNodeGrid[gridX][gridY], tNode) - } - } - allTraj = append(allTraj, alphaTraj) } From 8583ec654f86d72eb4dbffcf443c79c2e67585c7 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 16 Aug 2023 14:48:36 -0700 Subject: [PATCH 23/38] RDK is on older Go than my dev machine --- motionplan/tpSpaceRRT.go | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index a2cee315004..30af6ab826a 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -11,6 +11,7 @@ import ( "github.com/edaniels/golog" "github.com/golang/geo/r3" + "go.uber.org/multierr" "go.viam.com/utils" "go.viam.com/rdk/motionplan/tpspace" @@ -238,7 +239,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( seedMapNode := seedMapReached.node goalMapNode := goalMapReached.node - err := errors.Join(seedMapReached.error, goalMapReached.error) + err := multierr.Combine(seedMapReached.error, goalMapReached.error) if err != nil { rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} return From 4fdc37d6d0c3e603da7c076ff3d575f7173afb91 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 16 Aug 2023 15:21:49 -0700 Subject: [PATCH 24/38] Fix err join --- components/base/kinematicbase/ptgKinematics.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index e3b46b58025..ff381cb9780 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -126,7 +126,7 @@ func (ptgk *ptgBaseKinematics) GoToInputs(ctx context.Context, inputs []referenc selectedPTG := ptgk.ptgs[int(math.Round(inputs[ptgIndex].Value))] selectedTraj, err := selectedPTG.Trajectory(inputs[trajectoryIndexWithinPTG].Value, inputs[distanceAlongTrajectoryIndex].Value) if err != nil { - return errors.Join(err, ptgk.Base.Stop(ctx, nil)) + return multierr.Combine(err, ptgk.Base.Stop(ctx, nil)) } lastTime := 0. From 95b565bd1ac34c8f39a5221cd4e079aff41c9cf3 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 16 Aug 2023 17:46:07 -0700 Subject: [PATCH 25/38] Fix solving syncgroups --- motionplan/combinedInverseKinematics.go | 5 ++++- motionplan/motionPlanner.go | 7 ++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/motionplan/combinedInverseKinematics.go b/motionplan/combinedInverseKinematics.go index fcaa0a4cc06..c0f67ffbfba 100644 --- a/motionplan/combinedInverseKinematics.go +++ b/motionplan/combinedInverseKinematics.go @@ -63,7 +63,7 @@ func (ik *CombinedIK) Solve(ctx context.Context, ctxWithCancel, cancel := context.WithCancel(ctx) defer cancel() - errChan := make(chan error, len(ik.solvers)) + errChan := make(chan error, len(ik.solvers)+1) var activeSolvers sync.WaitGroup defer activeSolvers.Wait() activeSolvers.Add(len(ik.solvers)) @@ -89,6 +89,7 @@ func (ik *CombinedIK) Solve(ctx context.Context, for !done { select { case <-ctx.Done(): + activeSolvers.Wait() return ctx.Err() default: } @@ -110,6 +111,7 @@ func (ik *CombinedIK) Solve(ctx context.Context, // Collect return errors from all solvers select { case <-ctx.Done(): + activeSolvers.Wait() return ctx.Err() default: } @@ -120,6 +122,7 @@ func (ik *CombinedIK) Solve(ctx context.Context, collectedErrs = multierr.Combine(collectedErrs, err) } } + activeSolvers.Wait() return collectedErrs } diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 396c94ff1c6..d1c47a9ed58 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -5,6 +5,7 @@ import ( "context" "math/rand" "sort" + "sync" "time" "github.com/edaniels/golog" @@ -265,11 +266,15 @@ func (mp *planner) getSolutions(ctx context.Context, seed []frame.Input) ([]node ctxWithCancel, cancel := context.WithCancel(ctx) defer cancel() - solutionGen := make(chan *IKSolution) + solutionGen := make(chan *IKSolution, mp.planOpts.NumThreads) ikErr := make(chan error, 1) + var activeSolvers sync.WaitGroup + defer activeSolvers.Wait() + activeSolvers.Add(1) // Spawn the IK solver to generate solutions until done utils.PanicCapturingGo(func() { defer close(ikErr) + defer activeSolvers.Done() ikErr <- mp.solver.Solve(ctxWithCancel, solutionGen, seed, mp.planOpts.goalMetric, mp.randseed.Int()) }) From 5f242a04d925b29763fb26dbbb71ac3f30eadc58 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Tue, 22 Aug 2023 12:47:23 -0700 Subject: [PATCH 26/38] Move IK to its own package --- motionplan/cBiRRT.go | 27 +-- motionplan/cBiRRT_test.go | 5 +- motionplan/constraint.go | 193 ++++++++---------- motionplan/constraint_test.go | 17 +- .../{ => ik}/combinedInverseKinematics.go | 4 +- .../combinedInverseKinematics_windows.go | 2 +- motionplan/{ => ik}/inverseKinematics.go | 12 +- motionplan/{ => ik}/kinematic.go | 2 +- motionplan/{ => ik}/kinematic_test.go | 2 +- motionplan/{ => ik}/metrics.go | 39 ++-- motionplan/{ => ik}/metrics_test.go | 2 +- motionplan/{ => ik}/nloptInverseKinematics.go | 33 ++- .../{ => ik}/nloptInverseKinematics_test.go | 4 +- motionplan/motionPlanner.go | 17 +- motionplan/motionPlanner_test.go | 17 +- motionplan/nearestNeighbor.go | 7 +- motionplan/nearestNeighbor_test.go | 4 + motionplan/planManager.go | 7 +- motionplan/plannerOptions.go | 27 ++- motionplan/ptgIK.go | 141 ------------- motionplan/rrt.go | 5 +- motionplan/rrtStarConnect.go | 9 +- motionplan/tpSpaceRRT.go | 111 ++++++---- motionplan/tpSpaceRRT_test.go | 19 +- motionplan/tpspace/ptg.go | 20 +- motionplan/tpspace/ptgGridSim.go | 42 ++-- motionplan/tpspace/ptgGridSim_test.go | 9 - motionplan/{ => tpspace}/ptgGroupFrame.go | 57 ++++-- motionplan/tpspace/ptgIK.go | 152 ++++++++++++++ motionplan/utils.go | 5 +- 30 files changed, 539 insertions(+), 452 deletions(-) rename motionplan/{ => ik}/combinedInverseKinematics.go (96%) rename motionplan/{ => ik}/combinedInverseKinematics_windows.go (95%) rename motionplan/{ => ik}/inverseKinematics.go (80%) rename motionplan/{ => ik}/kinematic.go (99%) rename motionplan/{ => ik}/kinematic_test.go (99%) rename motionplan/{ => ik}/metrics.go (79%) rename motionplan/{ => ik}/metrics_test.go (98%) rename motionplan/{ => ik}/nloptInverseKinematics.go (92%) rename motionplan/{ => ik}/nloptInverseKinematics_test.go (92%) delete mode 100644 motionplan/ptgIK.go rename motionplan/{ => tpspace}/ptgGroupFrame.go (77%) create mode 100644 motionplan/tpspace/ptgIK.go diff --git a/motionplan/cBiRRT.go b/motionplan/cBiRRT.go index faf00f64451..e1a0bde2865 100644 --- a/motionplan/cBiRRT.go +++ b/motionplan/cBiRRT.go @@ -13,6 +13,7 @@ import ( "github.com/edaniels/golog" "go.viam.com/utils" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -55,7 +56,7 @@ func newCbirrtOptions(planOpts *plannerOptions) (*cbirrtOptions, error) { // https://ieeexplore.ieee.org/document/5152399/ type cBiRRTMotionPlanner struct { *planner - fastGradDescent *NloptIK + fastGradDescent *ik.NloptIK algOpts *cbirrtOptions } @@ -74,7 +75,7 @@ func newCBiRRTMotionPlanner( return nil, err } // nlopt should try only once - nlopt, err := CreateNloptIKSolver(frame, logger, 1, opt.GoalThreshold, false) + nlopt, err := ik.CreateNloptIKSolver(frame, logger, 1, true) if err != nil { return nil, err } @@ -206,7 +207,7 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( return } - reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) + reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) // Second iteration; extend maps 1 and 2 towards the halfway point between where they reached if reachedDelta > mp.planOpts.JointSolveDist { @@ -216,7 +217,7 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} return } - reachedDelta = mp.planOpts.DistanceFunc(&Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) + reachedDelta = mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) } // Solved! @@ -268,8 +269,8 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend( default: } - dist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: near.Q(), EndConfiguration: target.Q()}) - oldDist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: oldNear.Q(), EndConfiguration: target.Q()}) + dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: near.Q(), EndConfiguration: target.Q()}) + oldDist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: oldNear.Q(), EndConfiguration: target.Q()}) switch { case dist < mp.planOpts.JointSolveDist: mchan <- near @@ -286,7 +287,7 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend( newNear = mp.constrainNear(ctx, randseed, oldNear.Q(), newNear) if newNear != nil { - nearDist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: oldNear.Q(), EndConfiguration: newNear}) + nearDist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: oldNear.Q(), EndConfiguration: newNear}) if nearDist < math.Pow(mp.planOpts.JointSolveDist, 3) { if !doubled { doubled = true @@ -343,7 +344,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear( return nil } - newArc := &Segment{ + newArc := &ik.Segment{ StartPosition: seedPos, EndPosition: goalPos, StartConfiguration: seedInputs, @@ -356,11 +357,11 @@ func (mp *cBiRRTMotionPlanner) constrainNear( if ok { return target } - solutionGen := make(chan *IKSolution, 1) + solutionGen := make(chan *ik.IKSolution, 1) // Spawn the IK solver to generate solutions until done err = mp.fastGradDescent.Solve(ctx, solutionGen, target, mp.planOpts.pathMetric, randseed.Int()) // We should have zero or one solutions - var solved *IKSolution + var solved *ik.IKSolution select { case solved = <-solutionGen: default: @@ -371,14 +372,14 @@ func (mp *cBiRRTMotionPlanner) constrainNear( } ok, failpos := mp.planOpts.CheckSegmentAndStateValidity( - &Segment{StartConfiguration: seedInputs, EndConfiguration: solved.Configuration, Frame: mp.frame}, + &ik.Segment{StartConfiguration: seedInputs, EndConfiguration: solved.Configuration, Frame: mp.frame}, mp.planOpts.Resolution, ) if ok { return solved.Configuration } if failpos != nil { - dist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: target, EndConfiguration: failpos.EndConfiguration}) + dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: target, EndConfiguration: failpos.EndConfiguration}) if dist > mp.planOpts.JointSolveDist { // If we have a first failing position, and that target is updating (no infinite loop), then recurse seedInputs = failpos.StartConfiguration @@ -439,7 +440,7 @@ func (mp *cBiRRTMotionPlanner) smoothPath( // Note this could technically replace paths with "longer" paths i.e. with more waypoints. // However, smoothed paths are invariably more intuitive and smooth, and lend themselves to future shortening, // so we allow elongation here. - dist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: inputSteps[i].Q(), EndConfiguration: reached.Q()}) + dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: inputSteps[i].Q(), EndConfiguration: reached.Q()}) if dist < mp.planOpts.JointSolveDist { for _, hitCorner := range hitCorners { hitCorner.SetCorner(false) diff --git a/motionplan/cBiRRT_test.go b/motionplan/cBiRRT_test.go index 8c466918b78..6db3f5d9a5e 100644 --- a/motionplan/cBiRRT_test.go +++ b/motionplan/cBiRRT_test.go @@ -10,6 +10,7 @@ import ( "go.viam.com/test" "go.viam.com/utils" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" rutils "go.viam.com/rdk/utils" @@ -38,7 +39,7 @@ func TestSimpleLinearMotion(t *testing.T) { goalPos := spatialmath.NewPose(r3.Vector{X: 206, Y: 100, Z: 120.5}, &spatialmath.OrientationVectorDegrees{OY: -1}) opt := newBasicPlannerOptions(m) - opt.SetGoalMetric(NewSquaredNormMetric(goalPos)) + opt.SetGoalMetric(ik.NewSquaredNormMetric(goalPos)) mp, err := newCBiRRTMotionPlanner(m, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) cbirrt, _ := mp.(*cBiRRTMotionPlanner) @@ -80,7 +81,7 @@ func TestSimpleLinearMotion(t *testing.T) { cbirrt.constrainedExtend(ctx, cbirrt.randseed, goalMap, near2, seedReached, m1chan) }) goalReached := <-m1chan - dist := opt.DistanceFunc(&Segment{StartConfiguration: seedReached.Q(), EndConfiguration: goalReached.Q()}) + dist := opt.DistanceFunc(&ik.Segment{StartConfiguration: seedReached.Q(), EndConfiguration: goalReached.Q()}) test.That(t, dist < cbirrt.planOpts.JointSolveDist, test.ShouldBeTrue) seedReached.SetCorner(true) diff --git a/motionplan/constraint.go b/motionplan/constraint.go index 49358eacc43..623be1dc434 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -8,95 +8,18 @@ import ( pb "go.viam.com/api/service/motion/v1" "go.viam.com/rdk/pointcloud" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" spatial "go.viam.com/rdk/spatialmath" ) -// Segment contains all the information a constraint needs to determine validity for a movement. -// It contains the starting inputs, the ending inputs, corresponding poses, and the frame it refers to. -// Pose fields may be empty, and may be filled in by a constraint that needs them. -type Segment struct { - StartPosition spatial.Pose - EndPosition spatial.Pose - StartConfiguration []referenceframe.Input - EndConfiguration []referenceframe.Input - Frame referenceframe.Frame -} - -// Given a constraint input with only frames and input positions, calculates the corresponding poses as needed. -func resolveSegmentsToPositions(segment *Segment) error { - if segment.StartPosition == nil { - if segment.Frame != nil { - if segment.StartConfiguration != nil { - pos, err := segment.Frame.Transform(segment.StartConfiguration) - if err == nil { - segment.StartPosition = pos - } else { - return err - } - } else { - return errors.New("invalid constraint input") - } - } else { - return errors.New("invalid constraint input") - } - } - if segment.EndPosition == nil { - if segment.Frame != nil { - if segment.EndConfiguration != nil { - pos, err := segment.Frame.Transform(segment.EndConfiguration) - if err == nil { - segment.EndPosition = pos - } else { - return err - } - } else { - return errors.New("invalid constraint input") - } - } else { - return errors.New("invalid constraint input") - } - } - return nil -} - -// State contains all the information a constraint needs to determine validity for a movement. -// It contains the starting inputs, the ending inputs, corresponding poses, and the frame it refers to. -// Pose fields may be empty, and may be filled in by a constraint that needs them. -type State struct { - Position spatial.Pose - Configuration []referenceframe.Input - Frame referenceframe.Frame -} - -// Given a constraint input with only frames and input positions, calculates the corresponding poses as needed. -func resolveStatesToPositions(state *State) error { - if state.Position == nil { - if state.Frame != nil { - if state.Configuration != nil { - pos, err := state.Frame.Transform(state.Configuration) - if err == nil { - state.Position = pos - } else { - return err - } - } else { - return errors.New("invalid constraint input") - } - } else { - return errors.New("invalid constraint input") - } - } - return nil -} - // SegmentConstraint tests whether a transition from a starting robot configuration to an ending robot configuration is valid. // If the returned bool is true, the constraint is satisfied and the segment is valid. -type SegmentConstraint func(*Segment) bool +type SegmentConstraint func(*ik.Segment) bool // StateConstraint tests whether a given robot configuration is valid // If the returned bool is true, the constraint is satisfied and the state is valid. -type StateConstraint func(*State) bool +type StateConstraint func(*ik.State) bool // ConstraintHandler is a convenient wrapper for constraint handling which is likely to be common among most motion // planners. Including a constraint handler as an anonymous struct member allows reuse. @@ -109,7 +32,7 @@ type ConstraintHandler struct { // Return values are: // -- a bool representing whether all constraints passed // -- if failing, a string naming the failed constraint. -func (c *ConstraintHandler) CheckStateConstraints(state *State) (bool, string) { +func (c *ConstraintHandler) CheckStateConstraints(state *ik.State) (bool, string) { for name, cFunc := range c.stateConstraints { pass := cFunc(state) if !pass { @@ -123,7 +46,7 @@ func (c *ConstraintHandler) CheckStateConstraints(state *State) (bool, string) { // Return values are: // -- a bool representing whether all constraints passed // -- if failing, a string naming the failed constraint. -func (c *ConstraintHandler) CheckSegmentConstraints(segment *Segment) (bool, string) { +func (c *ConstraintHandler) CheckSegmentConstraints(segment *ik.Segment) (bool, string) { for name, cFunc := range c.segmentConstraints { pass := cFunc(segment) if !pass { @@ -137,7 +60,7 @@ func (c *ConstraintHandler) CheckSegmentConstraints(segment *Segment) (bool, str // states as well as both endpoints satisfy all state constraints. If all constraints are satisfied, then this will return `true, nil`. // If any constraints fail, this will return false, and an Segment representing the valid portion of the segment, if any. If no // part of the segment is valid, then `false, nil` is returned. -func (c *ConstraintHandler) CheckStateConstraintsAcrossSegment(ci *Segment, resolution float64) (bool, *Segment) { +func (c *ConstraintHandler) CheckStateConstraintsAcrossSegment(ci *ik.Segment, resolution float64) (bool, *ik.Segment) { // ensure we have cartesian positions err := resolveSegmentsToPositions(ci) if err != nil { @@ -150,7 +73,7 @@ func (c *ConstraintHandler) CheckStateConstraintsAcrossSegment(ci *Segment, reso for i := 0; i <= steps; i++ { interp := float64(i) / float64(steps) interpConfig := referenceframe.InterpolateInputs(ci.StartConfiguration, ci.EndConfiguration, interp) - interpC := &State{Frame: ci.Frame, Configuration: interpConfig} + interpC := &ik.State{Frame: ci.Frame, Configuration: interpConfig} err = resolveStatesToPositions(interpC) if err != nil { return false, nil @@ -161,7 +84,7 @@ func (c *ConstraintHandler) CheckStateConstraintsAcrossSegment(ci *Segment, reso // fail on start pos return false, nil } - return false, &Segment{StartConfiguration: ci.StartConfiguration, EndConfiguration: lastGood} + return false, &ik.Segment{StartConfiguration: ci.StartConfiguration, EndConfiguration: lastGood} } lastGood = interpC.Configuration } @@ -172,7 +95,7 @@ func (c *ConstraintHandler) CheckStateConstraintsAcrossSegment(ci *Segment, reso // CheckSegmentAndStateValidity will check an segment input and confirm that it 1) meets all segment constraints, and 2) meets all // state constraints across the segment at some resolution. If it fails an intermediate state, it will return the shortest valid segment, // provided that segment also meets segment constraints. -func (c *ConstraintHandler) CheckSegmentAndStateValidity(segment *Segment, resolution float64) (bool, *Segment) { +func (c *ConstraintHandler) CheckSegmentAndStateValidity(segment *ik.Segment, resolution float64) (bool, *ik.Segment) { valid, subSegment := c.CheckStateConstraintsAcrossSegment(segment, resolution) if !valid { if subSegment != nil { @@ -336,7 +259,7 @@ func newCollisionConstraint( } // create constraint from reference collision graph - constraint := func(state *State) bool { + constraint := func(state *ik.State) bool { var internalGeoms []spatial.Geometry switch { case state.Configuration != nil: @@ -373,12 +296,12 @@ func newCollisionConstraint( // NewAbsoluteLinearInterpolatingConstraint provides a Constraint whose valid manifold allows a specified amount of deviation from the // shortest straight-line path between the start and the goal. linTol is the allowed linear deviation in mm, orientTol is the allowed // orientation deviation measured by norm of the R3AA orientation difference to the slerp path between start/goal orientations. -func NewAbsoluteLinearInterpolatingConstraint(from, to spatial.Pose, linTol, orientTol float64) (StateConstraint, StateMetric) { +func NewAbsoluteLinearInterpolatingConstraint(from, to spatial.Pose, linTol, orientTol float64) (StateConstraint, ik.StateMetric) { orientConstraint, orientMetric := NewSlerpOrientationConstraint(from, to, orientTol) lineConstraint, lineMetric := NewLineConstraint(from.Point(), to.Point(), linTol) - interpMetric := CombineMetrics(orientMetric, lineMetric) + interpMetric := ik.CombineMetrics(orientMetric, lineMetric) - f := func(state *State) bool { + f := func(state *ik.State) bool { return orientConstraint(state) && lineConstraint(state) } return f, interpMetric @@ -386,8 +309,8 @@ func NewAbsoluteLinearInterpolatingConstraint(from, to spatial.Pose, linTol, ori // NewProportionalLinearInterpolatingConstraint will provide the same metric and constraint as NewAbsoluteLinearInterpolatingConstraint, // except that allowable linear and orientation deviation is scaled based on the distance from start to goal. -func NewProportionalLinearInterpolatingConstraint(from, to spatial.Pose, epsilon float64) (StateConstraint, StateMetric) { - orientTol := epsilon * orientDist(from.Orientation(), to.Orientation()) +func NewProportionalLinearInterpolatingConstraint(from, to spatial.Pose, epsilon float64) (StateConstraint, ik.StateMetric) { + orientTol := epsilon * ik.OrientDist(from.Orientation(), to.Orientation()) linTol := epsilon * from.Point().Distance(to.Point()) return NewAbsoluteLinearInterpolatingConstraint(from, to, linTol, orientTol) @@ -396,22 +319,22 @@ func NewProportionalLinearInterpolatingConstraint(from, to spatial.Pose, epsilon // NewSlerpOrientationConstraint will measure the orientation difference between the orientation of two poses, and return a constraint that // returns whether a given orientation is within a given tolerance distance of the shortest segment between the two orientations, as // well as a metric which returns the distance to that valid region. -func NewSlerpOrientationConstraint(start, goal spatial.Pose, tolerance float64) (StateConstraint, StateMetric) { - origDist := math.Max(orientDist(start.Orientation(), goal.Orientation()), defaultEpsilon) +func NewSlerpOrientationConstraint(start, goal spatial.Pose, tolerance float64) (StateConstraint, ik.StateMetric) { + origDist := math.Max(ik.OrientDist(start.Orientation(), goal.Orientation()), defaultEpsilon) - gradFunc := func(state *State) float64 { - sDist := orientDist(start.Orientation(), state.Position.Orientation()) + gradFunc := func(state *ik.State) float64 { + sDist := ik.OrientDist(start.Orientation(), state.Position.Orientation()) gDist := 0. // If origDist is less than or equal to defaultEpsilon, then the starting and ending orientations are the same and we do not need // to compute the distance to the ending orientation if origDist > defaultEpsilon { - gDist = orientDist(goal.Orientation(), state.Position.Orientation()) + gDist = ik.OrientDist(goal.Orientation(), state.Position.Orientation()) } return (sDist + gDist) - origDist } - validFunc := func(state *State) bool { + validFunc := func(state *ik.State) bool { err := resolveStatesToPositions(state) if err != nil { return false @@ -427,7 +350,7 @@ func NewSlerpOrientationConstraint(start, goal spatial.Pose, tolerance float64) // which will bring a pose into the valid constraint space. The plane normal is assumed to point towards the valid area. // angle refers to the maximum unit sphere segment length deviation from the ov // epsilon refers to the closeness to the plane necessary to be a valid pose. -func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (StateConstraint, StateMetric) { +func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (StateConstraint, ik.StateMetric) { // get the constant value for the plane pConst := -pt.Dot(pNorm) @@ -435,7 +358,7 @@ func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (Sta ov := &spatial.OrientationVector{OX: -pNorm.X, OY: -pNorm.Y, OZ: -pNorm.Z} ov.Normalize() - dFunc := orientDistToRegion(ov, writingAngle) + dFunc := ik.OrientDistToRegion(ov, writingAngle) // distance from plane to point planeDist := func(pt r3.Vector) float64 { @@ -443,13 +366,13 @@ func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (Sta } // TODO: do we need to care about trajectory here? Probably, but not yet implemented - gradFunc := func(state *State) float64 { + gradFunc := func(state *ik.State) float64 { pDist := planeDist(state.Position.Point()) oDist := dFunc(state.Position.Orientation()) return pDist*pDist + oDist*oDist } - validFunc := func(state *State) bool { + validFunc := func(state *ik.State) bool { err := resolveStatesToPositions(state) if err != nil { return false @@ -464,16 +387,16 @@ func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (Sta // function which will determine whether a point is on the line, and 2) a distance function // which will bring a pose into the valid constraint space. // tolerance refers to the closeness to the line necessary to be a valid pose in mm. -func NewLineConstraint(pt1, pt2 r3.Vector, tolerance float64) (StateConstraint, StateMetric) { +func NewLineConstraint(pt1, pt2 r3.Vector, tolerance float64) (StateConstraint, ik.StateMetric) { if pt1.Distance(pt2) < defaultEpsilon { tolerance = defaultEpsilon } - gradFunc := func(state *State) float64 { + gradFunc := func(state *ik.State) float64 { return math.Max(spatial.DistToLineSegment(pt1, pt2, state.Position.Point())-tolerance, 0) } - validFunc := func(state *State) bool { + validFunc := func(state *ik.State) bool { err := resolveStatesToPositions(state) if err != nil { return false @@ -488,7 +411,7 @@ func NewLineConstraint(pt1, pt2 r3.Vector, tolerance float64) (StateConstraint, // intersect with points in the octree. Threshold sets the confidence level required for a point to be considered, and buffer is the // distance to a point that is considered a collision in mm. func NewOctreeCollisionConstraint(octree *pointcloud.BasicOctree, threshold int, buffer float64) StateConstraint { - constraint := func(state *State) bool { + constraint := func(state *ik.State) bool { geometries, err := state.Frame.Geometries(state.Configuration) if err != nil && geometries == nil { return false @@ -504,3 +427,61 @@ func NewOctreeCollisionConstraint(octree *pointcloud.BasicOctree, threshold int, } return constraint } + +// Given a constraint input with only frames and input positions, calculates the corresponding poses as needed. +func resolveStatesToPositions(state *ik.State) error { + if state.Position == nil { + if state.Frame != nil { + if state.Configuration != nil { + pos, err := state.Frame.Transform(state.Configuration) + if err == nil { + state.Position = pos + } else { + return err + } + } else { + return errors.New("invalid constraint input") + } + } else { + return errors.New("invalid constraint input") + } + } + return nil +} + +// Given a constraint input with only frames and input positions, calculates the corresponding poses as needed. +func resolveSegmentsToPositions(segment *ik.Segment) error { + if segment.StartPosition == nil { + if segment.Frame != nil { + if segment.StartConfiguration != nil { + pos, err := segment.Frame.Transform(segment.StartConfiguration) + if err == nil { + segment.StartPosition = pos + } else { + return err + } + } else { + return errors.New("invalid constraint input") + } + } else { + return errors.New("invalid constraint input") + } + } + if segment.EndPosition == nil { + if segment.Frame != nil { + if segment.EndConfiguration != nil { + pos, err := segment.Frame.Transform(segment.EndConfiguration) + if err == nil { + segment.EndPosition = pos + } else { + return err + } + } else { + return errors.New("invalid constraint input") + } + } else { + return errors.New("invalid constraint input") + } + } + return nil +} diff --git a/motionplan/constraint_test.go b/motionplan/constraint_test.go index 54ff5034970..ba1920d6e28 100644 --- a/motionplan/constraint_test.go +++ b/motionplan/constraint_test.go @@ -12,6 +12,7 @@ import ( commonpb "go.viam.com/api/common/v1" "go.viam.com/test" + "go.viam.com/rdk/motionplan/ik" frame "go.viam.com/rdk/referenceframe" spatial "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/utils" @@ -39,7 +40,7 @@ func TestIKTolerances(t *testing.T) { // Now verify that setting tolerances to zero allows the same arm to reach that position opt := newBasicPlannerOptions(m) - opt.SetGoalMetric(NewPositionOnlyMetric(pos)) + opt.SetGoalMetric(ik.NewPositionOnlyMetric(pos)) opt.SetMaxSolutions(50) mp, err = newCBiRRTMotionPlanner(m, rand.New(rand.NewSource(1)), logger, opt) test.That(t, err, test.ShouldBeNil) @@ -54,7 +55,7 @@ func TestConstraintPath(t *testing.T) { modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") test.That(t, err, test.ShouldBeNil) - ci := &Segment{StartConfiguration: homePos, EndConfiguration: toPos, Frame: modelXarm} + ci := &ik.Segment{StartConfiguration: homePos, EndConfiguration: toPos, Frame: modelXarm} err = resolveSegmentsToPositions(ci) test.That(t, err, test.ShouldBeNil) @@ -75,7 +76,7 @@ func TestConstraintPath(t *testing.T) { test.That(t, len(handler.StateConstraints()), test.ShouldEqual, 1) badInterpPos := frame.FloatsToInputs([]float64{6.2, 0, 0, 0, 0, 0}) - ciBad := &Segment{StartConfiguration: homePos, EndConfiguration: badInterpPos, Frame: modelXarm} + ciBad := &ik.Segment{StartConfiguration: homePos, EndConfiguration: badInterpPos, Frame: modelXarm} err = resolveSegmentsToPositions(ciBad) test.That(t, err, test.ShouldBeNil) ok, failCI = handler.CheckSegmentAndStateValidity(ciBad, 0.5) @@ -133,7 +134,7 @@ func TestLineFollow(t *testing.T) { validFunc, gradFunc := NewLineConstraint(p1.Point(), p2.Point(), 0.001) - pointGrad := gradFunc(&State{Position: query}) + pointGrad := gradFunc(&ik.State{Position: query}) test.That(t, pointGrad, test.ShouldBeLessThan, 0.001*0.001) fs := frame.NewEmptyFrameSystem("test") @@ -159,7 +160,7 @@ func TestLineFollow(t *testing.T) { opt.AddStateConstraint("whiteboard", validFunc) ok, lastGood := opt.CheckSegmentAndStateValidity( - &Segment{ + &ik.Segment{ StartConfiguration: sf.InputFromProtobuf(mp1), EndConfiguration: sf.InputFromProtobuf(mp2), Frame: sf, @@ -169,7 +170,7 @@ func TestLineFollow(t *testing.T) { test.That(t, ok, test.ShouldBeFalse) // lastGood.StartConfiguration and EndConfiguration should pass constraints lastGood.Frame = sf - stateCheck := &State{Configuration: lastGood.StartConfiguration, Frame: lastGood.Frame} + stateCheck := &ik.State{Configuration: lastGood.StartConfiguration, Frame: lastGood.Frame} pass, _ := opt.CheckStateConstraints(stateCheck) test.That(t, pass, test.ShouldBeTrue) @@ -226,7 +227,7 @@ func TestCollisionConstraints(t *testing.T) { // loop through cases and check constraint handler processes them correctly for i, c := range cases { t.Run(fmt.Sprintf("Test %d", i), func(t *testing.T) { - response, failName := handler.CheckStateConstraints(&State{Configuration: c.input, Frame: model}) + response, failName := handler.CheckStateConstraints(&ik.State{Configuration: c.input, Frame: model}) test.That(t, response, test.ShouldEqual, c.expected) test.That(t, failName, test.ShouldEqual, c.failName) }) @@ -266,7 +267,7 @@ func BenchmarkCollisionConstraints(b *testing.B) { // loop through cases and check constraint handler processes them correctly for n = 0; n < b.N; n++ { rfloats := frame.GenerateRandomConfiguration(model, rseed) - b1, _ = handler.CheckStateConstraints(&State{Configuration: frame.FloatsToInputs(rfloats), Frame: model}) + b1, _ = handler.CheckStateConstraints(&ik.State{Configuration: frame.FloatsToInputs(rfloats), Frame: model}) } bt = b1 } diff --git a/motionplan/combinedInverseKinematics.go b/motionplan/ik/combinedInverseKinematics.go similarity index 96% rename from motionplan/combinedInverseKinematics.go rename to motionplan/ik/combinedInverseKinematics.go index c0f67ffbfba..f44a94d9bf8 100644 --- a/motionplan/combinedInverseKinematics.go +++ b/motionplan/ik/combinedInverseKinematics.go @@ -1,6 +1,6 @@ //go:build !windows -package motionplan +package ik import ( "context" @@ -30,7 +30,7 @@ func CreateCombinedIKSolver(model referenceframe.Frame, logger golog.Logger, nCP nCPU = 1 } for i := 1; i <= nCPU; i++ { - nlopt, err := CreateNloptIKSolver(model, logger, -1, goalThreshold, false) + nlopt, err := CreateNloptIKSolver(model, logger, -1, true) nlopt.id = i if err != nil { return nil, err diff --git a/motionplan/combinedInverseKinematics_windows.go b/motionplan/ik/combinedInverseKinematics_windows.go similarity index 95% rename from motionplan/combinedInverseKinematics_windows.go rename to motionplan/ik/combinedInverseKinematics_windows.go index a95507c87dd..5475b9633d1 100644 --- a/motionplan/combinedInverseKinematics_windows.go +++ b/motionplan/ik/combinedInverseKinematics_windows.go @@ -1,6 +1,6 @@ //go:build windows -package motionplan +package ik import ( "github.com/edaniels/golog" diff --git a/motionplan/inverseKinematics.go b/motionplan/ik/inverseKinematics.go similarity index 80% rename from motionplan/inverseKinematics.go rename to motionplan/ik/inverseKinematics.go index 9bc36538c69..f58d51b4727 100644 --- a/motionplan/inverseKinematics.go +++ b/motionplan/ik/inverseKinematics.go @@ -1,4 +1,4 @@ -package motionplan +package ik import ( "context" @@ -6,6 +6,14 @@ import ( "go.viam.com/rdk/referenceframe" ) +const ( + // Default distance below which two distances are considered equal. + defaultEpsilon = 0.001 + + // default amount of closeness to get to the goal. + defaultGoalThreshold = defaultEpsilon * defaultEpsilon +) + // InverseKinematics defines an interface which, provided with seed inputs and a Metric to minimize to zero, will output all found // solutions to the provided channel until cancelled or otherwise completes. type InverseKinematics interface { @@ -18,7 +26,7 @@ type InverseKinematics interface { type IKSolution struct { Configuration []referenceframe.Input Score float64 - Partial bool + Exact bool } func limitsToArrays(limits []referenceframe.Limit) ([]float64, []float64) { diff --git a/motionplan/kinematic.go b/motionplan/ik/kinematic.go similarity index 99% rename from motionplan/kinematic.go rename to motionplan/ik/kinematic.go index 57f5b5e4588..dbc5cd3149a 100644 --- a/motionplan/kinematic.go +++ b/motionplan/ik/kinematic.go @@ -1,4 +1,4 @@ -package motionplan +package ik import ( "math" diff --git a/motionplan/kinematic_test.go b/motionplan/ik/kinematic_test.go similarity index 99% rename from motionplan/kinematic_test.go rename to motionplan/ik/kinematic_test.go index 00b14a2055c..a2e93add684 100644 --- a/motionplan/kinematic_test.go +++ b/motionplan/ik/kinematic_test.go @@ -1,4 +1,4 @@ -package motionplan +package ik import ( "context" diff --git a/motionplan/metrics.go b/motionplan/ik/metrics.go similarity index 79% rename from motionplan/metrics.go rename to motionplan/ik/metrics.go index eb9453c0ac1..0ac33119aa9 100644 --- a/motionplan/metrics.go +++ b/motionplan/ik/metrics.go @@ -1,4 +1,4 @@ -package motionplan +package ik import ( "math" @@ -10,6 +10,26 @@ import ( const orientationDistanceScaling = 10. +// Segment contains all the information a constraint needs to determine validity for a movement. +// It contains the starting inputs, the ending inputs, corresponding poses, and the frame it refers to. +// Pose fields may be empty, and may be filled in by a constraint that needs them. +type Segment struct { + StartPosition spatial.Pose + EndPosition spatial.Pose + StartConfiguration []referenceframe.Input + EndConfiguration []referenceframe.Input + Frame referenceframe.Frame +} + +// State contains all the information a constraint needs to determine validity for a movement. +// It contains the starting inputs, the ending inputs, corresponding poses, and the frame it refers to. +// Pose fields may be empty, and may be filled in by a constraint that needs them. +type State struct { + Position spatial.Pose + Configuration []referenceframe.Input + Frame referenceframe.Frame +} + // StateMetric are functions which, given a State, produces some score. Lower is better. // This is used for gradient descent to converge upon a goal pose, for example. type StateMetric func(*State) float64 @@ -42,15 +62,15 @@ func CombineMetrics(metrics ...StateMetric) StateMetric { return cm.combinedDist } -// orientDist returns the arclength between two orientations in degrees. -func orientDist(o1, o2 spatial.Orientation) float64 { +// OrientDist returns the arclength between two orientations in degrees. +func OrientDist(o1, o2 spatial.Orientation) float64 { return utils.RadToDeg(spatial.QuatToR4AA(spatial.OrientationBetween(o1, o2).Quaternion()).Theta) } // orientDistToRegion will return a function which will tell you how far the unit sphere component of an orientation // vector is from a region defined by a point and an arclength around it. The theta value of OV is disregarded. // This is useful, for example, in defining the set of acceptable angles of attack for writing on a whiteboard. -func orientDistToRegion(goal spatial.Orientation, alpha float64) func(spatial.Orientation) float64 { +func OrientDistToRegion(goal spatial.Orientation, alpha float64) func(spatial.Orientation) float64 { ov1 := goal.OrientationVectorRadians() return func(o spatial.Orientation) float64 { ov2 := o.OrientationVectorRadians() @@ -81,7 +101,7 @@ func NewSquaredNormMetric(goal spatial.Pose) StateMetric { // NewPoseFlexOVMetric will provide a distance function which will converge on a pose with an OV within an arclength of `alpha` // of the ov of the goal given. func NewPoseFlexOVMetric(goal spatial.Pose, alpha float64) StateMetric { - oDistFunc := orientDistToRegion(goal.Orientation(), alpha) + oDistFunc := OrientDistToRegion(goal.Orientation(), alpha) return func(state *State) float64 { pDist := state.Position.Point().Distance(goal.Point()) oDist := oDistFunc(state.Position.Orientation()) @@ -114,17 +134,12 @@ func L2InputMetric(segment *Segment) float64 { } // NewSquaredNormSegmentMetric returns a metric which will return the cartesian distance between the two positions. -func NewSquaredNormSegmentMetric() SegmentMetric { - return NewSquaredNormSegmentMetricWithScaling(orientationDistanceScaling) -} - -// NewSquaredNormSegmentMetricWithScaling returns a metric which will return the cartesian distance between the two positions. // It allows the caller to choose the scaling level of orientation. -func NewSquaredNormSegmentMetricWithScaling(scaleFactor float64) SegmentMetric { +func NewSquaredNormSegmentMetric(orientationScaleFactor float64) SegmentMetric { return func(segment *Segment) float64 { delta := spatial.PoseDelta(segment.StartPosition, segment.EndPosition) // Increase weight for orientation since it's a small number - return delta.Point().Norm2() + spatial.QuatToR3AA(delta.Orientation().Quaternion()).Mul(scaleFactor).Norm2() + return delta.Point().Norm2() + spatial.QuatToR3AA(delta.Orientation().Quaternion()).Mul(orientationScaleFactor).Norm2() } } diff --git a/motionplan/metrics_test.go b/motionplan/ik/metrics_test.go similarity index 98% rename from motionplan/metrics_test.go rename to motionplan/ik/metrics_test.go index 89fb7f6dd56..d41b470d769 100644 --- a/motionplan/metrics_test.go +++ b/motionplan/ik/metrics_test.go @@ -1,4 +1,4 @@ -package motionplan +package ik import ( "math" diff --git a/motionplan/nloptInverseKinematics.go b/motionplan/ik/nloptInverseKinematics.go similarity index 92% rename from motionplan/nloptInverseKinematics.go rename to motionplan/ik/nloptInverseKinematics.go index 6a0512b0c87..25bfeb7827a 100644 --- a/motionplan/nloptInverseKinematics.go +++ b/motionplan/ik/nloptInverseKinematics.go @@ -1,6 +1,6 @@ //go:build !windows -package motionplan +package ik import ( "context" @@ -34,28 +34,27 @@ type NloptIK struct { lowerBound []float64 upperBound []float64 maxIterations int - epsilon float64 - solveEpsilon float64 + epsilon float64 logger golog.Logger jump float64 - // Nlopt will try to minimize a configuration for whatever is passed in. If partial is true, then the solver will emit partial + // Nlopt will try to minimize a configuration for whatever is passed in. If exact is false, then the solver will emit partial // solutions where it was not able to meet the goal criteria but still was able to improve upon the seed. - partial bool + exact bool } // CreateNloptIKSolver creates an nloptIK object that can perform gradient descent on metrics for Frames. The parameters are the Frame on // which Transform() will be called, a logger, and the number of iterations to run. If the iteration count is less than 1, it will be set // to the default of 5000. -func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int, solveEpsilon float64, partial bool) (*NloptIK, error) { +func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int, exact bool) (*NloptIK, error) { ik := &NloptIK{logger: logger} ik.model = mdl ik.id = 0 - // How close we want to get to the goal - ik.epsilon = defaultEpsilon + // Stop optimizing when iterations change by less than this much - ik.solveEpsilon = solveEpsilon + // Also, how close we want to get to the goal region. The metric should reflect any buffer. + ik.epsilon = defaultEpsilon * defaultEpsilon if iter < 1 { // default value iter = 5000 @@ -64,7 +63,7 @@ func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int ik.lowerBound, ik.upperBound = limitsToArrays(mdl.DoF()) // How much to adjust joints to determine slope ik.jump = 0.00000001 - ik.partial = partial + ik.exact = exact return ik, nil } @@ -139,13 +138,13 @@ func (ik *NloptIK) Solve(ctx context.Context, } err = multierr.Combine( - opt.SetFtolAbs(ik.solveEpsilon), - opt.SetFtolRel(ik.solveEpsilon), + opt.SetFtolAbs(ik.epsilon), + opt.SetFtolRel(ik.epsilon), opt.SetLowerBounds(ik.lowerBound), - opt.SetStopVal(ik.epsilon*ik.epsilon), + opt.SetStopVal(ik.epsilon), opt.SetUpperBounds(ik.upperBound), - opt.SetXtolAbs1(ik.solveEpsilon), - opt.SetXtolRel(ik.solveEpsilon), + opt.SetXtolAbs1(ik.epsilon), + opt.SetXtolRel(ik.epsilon), opt.SetMinObjective(nloptMinFunc), opt.SetMaxEval(nloptStepsPerIter), ) @@ -191,7 +190,7 @@ func (ik *NloptIK) Solve(ctx context.Context, err = multierr.Combine(err, nloptErr) } - if result < ik.epsilon*ik.epsilon || (solutionRaw != nil && ik.partial) { + if result < ik.epsilon || (solutionRaw != nil && !ik.exact) { select { case <-ctx.Done(): return err @@ -200,7 +199,7 @@ func (ik *NloptIK) Solve(ctx context.Context, solutionChan <- &IKSolution{ Configuration: referenceframe.FloatsToInputs(solutionRaw), Score: result, - Partial: result >= ik.epsilon*ik.epsilon, + Exact: result < ik.epsilon, } solutionsFound++ } diff --git a/motionplan/nloptInverseKinematics_test.go b/motionplan/ik/nloptInverseKinematics_test.go similarity index 92% rename from motionplan/nloptInverseKinematics_test.go rename to motionplan/ik/nloptInverseKinematics_test.go index 25095f95354..7e0fcbe9f48 100644 --- a/motionplan/nloptInverseKinematics_test.go +++ b/motionplan/ik/nloptInverseKinematics_test.go @@ -1,4 +1,4 @@ -package motionplan +package ik import ( "context" @@ -18,7 +18,7 @@ func TestCreateNloptIKSolver(t *testing.T) { logger := golog.NewTestLogger(t) m, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") test.That(t, err, test.ShouldBeNil) - ik, err := CreateNloptIKSolver(m, logger, -1, defaultGoalThreshold, false) + ik, err := CreateNloptIKSolver(m, logger, -1, false) test.That(t, err, test.ShouldBeNil) ik.id = 1 diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index d1c47a9ed58..bf4ed736fb5 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -13,6 +13,7 @@ import ( pb "go.viam.com/api/service/motion/v1" "go.viam.com/utils" + "go.viam.com/rdk/motionplan/ik" frame "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -144,7 +145,7 @@ func motionPlanInternal(ctx context.Context, } type planner struct { - solver InverseKinematics + solver ik.InverseKinematics frame frame.Frame logger golog.Logger randseed *rand.Rand @@ -153,12 +154,12 @@ type planner struct { } func newPlanner(frame frame.Frame, seed *rand.Rand, logger golog.Logger, opt *plannerOptions) (*planner, error) { - ik, err := CreateCombinedIKSolver(frame, logger, opt.NumThreads, opt.GoalThreshold) + solver, err := ik.CreateCombinedIKSolver(frame, logger, opt.NumThreads, opt.GoalThreshold) if err != nil { return nil, err } mp := &planner{ - solver: ik, + solver: solver, frame: frame, logger: logger, randseed: seed, @@ -168,7 +169,7 @@ func newPlanner(frame frame.Frame, seed *rand.Rand, logger golog.Logger, opt *pl } func (mp *planner) checkInputs(inputs []frame.Input) bool { - ok, _ := mp.planOpts.CheckStateConstraints(&State{ + ok, _ := mp.planOpts.CheckStateConstraints(&ik.State{ Configuration: inputs, Frame: mp.frame, }) @@ -177,7 +178,7 @@ func (mp *planner) checkInputs(inputs []frame.Input) bool { func (mp *planner) checkPath(seedInputs, target []frame.Input) bool { ok, _ := mp.planOpts.CheckSegmentAndStateValidity( - &Segment{ + &ik.Segment{ StartConfiguration: seedInputs, EndConfiguration: target, Frame: mp.frame, @@ -266,7 +267,7 @@ func (mp *planner) getSolutions(ctx context.Context, seed []frame.Input) ([]node ctxWithCancel, cancel := context.WithCancel(ctx) defer cancel() - solutionGen := make(chan *IKSolution, mp.planOpts.NumThreads) + solutionGen := make(chan *ik.IKSolution, mp.planOpts.NumThreads) ikErr := make(chan error, 1) var activeSolvers sync.WaitGroup defer activeSolvers.Wait() @@ -297,12 +298,12 @@ IK: case stepSolution := <-solutionGen: step := stepSolution.Configuration // Ensure the end state is a valid one - statePass, failName := mp.planOpts.CheckStateConstraints(&State{ + statePass, failName := mp.planOpts.CheckStateConstraints(&ik.State{ Configuration: step, Frame: mp.frame, }) if statePass { - stepArc := &Segment{ + stepArc := &ik.Segment{ StartConfiguration: seed, StartPosition: seedPos, EndConfiguration: step, diff --git a/motionplan/motionPlanner_test.go b/motionplan/motionPlanner_test.go index 290f2e9f57a..f1f7e614a13 100644 --- a/motionplan/motionPlanner_test.go +++ b/motionplan/motionPlanner_test.go @@ -13,6 +13,7 @@ import ( motionpb "go.viam.com/api/service/motion/v1" "go.viam.com/test" + "go.viam.com/rdk/motionplan/ik" frame "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/utils" @@ -96,17 +97,17 @@ func constrainedXArmMotion() (*planConfig, error) { pos := spatialmath.NewPoseFromProtobuf(&commonpb.Pose{X: -206, Y: 100, Z: 120, OZ: -1}) opt := newBasicPlannerOptions(model) - orientMetric := NewPoseFlexOVMetric(pos, 0.09) + orientMetric := ik.NewPoseFlexOVMetric(pos, 0.09) - oFunc := orientDistToRegion(pos.Orientation(), 0.1) - oFuncMet := func(from *State) float64 { + oFunc := ik.OrientDistToRegion(pos.Orientation(), 0.1) + oFuncMet := func(from *ik.State) float64 { err := resolveStatesToPositions(from) if err != nil { return math.Inf(1) } return oFunc(from.Position.Orientation()) } - orientConstraint := func(cInput *State) bool { + orientConstraint := func(cInput *ik.State) bool { err := resolveStatesToPositions(cInput) if err != nil { return false @@ -209,7 +210,7 @@ func simple2DMap() (*planConfig, error) { startInput := frame.StartPositions(fs) startInput[modelName] = frame.FloatsToInputs([]float64{-90., 90., 0}) goal := spatialmath.NewPoseFromPoint(r3.Vector{X: 90, Y: 90, Z: 0}) - opt.SetGoalMetric(NewSquaredNormMetric(goal)) + opt.SetGoalMetric(ik.NewSquaredNormMetric(goal)) sf, err := newSolverFrame(fs, modelName, frame.World, startInput) if err != nil { return nil, err @@ -247,7 +248,7 @@ func simpleXArmMotion() (*planConfig, error) { // setup planner options opt := newBasicPlannerOptions(xarm) - opt.SetGoalMetric(NewSquaredNormMetric(goal)) + opt.SetGoalMetric(ik.NewSquaredNormMetric(goal)) sf, err := newSolverFrame(fs, xarm.Name(), frame.World, frame.StartPositions(fs)) if err != nil { return nil, err @@ -282,7 +283,7 @@ func simpleUR5eMotion() (*planConfig, error) { // setup planner options opt := newBasicPlannerOptions(ur5e) - opt.SetGoalMetric(NewSquaredNormMetric(goal)) + opt.SetGoalMetric(ik.NewSquaredNormMetric(goal)) sf, err := newSolverFrame(fs, ur5e.Name(), frame.World, frame.StartPositions(fs)) if err != nil { return nil, err @@ -320,7 +321,7 @@ func testPlanner(t *testing.T, plannerFunc plannerConstructor, config planConfig // test that path doesn't violate constraints test.That(t, len(path), test.ShouldBeGreaterThanOrEqualTo, 2) for j := 0; j < len(path)-1; j++ { - ok, _ := cfg.Options.ConstraintHandler.CheckSegmentAndStateValidity(&Segment{ + ok, _ := cfg.Options.ConstraintHandler.CheckSegmentAndStateValidity(&ik.Segment{ StartConfiguration: path[j], EndConfiguration: path[j+1], Frame: cfg.RobotFrame, diff --git a/motionplan/nearestNeighbor.go b/motionplan/nearestNeighbor.go index 33fabd99fb4..3fa5bae069a 100644 --- a/motionplan/nearestNeighbor.go +++ b/motionplan/nearestNeighbor.go @@ -6,6 +6,7 @@ import ( "sort" "go.viam.com/utils" + "go.viam.com/rdk/motionplan/ik" ) const defaultNeighborsBeforeParallelization = 1000 @@ -32,7 +33,7 @@ func kNearestNeighbors(planOpts *plannerOptions, tree rrtMap, target node, neigh allCosts := make([]*neighbor, 0) for rrtnode := range tree { - dist := planOpts.DistanceFunc(&Segment{ + dist := planOpts.DistanceFunc(&ik.Segment{ StartConfiguration: target.Q(), EndConfiguration: rrtnode.Q(), }) @@ -79,7 +80,7 @@ func (nm *neighborManager) nearestNeighbor( bestDist := math.Inf(1) var best node for k := range tree { - seg := &Segment{ + seg := &ik.Segment{ StartConfiguration: seed.Q(), EndConfiguration: k.Q(), } @@ -158,7 +159,7 @@ func (nm *neighborManager) nnWorker(ctx context.Context, planOpts *plannerOption default: } - seg := &Segment{ + seg := &ik.Segment{ StartConfiguration: nm.seedPos.Q(), EndConfiguration: candidate.Q(), } diff --git a/motionplan/nearestNeighbor_test.go b/motionplan/nearestNeighbor_test.go index 920482c71aa..c3a5996ff6a 100644 --- a/motionplan/nearestNeighbor_test.go +++ b/motionplan/nearestNeighbor_test.go @@ -2,6 +2,8 @@ package motionplan import ( "context" + "math" + "runtime" "testing" "go.viam.com/test" @@ -9,6 +11,8 @@ import ( "go.viam.com/rdk/referenceframe" ) +var nCPU = int(math.Max(1.0, float64(runtime.NumCPU()/4))) + func TestNearestNeighbor(t *testing.T) { nm := &neighborManager{nCPU: 2, parallelNeighbors: 1000} rrtMap := map[node]node{} diff --git a/motionplan/planManager.go b/motionplan/planManager.go index e6c21ce27c3..17e96229eaf 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -14,6 +14,7 @@ import ( pb "go.viam.com/api/service/motion/v1" "go.viam.com/utils" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" @@ -446,7 +447,7 @@ func (pm *planManager) plannerSetupFromMoveRequest( // Start with normal options opt := newBasicPlannerOptions(pm.frame) - opt.SetGoalMetric(NewSquaredNormMetric(to)) + opt.SetGoalMetric(ik.NewSquaredNormMetric(to)) opt.extra = planningOpts @@ -516,7 +517,7 @@ func (pm *planManager) plannerSetupFromMoveRequest( // overwrite default with TP space opt.PlannerConstructor = newTPSpaceMotionPlanner // Distances are computed in cartesian space rather than configuration space - opt.DistanceFunc = NewSquaredNormSegmentMetricWithScaling(defaultTPspaceOrientationScale) + opt.DistanceFunc = ik.NewSquaredNormSegmentMetric(defaultTPspaceOrientationScale) planAlg = "tpspace" } @@ -556,7 +557,7 @@ func (pm *planManager) plannerSetupFromMoveRequest( opt.AddStateConstraint(defaultOrientationConstraintDesc, constraint) opt.pathMetric = pathMetric case PositionOnlyMotionProfile: - opt.SetGoalMetric(NewPositionOnlyMetric(to)) + opt.SetGoalMetric(ik.NewPositionOnlyMetric(to)) case FreeMotionProfile: // No restrictions on motion fallthrough diff --git a/motionplan/plannerOptions.go b/motionplan/plannerOptions.go index 6b5df4456a5..1eb9a14b00f 100644 --- a/motionplan/plannerOptions.go +++ b/motionplan/plannerOptions.go @@ -5,6 +5,7 @@ import ( pb "go.viam.com/api/service/motion/v1" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -40,9 +41,6 @@ const ( // default number of times to try to smooth the path. defaultSmoothIter = 200 - // default amount of closeness to get to the goal. - defaultGoalThreshold = defaultEpsilon * defaultEpsilon - // descriptions of constraints. defaultLinearConstraintDesc = "Constraint to follow linear path" defaultPseudolinearConstraintDesc = "Constraint to follow pseudolinear path, with tolerance scaled to path length" @@ -72,9 +70,9 @@ const ( // NewBasicPlannerOptions specifies a set of basic options for the planner. func newBasicPlannerOptions(frame referenceframe.Frame) *plannerOptions { opt := &plannerOptions{} - opt.goalArcScore = JointMetric - opt.DistanceFunc = L2InputMetric - opt.pathMetric = NewZeroMetric() // By default, the distance to the valid manifold is zero, unless constraints say otherwise + opt.goalArcScore = ik.JointMetric + opt.DistanceFunc = ik.L2InputMetric + opt.pathMetric = ik.NewZeroMetric() // By default, the distance to the valid manifold is zero, unless constraints say otherwise // opt.goalMetric is intentionally unset as it is likely dependent on the goal itself. // Set defaults @@ -82,7 +80,6 @@ func newBasicPlannerOptions(frame referenceframe.Frame) *plannerOptions { opt.MinScore = defaultMinIkScore opt.Resolution = defaultResolution opt.Timeout = defaultTimeout - opt.GoalThreshold = defaultGoalThreshold opt.PlanIter = defaultPlanIter opt.FrameStep = defaultFrameStep @@ -105,9 +102,9 @@ func newBasicPlannerOptions(frame referenceframe.Frame) *plannerOptions { // plannerOptions are a set of options to be passed to a planner which will specify how to solve a motion planning problem. type plannerOptions struct { ConstraintHandler - goalMetric StateMetric // Distance function which converges to the final goal position - goalArcScore SegmentMetric - pathMetric StateMetric // Distance function which converges on the valid manifold of intermediate path states + goalMetric ik.StateMetric // Distance function which converges to the final goal position + goalArcScore ik.SegmentMetric + pathMetric ik.StateMetric // Distance function which converges on the valid manifold of intermediate path states extra map[string]interface{} @@ -152,7 +149,7 @@ type plannerOptions struct { qstep []float64 // DistanceFunc is the function that the planner will use to measure the degree of "closeness" between two states of the robot - DistanceFunc SegmentMetric + DistanceFunc ik.SegmentMetric PlannerConstructor plannerConstructor @@ -160,12 +157,12 @@ type plannerOptions struct { } // SetMetric sets the distance metric for the solver. -func (p *plannerOptions) SetGoalMetric(m StateMetric) { +func (p *plannerOptions) SetGoalMetric(m ik.StateMetric) { p.goalMetric = m } // SetPathDist sets the distance metric for the solver to move a constraint-violating point into a valid manifold. -func (p *plannerOptions) SetPathMetric(m StateMetric) { +func (p *plannerOptions) SetPathMetric(m ik.StateMetric) { p.pathMetric = m } @@ -208,7 +205,7 @@ func (p *plannerOptions) addPbLinearConstraints(from, to spatialmath.Pose, pbCon constraint, pathDist := NewAbsoluteLinearInterpolatingConstraint(from, to, float64(linTol), float64(orientTol)) p.AddStateConstraint(defaultLinearConstraintDesc, constraint) - p.pathMetric = CombineMetrics(p.pathMetric, pathDist) + p.pathMetric = ik.CombineMetrics(p.pathMetric, pathDist) } func (p *plannerOptions) addPbOrientationConstraints(from, to spatialmath.Pose, pbConstraint *pb.OrientationConstraint) { @@ -218,5 +215,5 @@ func (p *plannerOptions) addPbOrientationConstraints(from, to spatialmath.Pose, } constraint, pathDist := NewSlerpOrientationConstraint(from, to, float64(orientTol)) p.AddStateConstraint(defaultOrientationConstraintDesc, constraint) - p.pathMetric = CombineMetrics(p.pathMetric, pathDist) + p.pathMetric = ik.CombineMetrics(p.pathMetric, pathDist) } diff --git a/motionplan/ptgIK.go b/motionplan/ptgIK.go deleted file mode 100644 index 33fd208912a..00000000000 --- a/motionplan/ptgIK.go +++ /dev/null @@ -1,141 +0,0 @@ -package motionplan - -import ( - "context" - "math" - "sync" - - "github.com/edaniels/golog" - - "go.viam.com/rdk/motionplan/tpspace" - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/spatialmath" -) - -const ( - defaultDiffT = 0.01 // seconds. Return trajectories updating velocities at this resolution. - nloptSeed = 42 // This should be fine to kepe constant - - defaultZeroDist = 1e-3 // Sometimes nlopt will minimize trajectories to zero. Ensure min traj dist is at least this -) - -// ptgGridSim will take a PrecomputePTG, and simulate out a number of trajectories through some requested time/distance for speed of lookup -// later. It will store the trajectories in a grid data structure allowing relatively fast lookups. -type ptgIK struct { - refDist float64 - simPTG tpspace.PrecomputePTG - ptgFrame referenceframe.Frame - fastGradDescent *NloptIK - - gridSim tpspace.PTG - - mu sync.RWMutex - trajCache map[float64][]*tpspace.TrajNode -} - -// NewPTGIK creates a new PTG by simulating a PrecomputePTG for some distance, then cacheing the results in a grid for fast lookup. -func NewPTGIK(simPTG tpspace.PrecomputePTG, logger golog.Logger, refDist float64, randSeed int) (tpspace.PTG, error) { - ptgFrame, err := tpspace.NewPTGIKFrame(simPTG, refDist) - if err != nil { - return nil, err - } - - nlopt, err := CreateNloptIKSolver(ptgFrame, logger, 1, defaultEpsilon*defaultEpsilon, true) - if err != nil { - return nil, err - } - - // create an ends-only grid sim for quick end-of-trajectory calculations - gridSim, err := tpspace.NewPTGGridSim(simPTG, 0, refDist, true) - if err != nil { - return nil, err - } - - ptg := &ptgIK{ - refDist: refDist, - simPTG: simPTG, - ptgFrame: ptgFrame, - fastGradDescent: nlopt, - gridSim: gridSim, - trajCache: map[float64][]*tpspace.TrajNode{}, - } - - return ptg, nil -} - -func (ptg *ptgIK) CToTP(ctx context.Context, distFunc func(spatialmath.Pose) float64) (*tpspace.TrajNode, error) { - solutionGen := make(chan *IKSolution, 1) - seedInput := []referenceframe.Input{{math.Pi / 2}, {ptg.refDist / 2}} // random value to seed the IK solver - goalMetric := func(state *State) float64 { - return distFunc(state.Position) - } - // Spawn the IK solver to generate a solution - err := ptg.fastGradDescent.Solve(ctx, solutionGen, seedInput, goalMetric, nloptSeed) - // We should have zero or one solutions - var solved *IKSolution - select { - case solved = <-solutionGen: - default: - } - close(solutionGen) - if err != nil || solved == nil || solved.Configuration[1].Value < defaultZeroDist { - return ptg.gridSim.CToTP(ctx, distFunc) - } - - if solved.Partial { - gridNode, err := ptg.gridSim.CToTP(ctx, distFunc) - if err == nil { - // Check if the grid has a better solution - if distFunc(gridNode.Pose) < solved.Score { - return gridNode, nil - } - } - } - - traj, err := ptg.Trajectory(solved.Configuration[0].Value, solved.Configuration[1].Value) - if err != nil { - return nil, err - } - return traj[len(traj)-1], nil -} - -func (ptg *ptgIK) RefDistance() float64 { - return ptg.refDist -} - -func (ptg *ptgIK) Trajectory(alpha, dist float64) ([]*tpspace.TrajNode, error) { - ptg.mu.RLock() - precomp := ptg.trajCache[alpha] - ptg.mu.RUnlock() - if precomp != nil { - if precomp[len(precomp)-1].Dist >= dist { - // Cacheing here provides a ~33% speedup to a solve call - subTraj := []*tpspace.TrajNode{} - for _, wp := range precomp { - if wp.Dist < dist { - subTraj = append(subTraj, wp) - } else { - break - } - } - time := 0. - if len(subTraj) > 0 { - time = subTraj[len(subTraj)-1].Time - } - lastNode, err := tpspace.ComputePTGNode(ptg.simPTG, alpha, dist, time) - if err != nil { - return nil, err - } - subTraj = append(subTraj, lastNode) - return subTraj, nil - } - } - traj, err := tpspace.ComputePTG(ptg.simPTG, alpha, dist, defaultDiffT) - if err != nil { - return nil, err - } - ptg.mu.Lock() - ptg.trajCache[alpha] = traj - ptg.mu.Unlock() - return traj, nil -} diff --git a/motionplan/rrt.go b/motionplan/rrt.go index 45fb1bf9ad0..37f16329336 100644 --- a/motionplan/rrt.go +++ b/motionplan/rrt.go @@ -4,6 +4,7 @@ import ( "context" "math" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -79,7 +80,7 @@ func initRRTSolutions(ctx context.Context, mp motionPlanner, seed []referencefra } // the smallest interpolated distance between the start and end input represents a lower bound on cost - optimalCost := mp.opt().DistanceFunc(&Segment{StartConfiguration: seed, EndConfiguration: solutions[0].Q()}) + optimalCost := mp.opt().DistanceFunc(&ik.Segment{StartConfiguration: seed, EndConfiguration: solutions[0].Q()}) rrt.maps.optNode = &basicNode{q: solutions[0].Q(), cost: optimalCost} // Check for direct interpolation for the subset of IK solutions within some multiple of optimal @@ -88,7 +89,7 @@ func initRRTSolutions(ctx context.Context, mp motionPlanner, seed []referencefra // initialize maps and check whether direct interpolation is an option for _, solution := range solutions { if canInterp { - cost := mp.opt().DistanceFunc(&Segment{StartConfiguration: seed, EndConfiguration: solution.Q()}) + cost := mp.opt().DistanceFunc(&ik.Segment{StartConfiguration: seed, EndConfiguration: solution.Q()}) if cost < optimalCost*defaultOptimalityMultiple { if mp.checkPath(seed, solution.Q()) { rrt.steps = []node{seedNode, solution} diff --git a/motionplan/rrtStarConnect.go b/motionplan/rrtStarConnect.go index 6cd3e374fc6..7d30df7ac29 100644 --- a/motionplan/rrtStarConnect.go +++ b/motionplan/rrtStarConnect.go @@ -9,6 +9,7 @@ import ( "github.com/edaniels/golog" "go.viam.com/utils" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -170,7 +171,7 @@ func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, return } - reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) + reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) // Second iteration; extend maps 1 and 2 towards the halfway point between where they reached if reachedDelta > mp.planOpts.JointSolveDist { @@ -180,7 +181,7 @@ func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} return } - reachedDelta = mp.planOpts.DistanceFunc(&Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) + reachedDelta = mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) } // Solved @@ -236,7 +237,7 @@ func (mp *rrtStarConnectMotionPlanner) extend( default: } - dist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: near.Q(), EndConfiguration: target.Q()}) + dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: near.Q(), EndConfiguration: target.Q()}) if dist < mp.planOpts.JointSolveDist { mchan <- near return @@ -261,7 +262,7 @@ func (mp *rrtStarConnectMotionPlanner) extend( } // check to see if a shortcut is possible, and rewire the node if it is - connectionCost := mp.planOpts.DistanceFunc(&Segment{ + connectionCost := mp.planOpts.DistanceFunc(&ik.Segment{ StartConfiguration: thisNeighbor.node.Q(), EndConfiguration: near.Q(), }) diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index 30af6ab826a..07aa7a25244 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -8,12 +8,14 @@ import ( "fmt" "math" "math/rand" + "sync" "github.com/edaniels/golog" "github.com/golang/geo/r3" "go.uber.org/multierr" "go.viam.com/utils" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" @@ -71,6 +73,9 @@ type tpspaceOptions struct { // Print very fine-grained debug info. Useful for observing the inner RRT tree structure directly pathdebug bool + + // random value to seed the IK solver. Can be anything in the middle of the valid manifold. + ikSeed []referenceframe.Input // Cached functions for calculating TP-space distances for each PTG distOptions map[tpspace.PTG]*plannerOptions @@ -94,6 +99,7 @@ type nodeAndError struct { // tpSpaceRRTMotionPlanner. type tpSpaceRRTMotionPlanner struct { *planner + mu sync.Mutex algOpts *tpspaceOptions tpFrame tpspace.PTGProvider } @@ -123,6 +129,9 @@ func newTPSpaceMotionPlanner( tpFrame: tpFrame, } tpPlanner.setupTPSpaceOptions() + + tpPlanner.algOpts.ikSeed = []referenceframe.Input{{math.Pi / 2}, {tpFrame.PTGs()[0].RefDistance() / 2}} + return tpPlanner, nil } @@ -201,7 +210,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( defer close(m1chan) defer close(m2chan) - dist := math.Sqrt(mp.planOpts.DistanceFunc(&Segment{StartPosition: startPose, EndPosition: goalPose})) + dist := math.Sqrt(mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: startPose, EndPosition: goalPose})) midPt := spatialmath.Interpolate(startPose, goalPose, 0.5) // used for initial seed var randPos spatialmath.Pose @@ -262,7 +271,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( if goalReached.node == nil { continue } - reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartPosition: seedReached.node.Pose(), EndPosition: goalReached.node.Pose()}) + reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: seedReached.node.Pose(), EndPosition: goalReached.node.Pose()}) if reachedDelta <= mp.algOpts.poseSolveDist { // If we've reached the goal, extract the path from the RRT trees and return path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: seedReached.node, b: goalReached.node}, false) @@ -281,7 +290,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( if seedReached.node == nil { continue } - reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartPosition: seedReached.node.Pose(), EndPosition: goalMapNode.Pose()}) + reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: seedReached.node.Pose(), EndPosition: goalMapNode.Pose()}) if reachedDelta <= mp.algOpts.poseSolveDist { // If we've reached the goal, extract the path from the RRT trees and return path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: seedReached.node, b: goalMapNode}, false) @@ -343,28 +352,49 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( // then dynamically expanding or contracting the limits of IK to be some fraction of that distance. // Get cartesian distance from NN to rand - var targetFunc func(spatialmath.Pose) float64 + //~ var targetFunc func(spatialmath.Pose) float64 + //~ if invert { + //~ sqMet := ik.NewSquaredNormMetric(randPosNode.Pose()) + //~ targetFunc = func(pose spatialmath.Pose) float64 { + //~ return sqMet(&ik.State{Position: spatialmath.Compose(nearest.Pose(), spatialmath.PoseInverse(pose))}) + //~ } + //~ } else { + //~ relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) + //~ sqMet := ik.NewSquaredNormMetric(relPose) + //~ targetFunc = func(pose spatialmath.Pose) float64 { + //~ return sqMet(&ik.State{Position: pose}) + //~ } + //~ } + + var targetFunc ik.StateMetric if invert { - sqMet := NewSquaredNormMetric(randPosNode.Pose()) - targetFunc = func(pose spatialmath.Pose) float64 { - return sqMet(&State{Position: spatialmath.Compose(nearest.Pose(), spatialmath.PoseInverse(pose))}) + sqMet := ik.NewSquaredNormMetric(randPosNode.Pose()) + targetFunc = func(pose *ik.State) float64 { + return sqMet(&ik.State{Position: spatialmath.Compose(nearest.Pose(), spatialmath.PoseInverse(pose.Position))}) } } else { relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) - sqMet := NewSquaredNormMetric(relPose) - targetFunc = func(pose spatialmath.Pose) float64 { - return sqMet(&State{Position: pose}) - } + targetFunc = ik.NewSquaredNormMetric(relPose) + } + solutionChan := make(chan *ik.IKSolution, 1) + err := curPtg.Solve(context.Background(), solutionChan, mp.algOpts.ikSeed, targetFunc, mp.randseed.Int()) + + var bestNode *ik.IKSolution + select { + case bestNode = <-solutionChan: + default: } - - // Convert cartesian distance to tp-space using inverse curPtg, yielding TP-space coordinates goalK and goalD - bestNode, err := curPtg.CToTP(ctx, targetFunc) if err != nil || bestNode == nil { return nil, err } - bestDist := targetFunc(bestNode.Pose) - goalAlpha := bestNode.Alpha - goalD := bestNode.Dist + pose, err := curPtg.Transform(bestNode.Configuration) + if err != nil { + return nil, err + } + + bestDist := targetFunc(&ik.State{Position: pose}) + goalAlpha := bestNode.Configuration[0].Value + goalD := bestNode.Configuration[1].Value // Check collisions along this traj and get the longest distance viable trajK, err := curPtg.Trajectory(goalAlpha, goalD) @@ -391,7 +421,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( } sinceLastCollideCheck += math.Abs(trajPt.Dist - lastDist) - trajState := &State{Position: spatialmath.Compose(arcStartPose, trajPt.Pose), Frame: mp.frame} + trajState := &ik.State{Position: spatialmath.Compose(arcStartPose, trajPt.Pose), Frame: mp.frame} nodePose = trajState.Position // This will get rewritten later for inverted trees if sinceLastCollideCheck > planOpts.Resolution { ok, _ := planOpts.CheckStateConstraints(trajState) @@ -419,7 +449,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( // Get nearest neighbor to new node that's already in the tree nearest = nm.nearestNeighbor(ctx, planOpts, successNode, rrt) if nearest != nil { - dist := planOpts.DistanceFunc(&Segment{StartPosition: successNode.Pose(), EndPosition: nearest.Pose()}) + dist := planOpts.DistanceFunc(&ik.Segment{StartPosition: successNode.Pose(), EndPosition: nearest.Pose()}) // Ensure successNode is sufficiently far from the nearest node already existing in the tree // If too close, don't add a new node if dist < defaultIdenticalNodeDistance { @@ -488,7 +518,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( if reseedCandidate == nil { return &nodeAndError{nil, nil} } - dist := mp.planOpts.DistanceFunc(&Segment{StartPosition: reseedCandidate.newNode.Pose(), EndPosition: goalNode.Pose()}) + dist := mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: reseedCandidate.newNode.Pose(), EndPosition: goalNode.Pose()}) if dist < mp.algOpts.poseSolveDist || lastIteration { // Reached the goal position, or otherwise failed to fully extend to the end of a trajectory return &nodeAndError{reseedCandidate.newNode, nil} @@ -548,7 +578,7 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( lastDist := 0. sinceLastNode := 0. - var trajState *State + var trajState *ik.State if mp.algOpts.addIntermediate { for i := 0; i < len(trajK); i++ { trajPt := trajK[i] @@ -558,7 +588,7 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( if ctx.Err() != nil { return nil, ctx.Err() } - trajState = &State{Position: spatialmath.Compose(arcStartPose, trajPt.Pose)} + trajState = &ik.State{Position: spatialmath.Compose(arcStartPose, trajPt.Pose)} if mp.algOpts.pathdebug { if !invert { mp.logger.Debugf("$FWDTREE,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) @@ -619,32 +649,43 @@ func (mp *tpSpaceRRTMotionPlanner) setupTPSpaceOptions() { // distances can be computed in TP space using the given PTG. func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, invert bool) *plannerOptions { opts := newBasicPlannerOptions(mp.frame) + mp.mu.Lock() + randSeed := rand.New(rand.NewSource(mp.randseed.Int63() + mp.randseed.Int63())) + mp.mu.Unlock() - segMetric := func(seg *Segment) float64 { + segMetric := func(seg *ik.Segment) float64 { // When running NearestNeighbor: // StartPosition is the seed/query // EndPosition is the pose already in the RRT tree if seg.StartPosition == nil || seg.EndPosition == nil { return math.Inf(1) } - var targetFunc func(spatialmath.Pose) float64 + var targetFunc ik.StateMetric if invert { - sqMet := NewSquaredNormMetric(seg.StartPosition) - targetFunc = func(pose spatialmath.Pose) float64 { - return sqMet(&State{Position: spatialmath.Compose(seg.EndPosition, spatialmath.PoseInverse(pose))}) + sqMet := ik.NewSquaredNormMetric(seg.StartPosition) + targetFunc = func(pose *ik.State) float64 { + return sqMet(&ik.State{Position: spatialmath.Compose(seg.EndPosition, spatialmath.PoseInverse(pose.Position))}) } } else { relPose := spatialmath.Compose(spatialmath.PoseInverse(seg.EndPosition), seg.StartPosition) - sqMet := NewSquaredNormMetric(relPose) - targetFunc = func(pose spatialmath.Pose) float64 { - return sqMet(&State{Position: pose}) - } + targetFunc = ik.NewSquaredNormMetric(relPose) + } + solutionChan := make(chan *ik.IKSolution, 1) + err := ptg.Solve(context.Background(), solutionChan, mp.algOpts.ikSeed, targetFunc, randSeed.Int()) + + var closeNode *ik.IKSolution + select { + case closeNode = <-solutionChan: + default: } - closeNode, err := ptg.CToTP(context.Background(), targetFunc) if err != nil || closeNode == nil { return math.Inf(1) } - return targetFunc(closeNode.Pose) + pose, err := ptg.Transform(closeNode.Configuration) + if err != nil { + return math.Inf(1) + } + return targetFunc(&ik.State{Position: pose}) } opts.DistanceFunc = segMetric return opts @@ -686,7 +727,7 @@ func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) maxCost = wp.Cost() } } - newFrame, err := newPTGFrameFromPTGFrame(mp.frame, maxCost*mp.algOpts.smoothScaleFactor) + newFrame, err := tpspace.NewPTGFrameFromPTGFrame(mp.frame, maxCost*mp.algOpts.smoothScaleFactor) if err != nil { return path } @@ -769,7 +810,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( return nil, errors.New("could not extend to smoothing destination") } - reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartPosition: reached.Pose(), EndPosition: path[secondEdge].Pose()}) + reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: reached.Pose(), EndPosition: path[secondEdge].Pose()}) // If we tried the goal and have a close-enough XY location, check if the node is good enough to be a final goal if reachedDelta > mp.algOpts.poseSolveDist { return nil, errors.New("could not precisely reach smoothing destination") diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 79027b1446e..686318d1066 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -12,6 +12,7 @@ import ( "github.com/golang/geo/r3" "go.viam.com/test" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" @@ -30,7 +31,7 @@ func TestPtgRrt(t *testing.T) { ctx := context.Background() - ackermanFrame, err := NewPTGFrameFromTurningRadius( + ackermanFrame, err := tpspace.NewPTGFrameFromTurningRadius( "ackframe", logger, 300., @@ -43,7 +44,7 @@ func TestPtgRrt(t *testing.T) { goalPos := spatialmath.NewPose(r3.Vector{X: 200, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 90}) opt := newBasicPlannerOptions(ackermanFrame) - opt.DistanceFunc = NewSquaredNormSegmentMetricWithScaling(30.) + opt.DistanceFunc = ik.NewSquaredNormSegmentMetric(30.) mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) tp, ok := mp.(*tpSpaceRRTMotionPlanner) @@ -103,7 +104,7 @@ func TestPtgWithObstacle(t *testing.T) { roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") test.That(t, err, test.ShouldBeNil) geometries := []spatialmath.Geometry{roverGeom} - ackermanFrame, err := NewPTGFrameFromTurningRadius( + ackermanFrame, err := tpspace.NewPTGFrameFromTurningRadius( "ackframe", logger, 300., @@ -121,7 +122,7 @@ func TestPtgWithObstacle(t *testing.T) { fs.AddFrame(ackermanFrame, fs.World()) opt := newBasicPlannerOptions(ackermanFrame) - opt.DistanceFunc = NewSquaredNormSegmentMetricWithScaling(30.) + opt.DistanceFunc = ik.NewSquaredNormSegmentMetric(30.) opt.GoalThreshold = 5 // obstacles obstacle1, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{3300, -500, 0}), r3.Vector{180, 1800, 1}, "") @@ -217,7 +218,7 @@ func TestIKPtgRrt(t *testing.T) { test.That(t, err, test.ShouldBeNil) geometries := []spatialmath.Geometry{roverGeom} - ackermanFrame, err := NewPTGFrameFromTurningRadius( + ackermanFrame, err := tpspace.NewPTGFrameFromTurningRadius( "ackframe", logger, 300., @@ -230,8 +231,8 @@ func TestIKPtgRrt(t *testing.T) { goalPos := spatialmath.NewPose(r3.Vector{X: 50, Y: 10, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 180}) opt := newBasicPlannerOptions(ackermanFrame) - opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) - opt.DistanceFunc = SquaredNormNoOrientSegmentMetric + opt.SetGoalMetric(ik.NewPositionOnlyMetric(goalPos)) + opt.DistanceFunc = ik.SquaredNormNoOrientSegmentMetric opt.GoalThreshold = 10. mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) @@ -253,7 +254,7 @@ func TestTPsmoothing(t *testing.T) { ctx := context.Background() - ackermanFrame, err := NewPTGFrameFromTurningRadius( + ackermanFrame, err := tpspace.NewPTGFrameFromTurningRadius( "ackframe", logger, 300., @@ -264,7 +265,7 @@ func TestTPsmoothing(t *testing.T) { test.That(t, err, test.ShouldBeNil) opt := newBasicPlannerOptions(ackermanFrame) - opt.DistanceFunc = NewSquaredNormSegmentMetricWithScaling(30.) + opt.DistanceFunc = ik.NewSquaredNormSegmentMetric(30.) mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) tp, _ := mp.(*tpSpaceRRTMotionPlanner) diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index 00d13421c57..ccb9eed7d11 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -2,11 +2,11 @@ package tpspace import ( - "context" "math" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/motionplan/ik" ) const floatEpsilon = 0.0001 // If floats are closer than this consider them equal @@ -15,8 +15,9 @@ const floatEpsilon = 0.0001 // If floats are closer than this consider them equa // PTG coordinates are specified in polar coordinates (alpha, d) // One of these is needed for each sort of motion that can be done. type PTG interface { - // CToTP will return the (alpha, dist) TP-space coordinates whose corresponding relative pose minimizes the given function - CToTP(context.Context, func(spatialmath.Pose) float64) (*TrajNode, error) + // Solve will return the (alpha, dist) TP-space coordinates whose corresponding relative pose minimizes the given function + ik.InverseKinematics + PrecomputePTG // RefDistance returns the maximum distance that a single trajectory may travel RefDistance() float64 @@ -49,9 +50,6 @@ type TrajNode struct { Alpha float64 // alpha k-value at this node LinVelMMPS float64 // linvel in millimeters per second at this node AngVelRPS float64 // angvel in radians per second at this node - - ptX float64 - ptY float64 } // discretized path to alpha. @@ -91,7 +89,7 @@ func ComputePTG( // Step through each time point for this alpha for math.Abs(dist) < math.Abs(refDist) { t += diffT - nextNode, err := ComputePTGNode(simPTG, alpha, dist, t) + nextNode, err := computePTGNode(simPTG, alpha, dist, t) if err != nil { return nil, err } @@ -113,14 +111,14 @@ func ComputePTG( if err != nil { return nil, err } - tNode := &TrajNode{pose, t, refDist, alpha, v, w, pose.Point().X, pose.Point().Y} + tNode := &TrajNode{pose, t, refDist, alpha, v, w} alphaTraj = append(alphaTraj, tNode) return alphaTraj, nil } -// ComputePTGNode will return the TrajNode of the requested PTG, at the specified alpha and dist. The provided time is used +// computePTGNode will return the TrajNode of the requested PTG, at the specified alpha and dist. The provided time is used // to fill in the time field. -func ComputePTGNode( +func computePTGNode( simPTG PrecomputePTG, alpha, dist, atT float64, ) (*TrajNode, error) { @@ -134,5 +132,5 @@ func ComputePTGNode( if err != nil { return nil, err } - return &TrajNode{pose, atT, dist, alpha, v, w, pose.Point().X, pose.Point().Y}, nil + return &TrajNode{pose, atT, dist, alpha, v, w}, nil } diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index c55833f6ffe..8ffe327e494 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -4,7 +4,8 @@ import ( "context" "math" - "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/motionplan/ik" ) const ( @@ -16,14 +17,13 @@ const ( // ptgGridSim will take a PrecomputePTG, and simulate out a number of trajectories through some requested time/distance for speed of lookup // later. It will store the trajectories in a grid data structure allowing relatively fast lookups. type ptgGridSim struct { + PrecomputePTG refDist float64 alphaCnt uint maxTime float64 // secs of robot execution to simulate diffT float64 // discretize trajectory simulation to this time granularity - simPTG PrecomputePTG - precomputeTraj [][]*TrajNode // If true, then CToTP calls will *only* check the furthest end of each precomputed trajectory. @@ -44,9 +44,9 @@ func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64, endsOnly bo diffT: defaultDiffT, endsOnly: endsOnly, } - ptg.simPTG = simPTG + ptg.PrecomputePTG = simPTG - precomp, err := ptg.simulateTrajectories(ptg.simPTG) + precomp, err := ptg.simulateTrajectories() if err != nil { return nil, err } @@ -55,7 +55,13 @@ func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64, endsOnly bo return ptg, nil } -func (ptg *ptgGridSim) CToTP(ctx context.Context, distFunc func(spatialmath.Pose) float64) (*TrajNode, error) { +func (ptg *ptgGridSim) Solve( + ctx context.Context, + solutionChan chan<- *ik.IKSolution, + seed []referenceframe.Input, + solveMetric ik.StateMetric, + rseed int, +) error { // Try to find a closest point to the paths: bestDist := math.Inf(1) var bestNode *TrajNode @@ -64,7 +70,7 @@ func (ptg *ptgGridSim) CToTP(ctx context.Context, distFunc func(spatialmath.Pose for k := 0; k < int(ptg.alphaCnt); k++ { nMax := len(ptg.precomputeTraj[k]) - 1 for n := 0; n <= nMax; n++ { - distToPoint := distFunc(ptg.precomputeTraj[k][n].Pose) + distToPoint := solveMetric(&ik.State{Position: ptg.precomputeTraj[k][n].Pose}) if distToPoint < bestDist { bestDist = distToPoint @@ -74,7 +80,12 @@ func (ptg *ptgGridSim) CToTP(ctx context.Context, distFunc func(spatialmath.Pose } if bestNode != nil { - return bestNode, nil + solutionChan <- &ik.IKSolution{ + Configuration: []referenceframe.Input{{bestNode.Alpha}, {bestNode.Dist}}, + Score: bestDist, + Exact: false, + } + return nil } } @@ -83,7 +94,7 @@ func (ptg *ptgGridSim) CToTP(ctx context.Context, distFunc func(spatialmath.Pose // which can be normalized by "1/refDistance" to get TP-Space distances. for k := 0; k < int(ptg.alphaCnt); k++ { n := len(ptg.precomputeTraj[k]) - 1 - distToPoint := distFunc(ptg.precomputeTraj[k][n].Pose) + distToPoint := solveMetric(&ik.State{Position: ptg.precomputeTraj[k][n].Pose}) if distToPoint < bestDist { bestDist = distToPoint @@ -91,7 +102,12 @@ func (ptg *ptgGridSim) CToTP(ctx context.Context, distFunc func(spatialmath.Pose } } - return bestNode, nil + solutionChan <- &ik.IKSolution{ + Configuration: []referenceframe.Input{{bestNode.Alpha}, {bestNode.Dist}}, + Score: bestDist, + Exact: false, + } + return nil } func (ptg *ptgGridSim) RefDistance() float64 { @@ -99,17 +115,17 @@ func (ptg *ptgGridSim) RefDistance() float64 { } func (ptg *ptgGridSim) Trajectory(alpha, dist float64) ([]*TrajNode, error) { - return ComputePTG(ptg.simPTG, alpha, dist, defaultDiffT) + return ComputePTG(ptg, alpha, dist, defaultDiffT) } -func (ptg *ptgGridSim) simulateTrajectories(simPTG PrecomputePTG) ([][]*TrajNode, error) { +func (ptg *ptgGridSim) simulateTrajectories() ([][]*TrajNode, error) { // C-space path structure allTraj := make([][]*TrajNode, 0, ptg.alphaCnt) for k := uint(0); k < ptg.alphaCnt; k++ { alpha := index2alpha(k, ptg.alphaCnt) - alphaTraj, err := ComputePTG(simPTG, alpha, ptg.refDist, ptg.diffT) + alphaTraj, err := ComputePTG(ptg, alpha, ptg.refDist, ptg.diffT) if err != nil { return nil, err } diff --git a/motionplan/tpspace/ptgGridSim_test.go b/motionplan/tpspace/ptgGridSim_test.go index 2f783a7f981..6551103e68d 100644 --- a/motionplan/tpspace/ptgGridSim_test.go +++ b/motionplan/tpspace/ptgGridSim_test.go @@ -6,15 +6,6 @@ import ( "go.viam.com/test" ) -type ptgFactory func(float64, float64) PrecomputePTG - -var defaultPTGs = []ptgFactory{ - NewCirclePTG, - NewCCPTG, - NewCCSPTG, - NewCSPTG, -} - var ( defaultMMps = 300. turnRadMeters = 0.3 diff --git a/motionplan/ptgGroupFrame.go b/motionplan/tpspace/ptgGroupFrame.go similarity index 77% rename from motionplan/ptgGroupFrame.go rename to motionplan/tpspace/ptgGroupFrame.go index 02e7655a46d..fdaf598e2e4 100644 --- a/motionplan/ptgGroupFrame.go +++ b/motionplan/tpspace/ptgGroupFrame.go @@ -1,4 +1,4 @@ -package motionplan +package tpspace import ( "errors" @@ -8,7 +8,6 @@ import ( "github.com/edaniels/golog" pb "go.viam.com/api/component/arm/v1" - "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -19,24 +18,27 @@ const ( distanceAlongTrajectoryIndex ) -type ptgFactory func(float64, float64) tpspace.PrecomputePTG +// If refDist is not explicitly set, default to pi radians times this adjustment value +const refDistHalfCircles = 0.9 + +type ptgFactory func(float64, float64) PrecomputePTG var defaultPTGs = []ptgFactory{ - tpspace.NewCirclePTG, - tpspace.NewCCPTG, - tpspace.NewCCSPTG, - tpspace.NewCSPTG, - tpspace.NewSideSPTG, - tpspace.NewSideSOverturnPTG, + NewCirclePTG, + NewCCPTG, + NewCCSPTG, + NewCSPTG, + NewSideSPTG, + NewSideSOverturnPTG, } type ptgGroupFrame struct { name string limits []referenceframe.Limit geometries []spatialmath.Geometry - ptgs []tpspace.PTG + ptgs []PTG velocityMMps float64 - turnRadMeters float64 + turnRadMillimeters float64 logger golog.Logger } @@ -54,14 +56,20 @@ func NewPTGFrameFromTurningRadius( if turnRadMeters <= 0 { return nil, fmt.Errorf("cannot create ptg frame, turning radius %f must be >0", turnRadMeters) } + if refDist < 0 { + return nil, fmt.Errorf("cannot create ptg frame, refDist %f must be >=0", refDist) + } + + turnRadMillimeters := turnRadMeters * 1000 - if refDist <= 0 { + if refDist == 0 { // Default to a distance of just over one half of a circle turning at max radius - refDist = 1000. * turnRadMeters * math.Pi * 0.9 + refDist = turnRadMillimeters * math.Pi * refDistHalfCircles + logger.Debugf("refDist was zero, calculating default %f", refDist) } // Get max angular velocity in radians per second - maxRPS := velocityMMps / (1000. * turnRadMeters) + maxRPS := velocityMMps / turnRadMillimeters pf := &ptgGroupFrame{name: name} err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist) if err != nil { @@ -70,7 +78,7 @@ func NewPTGFrameFromTurningRadius( pf.geometries = geoms pf.velocityMMps = velocityMMps - pf.turnRadMeters = turnRadMeters + pf.turnRadMillimeters = turnRadMillimeters pf.limits = []referenceframe.Limit{ {Min: 0, Max: float64(len(pf.ptgs) - 1)}, @@ -81,20 +89,24 @@ func NewPTGFrameFromTurningRadius( return pf, nil } -// newPTGFrameFromPTGFrame will create a new Frame from a preexisting ptgGroupFrame, allowing the adjustment of `refDist` while keeping +// NewPTGFrameFromPTGFrame will create a new Frame from a preexisting ptgGroupFrame, allowing the adjustment of `refDist` while keeping // other params the same. This may be expanded to allow altering turning radius, geometries, etc. -func newPTGFrameFromPTGFrame(frame referenceframe.Frame, refDist float64) (referenceframe.Frame, error) { +func NewPTGFrameFromPTGFrame(frame referenceframe.Frame, refDist float64) (referenceframe.Frame, error) { ptgFrame, ok := frame.(*ptgGroupFrame) if !ok { return nil, errors.New("cannot create ptg framem given frame is not a ptgGroupFrame") } + if refDist < 0 { + return nil, fmt.Errorf("cannot create ptg frame, refDist %f must be >=0", refDist) + } if refDist <= 0 { - refDist = 1000. * ptgFrame.turnRadMeters * math.Pi * 1.5 + refDist = ptgFrame.turnRadMillimeters * math.Pi * refDistHalfCircles + ptgFrame.logger.Debugf("refDist was zero, calculating default %f", refDist) } // Get max angular velocity in radians per second - maxRPS := ptgFrame.velocityMMps / (1000. * ptgFrame.turnRadMeters) + maxRPS := ptgFrame.velocityMMps / ptgFrame.turnRadMillimeters pf := &ptgGroupFrame{name: ptgFrame.name} err := pf.initPTGs(ptgFrame.logger, ptgFrame.velocityMMps, maxRPS, refDist) if err != nil { @@ -127,6 +139,9 @@ func (pf *ptgGroupFrame) MarshalJSON() ([]byte, error) { // Inputs are: [0] index of PTG to use, [1] index of the trajectory within that PTG, and [2] distance to travel along that trajectory. func (pf *ptgGroupFrame) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + if len(inputs) != len(pf.DoF()) { + return nil, referenceframe.NewIncorrectInputLengthError(len(inputs), len(pf.DoF())) + } alpha := inputs[trajectoryAlphaWithinPTG].Value dist := inputs[distanceAlongTrajectoryIndex].Value @@ -172,12 +187,12 @@ func (pf *ptgGroupFrame) Geometries(inputs []referenceframe.Input) (*referencefr return referenceframe.NewGeometriesInFrame(pf.name, geoms), nil } -func (pf *ptgGroupFrame) PTGs() []tpspace.PTG { +func (pf *ptgGroupFrame) PTGs() []PTG { return pf.ptgs } func (pf *ptgGroupFrame) initPTGs(logger golog.Logger, maxMps, maxRPS, simDist float64) error { - ptgs := []tpspace.PTG{} + ptgs := []PTG{} for _, ptg := range defaultPTGs { ptgGen := ptg(maxMps, maxRPS) if ptgGen != nil { diff --git a/motionplan/tpspace/ptgIK.go b/motionplan/tpspace/ptgIK.go new file mode 100644 index 00000000000..57408110fe7 --- /dev/null +++ b/motionplan/tpspace/ptgIK.go @@ -0,0 +1,152 @@ +package tpspace + +import ( + "context" + "errors" + "sync" + + "github.com/edaniels/golog" + + "go.viam.com/rdk/motionplan/ik" + "go.viam.com/rdk/referenceframe" +) + +const ( + defaultResolutionSeconds = 0.01 // seconds. Return trajectories updating velocities at this resolution. + + defaultZeroDist = 1e-3 // Sometimes nlopt will minimize trajectories to zero. Ensure min traj dist is at least this +) + +type ptgIK struct { + PrecomputePTG + refDist float64 + ptgFrame referenceframe.Frame + fastGradDescent *ik.NloptIK + + gridSim PTG + + mu sync.RWMutex + trajCache map[float64][]*TrajNode +} + +// NewPTGIK creates a new ptgIK, which creates a frame using the provided PrecomputePTG, and wraps it providing functions to fill the PTG +// interface, allowing inverse kinematics queries to be run against it. +func NewPTGIK(simPTG PrecomputePTG, logger golog.Logger, refDist float64, randSeed int) (PTG, error) { + if refDist <= 0 { + return nil, errors.New("refDist must be greater than zero") + } + + ptgFrame, err := NewPTGIKFrame(simPTG, refDist) + if err != nil { + return nil, err + } + + nlopt, err := ik.CreateNloptIKSolver(ptgFrame, logger, 1, false) + if err != nil { + return nil, err + } + + // create an ends-only grid sim for quick end-of-trajectory calculations + gridSim, err := NewPTGGridSim(simPTG, 0, refDist, true) + if err != nil { + return nil, err + } + + ptg := &ptgIK{ + PrecomputePTG: simPTG, + refDist: refDist, + ptgFrame: ptgFrame, + fastGradDescent: nlopt, + gridSim: gridSim, + trajCache: map[float64][]*TrajNode{}, + } + + return ptg, nil +} + +func (ptg *ptgIK) Solve( + ctx context.Context, + solutionChan chan<- *ik.IKSolution, + seed []referenceframe.Input, + solveMetric ik.StateMetric, + nloptSeed int, +) error { + internalSolutionGen := make(chan *ik.IKSolution, 1) + defer close(internalSolutionGen) + var solved *ik.IKSolution + + // Spawn the IK solver to generate a solution + err := ptg.fastGradDescent.Solve(ctx, internalSolutionGen, seed, solveMetric, nloptSeed) + // We should have zero or one solutions + + select { + case solved = <-internalSolutionGen: + default: + } + if err != nil || solved == nil || solved.Configuration[1].Value < defaultZeroDist { + // nlopt did not return a valid solution or otherwise errored. Fall back fully to the grid check. + return ptg.gridSim.Solve(ctx, solutionChan, seed, solveMetric, nloptSeed) + } + + if !solved.Exact { + // nlopt returned something but was unable to complete the solve. See if the grid check produces something better. + err = ptg.gridSim.Solve(ctx, internalSolutionGen, seed, solveMetric, nloptSeed) + if err == nil { + var gridSolved *ik.IKSolution + select { + case gridSolved = <-internalSolutionGen: + default: + } + // Check if the grid has a better solution + if gridSolved != nil { + if gridSolved.Score < solved.Score { + solved = gridSolved + } + } + } + } + + solutionChan <- solved + return nil +} + +func (ptg *ptgIK) RefDistance() float64 { + return ptg.refDist +} + +func (ptg *ptgIK) Trajectory(alpha, dist float64) ([]*TrajNode, error) { + ptg.mu.RLock() + precomp := ptg.trajCache[alpha] + ptg.mu.RUnlock() + if precomp != nil { + if precomp[len(precomp)-1].Dist >= dist { + // Cacheing here provides a ~33% speedup to a solve call + subTraj := []*TrajNode{} + for _, wp := range precomp { + if wp.Dist < dist { + subTraj = append(subTraj, wp) + } else { + break + } + } + time := 0. + if len(subTraj) > 0 { + time = subTraj[len(subTraj)-1].Time + } + lastNode, err := computePTGNode(ptg, alpha, dist, time) + if err != nil { + return nil, err + } + subTraj = append(subTraj, lastNode) + return subTraj, nil + } + } + traj, err := ComputePTG(ptg, alpha, dist, defaultResolutionSeconds) + if err != nil { + return nil, err + } + ptg.mu.Lock() + ptg.trajCache[alpha] = traj + ptg.mu.Unlock() + return traj, nil +} diff --git a/motionplan/utils.go b/motionplan/utils.go index 7bc348dba33..233f8d2c610 100644 --- a/motionplan/utils.go +++ b/motionplan/utils.go @@ -5,6 +5,7 @@ import ( "fmt" "math" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/utils" @@ -40,12 +41,12 @@ func PathStepCount(seedPos, goalPos spatialmath.Pose, stepSize float64) int { } // EvaluatePlan assigns a numeric score to a plan that corresponds to the cumulative distance between input waypoints in the plan. -func EvaluatePlan(plan [][]referenceframe.Input, distFunc SegmentMetric) (totalCost float64) { +func EvaluatePlan(plan [][]referenceframe.Input, distFunc ik.SegmentMetric) (totalCost float64) { if len(plan) < 2 { return math.Inf(1) } for i := 0; i < len(plan)-1; i++ { - cost := distFunc(&Segment{StartConfiguration: plan[i], EndConfiguration: plan[i+1]}) + cost := distFunc(&ik.Segment{StartConfiguration: plan[i], EndConfiguration: plan[i+1]}) totalCost += cost } return totalCost From 0e1748bedfcf825f8ee1a3fbe1e26629a53e4468 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Tue, 22 Aug 2023 15:06:23 -0700 Subject: [PATCH 27/38] Address all PR comments, including a big reshuffle of IK --- .../base/kinematicbase/differentialDrive.go | 4 +- .../base/kinematicbase/ptgKinematics.go | 3 +- motionplan/cBiRRT.go | 18 +- motionplan/constraint.go | 2 +- motionplan/ik/combinedInverseKinematics.go | 4 +- motionplan/ik/inverseKinematics.go | 8 +- motionplan/ik/inverseKinematics_test.go | 117 ++++++ motionplan/ik/metrics.go | 2 +- motionplan/ik/nloptInverseKinematics.go | 8 +- motionplan/kinematic.go | 111 ++++++ motionplan/kinematic_test.go | 351 ++++++++++++++++++ motionplan/motionPlanner.go | 13 +- motionplan/nearestNeighbor.go | 1 + motionplan/rrtStarConnect.go | 10 +- motionplan/tpSpaceRRT.go | 88 +++-- motionplan/tpspace/ptg.go | 2 +- motionplan/tpspace/ptgGridSim.go | 16 +- motionplan/tpspace/ptgGroupFrame.go | 16 +- motionplan/tpspace/ptgIK.go | 16 +- motionplan/tpspace/ptgSideS.go | 4 +- 20 files changed, 688 insertions(+), 106 deletions(-) create mode 100644 motionplan/ik/inverseKinematics_test.go create mode 100644 motionplan/kinematic.go create mode 100644 motionplan/kinematic_test.go diff --git a/components/base/kinematicbase/differentialDrive.go b/components/base/kinematicbase/differentialDrive.go index 9a66814bb98..9be0a275886 100644 --- a/components/base/kinematicbase/differentialDrive.go +++ b/components/base/kinematicbase/differentialDrive.go @@ -13,7 +13,7 @@ import ( utils "go.viam.com/utils" "go.viam.com/rdk/components/base" - "go.viam.com/rdk/motionplan" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/services/motion" "go.viam.com/rdk/spatialmath" @@ -186,7 +186,7 @@ func (ddk *differentialDriveKinematics) GoToInputs(ctx context.Context, desired if prevInputs == nil { prevInputs = currentInputs } - positionChange := motionplan.L2InputMetric(&motionplan.Segment{ + positionChange := ik.L2InputMetric(&ik.Segment{ StartConfiguration: prevInputs, EndConfiguration: currentInputs, }) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index ff381cb9780..1d9883dda1f 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -14,7 +14,6 @@ import ( utils "go.viam.com/utils" "go.viam.com/rdk/components/base" - "go.viam.com/rdk/motionplan" "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/referenceframe" rdkutils "go.viam.com/rdk/utils" @@ -75,7 +74,7 @@ func wrapWithPTGKinematics( return nil, err } - frame, err := motionplan.NewPTGFrameFromTurningRadius( + frame, err := tpspace.NewPTGFrameFromTurningRadius( b.Name().ShortName(), logger, baseMillimetersPerSecond, diff --git a/motionplan/cBiRRT.go b/motionplan/cBiRRT.go index e1a0bde2865..32e16e4b637 100644 --- a/motionplan/cBiRRT.go +++ b/motionplan/cBiRRT.go @@ -133,7 +133,7 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( rrt.maps = planSeed.maps } mp.logger.Infof("goal node: %v\n", rrt.maps.optNode.Q()) - target := referenceframe.InterpolateInputs(seed, rrt.maps.optNode.Q(), 0.5) + target := newConfigurationNode(referenceframe.InterpolateInputs(seed, rrt.maps.optNode.Q(), 0.5)) map1, map2 := rrt.maps.startMap, rrt.maps.goalMap @@ -164,13 +164,13 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( default: } - tryExtend := func(target []referenceframe.Input) (node, node, error) { + tryExtend := func(target node) (node, node, error) { // attempt to extend maps 1 and 2 towards the target utils.PanicCapturingGo(func() { - m1chan <- nm1.nearestNeighbor(nmContext, mp.planOpts, newConfigurationNode(target), map1) + m1chan <- nm1.nearestNeighbor(nmContext, mp.planOpts, target, map1) }) utils.PanicCapturingGo(func() { - m2chan <- nm2.nearestNeighbor(nmContext, mp.planOpts, newConfigurationNode(target), map2) + m2chan <- nm2.nearestNeighbor(nmContext, mp.planOpts, target, map2) }) nearest1 := <-m1chan nearest2 := <-m2chan @@ -187,10 +187,10 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( rseed2 := rand.New(rand.NewSource(int64(mp.randseed.Int()))) utils.PanicCapturingGo(func() { - mp.constrainedExtend(ctx, rseed1, map1, nearest1, newConfigurationNode(target), m1chan) + mp.constrainedExtend(ctx, rseed1, map1, nearest1, target, m1chan) }) utils.PanicCapturingGo(func() { - mp.constrainedExtend(ctx, rseed2, map2, nearest2, newConfigurationNode(target), m2chan) + mp.constrainedExtend(ctx, rseed2, map2, nearest2, target, m2chan) }) map1reached := <-m1chan map2reached := <-m2chan @@ -211,7 +211,7 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( // Second iteration; extend maps 1 and 2 towards the halfway point between where they reached if reachedDelta > mp.planOpts.JointSolveDist { - target = referenceframe.InterpolateInputs(map1reached.Q(), map2reached.Q(), 0.5) + target = newConfigurationNode(referenceframe.InterpolateInputs(map1reached.Q(), map2reached.Q(), 0.5)) map1reached, map2reached, err = tryExtend(target) if err != nil { rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} @@ -357,11 +357,11 @@ func (mp *cBiRRTMotionPlanner) constrainNear( if ok { return target } - solutionGen := make(chan *ik.IKSolution, 1) + solutionGen := make(chan *ik.Solution, 1) // Spawn the IK solver to generate solutions until done err = mp.fastGradDescent.Solve(ctx, solutionGen, target, mp.planOpts.pathMetric, randseed.Int()) // We should have zero or one solutions - var solved *ik.IKSolution + var solved *ik.Solution select { case solved = <-solutionGen: default: diff --git a/motionplan/constraint.go b/motionplan/constraint.go index 623be1dc434..954b12d91dd 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -7,8 +7,8 @@ import ( "github.com/golang/geo/r3" pb "go.viam.com/api/service/motion/v1" - "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/motionplan/ik" + "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/referenceframe" spatial "go.viam.com/rdk/spatialmath" ) diff --git a/motionplan/ik/combinedInverseKinematics.go b/motionplan/ik/combinedInverseKinematics.go index f44a94d9bf8..0c2ceacf728 100644 --- a/motionplan/ik/combinedInverseKinematics.go +++ b/motionplan/ik/combinedInverseKinematics.go @@ -43,7 +43,7 @@ func CreateCombinedIKSolver(model referenceframe.Frame, logger golog.Logger, nCP func runSolver(ctx context.Context, solver InverseKinematics, - c chan<- *IKSolution, + c chan<- *Solution, seed []referenceframe.Input, m StateMetric, rseed int, @@ -54,7 +54,7 @@ func runSolver(ctx context.Context, // Solve will initiate solving for the given position in all child solvers, seeding with the specified initial joint // positions. If unable to solve, the returned error will be non-nil. func (ik *CombinedIK) Solve(ctx context.Context, - c chan<- *IKSolution, + c chan<- *Solution, seed []referenceframe.Input, m StateMetric, rseed int, diff --git a/motionplan/ik/inverseKinematics.go b/motionplan/ik/inverseKinematics.go index f58d51b4727..d9bf45796dd 100644 --- a/motionplan/ik/inverseKinematics.go +++ b/motionplan/ik/inverseKinematics.go @@ -1,3 +1,5 @@ +// Package ik contains tols for doing gradient-descent based inverse kinematics, allowing for the minimization of arbitrary metrics +// based on the output of calling `Transform` on the given frame. package ik import ( @@ -18,12 +20,12 @@ const ( // solutions to the provided channel until cancelled or otherwise completes. type InverseKinematics interface { // Solve receives a context, the goal arm position, and current joint angles. - Solve(context.Context, chan<- *IKSolution, []referenceframe.Input, StateMetric, int) error + Solve(context.Context, chan<- *Solution, []referenceframe.Input, StateMetric, int) error } -// IKSolution is the struct returned from an IK solver. It contains the solution configuration, the score of the solution, and a flag +// Solution is the struct returned from an IK solver. It contains the solution configuration, the score of the solution, and a flag // indicating whether that configuration and score met the solution criteria requested by the caller. -type IKSolution struct { +type Solution struct { Configuration []referenceframe.Input Score float64 Exact bool diff --git a/motionplan/ik/inverseKinematics_test.go b/motionplan/ik/inverseKinematics_test.go new file mode 100644 index 00000000000..49831d4cfd2 --- /dev/null +++ b/motionplan/ik/inverseKinematics_test.go @@ -0,0 +1,117 @@ +package ik + +import ( + "context" + "errors" + "math" + "runtime" + "testing" + + "github.com/edaniels/golog" + "github.com/golang/geo/r3" + "go.viam.com/test" + + frame "go.viam.com/rdk/referenceframe" + spatial "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils" +) + +var ( + home = frame.FloatsToInputs([]float64{0, 0, 0, 0, 0, 0}) + nCPU = int(math.Max(1.0, float64(runtime.NumCPU()/4))) +) + +func TestCombinedIKinematics(t *testing.T) { + logger := golog.NewTestLogger(t) + m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + test.That(t, err, test.ShouldBeNil) + ik, err := CreateCombinedIKSolver(m, logger, nCPU, defaultGoalThreshold) + test.That(t, err, test.ShouldBeNil) + + // Test ability to arrive at another position + pos := spatial.NewPose( + r3.Vector{X: -46, Y: -133, Z: 372}, + &spatial.OrientationVectorDegrees{OX: 1.79, OY: -1.32, OZ: -1.11}, + ) + solution, err := solveTest(context.Background(), ik, pos, home) + test.That(t, err, test.ShouldBeNil) + + // Test moving forward 20 in X direction from previous position + pos = spatial.NewPose( + r3.Vector{X: -66, Y: -133, Z: 372}, + &spatial.OrientationVectorDegrees{OX: 1.78, OY: -3.3, OZ: -1.11}, + ) + _, err = solveTest(context.Background(), ik, pos, solution[0]) + test.That(t, err, test.ShouldBeNil) +} + +func TestUR5NloptIKinematics(t *testing.T) { + logger := golog.NewTestLogger(t) + + m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/universalrobots/ur5e.json"), "") + test.That(t, err, test.ShouldBeNil) + ik, err := CreateCombinedIKSolver(m, logger, nCPU, defaultGoalThreshold) + test.That(t, err, test.ShouldBeNil) + + goalJP := frame.JointPositionsFromRadians([]float64{-4.128, 2.71, 2.798, 2.3, 1.291, 0.62}) + goal, err := m.Transform(m.InputFromProtobuf(goalJP)) + test.That(t, err, test.ShouldBeNil) + _, err = solveTest(context.Background(), ik, goal, home) + test.That(t, err, test.ShouldBeNil) +} + +func TestCombinedCPUs(t *testing.T) { + logger := golog.NewTestLogger(t) + m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") + test.That(t, err, test.ShouldBeNil) + ik, err := CreateCombinedIKSolver(m, logger, runtime.NumCPU()/400000, defaultGoalThreshold) + test.That(t, err, test.ShouldBeNil) + test.That(t, len(ik.solvers), test.ShouldEqual, 1) +} + +func solveTest(ctx context.Context, solver InverseKinematics, goal spatial.Pose, seed []frame.Input) ([][]frame.Input, error) { + solutionGen := make(chan *Solution) + ikErr := make(chan error) + ctxWithCancel, cancel := context.WithCancel(ctx) + defer cancel() + + // Spawn the IK solver to generate solutions until done + go func() { + defer close(ikErr) + ikErr <- solver.Solve(ctxWithCancel, solutionGen, seed, NewSquaredNormMetric(goal), 1) + }() + + var solutions [][]frame.Input + + // Solve the IK solver. Loop labels are required because `break` etc in a `select` will break only the `select`. +IK: + for { + select { + case <-ctx.Done(): + return nil, ctx.Err() + default: + } + + select { + case step := <-solutionGen: + solutions = append(solutions, step.Configuration) + // Skip the return check below until we have nothing left to read from solutionGen + continue IK + default: + } + + select { + case <-ikErr: + // If we have a return from the IK solver, there are no more solutions, so we finish processing above + // until we've drained the channel + break IK + default: + } + } + cancel() + if len(solutions) == 0 { + return nil, errors.New("unable to solve for position") + } + + return solutions, nil +} diff --git a/motionplan/ik/metrics.go b/motionplan/ik/metrics.go index 0ac33119aa9..38012b0ed2f 100644 --- a/motionplan/ik/metrics.go +++ b/motionplan/ik/metrics.go @@ -67,7 +67,7 @@ func OrientDist(o1, o2 spatial.Orientation) float64 { return utils.RadToDeg(spatial.QuatToR4AA(spatial.OrientationBetween(o1, o2).Quaternion()).Theta) } -// orientDistToRegion will return a function which will tell you how far the unit sphere component of an orientation +// OrientDistToRegion will return a function which will tell you how far the unit sphere component of an orientation // vector is from a region defined by a point and an arclength around it. The theta value of OV is disregarded. // This is useful, for example, in defining the set of acceptable angles of attack for writing on a whiteboard. func OrientDistToRegion(goal spatial.Orientation, alpha float64) func(spatial.Orientation) float64 { diff --git a/motionplan/ik/nloptInverseKinematics.go b/motionplan/ik/nloptInverseKinematics.go index 25bfeb7827a..912f2bcba57 100644 --- a/motionplan/ik/nloptInverseKinematics.go +++ b/motionplan/ik/nloptInverseKinematics.go @@ -34,7 +34,7 @@ type NloptIK struct { lowerBound []float64 upperBound []float64 maxIterations int - epsilon float64 + epsilon float64 logger golog.Logger jump float64 @@ -51,7 +51,7 @@ func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int ik.model = mdl ik.id = 0 - + // Stop optimizing when iterations change by less than this much // Also, how close we want to get to the goal region. The metric should reflect any buffer. ik.epsilon = defaultEpsilon * defaultEpsilon @@ -70,7 +70,7 @@ func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int // Solve runs the actual solver and sends any solutions found to the given channel. func (ik *NloptIK) Solve(ctx context.Context, - solutionChan chan<- *IKSolution, + solutionChan chan<- *Solution, seed []referenceframe.Input, solveMetric StateMetric, rseed int, @@ -196,7 +196,7 @@ func (ik *NloptIK) Solve(ctx context.Context, return err default: } - solutionChan <- &IKSolution{ + solutionChan <- &Solution{ Configuration: referenceframe.FloatsToInputs(solutionRaw), Score: result, Exact: result < ik.epsilon, diff --git a/motionplan/kinematic.go b/motionplan/kinematic.go new file mode 100644 index 00000000000..57f5b5e4588 --- /dev/null +++ b/motionplan/kinematic.go @@ -0,0 +1,111 @@ +package motionplan + +import ( + "math" + "strings" + + "github.com/pkg/errors" + pb "go.viam.com/api/component/arm/v1" + "gonum.org/v1/gonum/floats" + "gonum.org/v1/gonum/num/quat" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" +) + +// ComputePosition takes a model and a protobuf JointPositions in degrees and returns the cartesian position of the +// end effector as a protobuf ArmPosition. This is performed statelessly without changing any data. +func ComputePosition(model referenceframe.Frame, joints *pb.JointPositions) (spatialmath.Pose, error) { + if len(joints.Values) != len(model.DoF()) { + return nil, errors.Errorf( + "incorrect number of joints passed to ComputePosition. Want: %d, got: %d", + len(model.DoF()), + len(joints.Values), + ) + } + + pose, err := model.Transform(model.InputFromProtobuf(joints)) + if err != nil { + return nil, err + } + + return pose, nil +} + +// ComputeOOBPosition takes a model and a protobuf JointPositions in degrees and returns the cartesian +// position of the end effector as a protobuf ArmPosition even when the arm is in an out of bounds state. +// This is performed statelessly without changing any data. +func ComputeOOBPosition(model referenceframe.Frame, joints *pb.JointPositions) (spatialmath.Pose, error) { + if joints == nil { + return nil, referenceframe.ErrNilJointPositions + } + if model == nil { + return nil, referenceframe.ErrNilModelFrame + } + + if len(joints.Values) != len(model.DoF()) { + return nil, errors.Errorf( + "incorrect number of joints passed to ComputePosition. Want: %d, got: %d", + len(model.DoF()), + len(joints.Values), + ) + } + + pose, err := model.Transform(model.InputFromProtobuf(joints)) + if err != nil && !strings.Contains(err.Error(), referenceframe.OOBErrString) { + return nil, err + } + + return pose, nil +} + +// deriv will compute D(q), the derivative of q = e^w with respect to w +// Note that for prismatic joints, this will need to be expanded to dual quaternions. +func deriv(q quat.Number) []quat.Number { + w := quat.Log(q) + + qNorm := math.Sqrt(w.Imag*w.Imag + w.Jmag*w.Jmag + w.Kmag*w.Kmag) + // qNorm hits a singularity every 2pi + // But if we flip the axis we get the same rotation but away from a singularity + + var quatD []quat.Number + + // qNorm is non-zero if our joint has a non-zero rotation + if qNorm > 0 { + b := math.Sin(qNorm) / qNorm + c := (math.Cos(qNorm) / (qNorm * qNorm)) - (math.Sin(qNorm) / (qNorm * qNorm * qNorm)) + + quatD = append(quatD, quat.Number{ + Real: -1 * w.Imag * b, + Imag: b + w.Imag*w.Imag*c, + Jmag: w.Imag * w.Jmag * c, + Kmag: w.Imag * w.Kmag * c, + }) + quatD = append(quatD, quat.Number{ + Real: -1 * w.Jmag * b, + Imag: w.Jmag * w.Imag * c, + Jmag: b + w.Jmag*w.Jmag*c, + Kmag: w.Jmag * w.Kmag * c, + }) + quatD = append(quatD, quat.Number{ + Real: -1 * w.Kmag * b, + Imag: w.Kmag * w.Imag * c, + Jmag: w.Kmag * w.Jmag * c, + Kmag: b + w.Kmag*w.Kmag*c, + }) + } else { + quatD = append(quatD, quat.Number{0, 1, 0, 0}) + quatD = append(quatD, quat.Number{0, 0, 1, 0}) + quatD = append(quatD, quat.Number{0, 0, 0, 1}) + } + return quatD +} + +// L2Distance returns the L2 normalized difference between two equal length arrays. +func L2Distance(q1, q2 []float64) float64 { + for i := 0; i < len(q1); i++ { + q1[i] -= q2[i] + } + // 2 is the L value returning a standard L2 Normalization + return floats.Norm(q1, 2) +} diff --git a/motionplan/kinematic_test.go b/motionplan/kinematic_test.go new file mode 100644 index 00000000000..2bc36add521 --- /dev/null +++ b/motionplan/kinematic_test.go @@ -0,0 +1,351 @@ +package motionplan + +import ( + "math" + "math/rand" + "testing" + + "github.com/golang/geo/r3" + pb "go.viam.com/api/component/arm/v1" + "go.viam.com/test" + "gonum.org/v1/gonum/num/quat" + + frame "go.viam.com/rdk/referenceframe" + spatial "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils" +) + +func BenchmarkFK(b *testing.B) { + m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") + test.That(b, err, test.ShouldBeNil) + for n := 0; n < b.N; n++ { + _, err := ComputePosition(m, &pb.JointPositions{Values: make([]float64, 7)}) + test.That(b, err, test.ShouldBeNil) + } +} + +// This should test forward kinematics functions. +func TestForwardKinematics(t *testing.T) { + // Test the 5DOF yahboom arm to confirm kinematics works with non-6dof arms + m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/yahboom/dofbot.json"), "") + test.That(t, err, test.ShouldBeNil) + + // Confirm end effector starts at 248.55, 0, 115 + expect := spatial.NewPose( + r3.Vector{X: 248.55, Y: 0, Z: 115}, + &spatial.OrientationVectorDegrees{Theta: 0, OX: 0, OY: 0, OZ: 1}, + ) + pos, err := ComputePosition(m, &pb.JointPositions{Values: make([]float64, 5)}) + test.That(t, err, test.ShouldBeNil) + test.That(t, spatial.PoseAlmostEqual(expect, pos), test.ShouldBeTrue) + + // Test the 6dof xarm we actually have + m, err = frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + test.That(t, err, test.ShouldBeNil) + + // Confirm end effector starts at 207, 0, 112 + expect = spatial.NewPose( + r3.Vector{X: 207, Y: 0, Z: 112}, + &spatial.OrientationVectorDegrees{Theta: 0, OX: 0, OY: 0, OZ: -1}, + ) + pos, err = ComputePosition(m, &pb.JointPositions{Values: make([]float64, 6)}) + test.That(t, err, test.ShouldBeNil) + test.That(t, spatial.PoseAlmostEqual(expect, pos), test.ShouldBeTrue) + + // Test incorrect joints + _, err = ComputePosition(m, &pb.JointPositions{Values: []float64{}}) + test.That(t, err, test.ShouldNotBeNil) + _, err = ComputePosition(m, &pb.JointPositions{Values: make([]float64, 7)}) + test.That(t, err, test.ShouldNotBeNil) + + newPos := []float64{45, -45, 0, 0, 0, 0} + pos, err = ComputePosition(m, &pb.JointPositions{Values: newPos}) + test.That(t, err, test.ShouldBeNil) + expect = spatial.NewPose( + r3.Vector{X: 181, Y: 181, Z: 303.76}, + &spatial.OrientationVectorDegrees{Theta: 0, OX: 0.5, OY: 0.5, OZ: -0.707}, + ) + test.That(t, spatial.PoseAlmostEqualEps(expect, pos, 0.01), test.ShouldBeTrue) + + newPos = []float64{-45, 0, 0, 0, 0, 45} + pos, err = ComputePosition(m, &pb.JointPositions{Values: newPos}) + test.That(t, err, test.ShouldBeNil) + expect = spatial.NewPose( + r3.Vector{X: 146.37, Y: -146.37, Z: 112}, + &spatial.OrientationVectorDegrees{Theta: 90, OX: 0, OY: 0, OZ: -1}, + ) + test.That(t, spatial.PoseAlmostEqualEps(expect, pos, 0.01), test.ShouldBeTrue) + + // Test out of bounds. Note that ComputePosition will return nil on OOB. + newPos = []float64{-45, 0, 0, 0, 0, 999} + pos, err = ComputePosition(m, &pb.JointPositions{Values: newPos}) + test.That(t, pos, test.ShouldBeNil) + test.That(t, err, test.ShouldNotBeNil) + + // Test out of bounds. Note that ComputeOOBPosition will NOT return nil on OOB. + newPos = []float64{-45, 0, 0, 0, 0, 999} + pos, err = ComputeOOBPosition(m, &pb.JointPositions{Values: newPos}) + expect = spatial.NewPose( + r3.Vector{X: 146.37, Y: -146.37, Z: 112}, + &spatial.R4AA{Theta: math.Pi, RX: 0.31, RY: -0.95, RZ: 0}, + ) + test.That(t, err, test.ShouldBeNil) + test.That(t, spatial.PoseAlmostEqualEps(expect, pos, 0.01), test.ShouldBeTrue) +} + +const derivEqualityEpsilon = 1e-16 + +func derivComponentAlmostEqual(left, right float64) bool { + return math.Abs(left-right) <= derivEqualityEpsilon +} + +func areDerivsEqual(q1, q2 []quat.Number) bool { + if len(q1) != len(q2) { + return false + } + for i, dq1 := range q1 { + dq2 := q2[i] + if !derivComponentAlmostEqual(dq1.Real, dq2.Real) { + return false + } + if !derivComponentAlmostEqual(dq1.Imag, dq2.Imag) { + return false + } + if !derivComponentAlmostEqual(dq1.Jmag, dq2.Jmag) { + return false + } + if !derivComponentAlmostEqual(dq1.Kmag, dq2.Kmag) { + return false + } + } + return true +} + +func TestDeriv(t *testing.T) { + // Test identity quaternion + q := quat.Number{1, 0, 0, 0} + qDeriv := []quat.Number{{0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}} + + match := areDerivsEqual(qDeriv, deriv(q)) + test.That(t, match, test.ShouldBeTrue) + + // Test non-identity single-axis unit quaternion + q = quat.Exp(quat.Number{0, 2, 0, 0}) + + qDeriv = []quat.Number{ + {-0.9092974268256816, -0.4161468365471424, 0, 0}, + {0, 0, 0.4546487134128408, 0}, + {0, 0, 0, 0.4546487134128408}, + } + + match = areDerivsEqual(qDeriv, deriv(q)) + test.That(t, match, test.ShouldBeTrue) + + // Test non-identity multi-axis unit quaternion + q = quat.Exp(quat.Number{0, 2, 1.5, 0.2}) + + qDeriv = []quat.Number{ + {-0.472134934000233, -0.42654977821280804, -0.4969629339096933, -0.06626172452129245}, + {-0.35410120050017474, -0.4969629339096933, -0.13665473343215354, -0.049696293390969336}, + {-0.0472134934000233, -0.06626172452129245, -0.049696293390969336, 0.22944129454798728}, + } + + match = areDerivsEqual(qDeriv, deriv(q)) + test.That(t, match, test.ShouldBeTrue) +} + +// Test dynamic frame systems +// Since kinematics imports reference frame, this needs to be here to avoid circular dependencies. +func TestDynamicFrameSystemXArm(t *testing.T) { + fs := frame.NewEmptyFrameSystem("test") + + model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + test.That(t, err, test.ShouldBeNil) + fs.AddFrame(model, fs.World()) + + positions := frame.StartPositions(fs) + + // World point of xArm at 0 position + poseWorld1 := spatial.NewPoseFromPoint(r3.Vector{207, 0, 112}) + // World point of xArm at (90,-90,90,-90,90,-90) joint positions + poseWorld2 := spatial.NewPoseFromPoint(r3.Vector{97, -207, -98}) + + // Note that because the arm is pointing in a different direction, this point is not a direct inverse of pointWorld2 + pointXarm := spatial.NewPoseFromPoint(r3.Vector{207, 98, -97}) + + transformPoint1, err := fs.Transform(positions, frame.NewPoseInFrame("xArm6", spatial.NewZeroPose()), frame.World) + test.That(t, err, test.ShouldBeNil) + test.That(t, spatial.PoseAlmostCoincident(transformPoint1.(*frame.PoseInFrame).Pose(), poseWorld1), test.ShouldBeTrue) + + // Test ability to calculate hypothetical out-of-bounds positions for the arm, but still return an error + positions["xArm6"] = frame.FloatsToInputs( + []float64{math.Pi / 2, -math.Pi / 2, math.Pi / 2, -math.Pi / 2, math.Pi / 2, -math.Pi / 2}) + transformPoint2, err := fs.Transform(positions, frame.NewPoseInFrame("xArm6", spatial.NewZeroPose()), frame.World) + test.That(t, err, test.ShouldBeNil) + test.That(t, spatial.PoseAlmostCoincident(transformPoint2.(*frame.PoseInFrame).Pose(), poseWorld2), test.ShouldBeTrue) + + transformPoint3, err := fs.Transform(positions, frame.NewPoseInFrame(frame.World, spatial.NewZeroPose()), "xArm6") + test.That(t, err, test.ShouldBeNil) + test.That(t, spatial.PoseAlmostCoincident(transformPoint3.(*frame.PoseInFrame).Pose(), pointXarm), test.ShouldBeTrue) +} + +// Test a complicated dynamic frame system. We model a UR5 at (100,100,200) holding a camera pointing in line with the +// gripper on a 3cm stick. We also model a xArm6 which is placed on an XY gantry, which is zeroed at (-50,-50,-200). +// Ensure that we are able to transform points from the camera frame into world frame, to gantry frame, and to xarm frame. +func TestComplicatedDynamicFrameSystem(t *testing.T) { + fs := frame.NewEmptyFrameSystem("test") + + // robot offsets + urOffset, err := frame.NewStaticFrame("urOffset", spatial.NewPoseFromPoint(r3.Vector{100, 100, 200})) + test.That(t, err, test.ShouldBeNil) + fs.AddFrame(urOffset, fs.World()) + gantryOffset, err := frame.NewStaticFrame("gantryXOffset", spatial.NewPoseFromPoint(r3.Vector{-50, -50, -200})) + test.That(t, err, test.ShouldBeNil) + fs.AddFrame(gantryOffset, fs.World()) + + // build 2 axis gantry manually + gantryX, err := frame.NewTranslationalFrame("gantryX", r3.Vector{1, 0, 0}, frame.Limit{math.Inf(-1), math.Inf(1)}) + test.That(t, err, test.ShouldBeNil) + fs.AddFrame(gantryX, gantryOffset) + gantryY, err := frame.NewTranslationalFrame("gantryY", r3.Vector{0, 1, 0}, frame.Limit{math.Inf(-1), math.Inf(1)}) + test.That(t, err, test.ShouldBeNil) + fs.AddFrame(gantryY, gantryX) + + // xarm on gantry + modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") + test.That(t, err, test.ShouldBeNil) + fs.AddFrame(modelXarm, gantryY) + + // ur5 + modelUR5e, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/universalrobots/ur5e.json"), "") + test.That(t, err, test.ShouldBeNil) + fs.AddFrame(modelUR5e, urOffset) + + // Note that positive Z is always "forwards". If the position of the arm is such that it is pointing elsewhere, + // the resulting translation will be similarly oriented + urCamera, err := frame.NewStaticFrame("urCamera", spatial.NewPoseFromPoint(r3.Vector{0, 0, 30})) + test.That(t, err, test.ShouldBeNil) + fs.AddFrame(urCamera, modelUR5e) + + positions := frame.StartPositions(fs) + + poseUR5e := spatial.NewPoseFromPoint(r3.Vector{-717.2, -132.9, 262.8}) + // Camera translates by 30, gripper is pointed at -Y + poseUR5eCam := spatial.NewPoseFromPoint(r3.Vector{-717.2, -162.9, 262.8}) + + poseXarm := spatial.NewPoseFromPoint(r3.Vector{157., -50, -88}) + poseXarmFromCam := spatial.NewPoseFromPoint(r3.Vector{874.2, -112.9, -350.8}) + + // Check the UR5e and camera default positions + transformPoint1, err := fs.Transform(positions, frame.NewPoseInFrame("UR5e", spatial.NewZeroPose()), frame.World) + test.That(t, err, test.ShouldBeNil) + transformPoint2, err := fs.Transform(positions, frame.NewPoseInFrame("urCamera", spatial.NewZeroPose()), frame.World) + test.That(t, err, test.ShouldBeNil) + transformPoint3, err := fs.Transform(positions, frame.NewPoseInFrame("xArm6", spatial.NewZeroPose()), frame.World) + test.That(t, err, test.ShouldBeNil) + transformPoint4, err := fs.Transform(positions, frame.NewPoseInFrame("urCamera", spatial.NewZeroPose()), "xArm6") + test.That(t, err, test.ShouldBeNil) + test.That(t, spatial.PoseAlmostCoincident(transformPoint1.(*frame.PoseInFrame).Pose(), poseUR5e), test.ShouldBeTrue) + test.That(t, spatial.PoseAlmostCoincident(transformPoint2.(*frame.PoseInFrame).Pose(), poseUR5eCam), test.ShouldBeTrue) + test.That(t, spatial.PoseAlmostCoincident(transformPoint3.(*frame.PoseInFrame).Pose(), poseXarm), test.ShouldBeTrue) + test.That(t, spatial.PoseAlmostCoincident(transformPoint4.(*frame.PoseInFrame).Pose(), poseXarmFromCam), test.ShouldBeTrue) + + // Move the UR5e so its local Z axis is pointing approximately towards the xArm (at positive X) + positions["UR5e"] = frame.FloatsToInputs([]float64{0, 0, 0, 0, -math.Pi / 2, -math.Pi / 2}) + + // A point that is 813.6, -50, 200 from the camera + // This puts the point in the Z plane of the xArm6 + targetPoint := spatial.NewPoseFromPoint(r3.Vector{350.8, -50, 200}) + // Target point in world + tf, err := fs.Transform(positions, frame.NewPoseInFrame("urCamera", targetPoint), frame.World) + test.That(t, err, test.ShouldBeNil) + worldPointLoc := spatial.NewPoseFromPoint(tf.(*frame.PoseInFrame).Pose().Point()) + + // Move the XY gantry such that the xArm6 is now at the point specified + positions["gantryX"] = frame.FloatsToInputs([]float64{worldPointLoc.Point().X - poseXarm.Point().X}) + positions["gantryY"] = frame.FloatsToInputs([]float64{worldPointLoc.Point().Y - poseXarm.Point().Y}) + + // Confirm the xArm6 is now at the same location as the point + newPointXarm, err := fs.Transform(positions, frame.NewPoseInFrame("xArm6", spatial.NewZeroPose()), frame.World) + test.That(t, err, test.ShouldBeNil) + test.That(t, spatial.PoseAlmostCoincident(newPointXarm.(*frame.PoseInFrame).Pose(), worldPointLoc), test.ShouldBeTrue) + + // If the above passes, then converting one directly to the other should be (0,0,0) + pointCamToXarm, err := fs.Transform(positions, frame.NewPoseInFrame("urCamera", targetPoint), "xArm6") + test.That(t, err, test.ShouldBeNil) + test.That(t, spatial.PoseAlmostCoincident(pointCamToXarm.(*frame.PoseInFrame).Pose(), spatial.NewZeroPose()), test.ShouldBeTrue) +} + +func TestSVAvsDH(t *testing.T) { + mSVA, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/universalrobots/ur5e.json"), "") + test.That(t, err, test.ShouldBeNil) + mDH, err := frame.ParseModelJSONFile(utils.ResolveFile("referenceframe/testjson/ur5eDH.json"), "") + test.That(t, err, test.ShouldBeNil) + + numTests := 10000 + + seed := rand.New(rand.NewSource(23)) + for i := 0; i < numTests; i++ { + joints := mSVA.ProtobufFromInput(frame.RandomFrameInputs(mSVA, seed)) + + posSVA, err := ComputePosition(mSVA, joints) + test.That(t, err, test.ShouldBeNil) + posDH, err := ComputePosition(mDH, joints) + test.That(t, err, test.ShouldBeNil) + test.That(t, spatial.PoseAlmostEqual(posSVA, posDH), test.ShouldBeTrue) + } +} + +// Test loading model kinematics of the same arm via ModelJSON parsing and URDF parsing and comparing results. +func TestKinematicsJSONvsURDF(t *testing.T) { + numTests := 100 + + mJSON, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/universalrobots/ur5e.json"), "") + test.That(t, err, test.ShouldBeNil) + mURDF, err := frame.ParseURDFFile(utils.ResolveFile("referenceframe/testurdf/ur5_viam.urdf"), "") + test.That(t, err, test.ShouldBeNil) + + seed := rand.New(rand.NewSource(50)) + for i := 0; i < numTests; i++ { + joints := frame.JointPositionsFromRadians(frame.GenerateRandomConfiguration(mJSON, seed)) + + posJSON, err := ComputePosition(mJSON, joints) + test.That(t, err, test.ShouldBeNil) + posURDF, err := ComputePosition(mURDF, joints) + test.That(t, err, test.ShouldBeNil) + + test.That(t, spatial.PoseAlmostEqual(posJSON, posURDF), test.ShouldBeTrue) + } +} + +func TestComputeOOBPosition(t *testing.T) { + model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "foo") + test.That(t, err, test.ShouldBeNil) + test.That(t, model.Name(), test.ShouldEqual, "foo") + + jointPositions := &pb.JointPositions{Values: []float64{1.1, 2.2, 3.3, 1.1, 2.2, 3.3}} + + t.Run("succeed", func(t *testing.T) { + pose, err := ComputeOOBPosition(model, jointPositions) + test.That(t, err, test.ShouldBeNil) + test.That(t, pose, test.ShouldNotBeNil) + }) + + t.Run("fail when JointPositions are nil", func(t *testing.T) { + var NilJointPositions *pb.JointPositions + + pose, err := ComputeOOBPosition(model, NilJointPositions) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, pose, test.ShouldBeNil) + test.That(t, err, test.ShouldEqual, frame.ErrNilJointPositions) + }) + + t.Run("fail when model frame is nil", func(t *testing.T) { + var NilModel frame.Model + + pose, err := ComputeOOBPosition(NilModel, jointPositions) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, pose, test.ShouldBeNil) + test.That(t, err, test.ShouldEqual, frame.ErrNilModelFrame) + }) +} diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index bf4ed736fb5..0c548e21a47 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -33,6 +33,7 @@ type motionPlanner interface { checkInputs([]frame.Input) bool getSolutions(context.Context, []frame.Input) ([]node, error) opt() *plannerOptions + sample(node, int) (node, error) } type plannerConstructor func(frame.Frame, *rand.Rand, golog.Logger, *plannerOptions) (motionPlanner, error) @@ -188,14 +189,18 @@ func (mp *planner) checkPath(seedInputs, target []frame.Input) bool { return ok } -func (mp *planner) sample(rSeed node, sampleNum int) ([]frame.Input, error) { +func (mp *planner) sample(rSeed node, sampleNum int) (node, error) { // If we have done more than 50 iterations, start seeding off completely random positions 2 at a time // The 2 at a time is to ensure random seeds are added onto both the seed and goal maps. if sampleNum >= mp.planOpts.IterBeforeRand && sampleNum%4 >= 2 { - return frame.RandomFrameInputs(mp.frame, mp.randseed), nil + return newConfigurationNode(frame.RandomFrameInputs(mp.frame, mp.randseed)), nil } // Seeding nearby to valid points results in much faster convergence in less constrained space - return frame.RestrictedRandomFrameInputs(mp.frame, mp.randseed, 0.1, rSeed.Q()) + q, err := frame.RestrictedRandomFrameInputs(mp.frame, mp.randseed, 0.1, rSeed.Q()) + if err != nil { + return nil, err + } + return newConfigurationNode(q), nil } func (mp *planner) opt() *plannerOptions { @@ -267,7 +272,7 @@ func (mp *planner) getSolutions(ctx context.Context, seed []frame.Input) ([]node ctxWithCancel, cancel := context.WithCancel(ctx) defer cancel() - solutionGen := make(chan *ik.IKSolution, mp.planOpts.NumThreads) + solutionGen := make(chan *ik.Solution, mp.planOpts.NumThreads) ikErr := make(chan error, 1) var activeSolvers sync.WaitGroup defer activeSolvers.Wait() diff --git a/motionplan/nearestNeighbor.go b/motionplan/nearestNeighbor.go index 3fa5bae069a..2ff20bc46e3 100644 --- a/motionplan/nearestNeighbor.go +++ b/motionplan/nearestNeighbor.go @@ -6,6 +6,7 @@ import ( "sort" "go.viam.com/utils" + "go.viam.com/rdk/motionplan/ik" ) diff --git a/motionplan/rrtStarConnect.go b/motionplan/rrtStarConnect.go index 7d30df7ac29..56811915e28 100644 --- a/motionplan/rrtStarConnect.go +++ b/motionplan/rrtStarConnect.go @@ -116,7 +116,7 @@ func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, } rrt.maps = planSeed.maps } - target := referenceframe.InterpolateInputs(seed, rrt.maps.optNode.Q(), 0.5) + target := newConfigurationNode(referenceframe.InterpolateInputs(seed, rrt.maps.optNode.Q(), 0.5)) map1, map2 := rrt.maps.startMap, rrt.maps.goalMap // Keep a list of the node pairs that have the same inputs @@ -144,7 +144,7 @@ func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, default: } - tryExtend := func(target []referenceframe.Input) (node, node, error) { + tryExtend := func(target node) (node, node, error) { // attempt to extend maps 1 and 2 towards the target // If ctx is done, nearest neighbors will be invalid and we want to return immediately select { @@ -154,10 +154,10 @@ func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, } utils.PanicCapturingGo(func() { - mp.extend(ctx, map1, newConfigurationNode(target), m1chan) + mp.extend(ctx, map1, target, m1chan) }) utils.PanicCapturingGo(func() { - mp.extend(ctx, map2, newConfigurationNode(target), m2chan) + mp.extend(ctx, map2, target, m2chan) }) map1reached := <-m1chan map2reached := <-m2chan @@ -175,7 +175,7 @@ func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, // Second iteration; extend maps 1 and 2 towards the halfway point between where they reached if reachedDelta > mp.planOpts.JointSolveDist { - target = referenceframe.InterpolateInputs(map1reached.Q(), map2reached.Q(), 0.5) + target = newConfigurationNode(referenceframe.InterpolateInputs(map1reached.Q(), map2reached.Q(), 0.5)) map1reached, map2reached, err = tryExtend(target) if err != nil { rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index 07aa7a25244..7653236b06b 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -41,6 +41,8 @@ const ( // When extending the RRT tree towards some point, do not extend more than this many times in a single RRT invocation. defaultMaxReseeds = 50 + // For whatever `refDist` is used for the generation of the original path, scale that by this amount when smoothing. + // This can help to find paths. defaultSmoothScaleFactor = 0.5 // Make an attempt to solve the tree every this many iterations @@ -73,7 +75,7 @@ type tpspaceOptions struct { // Print very fine-grained debug info. Useful for observing the inner RRT tree structure directly pathdebug bool - + // random value to seed the IK solver. Can be anything in the middle of the valid manifold. ikSeed []referenceframe.Input @@ -99,7 +101,7 @@ type nodeAndError struct { // tpSpaceRRTMotionPlanner. type tpSpaceRRTMotionPlanner struct { *planner - mu sync.Mutex + mu sync.Mutex algOpts *tpspaceOptions tpFrame tpspace.PTGProvider } @@ -129,9 +131,9 @@ func newTPSpaceMotionPlanner( tpFrame: tpFrame, } tpPlanner.setupTPSpaceOptions() - + tpPlanner.algOpts.ikSeed = []referenceframe.Input{{math.Pi / 2}, {tpFrame.PTGs()[0].RefDistance() / 2}} - + return tpPlanner, nil } @@ -203,39 +205,21 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( } } - bidirectional := true - m1chan := make(chan *nodeAndError, 1) m2chan := make(chan *nodeAndError, 1) defer close(m1chan) defer close(m2chan) dist := math.Sqrt(mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: startPose, EndPosition: goalPose})) - midPt := spatialmath.Interpolate(startPose, goalPose, 0.5) // used for initial seed + midptNode := &basicNode{pose: spatialmath.Interpolate(startPose, goalPose, 0.5), cost: dist} // used for initial seed + var randPosNode node = midptNode - var randPos spatialmath.Pose for iter := 0; iter < mp.planOpts.PlanIter; iter++ { if ctx.Err() != nil { mp.logger.Debugf("TP Space RRT timed out after %d iterations", iter) rrt.solutionChan <- &rrtPlanReturn{planerr: fmt.Errorf("TP Space RRT timeout %w", ctx.Err()), maps: rrt.maps} return } - // Get random cartesian configuration - rDist := dist * (mp.algOpts.autoBB + float64(iter)/10.) - randPosX := float64(mp.randseed.Intn(int(rDist))) - randPosY := float64(mp.randseed.Intn(int(rDist))) - randPosTheta := math.Pi * (mp.randseed.Float64() - 0.5) - randPos = spatialmath.NewPose( - r3.Vector{midPt.Point().X + (randPosX - rDist/2.), midPt.Point().Y + (randPosY - rDist/2.), 0}, - &spatialmath.OrientationVector{OZ: 1, Theta: randPosTheta}, - ) - if iter == 0 { - randPos = midPt - } - if !bidirectional && iter%mp.algOpts.attemptSolveEvery == 0 { - randPos = goalPose - } - randPosNode := &basicNode{pose: randPos} utils.PanicCapturingGo(func() { m1chan <- mp.attemptExtension(ctx, randPosNode, rrt.maps.startMap, false) @@ -280,6 +264,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( } } if iter%mp.algOpts.attemptSolveEvery == 0 { + // Attempt a solve; we exhaustively iterate through our goal tree and attempt to find any connection to the seed tree paths := [][]node{} for goalMapNode := range rrt.maps.goalMap { seedReached := mp.attemptExtension(ctx, goalMapNode, rrt.maps.startMap, false) @@ -316,6 +301,13 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( return } } + + // Get random cartesian configuration + randPosNode, err = mp.sample(midptNode, iter+1) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return + } } rrt.solutionChan <- &rrtPlanReturn{maps: rrt.maps, planerr: errors.New("tpspace RRT unable to create valid path")} } @@ -352,20 +344,6 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( // then dynamically expanding or contracting the limits of IK to be some fraction of that distance. // Get cartesian distance from NN to rand - //~ var targetFunc func(spatialmath.Pose) float64 - //~ if invert { - //~ sqMet := ik.NewSquaredNormMetric(randPosNode.Pose()) - //~ targetFunc = func(pose spatialmath.Pose) float64 { - //~ return sqMet(&ik.State{Position: spatialmath.Compose(nearest.Pose(), spatialmath.PoseInverse(pose))}) - //~ } - //~ } else { - //~ relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) - //~ sqMet := ik.NewSquaredNormMetric(relPose) - //~ targetFunc = func(pose spatialmath.Pose) float64 { - //~ return sqMet(&ik.State{Position: pose}) - //~ } - //~ } - var targetFunc ik.StateMetric if invert { sqMet := ik.NewSquaredNormMetric(randPosNode.Pose()) @@ -376,10 +354,10 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) targetFunc = ik.NewSquaredNormMetric(relPose) } - solutionChan := make(chan *ik.IKSolution, 1) + solutionChan := make(chan *ik.Solution, 1) err := curPtg.Solve(context.Background(), solutionChan, mp.algOpts.ikSeed, targetFunc, mp.randseed.Int()) - - var bestNode *ik.IKSolution + + var bestNode *ik.Solution select { case bestNode = <-solutionChan: default: @@ -391,7 +369,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( if err != nil { return nil, err } - + bestDist := targetFunc(&ik.State{Position: pose}) goalAlpha := bestNode.Configuration[0].Value goalD := bestNode.Configuration[1].Value @@ -650,6 +628,7 @@ func (mp *tpSpaceRRTMotionPlanner) setupTPSpaceOptions() { func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, invert bool) *plannerOptions { opts := newBasicPlannerOptions(mp.frame) mp.mu.Lock() + //nolint: gosec randSeed := rand.New(rand.NewSource(mp.randseed.Int63() + mp.randseed.Int63())) mp.mu.Unlock() @@ -670,10 +649,10 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, relPose := spatialmath.Compose(spatialmath.PoseInverse(seg.EndPosition), seg.StartPosition) targetFunc = ik.NewSquaredNormMetric(relPose) } - solutionChan := make(chan *ik.IKSolution, 1) + solutionChan := make(chan *ik.Solution, 1) err := ptg.Solve(context.Background(), solutionChan, mp.algOpts.ikSeed, targetFunc, randSeed.Int()) - - var closeNode *ik.IKSolution + + var closeNode *ik.Solution select { case closeNode = <-solutionChan: default: @@ -771,7 +750,8 @@ func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) return path } -// firstEdge and secondEdge must not be adjacent. +// attemptSmooth attempts to connect two given points in a path. The points must not be adjacent. +// Strategy is to subdivide the seed-side trajectories to give a greater probability of solving. func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( ctx context.Context, path []node, @@ -823,3 +803,19 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( } return mp.rectifyPath(newInputSteps) } + +func (mp *tpSpaceRRTMotionPlanner) sample(rSeed node, iter int) (node, error) { + dist := rSeed.Cost() + if dist == 0 { + dist = 1.0 + } + rDist := dist * (mp.algOpts.autoBB + float64(iter)/10.) + randPosX := float64(mp.randseed.Intn(int(rDist))) + randPosY := float64(mp.randseed.Intn(int(rDist))) + randPosTheta := math.Pi * (mp.randseed.Float64() - 0.5) + randPos := spatialmath.NewPose( + r3.Vector{rSeed.Pose().Point().X + (randPosX - rDist/2.), rSeed.Pose().Point().Y + (randPosY - rDist/2.), 0}, + &spatialmath.OrientationVector{OZ: 1, Theta: randPosTheta}, + ) + return &basicNode{pose: randPos}, nil +} diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index ccb9eed7d11..8c68cbc251d 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -4,9 +4,9 @@ package tpspace import ( "math" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" - "go.viam.com/rdk/motionplan/ik" ) const floatEpsilon = 0.0001 // If floats are closer than this consider them equal diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index 8ffe327e494..ca0bdfb6f61 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -4,8 +4,8 @@ import ( "context" "math" - "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/motionplan/ik" + "go.viam.com/rdk/referenceframe" ) const ( @@ -57,7 +57,7 @@ func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64, endsOnly bo func (ptg *ptgGridSim) Solve( ctx context.Context, - solutionChan chan<- *ik.IKSolution, + solutionChan chan<- *ik.Solution, seed []referenceframe.Input, solveMetric ik.StateMetric, rseed int, @@ -80,10 +80,10 @@ func (ptg *ptgGridSim) Solve( } if bestNode != nil { - solutionChan <- &ik.IKSolution{ + solutionChan <- &ik.Solution{ Configuration: []referenceframe.Input{{bestNode.Alpha}, {bestNode.Dist}}, - Score: bestDist, - Exact: false, + Score: bestDist, + Exact: false, } return nil } @@ -102,10 +102,10 @@ func (ptg *ptgGridSim) Solve( } } - solutionChan <- &ik.IKSolution{ + solutionChan <- &ik.Solution{ Configuration: []referenceframe.Input{{bestNode.Alpha}, {bestNode.Dist}}, - Score: bestDist, - Exact: false, + Score: bestDist, + Exact: false, } return nil } diff --git a/motionplan/tpspace/ptgGroupFrame.go b/motionplan/tpspace/ptgGroupFrame.go index fdaf598e2e4..61720ac379e 100644 --- a/motionplan/tpspace/ptgGroupFrame.go +++ b/motionplan/tpspace/ptgGroupFrame.go @@ -18,7 +18,7 @@ const ( distanceAlongTrajectoryIndex ) -// If refDist is not explicitly set, default to pi radians times this adjustment value +// If refDist is not explicitly set, default to pi radians times this adjustment value. const refDistHalfCircles = 0.9 type ptgFactory func(float64, float64) PrecomputePTG @@ -33,13 +33,13 @@ var defaultPTGs = []ptgFactory{ } type ptgGroupFrame struct { - name string - limits []referenceframe.Limit - geometries []spatialmath.Geometry - ptgs []PTG - velocityMMps float64 + name string + limits []referenceframe.Limit + geometries []spatialmath.Geometry + ptgs []PTG + velocityMMps float64 turnRadMillimeters float64 - logger golog.Logger + logger golog.Logger } // NewPTGFrameFromTurningRadius will create a new Frame which is also a PTGProvider. It will precompute the default set of @@ -59,7 +59,7 @@ func NewPTGFrameFromTurningRadius( if refDist < 0 { return nil, fmt.Errorf("cannot create ptg frame, refDist %f must be >=0", refDist) } - + turnRadMillimeters := turnRadMeters * 1000 if refDist == 0 { diff --git a/motionplan/tpspace/ptgIK.go b/motionplan/tpspace/ptgIK.go index 57408110fe7..3ec6750eba7 100644 --- a/motionplan/tpspace/ptgIK.go +++ b/motionplan/tpspace/ptgIK.go @@ -35,7 +35,7 @@ func NewPTGIK(simPTG PrecomputePTG, logger golog.Logger, refDist float64, randSe if refDist <= 0 { return nil, errors.New("refDist must be greater than zero") } - + ptgFrame, err := NewPTGIKFrame(simPTG, refDist) if err != nil { return nil, err @@ -66,19 +66,19 @@ func NewPTGIK(simPTG PrecomputePTG, logger golog.Logger, refDist float64, randSe func (ptg *ptgIK) Solve( ctx context.Context, - solutionChan chan<- *ik.IKSolution, + solutionChan chan<- *ik.Solution, seed []referenceframe.Input, solveMetric ik.StateMetric, nloptSeed int, ) error { - internalSolutionGen := make(chan *ik.IKSolution, 1) + internalSolutionGen := make(chan *ik.Solution, 1) defer close(internalSolutionGen) - var solved *ik.IKSolution - + var solved *ik.Solution + // Spawn the IK solver to generate a solution err := ptg.fastGradDescent.Solve(ctx, internalSolutionGen, seed, solveMetric, nloptSeed) // We should have zero or one solutions - + select { case solved = <-internalSolutionGen: default: @@ -92,7 +92,7 @@ func (ptg *ptgIK) Solve( // nlopt returned something but was unable to complete the solve. See if the grid check produces something better. err = ptg.gridSim.Solve(ctx, internalSolutionGen, seed, solveMetric, nloptSeed) if err == nil { - var gridSolved *ik.IKSolution + var gridSolved *ik.Solution select { case gridSolved = <-internalSolutionGen: default: @@ -120,7 +120,7 @@ func (ptg *ptgIK) Trajectory(alpha, dist float64) ([]*TrajNode, error) { ptg.mu.RUnlock() if precomp != nil { if precomp[len(precomp)-1].Dist >= dist { - // Cacheing here provides a ~33% speedup to a solve call + // Caching here provides a ~33% speedup to a solve call subTraj := []*TrajNode{} for _, wp := range precomp { if wp.Dist < dist { diff --git a/motionplan/tpspace/ptgSideS.go b/motionplan/tpspace/ptgSideS.go index 8de4e58249d..5c8ab838895 100644 --- a/motionplan/tpspace/ptgSideS.go +++ b/motionplan/tpspace/ptgSideS.go @@ -14,8 +14,8 @@ const defaultCountersteer = 1.5 type ptgDiffDriveSideS struct { maxMMPS float64 // millimeters per second velocity to target maxRPS float64 // radians per second of rotation when driving at maxMMPS and turning at max turning radius - r float64 - countersteer float64 + r float64 // turning radius + countersteer float64 // scale the length of the second arc by this much circle *ptgDiffDriveC } From c534fa08120fbca7b5abefbf414421331fa89cf9 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Tue, 22 Aug 2023 15:11:07 -0700 Subject: [PATCH 28/38] Delete files --- motionplan/ik/kinematic.go | 111 -------- motionplan/ik/kinematic_test.go | 455 -------------------------------- 2 files changed, 566 deletions(-) delete mode 100644 motionplan/ik/kinematic.go delete mode 100644 motionplan/ik/kinematic_test.go diff --git a/motionplan/ik/kinematic.go b/motionplan/ik/kinematic.go deleted file mode 100644 index dbc5cd3149a..00000000000 --- a/motionplan/ik/kinematic.go +++ /dev/null @@ -1,111 +0,0 @@ -package ik - -import ( - "math" - "strings" - - "github.com/pkg/errors" - pb "go.viam.com/api/component/arm/v1" - "gonum.org/v1/gonum/floats" - "gonum.org/v1/gonum/num/quat" - - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/spatialmath" -) - -// ComputePosition takes a model and a protobuf JointPositions in degrees and returns the cartesian position of the -// end effector as a protobuf ArmPosition. This is performed statelessly without changing any data. -func ComputePosition(model referenceframe.Frame, joints *pb.JointPositions) (spatialmath.Pose, error) { - if len(joints.Values) != len(model.DoF()) { - return nil, errors.Errorf( - "incorrect number of joints passed to ComputePosition. Want: %d, got: %d", - len(model.DoF()), - len(joints.Values), - ) - } - - pose, err := model.Transform(model.InputFromProtobuf(joints)) - if err != nil { - return nil, err - } - - return pose, nil -} - -// ComputeOOBPosition takes a model and a protobuf JointPositions in degrees and returns the cartesian -// position of the end effector as a protobuf ArmPosition even when the arm is in an out of bounds state. -// This is performed statelessly without changing any data. -func ComputeOOBPosition(model referenceframe.Frame, joints *pb.JointPositions) (spatialmath.Pose, error) { - if joints == nil { - return nil, referenceframe.ErrNilJointPositions - } - if model == nil { - return nil, referenceframe.ErrNilModelFrame - } - - if len(joints.Values) != len(model.DoF()) { - return nil, errors.Errorf( - "incorrect number of joints passed to ComputePosition. Want: %d, got: %d", - len(model.DoF()), - len(joints.Values), - ) - } - - pose, err := model.Transform(model.InputFromProtobuf(joints)) - if err != nil && !strings.Contains(err.Error(), referenceframe.OOBErrString) { - return nil, err - } - - return pose, nil -} - -// deriv will compute D(q), the derivative of q = e^w with respect to w -// Note that for prismatic joints, this will need to be expanded to dual quaternions. -func deriv(q quat.Number) []quat.Number { - w := quat.Log(q) - - qNorm := math.Sqrt(w.Imag*w.Imag + w.Jmag*w.Jmag + w.Kmag*w.Kmag) - // qNorm hits a singularity every 2pi - // But if we flip the axis we get the same rotation but away from a singularity - - var quatD []quat.Number - - // qNorm is non-zero if our joint has a non-zero rotation - if qNorm > 0 { - b := math.Sin(qNorm) / qNorm - c := (math.Cos(qNorm) / (qNorm * qNorm)) - (math.Sin(qNorm) / (qNorm * qNorm * qNorm)) - - quatD = append(quatD, quat.Number{ - Real: -1 * w.Imag * b, - Imag: b + w.Imag*w.Imag*c, - Jmag: w.Imag * w.Jmag * c, - Kmag: w.Imag * w.Kmag * c, - }) - quatD = append(quatD, quat.Number{ - Real: -1 * w.Jmag * b, - Imag: w.Jmag * w.Imag * c, - Jmag: b + w.Jmag*w.Jmag*c, - Kmag: w.Jmag * w.Kmag * c, - }) - quatD = append(quatD, quat.Number{ - Real: -1 * w.Kmag * b, - Imag: w.Kmag * w.Imag * c, - Jmag: w.Kmag * w.Jmag * c, - Kmag: b + w.Kmag*w.Kmag*c, - }) - } else { - quatD = append(quatD, quat.Number{0, 1, 0, 0}) - quatD = append(quatD, quat.Number{0, 0, 1, 0}) - quatD = append(quatD, quat.Number{0, 0, 0, 1}) - } - return quatD -} - -// L2Distance returns the L2 normalized difference between two equal length arrays. -func L2Distance(q1, q2 []float64) float64 { - for i := 0; i < len(q1); i++ { - q1[i] -= q2[i] - } - // 2 is the L value returning a standard L2 Normalization - return floats.Norm(q1, 2) -} diff --git a/motionplan/ik/kinematic_test.go b/motionplan/ik/kinematic_test.go deleted file mode 100644 index a2e93add684..00000000000 --- a/motionplan/ik/kinematic_test.go +++ /dev/null @@ -1,455 +0,0 @@ -package ik - -import ( - "context" - "errors" - "math" - "math/rand" - "runtime" - "testing" - - "github.com/edaniels/golog" - "github.com/golang/geo/r3" - pb "go.viam.com/api/component/arm/v1" - "go.viam.com/test" - "gonum.org/v1/gonum/num/quat" - - frame "go.viam.com/rdk/referenceframe" - spatial "go.viam.com/rdk/spatialmath" - "go.viam.com/rdk/utils" -) - -var ( - home = frame.FloatsToInputs([]float64{0, 0, 0, 0, 0, 0}) - nCPU = int(math.Max(1.0, float64(runtime.NumCPU()/4))) -) - -func BenchmarkFK(b *testing.B) { - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") - test.That(b, err, test.ShouldBeNil) - for n := 0; n < b.N; n++ { - _, err := ComputePosition(m, &pb.JointPositions{Values: make([]float64, 7)}) - test.That(b, err, test.ShouldBeNil) - } -} - -// This should test forward kinematics functions. -func TestForwardKinematics(t *testing.T) { - // Test the 5DOF yahboom arm to confirm kinematics works with non-6dof arms - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/yahboom/dofbot.json"), "") - test.That(t, err, test.ShouldBeNil) - - // Confirm end effector starts at 248.55, 0, 115 - expect := spatial.NewPose( - r3.Vector{X: 248.55, Y: 0, Z: 115}, - &spatial.OrientationVectorDegrees{Theta: 0, OX: 0, OY: 0, OZ: 1}, - ) - pos, err := ComputePosition(m, &pb.JointPositions{Values: make([]float64, 5)}) - test.That(t, err, test.ShouldBeNil) - test.That(t, spatial.PoseAlmostEqual(expect, pos), test.ShouldBeTrue) - - // Test the 6dof xarm we actually have - m, err = frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") - test.That(t, err, test.ShouldBeNil) - - // Confirm end effector starts at 207, 0, 112 - expect = spatial.NewPose( - r3.Vector{X: 207, Y: 0, Z: 112}, - &spatial.OrientationVectorDegrees{Theta: 0, OX: 0, OY: 0, OZ: -1}, - ) - pos, err = ComputePosition(m, &pb.JointPositions{Values: make([]float64, 6)}) - test.That(t, err, test.ShouldBeNil) - test.That(t, spatial.PoseAlmostEqual(expect, pos), test.ShouldBeTrue) - - // Test incorrect joints - _, err = ComputePosition(m, &pb.JointPositions{Values: []float64{}}) - test.That(t, err, test.ShouldNotBeNil) - _, err = ComputePosition(m, &pb.JointPositions{Values: make([]float64, 7)}) - test.That(t, err, test.ShouldNotBeNil) - - newPos := []float64{45, -45, 0, 0, 0, 0} - pos, err = ComputePosition(m, &pb.JointPositions{Values: newPos}) - test.That(t, err, test.ShouldBeNil) - expect = spatial.NewPose( - r3.Vector{X: 181, Y: 181, Z: 303.76}, - &spatial.OrientationVectorDegrees{Theta: 0, OX: 0.5, OY: 0.5, OZ: -0.707}, - ) - test.That(t, spatial.PoseAlmostEqualEps(expect, pos, 0.01), test.ShouldBeTrue) - - newPos = []float64{-45, 0, 0, 0, 0, 45} - pos, err = ComputePosition(m, &pb.JointPositions{Values: newPos}) - test.That(t, err, test.ShouldBeNil) - expect = spatial.NewPose( - r3.Vector{X: 146.37, Y: -146.37, Z: 112}, - &spatial.OrientationVectorDegrees{Theta: 90, OX: 0, OY: 0, OZ: -1}, - ) - test.That(t, spatial.PoseAlmostEqualEps(expect, pos, 0.01), test.ShouldBeTrue) - - // Test out of bounds. Note that ComputePosition will return nil on OOB. - newPos = []float64{-45, 0, 0, 0, 0, 999} - pos, err = ComputePosition(m, &pb.JointPositions{Values: newPos}) - test.That(t, pos, test.ShouldBeNil) - test.That(t, err, test.ShouldNotBeNil) - - // Test out of bounds. Note that ComputeOOBPosition will NOT return nil on OOB. - newPos = []float64{-45, 0, 0, 0, 0, 999} - pos, err = ComputeOOBPosition(m, &pb.JointPositions{Values: newPos}) - expect = spatial.NewPose( - r3.Vector{X: 146.37, Y: -146.37, Z: 112}, - &spatial.R4AA{Theta: math.Pi, RX: 0.31, RY: -0.95, RZ: 0}, - ) - test.That(t, err, test.ShouldBeNil) - test.That(t, spatial.PoseAlmostEqualEps(expect, pos, 0.01), test.ShouldBeTrue) -} - -const derivEqualityEpsilon = 1e-16 - -func derivComponentAlmostEqual(left, right float64) bool { - return math.Abs(left-right) <= derivEqualityEpsilon -} - -func areDerivsEqual(q1, q2 []quat.Number) bool { - if len(q1) != len(q2) { - return false - } - for i, dq1 := range q1 { - dq2 := q2[i] - if !derivComponentAlmostEqual(dq1.Real, dq2.Real) { - return false - } - if !derivComponentAlmostEqual(dq1.Imag, dq2.Imag) { - return false - } - if !derivComponentAlmostEqual(dq1.Jmag, dq2.Jmag) { - return false - } - if !derivComponentAlmostEqual(dq1.Kmag, dq2.Kmag) { - return false - } - } - return true -} - -func TestDeriv(t *testing.T) { - // Test identity quaternion - q := quat.Number{1, 0, 0, 0} - qDeriv := []quat.Number{{0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}} - - match := areDerivsEqual(qDeriv, deriv(q)) - test.That(t, match, test.ShouldBeTrue) - - // Test non-identity single-axis unit quaternion - q = quat.Exp(quat.Number{0, 2, 0, 0}) - - qDeriv = []quat.Number{ - {-0.9092974268256816, -0.4161468365471424, 0, 0}, - {0, 0, 0.4546487134128408, 0}, - {0, 0, 0, 0.4546487134128408}, - } - - match = areDerivsEqual(qDeriv, deriv(q)) - test.That(t, match, test.ShouldBeTrue) - - // Test non-identity multi-axis unit quaternion - q = quat.Exp(quat.Number{0, 2, 1.5, 0.2}) - - qDeriv = []quat.Number{ - {-0.472134934000233, -0.42654977821280804, -0.4969629339096933, -0.06626172452129245}, - {-0.35410120050017474, -0.4969629339096933, -0.13665473343215354, -0.049696293390969336}, - {-0.0472134934000233, -0.06626172452129245, -0.049696293390969336, 0.22944129454798728}, - } - - match = areDerivsEqual(qDeriv, deriv(q)) - test.That(t, match, test.ShouldBeTrue) -} - -// Test dynamic frame systems -// Since kinematics imports reference frame, this needs to be here to avoid circular dependencies. -func TestDynamicFrameSystemXArm(t *testing.T) { - fs := frame.NewEmptyFrameSystem("test") - - model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") - test.That(t, err, test.ShouldBeNil) - fs.AddFrame(model, fs.World()) - - positions := frame.StartPositions(fs) - - // World point of xArm at 0 position - poseWorld1 := spatial.NewPoseFromPoint(r3.Vector{207, 0, 112}) - // World point of xArm at (90,-90,90,-90,90,-90) joint positions - poseWorld2 := spatial.NewPoseFromPoint(r3.Vector{97, -207, -98}) - - // Note that because the arm is pointing in a different direction, this point is not a direct inverse of pointWorld2 - pointXarm := spatial.NewPoseFromPoint(r3.Vector{207, 98, -97}) - - transformPoint1, err := fs.Transform(positions, frame.NewPoseInFrame("xArm6", spatial.NewZeroPose()), frame.World) - test.That(t, err, test.ShouldBeNil) - test.That(t, spatial.PoseAlmostCoincident(transformPoint1.(*frame.PoseInFrame).Pose(), poseWorld1), test.ShouldBeTrue) - - // Test ability to calculate hypothetical out-of-bounds positions for the arm, but still return an error - positions["xArm6"] = frame.FloatsToInputs( - []float64{math.Pi / 2, -math.Pi / 2, math.Pi / 2, -math.Pi / 2, math.Pi / 2, -math.Pi / 2}) - transformPoint2, err := fs.Transform(positions, frame.NewPoseInFrame("xArm6", spatial.NewZeroPose()), frame.World) - test.That(t, err, test.ShouldBeNil) - test.That(t, spatial.PoseAlmostCoincident(transformPoint2.(*frame.PoseInFrame).Pose(), poseWorld2), test.ShouldBeTrue) - - transformPoint3, err := fs.Transform(positions, frame.NewPoseInFrame(frame.World, spatial.NewZeroPose()), "xArm6") - test.That(t, err, test.ShouldBeNil) - test.That(t, spatial.PoseAlmostCoincident(transformPoint3.(*frame.PoseInFrame).Pose(), pointXarm), test.ShouldBeTrue) -} - -// Test a complicated dynamic frame system. We model a UR5 at (100,100,200) holding a camera pointing in line with the -// gripper on a 3cm stick. We also model a xArm6 which is placed on an XY gantry, which is zeroed at (-50,-50,-200). -// Ensure that we are able to transform points from the camera frame into world frame, to gantry frame, and to xarm frame. -func TestComplicatedDynamicFrameSystem(t *testing.T) { - fs := frame.NewEmptyFrameSystem("test") - - // robot offsets - urOffset, err := frame.NewStaticFrame("urOffset", spatial.NewPoseFromPoint(r3.Vector{100, 100, 200})) - test.That(t, err, test.ShouldBeNil) - fs.AddFrame(urOffset, fs.World()) - gantryOffset, err := frame.NewStaticFrame("gantryXOffset", spatial.NewPoseFromPoint(r3.Vector{-50, -50, -200})) - test.That(t, err, test.ShouldBeNil) - fs.AddFrame(gantryOffset, fs.World()) - - // build 2 axis gantry manually - gantryX, err := frame.NewTranslationalFrame("gantryX", r3.Vector{1, 0, 0}, frame.Limit{math.Inf(-1), math.Inf(1)}) - test.That(t, err, test.ShouldBeNil) - fs.AddFrame(gantryX, gantryOffset) - gantryY, err := frame.NewTranslationalFrame("gantryY", r3.Vector{0, 1, 0}, frame.Limit{math.Inf(-1), math.Inf(1)}) - test.That(t, err, test.ShouldBeNil) - fs.AddFrame(gantryY, gantryX) - - // xarm on gantry - modelXarm, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") - test.That(t, err, test.ShouldBeNil) - fs.AddFrame(modelXarm, gantryY) - - // ur5 - modelUR5e, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/universalrobots/ur5e.json"), "") - test.That(t, err, test.ShouldBeNil) - fs.AddFrame(modelUR5e, urOffset) - - // Note that positive Z is always "forwards". If the position of the arm is such that it is pointing elsewhere, - // the resulting translation will be similarly oriented - urCamera, err := frame.NewStaticFrame("urCamera", spatial.NewPoseFromPoint(r3.Vector{0, 0, 30})) - test.That(t, err, test.ShouldBeNil) - fs.AddFrame(urCamera, modelUR5e) - - positions := frame.StartPositions(fs) - - poseUR5e := spatial.NewPoseFromPoint(r3.Vector{-717.2, -132.9, 262.8}) - // Camera translates by 30, gripper is pointed at -Y - poseUR5eCam := spatial.NewPoseFromPoint(r3.Vector{-717.2, -162.9, 262.8}) - - poseXarm := spatial.NewPoseFromPoint(r3.Vector{157., -50, -88}) - poseXarmFromCam := spatial.NewPoseFromPoint(r3.Vector{874.2, -112.9, -350.8}) - - // Check the UR5e and camera default positions - transformPoint1, err := fs.Transform(positions, frame.NewPoseInFrame("UR5e", spatial.NewZeroPose()), frame.World) - test.That(t, err, test.ShouldBeNil) - transformPoint2, err := fs.Transform(positions, frame.NewPoseInFrame("urCamera", spatial.NewZeroPose()), frame.World) - test.That(t, err, test.ShouldBeNil) - transformPoint3, err := fs.Transform(positions, frame.NewPoseInFrame("xArm6", spatial.NewZeroPose()), frame.World) - test.That(t, err, test.ShouldBeNil) - transformPoint4, err := fs.Transform(positions, frame.NewPoseInFrame("urCamera", spatial.NewZeroPose()), "xArm6") - test.That(t, err, test.ShouldBeNil) - test.That(t, spatial.PoseAlmostCoincident(transformPoint1.(*frame.PoseInFrame).Pose(), poseUR5e), test.ShouldBeTrue) - test.That(t, spatial.PoseAlmostCoincident(transformPoint2.(*frame.PoseInFrame).Pose(), poseUR5eCam), test.ShouldBeTrue) - test.That(t, spatial.PoseAlmostCoincident(transformPoint3.(*frame.PoseInFrame).Pose(), poseXarm), test.ShouldBeTrue) - test.That(t, spatial.PoseAlmostCoincident(transformPoint4.(*frame.PoseInFrame).Pose(), poseXarmFromCam), test.ShouldBeTrue) - - // Move the UR5e so its local Z axis is pointing approximately towards the xArm (at positive X) - positions["UR5e"] = frame.FloatsToInputs([]float64{0, 0, 0, 0, -math.Pi / 2, -math.Pi / 2}) - - // A point that is 813.6, -50, 200 from the camera - // This puts the point in the Z plane of the xArm6 - targetPoint := spatial.NewPoseFromPoint(r3.Vector{350.8, -50, 200}) - // Target point in world - tf, err := fs.Transform(positions, frame.NewPoseInFrame("urCamera", targetPoint), frame.World) - test.That(t, err, test.ShouldBeNil) - worldPointLoc := spatial.NewPoseFromPoint(tf.(*frame.PoseInFrame).Pose().Point()) - - // Move the XY gantry such that the xArm6 is now at the point specified - positions["gantryX"] = frame.FloatsToInputs([]float64{worldPointLoc.Point().X - poseXarm.Point().X}) - positions["gantryY"] = frame.FloatsToInputs([]float64{worldPointLoc.Point().Y - poseXarm.Point().Y}) - - // Confirm the xArm6 is now at the same location as the point - newPointXarm, err := fs.Transform(positions, frame.NewPoseInFrame("xArm6", spatial.NewZeroPose()), frame.World) - test.That(t, err, test.ShouldBeNil) - test.That(t, spatial.PoseAlmostCoincident(newPointXarm.(*frame.PoseInFrame).Pose(), worldPointLoc), test.ShouldBeTrue) - - // If the above passes, then converting one directly to the other should be (0,0,0) - pointCamToXarm, err := fs.Transform(positions, frame.NewPoseInFrame("urCamera", targetPoint), "xArm6") - test.That(t, err, test.ShouldBeNil) - test.That(t, spatial.PoseAlmostCoincident(pointCamToXarm.(*frame.PoseInFrame).Pose(), spatial.NewZeroPose()), test.ShouldBeTrue) -} - -func TestCombinedIKinematics(t *testing.T) { - logger := golog.NewTestLogger(t) - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") - test.That(t, err, test.ShouldBeNil) - ik, err := CreateCombinedIKSolver(m, logger, nCPU, defaultGoalThreshold) - test.That(t, err, test.ShouldBeNil) - - // Test ability to arrive at another position - pos := spatial.NewPose( - r3.Vector{X: -46, Y: -133, Z: 372}, - &spatial.OrientationVectorDegrees{OX: 1.79, OY: -1.32, OZ: -1.11}, - ) - solution, err := solveTest(context.Background(), ik, pos, home) - test.That(t, err, test.ShouldBeNil) - - // Test moving forward 20 in X direction from previous position - pos = spatial.NewPose( - r3.Vector{X: -66, Y: -133, Z: 372}, - &spatial.OrientationVectorDegrees{OX: 1.78, OY: -3.3, OZ: -1.11}, - ) - _, err = solveTest(context.Background(), ik, pos, solution[0]) - test.That(t, err, test.ShouldBeNil) -} - -func TestUR5NloptIKinematics(t *testing.T) { - logger := golog.NewTestLogger(t) - - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/universalrobots/ur5e.json"), "") - test.That(t, err, test.ShouldBeNil) - ik, err := CreateCombinedIKSolver(m, logger, nCPU, defaultGoalThreshold) - test.That(t, err, test.ShouldBeNil) - - goalJP := frame.JointPositionsFromRadians([]float64{-4.128, 2.71, 2.798, 2.3, 1.291, 0.62}) - goal, err := ComputePosition(m, goalJP) - test.That(t, err, test.ShouldBeNil) - _, err = solveTest(context.Background(), ik, goal, home) - test.That(t, err, test.ShouldBeNil) -} - -func TestSVAvsDH(t *testing.T) { - mSVA, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/universalrobots/ur5e.json"), "") - test.That(t, err, test.ShouldBeNil) - mDH, err := frame.ParseModelJSONFile(utils.ResolveFile("referenceframe/testjson/ur5eDH.json"), "") - test.That(t, err, test.ShouldBeNil) - - numTests := 10000 - - seed := rand.New(rand.NewSource(23)) - for i := 0; i < numTests; i++ { - joints := mSVA.ProtobufFromInput(frame.RandomFrameInputs(mSVA, seed)) - - posSVA, err := ComputePosition(mSVA, joints) - test.That(t, err, test.ShouldBeNil) - posDH, err := ComputePosition(mDH, joints) - test.That(t, err, test.ShouldBeNil) - test.That(t, spatial.PoseAlmostEqual(posSVA, posDH), test.ShouldBeTrue) - } -} - -func TestCombinedCPUs(t *testing.T) { - logger := golog.NewTestLogger(t) - m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") - test.That(t, err, test.ShouldBeNil) - ik, err := CreateCombinedIKSolver(m, logger, runtime.NumCPU()/400000, defaultGoalThreshold) - test.That(t, err, test.ShouldBeNil) - test.That(t, len(ik.solvers), test.ShouldEqual, 1) -} - -func solveTest(ctx context.Context, solver InverseKinematics, goal spatial.Pose, seed []frame.Input) ([][]frame.Input, error) { - solutionGen := make(chan *IKSolution) - ikErr := make(chan error) - ctxWithCancel, cancel := context.WithCancel(ctx) - defer cancel() - - // Spawn the IK solver to generate solutions until done - go func() { - defer close(ikErr) - ikErr <- solver.Solve(ctxWithCancel, solutionGen, seed, NewSquaredNormMetric(goal), 1) - }() - - var solutions [][]frame.Input - - // Solve the IK solver. Loop labels are required because `break` etc in a `select` will break only the `select`. -IK: - for { - select { - case <-ctx.Done(): - return nil, ctx.Err() - default: - } - - select { - case step := <-solutionGen: - solutions = append(solutions, step.Configuration) - // Skip the return check below until we have nothing left to read from solutionGen - continue IK - default: - } - - select { - case <-ikErr: - // If we have a return from the IK solver, there are no more solutions, so we finish processing above - // until we've drained the channel - break IK - default: - } - } - cancel() - if len(solutions) == 0 { - return nil, errors.New("unable to solve for position") - } - - return solutions, nil -} - -// Test loading model kinematics of the same arm via ModelJSON parsing and URDF parsing and comparing results. -func TestKinematicsJSONvsURDF(t *testing.T) { - numTests := 100 - - mJSON, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/universalrobots/ur5e.json"), "") - test.That(t, err, test.ShouldBeNil) - mURDF, err := frame.ParseURDFFile(utils.ResolveFile("referenceframe/testurdf/ur5_viam.urdf"), "") - test.That(t, err, test.ShouldBeNil) - - seed := rand.New(rand.NewSource(50)) - for i := 0; i < numTests; i++ { - joints := frame.JointPositionsFromRadians(frame.GenerateRandomConfiguration(mJSON, seed)) - - posJSON, err := ComputePosition(mJSON, joints) - test.That(t, err, test.ShouldBeNil) - posURDF, err := ComputePosition(mURDF, joints) - test.That(t, err, test.ShouldBeNil) - - test.That(t, spatial.PoseAlmostEqual(posJSON, posURDF), test.ShouldBeTrue) - } -} - -func TestComputeOOBPosition(t *testing.T) { - model, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "foo") - test.That(t, err, test.ShouldBeNil) - test.That(t, model.Name(), test.ShouldEqual, "foo") - - jointPositions := &pb.JointPositions{Values: []float64{1.1, 2.2, 3.3, 1.1, 2.2, 3.3}} - - t.Run("succeed", func(t *testing.T) { - pose, err := ComputeOOBPosition(model, jointPositions) - test.That(t, err, test.ShouldBeNil) - test.That(t, pose, test.ShouldNotBeNil) - }) - - t.Run("fail when JointPositions are nil", func(t *testing.T) { - var NilJointPositions *pb.JointPositions - - pose, err := ComputeOOBPosition(model, NilJointPositions) - test.That(t, err, test.ShouldNotBeNil) - test.That(t, pose, test.ShouldBeNil) - test.That(t, err, test.ShouldEqual, frame.ErrNilJointPositions) - }) - - t.Run("fail when model frame is nil", func(t *testing.T) { - var NilModel frame.Model - - pose, err := ComputeOOBPosition(NilModel, jointPositions) - test.That(t, err, test.ShouldNotBeNil) - test.That(t, pose, test.ShouldBeNil) - test.That(t, err, test.ShouldEqual, frame.ErrNilModelFrame) - }) -} From 8e343c2f0a65672eccb9d6b8347a06bfeba07f35 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Tue, 22 Aug 2023 15:58:12 -0700 Subject: [PATCH 29/38] Remove extraneous tpspace rrt parameter --- motionplan/tpSpaceRRT.go | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index 7653236b06b..ddbac19cfa3 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -320,7 +320,6 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( curPtg tpspace.PTG, rrt rrtMap, nearest node, - planOpts *plannerOptions, invert bool, ) (*candidate, error) { nm := &neighborManager{nCPU: mp.planOpts.NumThreads / len(mp.tpFrame.PTGs())} @@ -401,8 +400,8 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( sinceLastCollideCheck += math.Abs(trajPt.Dist - lastDist) trajState := &ik.State{Position: spatialmath.Compose(arcStartPose, trajPt.Pose), Frame: mp.frame} nodePose = trajState.Position // This will get rewritten later for inverted trees - if sinceLastCollideCheck > planOpts.Resolution { - ok, _ := planOpts.CheckStateConstraints(trajState) + if sinceLastCollideCheck > mp.planOpts.Resolution { + ok, _ := mp.planOpts.CheckStateConstraints(trajState) if !ok { return nil, errInvalidCandidate } @@ -425,9 +424,9 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( cand := &candidate{dist: bestDist, treeNode: nearest, newNode: successNode, lastInTraj: isLastNode} // check if this successNode is too close to nodes already in the tree, and if so, do not add. // Get nearest neighbor to new node that's already in the tree - nearest = nm.nearestNeighbor(ctx, planOpts, successNode, rrt) + nearest = nm.nearestNeighbor(ctx, mp.planOpts, successNode, rrt) if nearest != nil { - dist := planOpts.DistanceFunc(&ik.Segment{StartPosition: successNode.Pose(), EndPosition: nearest.Pose()}) + dist := mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: successNode.Pose(), EndPosition: nearest.Pose()}) // Ensure successNode is sufficiently far from the nearest node already existing in the tree // If too close, don't add a new node if dist < defaultIdenticalNodeDistance { @@ -463,7 +462,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( // Find the best traj point for each traj family, and store for later comparison ptgNumPar, curPtgPar := ptgNum, curPtg utils.PanicCapturingGo(func() { - cand, err := mp.getExtensionCandidate(ctx, goalNode, ptgNumPar, curPtgPar, rrt, seedNode, mp.planOpts, invert) + cand, err := mp.getExtensionCandidate(ctx, goalNode, ptgNumPar, curPtgPar, rrt, seedNode, invert) if err != nil && !errors.Is(err, errNoNeighbors) && !errors.Is(err, errInvalidCandidate) { candChan <- nil return From e283a10dbca6ac3176c3a28f16c5d6ec5c7bb469 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Tue, 22 Aug 2023 17:15:05 -0700 Subject: [PATCH 30/38] Fix flaky test --- motionplan/cBiRRT.go | 4 ++- motionplan/ik/combinedInverseKinematics.go | 13 ++----- motionplan/ik/nloptInverseKinematics.go | 40 +++++++++++++++++----- motionplan/motionPlanner.go | 11 +++++- motionplan/planManager.go | 4 +++ 5 files changed, 51 insertions(+), 21 deletions(-) diff --git a/motionplan/cBiRRT.go b/motionplan/cBiRRT.go index 32e16e4b637..4dae628a54b 100644 --- a/motionplan/cBiRRT.go +++ b/motionplan/cBiRRT.go @@ -125,7 +125,9 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( mp.start = time.Now() if rrt.maps == nil || len(rrt.maps.goalMap) == 0 { + mp.logger.Info("initing solutions") planSeed := initRRTSolutions(ctx, mp, seed) + mp.logger.Info("init solutions done") if planSeed.planerr != nil || planSeed.steps != nil { rrt.solutionChan <- planSeed return @@ -367,7 +369,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear( default: } close(solutionGen) - if err != nil { + if err != nil || solved == nil { return nil } diff --git a/motionplan/ik/combinedInverseKinematics.go b/motionplan/ik/combinedInverseKinematics.go index 0c2ceacf728..1f10f133ed2 100644 --- a/motionplan/ik/combinedInverseKinematics.go +++ b/motionplan/ik/combinedInverseKinematics.go @@ -41,16 +41,6 @@ func CreateCombinedIKSolver(model referenceframe.Frame, logger golog.Logger, nCP return ik, nil } -func runSolver(ctx context.Context, - solver InverseKinematics, - c chan<- *Solution, - seed []referenceframe.Input, - m StateMetric, - rseed int, -) error { - return solver.Solve(ctx, c, seed, m, rseed) -} - // Solve will initiate solving for the given position in all child solvers, seeding with the specified initial joint // positions. If unable to solve, the returned error will be non-nil. func (ik *CombinedIK) Solve(ctx context.Context, @@ -75,7 +65,8 @@ func (ik *CombinedIK) Solve(ctx context.Context, utils.PanicCapturingGo(func() { defer activeSolvers.Done() - errChan <- runSolver(ctxWithCancel, thisSolver, c, seed, m, parseed) + + errChan <- thisSolver.Solve(ctxWithCancel, c, seed, m, parseed) }) } diff --git a/motionplan/ik/nloptInverseKinematics.go b/motionplan/ik/nloptInverseKinematics.go index 912f2bcba57..fcd63587eef 100644 --- a/motionplan/ik/nloptInverseKinematics.go +++ b/motionplan/ik/nloptInverseKinematics.go @@ -7,11 +7,13 @@ import ( "math" "math/rand" "strings" + "sync" "github.com/edaniels/golog" "github.com/go-nlopt/nlopt" "github.com/pkg/errors" "go.uber.org/multierr" + "go.viam.com/utils" "go.viam.com/rdk/referenceframe" ) @@ -43,6 +45,12 @@ type NloptIK struct { exact bool } +type optimizeReturn struct { + solution []float64 + score float64 + err error +} + // CreateNloptIKSolver creates an nloptIK object that can perform gradient descent on metrics for Frames. The parameters are the Frame on // which Transform() will be called, a logger, and the number of iterations to run. If the iteration count is less than 1, it will be set // to the default of 5000. @@ -94,6 +102,7 @@ func (ik *NloptIK) Solve(ctx context.Context, return errBadBounds } mInput := &State{Frame: ik.model} + var activeSolvers sync.WaitGroup // x is our joint positions // Gradient is, under the hood, a unsafe C structure that we are meant to mutate in place. @@ -169,21 +178,36 @@ func (ik *NloptIK) Solve(ctx context.Context, } } - select { - case <-ctx.Done(): - ik.logger.Info("solver halted before solving start; possibly solving twice in a row?") - return err - default: - } - + solveChan := make(chan *optimizeReturn, 1) + defer close(solveChan) for iterations < ik.maxIterations { select { case <-ctx.Done(): return ctx.Err() default: } + + var solutionRaw []float64 + var result float64 + var nloptErr error + iterations++ - solutionRaw, result, nloptErr := opt.Optimize(referenceframe.InputsToFloats(startingPos)) + activeSolvers.Add(1) + utils.PanicCapturingGo(func() { + defer activeSolvers.Done() + solutionRaw, result, nloptErr := opt.Optimize(referenceframe.InputsToFloats(startingPos)) + solveChan <- &optimizeReturn{solutionRaw, result, nloptErr} + }) + select { + case <-ctx.Done(): + err = multierr.Combine(err, opt.ForceStop()) + activeSolvers.Wait() + return multierr.Combine(err, ctx.Err()) + case solution := <-solveChan: + solutionRaw = solution.solution + result = solution.score + nloptErr = solution.err + } if nloptErr != nil { // This just *happens* sometimes due to weirdnesses in nonlinear randomized problems. // Ignore it, something else will find a solution diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 0c548e21a47..8a82fe5ab39 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -131,6 +131,7 @@ func motionPlanInternal(ctx context.Context, if err != nil { return nil, err } + logger.Debug("created new plan manager") resultSlices, err := sfPlanner.PlanSingleWaypoint(ctx, seedMap, goal.Pose(), worldState, constraintSpec, motionConfig) if err != nil { return nil, err @@ -272,7 +273,7 @@ func (mp *planner) getSolutions(ctx context.Context, seed []frame.Input) ([]node ctxWithCancel, cancel := context.WithCancel(ctx) defer cancel() - solutionGen := make(chan *ik.Solution, mp.planOpts.NumThreads) + solutionGen := make(chan *ik.Solution, mp.planOpts.NumThreads*2) ikErr := make(chan error, 1) var activeSolvers sync.WaitGroup defer activeSolvers.Wait() @@ -354,6 +355,14 @@ IK: // Cancel any ongoing processing within the IK solvers if we're done receiving solutions cancel() + done := false + for !done { + select { + case <-solutionGen: + default: + done = true + } + } if len(solutions) == 0 { // We have failed to produce a usable IK solution. Let the user know if zero IK solutions were produced, or if non-zero solutions diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 17e96229eaf..f891b7fde70 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -144,6 +144,7 @@ func (pm *planManager) PlanSingleWaypoint(ctx context.Context, } goals = append(goals, goalPos) opt, err := pm.plannerSetupFromMoveRequest(seedPos, goalPos, seedMap, worldState, constraintSpec, motionConfig) + pm.logger.Debug("set up planner options for waypoint") if err != nil { return nil, err } @@ -174,6 +175,7 @@ func (pm *planManager) PlanSingleWaypoint(ctx context.Context, return nil, err } } + pm.logger.Debug("checked goal viability: success") resultSlices, err := pm.planAtomicWaypoints(ctx, goals, seed, planners) pm.activeBackgroundWorkers.Wait() @@ -301,7 +303,9 @@ func (pm *planManager) planParallelRRTMotion( var err error // If we don't pass in pre-made maps, initialize and seed with IK solutions here if maps == nil { + pm.logger.Debug("initializing RRT solutions") planSeed := initRRTSolutions(ctx, pathPlanner, seed) + pm.logger.Debug("RRT solutions initialized") if planSeed.planerr != nil || planSeed.steps != nil { solutionChan <- planSeed return From e3b110d0a73ca4e02c8c3cf6c34e6652f8345a2a Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 23 Aug 2023 08:47:42 -0700 Subject: [PATCH 31/38] Fix race --- motionplan/tpSpaceRRT.go | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index ddbac19cfa3..b39e4ad5ea2 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -354,7 +354,10 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( targetFunc = ik.NewSquaredNormMetric(relPose) } solutionChan := make(chan *ik.Solution, 1) - err := curPtg.Solve(context.Background(), solutionChan, mp.algOpts.ikSeed, targetFunc, mp.randseed.Int()) + mp.mu.Lock() + rseed := mp.randseed.Int() + mp.mu.Unlock() + err := curPtg.Solve(context.Background(), solutionChan, mp.algOpts.ikSeed, targetFunc, rseed) var bestNode *ik.Solution select { From 7eb28dc1f1e7a090d8afb87e9cd70866e59e8be9 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 23 Aug 2023 09:15:02 -0700 Subject: [PATCH 32/38] Allow use of rectifyTPspacePath without a tpspace planner --- motionplan/tpSpaceRRT.go | 56 ++++++++++++++++++++-------------------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index b39e4ad5ea2..5bd9b9d63c5 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -292,7 +292,7 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( bestPath = goodPath } } - correctedPath, err := mp.rectifyPath(bestPath) + correctedPath, err := rectifyTPspacePath(bestPath, mp.frame) if err != nil { rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} return @@ -672,32 +672,6 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, return opts } -// rectifyPath is needed because of how trees are currently stored. As trees grow from the start or goal, the Pose stored in the node -// is the distal pose away from the root of the tree, which in the case of the goal tree is in fact the 0-distance point of the traj. -// When this becomes a single path, poses should reflect the transformation at the end of each traj. Here we go through and recompute -// each pose in order to ensure correctness. -// TODO: if trees are stored as segments rather than nodes, then this becomes simpler/unnecessary. Related to RSDK-4139. -func (mp *tpSpaceRRTMotionPlanner) rectifyPath(path []node) ([]node, error) { - correctedPath := []node{} - runningPose := spatialmath.NewZeroPose() - for _, wp := range path { - wpPose, err := mp.frame.Transform(wp.Q()) - if err != nil { - return nil, err - } - runningPose = spatialmath.Compose(runningPose, wpPose) - - thisNode := &basicNode{ - q: wp.Q(), - cost: wp.Cost(), - pose: runningPose, - corner: wp.Corner(), - } - correctedPath = append(correctedPath, thisNode) - } - return correctedPath, nil -} - func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) []node { toIter := int(math.Min(float64(len(path)*len(path))/2, float64(mp.planOpts.SmoothIter))) currCost := sumCosts(path) @@ -803,7 +777,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( if secondEdge < len(path)-1 { newInputSteps = append(newInputSteps, path[secondEdge+1:]...) } - return mp.rectifyPath(newInputSteps) + return rectifyTPspacePath(newInputSteps, mp.frame) } func (mp *tpSpaceRRTMotionPlanner) sample(rSeed node, iter int) (node, error) { @@ -821,3 +795,29 @@ func (mp *tpSpaceRRTMotionPlanner) sample(rSeed node, iter int) (node, error) { ) return &basicNode{pose: randPos}, nil } + +// rectifyTPspacePath is needed because of how trees are currently stored. As trees grow from the start or goal, the Pose stored in the node +// is the distal pose away from the root of the tree, which in the case of the goal tree is in fact the 0-distance point of the traj. +// When this becomes a single path, poses should reflect the transformation at the end of each traj. Here we go through and recompute +// each pose in order to ensure correctness. +// TODO: if trees are stored as segments rather than nodes, then this becomes simpler/unnecessary. Related to RSDK-4139. +func rectifyTPspacePath(path []node, frame referenceframe.Frame) ([]node, error) { + correctedPath := []node{} + runningPose := spatialmath.NewZeroPose() + for _, wp := range path { + wpPose, err := frame.Transform(wp.Q()) + if err != nil { + return nil, err + } + runningPose = spatialmath.Compose(runningPose, wpPose) + + thisNode := &basicNode{ + q: wp.Q(), + cost: wp.Cost(), + pose: runningPose, + corner: wp.Corner(), + } + correctedPath = append(correctedPath, thisNode) + } + return correctedPath, nil +} From 72c79d35caa31ec20b642ecd59f491c7cc055da2 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 23 Aug 2023 09:22:35 -0700 Subject: [PATCH 33/38] Fix test --- motionplan/tpSpaceRRT_test.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 686318d1066..0d86e7d0909 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -297,7 +297,7 @@ func TestTPsmoothing(t *testing.T) { } plan = append(plan, thisNode) } - plan, err = tp.rectifyPath(plan) + plan, err = rectifyTPspacePath(plan, tp.frame) test.That(t, err, test.ShouldBeNil) plan = tp.smoothPath(ctx, plan) From 9c92456e14ee34c4f62ebd71e0cda52a0d97a201 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 23 Aug 2023 10:05:05 -0700 Subject: [PATCH 34/38] Put moved code back where it was --- motionplan/constraint.go | 116 +++++++++++++++++++-------------------- 1 file changed, 58 insertions(+), 58 deletions(-) diff --git a/motionplan/constraint.go b/motionplan/constraint.go index 954b12d91dd..9344c627b77 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -13,6 +13,64 @@ import ( spatial "go.viam.com/rdk/spatialmath" ) +// Given a constraint input with only frames and input positions, calculates the corresponding poses as needed. +func resolveStatesToPositions(state *ik.State) error { + if state.Position == nil { + if state.Frame != nil { + if state.Configuration != nil { + pos, err := state.Frame.Transform(state.Configuration) + if err == nil { + state.Position = pos + } else { + return err + } + } else { + return errors.New("invalid constraint input") + } + } else { + return errors.New("invalid constraint input") + } + } + return nil +} + +// Given a constraint input with only frames and input positions, calculates the corresponding poses as needed. +func resolveSegmentsToPositions(segment *ik.Segment) error { + if segment.StartPosition == nil { + if segment.Frame != nil { + if segment.StartConfiguration != nil { + pos, err := segment.Frame.Transform(segment.StartConfiguration) + if err == nil { + segment.StartPosition = pos + } else { + return err + } + } else { + return errors.New("invalid constraint input") + } + } else { + return errors.New("invalid constraint input") + } + } + if segment.EndPosition == nil { + if segment.Frame != nil { + if segment.EndConfiguration != nil { + pos, err := segment.Frame.Transform(segment.EndConfiguration) + if err == nil { + segment.EndPosition = pos + } else { + return err + } + } else { + return errors.New("invalid constraint input") + } + } else { + return errors.New("invalid constraint input") + } + } + return nil +} + // SegmentConstraint tests whether a transition from a starting robot configuration to an ending robot configuration is valid. // If the returned bool is true, the constraint is satisfied and the segment is valid. type SegmentConstraint func(*ik.Segment) bool @@ -427,61 +485,3 @@ func NewOctreeCollisionConstraint(octree *pointcloud.BasicOctree, threshold int, } return constraint } - -// Given a constraint input with only frames and input positions, calculates the corresponding poses as needed. -func resolveStatesToPositions(state *ik.State) error { - if state.Position == nil { - if state.Frame != nil { - if state.Configuration != nil { - pos, err := state.Frame.Transform(state.Configuration) - if err == nil { - state.Position = pos - } else { - return err - } - } else { - return errors.New("invalid constraint input") - } - } else { - return errors.New("invalid constraint input") - } - } - return nil -} - -// Given a constraint input with only frames and input positions, calculates the corresponding poses as needed. -func resolveSegmentsToPositions(segment *ik.Segment) error { - if segment.StartPosition == nil { - if segment.Frame != nil { - if segment.StartConfiguration != nil { - pos, err := segment.Frame.Transform(segment.StartConfiguration) - if err == nil { - segment.StartPosition = pos - } else { - return err - } - } else { - return errors.New("invalid constraint input") - } - } else { - return errors.New("invalid constraint input") - } - } - if segment.EndPosition == nil { - if segment.Frame != nil { - if segment.EndConfiguration != nil { - pos, err := segment.Frame.Transform(segment.EndConfiguration) - if err == nil { - segment.EndPosition = pos - } else { - return err - } - } else { - return errors.New("invalid constraint input") - } - } else { - return errors.New("invalid constraint input") - } - } - return nil -} From 716404db06733804bb3599507991bb4dda1e2058 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 23 Aug 2023 10:06:08 -0700 Subject: [PATCH 35/38] Fix function ordering --- motionplan/constraint.go | 40 ++++++++++++++++++++-------------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/motionplan/constraint.go b/motionplan/constraint.go index 9344c627b77..4f44c17e2b6 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -14,13 +14,13 @@ import ( ) // Given a constraint input with only frames and input positions, calculates the corresponding poses as needed. -func resolveStatesToPositions(state *ik.State) error { - if state.Position == nil { - if state.Frame != nil { - if state.Configuration != nil { - pos, err := state.Frame.Transform(state.Configuration) +func resolveSegmentsToPositions(segment *ik.Segment) error { + if segment.StartPosition == nil { + if segment.Frame != nil { + if segment.StartConfiguration != nil { + pos, err := segment.Frame.Transform(segment.StartConfiguration) if err == nil { - state.Position = pos + segment.StartPosition = pos } else { return err } @@ -31,17 +31,12 @@ func resolveStatesToPositions(state *ik.State) error { return errors.New("invalid constraint input") } } - return nil -} - -// Given a constraint input with only frames and input positions, calculates the corresponding poses as needed. -func resolveSegmentsToPositions(segment *ik.Segment) error { - if segment.StartPosition == nil { + if segment.EndPosition == nil { if segment.Frame != nil { - if segment.StartConfiguration != nil { - pos, err := segment.Frame.Transform(segment.StartConfiguration) + if segment.EndConfiguration != nil { + pos, err := segment.Frame.Transform(segment.EndConfiguration) if err == nil { - segment.StartPosition = pos + segment.EndPosition = pos } else { return err } @@ -52,12 +47,17 @@ func resolveSegmentsToPositions(segment *ik.Segment) error { return errors.New("invalid constraint input") } } - if segment.EndPosition == nil { - if segment.Frame != nil { - if segment.EndConfiguration != nil { - pos, err := segment.Frame.Transform(segment.EndConfiguration) + return nil +} + +// Given a constraint input with only frames and input positions, calculates the corresponding poses as needed. +func resolveStatesToPositions(state *ik.State) error { + if state.Position == nil { + if state.Frame != nil { + if state.Configuration != nil { + pos, err := state.Frame.Transform(state.Configuration) if err == nil { - segment.EndPosition = pos + state.Position = pos } else { return err } From dc361dcee96fafa8e2f1fc63b7c8dd9261ec2506 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 23 Aug 2023 10:07:22 -0700 Subject: [PATCH 36/38] Correct errChan capacity --- motionplan/ik/combinedInverseKinematics.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motionplan/ik/combinedInverseKinematics.go b/motionplan/ik/combinedInverseKinematics.go index 1f10f133ed2..2247b7d92a8 100644 --- a/motionplan/ik/combinedInverseKinematics.go +++ b/motionplan/ik/combinedInverseKinematics.go @@ -53,7 +53,7 @@ func (ik *CombinedIK) Solve(ctx context.Context, ctxWithCancel, cancel := context.WithCancel(ctx) defer cancel() - errChan := make(chan error, len(ik.solvers)+1) + errChan := make(chan error, len(ik.solvers)) var activeSolvers sync.WaitGroup defer activeSolvers.Wait() activeSolvers.Add(len(ik.solvers)) From 8ae6899c4b36e6f8006961f058872c7be1d69fb2 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 23 Aug 2023 11:20:35 -0700 Subject: [PATCH 37/38] Remove log statements --- motionplan/cBiRRT.go | 2 -- 1 file changed, 2 deletions(-) diff --git a/motionplan/cBiRRT.go b/motionplan/cBiRRT.go index 4dae628a54b..8d4623cf381 100644 --- a/motionplan/cBiRRT.go +++ b/motionplan/cBiRRT.go @@ -125,9 +125,7 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( mp.start = time.Now() if rrt.maps == nil || len(rrt.maps.goalMap) == 0 { - mp.logger.Info("initing solutions") planSeed := initRRTSolutions(ctx, mp, seed) - mp.logger.Info("init solutions done") if planSeed.planerr != nil || planSeed.steps != nil { rrt.solutionChan <- planSeed return From 0c6734fcb8bf91fe3ae096f0d529587297f92d58 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 23 Aug 2023 15:38:07 -0700 Subject: [PATCH 38/38] PR feedback --- motionplan/ik/inverseKinematics.go | 9 ------ motionplan/ik/nloptInverseKinematics.go | 9 ++++++ motionplan/motionPlanner.go | 3 +- motionplan/planManager.go | 4 --- motionplan/tpSpaceRRT.go | 4 +-- motionplan/tpspace/ptg.go | 37 ++++++++++--------------- motionplan/tpspace/ptgC.go | 3 ++ motionplan/tpspace/ptgCCS.go | 1 - motionplan/tpspace/ptgGridSim.go | 2 +- motionplan/tpspace/ptgIK.go | 7 ++--- motionplan/tpspace/ptgIKFrame.go | 6 ++-- motionplan/tpspace/ptg_test.go | 7 +++++ 12 files changed, 42 insertions(+), 50 deletions(-) diff --git a/motionplan/ik/inverseKinematics.go b/motionplan/ik/inverseKinematics.go index d9bf45796dd..19993aa6d8f 100644 --- a/motionplan/ik/inverseKinematics.go +++ b/motionplan/ik/inverseKinematics.go @@ -30,12 +30,3 @@ type Solution struct { Score float64 Exact bool } - -func limitsToArrays(limits []referenceframe.Limit) ([]float64, []float64) { - var min, max []float64 - for _, limit := range limits { - min = append(min, limit.Min) - max = append(max, limit.Max) - } - return min, max -} diff --git a/motionplan/ik/nloptInverseKinematics.go b/motionplan/ik/nloptInverseKinematics.go index fcd63587eef..a454f5a4771 100644 --- a/motionplan/ik/nloptInverseKinematics.go +++ b/motionplan/ik/nloptInverseKinematics.go @@ -299,3 +299,12 @@ func (ik *NloptIK) updateBounds(seed []referenceframe.Input, tries int, opt *nlo opt.SetUpperBounds(newUpper), ) } + +func limitsToArrays(limits []referenceframe.Limit) ([]float64, []float64) { + var min, max []float64 + for _, limit := range limits { + min = append(min, limit.Min) + max = append(max, limit.Max) + } + return min, max +} diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 8a82fe5ab39..b3aa9105d25 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -355,8 +355,7 @@ IK: // Cancel any ongoing processing within the IK solvers if we're done receiving solutions cancel() - done := false - for !done { + for done := false; !done; { select { case <-solutionGen: default: diff --git a/motionplan/planManager.go b/motionplan/planManager.go index f891b7fde70..17e96229eaf 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -144,7 +144,6 @@ func (pm *planManager) PlanSingleWaypoint(ctx context.Context, } goals = append(goals, goalPos) opt, err := pm.plannerSetupFromMoveRequest(seedPos, goalPos, seedMap, worldState, constraintSpec, motionConfig) - pm.logger.Debug("set up planner options for waypoint") if err != nil { return nil, err } @@ -175,7 +174,6 @@ func (pm *planManager) PlanSingleWaypoint(ctx context.Context, return nil, err } } - pm.logger.Debug("checked goal viability: success") resultSlices, err := pm.planAtomicWaypoints(ctx, goals, seed, planners) pm.activeBackgroundWorkers.Wait() @@ -303,9 +301,7 @@ func (pm *planManager) planParallelRRTMotion( var err error // If we don't pass in pre-made maps, initialize and seed with IK solutions here if maps == nil { - pm.logger.Debug("initializing RRT solutions") planSeed := initRRTSolutions(ctx, pathPlanner, seed) - pm.logger.Debug("RRT solutions initialized") if planSeed.planerr != nil || planSeed.steps != nil { solutionChan <- planSeed return diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index 5bd9b9d63c5..c185903843a 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -132,7 +132,7 @@ func newTPSpaceMotionPlanner( } tpPlanner.setupTPSpaceOptions() - tpPlanner.algOpts.ikSeed = []referenceframe.Input{{math.Pi / 2}, {tpFrame.PTGs()[0].RefDistance() / 2}} + tpPlanner.algOpts.ikSeed = []referenceframe.Input{{math.Pi / 2}, {tpFrame.PTGs()[0].MaxDistance() / 2}} return tpPlanner, nil } @@ -414,7 +414,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( lastDist = trajPt.Dist } - isLastNode := math.Abs(finalTrajNode.Dist-curPtg.RefDistance()) < 0.1 + isLastNode := math.Abs(finalTrajNode.Dist-curPtg.MaxDistance()) < 0.1 // add the last node in trajectory successNode = &basicNode{ diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index 8c68cbc251d..4f40fb0ea8f 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -19,8 +19,8 @@ type PTG interface { ik.InverseKinematics PrecomputePTG - // RefDistance returns the maximum distance that a single trajectory may travel - RefDistance() float64 + // MaxDistance returns the maximum distance that a single trajectory may travel + MaxDistance() float64 // Returns the set of trajectory nodes along the given trajectory, out to the requested distance Trajectory(alpha, dist float64) ([]*TrajNode, error) @@ -63,65 +63,56 @@ func index2alpha(k, numPaths uint) float64 { return math.Pi * (-1.0 + 2.0*(float64(k)+0.5)/float64(numPaths)) } -func alpha2index(alpha float64, numPaths uint) uint { - alpha = wrapTo2Pi(alpha+math.Pi) - math.Pi - idx := uint(math.Round(0.5 * (float64(numPaths)*(1.0+alpha/math.Pi) - 1.0))) - return idx -} - // Returns a given angle in the [0, 2pi) range. func wrapTo2Pi(theta float64) float64 { return theta - 2*math.Pi*math.Floor(theta/(2*math.Pi)) } // ComputePTG will compute all nodes of simPTG at the requested alpha, out to the requested distance, at the specified diffT resolution. -func ComputePTG( - simPTG PrecomputePTG, - alpha, refDist, diffT float64, -) ([]*TrajNode, error) { +func ComputePTG(simPTG PrecomputePTG, alpha, dist, diffT float64) ([]*TrajNode, error) { // Initialize trajectory with an all-zero node alphaTraj := []*TrajNode{{Pose: spatialmath.NewZeroPose()}} var err error var t, v, w float64 - dist := math.Copysign(math.Abs(v)*diffT, refDist) + distTravelled := math.Copysign(math.Abs(v)*diffT, dist) // Step through each time point for this alpha - for math.Abs(dist) < math.Abs(refDist) { + for math.Abs(distTravelled) < math.Abs(dist) { t += diffT - nextNode, err := computePTGNode(simPTG, alpha, dist, t) + nextNode, err := computePTGNode(simPTG, alpha, distTravelled, t) if err != nil { return nil, err } v = nextNode.LinVelMMPS w = nextNode.AngVelRPS - // Update velocities of last node because the computed velocities at this node are what should be set after passing the last node + // Update velocities of last node because the computed velocities at this node are what should be set after passing the last node. + // Reasoning: if the distance passed in is 0, then we want the first node to return velocity 0. However, if we want a nonzero + // distance such that we return two nodes, then the first node, which has zero translation, should set a nonzero velocity so that + // the next node, which has a nonzero translation, is arrived at when it ought to be. alphaTraj[len(alphaTraj)-1].LinVelMMPS = v alphaTraj[len(alphaTraj)-1].AngVelRPS = w alphaTraj = append(alphaTraj, nextNode) - dist += math.Copysign(math.Abs(v)*diffT, refDist) + distTravelled += math.Copysign(math.Max(diffT, math.Abs(v)*diffT), dist) } // Add final node alphaTraj[len(alphaTraj)-1].LinVelMMPS = v alphaTraj[len(alphaTraj)-1].AngVelRPS = w - pose, err := simPTG.Transform([]referenceframe.Input{{alpha}, {refDist}}) + pose, err := simPTG.Transform([]referenceframe.Input{{alpha}, {dist}}) if err != nil { return nil, err } - tNode := &TrajNode{pose, t, refDist, alpha, v, w} + tNode := &TrajNode{pose, t, dist, alpha, v, w} alphaTraj = append(alphaTraj, tNode) return alphaTraj, nil } // computePTGNode will return the TrajNode of the requested PTG, at the specified alpha and dist. The provided time is used // to fill in the time field. -func computePTGNode( - simPTG PrecomputePTG, - alpha, dist, atT float64, -) (*TrajNode, error) { +func computePTGNode(simPTG PrecomputePTG, alpha, dist, atT float64) (*TrajNode, error) { v, w, err := simPTG.PTGVelocities(alpha, dist) if err != nil { return nil, err diff --git a/motionplan/tpspace/ptgC.go b/motionplan/tpspace/ptgC.go index acf7b100c5f..f51f42a44b6 100644 --- a/motionplan/tpspace/ptgC.go +++ b/motionplan/tpspace/ptgC.go @@ -29,6 +29,9 @@ func NewCirclePTG(maxMMPS, maxRPS float64) PrecomputePTG { // because they will have zero linear velocity through their turns, not max. func (ptg *ptgDiffDriveC) PTGVelocities(alpha, dist float64) (float64, float64, error) { // (v,w) + if dist == 0 { + return 0, 0, nil + } k := math.Copysign(1.0, dist) v := ptg.maxMMPS * k w := (alpha / math.Pi) * ptg.maxRPS * k diff --git a/motionplan/tpspace/ptgCCS.go b/motionplan/tpspace/ptgCCS.go index c5bf70362fe..0675ca3d830 100644 --- a/motionplan/tpspace/ptgCCS.go +++ b/motionplan/tpspace/ptgCCS.go @@ -61,7 +61,6 @@ func (ptg *ptgDiffDriveCCS) PTGVelocities(alpha, dist float64) (float64, float64 } func (ptg *ptgDiffDriveCCS) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { - // ~ fmt.Println("CCS") alpha := inputs[0].Value dist := inputs[1].Value diff --git a/motionplan/tpspace/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index ca0bdfb6f61..cef65c82a75 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -110,7 +110,7 @@ func (ptg *ptgGridSim) Solve( return nil } -func (ptg *ptgGridSim) RefDistance() float64 { +func (ptg *ptgGridSim) MaxDistance() float64 { return ptg.refDist } diff --git a/motionplan/tpspace/ptgIK.go b/motionplan/tpspace/ptgIK.go index 3ec6750eba7..23c5d17c941 100644 --- a/motionplan/tpspace/ptgIK.go +++ b/motionplan/tpspace/ptgIK.go @@ -36,10 +36,7 @@ func NewPTGIK(simPTG PrecomputePTG, logger golog.Logger, refDist float64, randSe return nil, errors.New("refDist must be greater than zero") } - ptgFrame, err := NewPTGIKFrame(simPTG, refDist) - if err != nil { - return nil, err - } + ptgFrame := newPTGIKFrame(simPTG, refDist) nlopt, err := ik.CreateNloptIKSolver(ptgFrame, logger, 1, false) if err != nil { @@ -110,7 +107,7 @@ func (ptg *ptgIK) Solve( return nil } -func (ptg *ptgIK) RefDistance() float64 { +func (ptg *ptgIK) MaxDistance() float64 { return ptg.refDist } diff --git a/motionplan/tpspace/ptgIKFrame.go b/motionplan/tpspace/ptgIKFrame.go index 7408cc86295..57f6ac99c36 100644 --- a/motionplan/tpspace/ptgIKFrame.go +++ b/motionplan/tpspace/ptgIKFrame.go @@ -17,14 +17,14 @@ type ptgIKFrame struct { // NewPTGIKFrame will create a new frame intended to be passed to an Inverse Kinematics solver, allowing IK to solve for parameters // for the passed in PTG. -func NewPTGIKFrame(ptg PrecomputePTG, dist float64) (referenceframe.Frame, error) { +func newPTGIKFrame(ptg PrecomputePTG, dist float64) referenceframe.Frame { pf := &ptgIKFrame{PrecomputePTG: ptg} pf.limits = []referenceframe.Limit{ {Min: -math.Pi, Max: math.Pi}, - {Min: -dist, Max: dist}, + {Min: 0, Max: dist}, } - return pf, nil + return pf } func (pf *ptgIKFrame) DoF() []referenceframe.Limit { diff --git a/motionplan/tpspace/ptg_test.go b/motionplan/tpspace/ptg_test.go index 7b9ef65f631..081d8339ade 100644 --- a/motionplan/tpspace/ptg_test.go +++ b/motionplan/tpspace/ptg_test.go @@ -1,6 +1,7 @@ package tpspace import ( + "math" "testing" "go.viam.com/test" @@ -13,3 +14,9 @@ func TestAlphaIdx(t *testing.T) { test.That(t, i, test.ShouldEqual, i2) } } + +func alpha2index(alpha float64, numPaths uint) uint { + alpha = wrapTo2Pi(alpha+math.Pi) - math.Pi + idx := uint(math.Round(0.5 * (float64(numPaths)*(1.0+alpha/math.Pi) - 1.0))) + return idx +}