diff --git a/components/base/kinematicbase/differentialDrive.go b/components/base/kinematicbase/differentialDrive.go index e09d7a94000..dcbc5ca9daf 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 8c9668f5989..1d9883dda1f 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -74,11 +74,12 @@ func wrapWithPTGKinematics( return nil, err } - frame, err := referenceframe.NewPTGFrameFromTurningRadius( + frame, err := tpspace.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 +123,15 @@ 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 multierr.Combine(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 { + // 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/components/base/kinematicbase/ptgKinematics_test.go b/components/base/kinematicbase/ptgKinematics_test.go index 5985c8fd94c..124c7be0cb9 100644 --- a/components/base/kinematicbase/ptgKinematics_test.go +++ b/components/base/kinematicbase/ptgKinematics_test.go @@ -51,3 +51,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/cBiRRT.go b/motionplan/cBiRRT.go index 40db093f062..8d4623cf381 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) + nlopt, err := ik.CreateNloptIKSolver(frame, logger, 1, true) if err != nil { return nil, err } @@ -132,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 @@ -163,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 @@ -186,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 @@ -206,24 +207,24 @@ 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 { - 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} 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! if reachedDelta <= mp.planOpts.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 } @@ -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,29 +357,29 @@ func (mp *cBiRRTMotionPlanner) constrainNear( if ok { return target } - solutionGen := make(chan []referenceframe.Input, 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 []referenceframe.Input + var solved *ik.Solution select { case solved = <-solutionGen: default: } close(solutionGen) - if err != nil { + if err != nil || solved == nil { return nil } ok, failpos := mp.planOpts.CheckSegmentAndStateValidity( - &Segment{StartConfiguration: seedInputs, EndConfiguration: solved, Frame: mp.frame}, + &ik.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}) + 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..4f44c17e2b6 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -7,24 +7,14 @@ import ( "github.com/golang/geo/r3" pb "go.viam.com/api/service/motion/v1" + "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/pointcloud" "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 { +func resolveSegmentsToPositions(segment *ik.Segment) error { if segment.StartPosition == nil { if segment.Frame != nil { if segment.StartConfiguration != nil { @@ -60,17 +50,8 @@ func resolveSegmentsToPositions(segment *Segment) error { 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 { +func resolveStatesToPositions(state *ik.State) error { if state.Position == nil { if state.Frame != nil { if state.Configuration != nil { @@ -92,11 +73,11 @@ func resolveStatesToPositions(state *State) error { // 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 +90,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 +104,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 +118,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 +131,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 +142,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 +153,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 +317,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 +354,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 +367,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 +377,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 +408,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 +416,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 +424,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 +445,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 +469,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 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/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/combinedInverseKinematics.go b/motionplan/ik/combinedInverseKinematics.go similarity index 86% rename from motionplan/combinedInverseKinematics.go rename to motionplan/ik/combinedInverseKinematics.go index 6b528802961..2247b7d92a8 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) + nlopt, err := CreateNloptIKSolver(model, logger, -1, true) nlopt.id = i if err != nil { return nil, err @@ -41,20 +41,10 @@ func CreateCombinedIKSolver(model referenceframe.Frame, logger golog.Logger, nCP return ik, nil } -func runSolver(ctx context.Context, - solver InverseKinematics, - c chan<- []referenceframe.Input, - 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, - c chan<- []referenceframe.Input, + c chan<- *Solution, seed []referenceframe.Input, m StateMetric, rseed int, @@ -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) }) } @@ -89,6 +80,7 @@ func (ik *CombinedIK) Solve(ctx context.Context, for !done { select { case <-ctx.Done(): + activeSolvers.Wait() return ctx.Err() default: } @@ -110,6 +102,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 +113,7 @@ func (ik *CombinedIK) Solve(ctx context.Context, collectedErrs = multierr.Combine(collectedErrs, err) } } + activeSolvers.Wait() return collectedErrs } 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/ik/inverseKinematics.go b/motionplan/ik/inverseKinematics.go new file mode 100644 index 00000000000..19993aa6d8f --- /dev/null +++ b/motionplan/ik/inverseKinematics.go @@ -0,0 +1,32 @@ +// 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 ( + "context" + + "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 { + // Solve receives a context, the goal arm position, and current joint angles. + Solve(context.Context, chan<- *Solution, []referenceframe.Input, StateMetric, int) error +} + +// 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 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/metrics.go b/motionplan/ik/metrics.go similarity index 71% rename from motionplan/metrics.go rename to motionplan/ik/metrics.go index 6dbcb62636a..38012b0ed2f 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 +// 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()) @@ -113,11 +133,14 @@ 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. +// It allows the caller to choose the scaling level of orientation. +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(orientationScaleFactor).Norm2() + } } // SquaredNormNoOrientSegmentMetric is a metric which will return the cartesian distance between the two positions. 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 77% rename from motionplan/nloptInverseKinematics.go rename to motionplan/ik/nloptInverseKinematics.go index b5f8a43d6f8..a454f5a4771 100644 --- a/motionplan/nloptInverseKinematics.go +++ b/motionplan/ik/nloptInverseKinematics.go @@ -1,17 +1,19 @@ //go:build !windows -package motionplan +package ik import ( "context" "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" ) @@ -35,23 +37,32 @@ type NloptIK struct { upperBound []float64 maxIterations int epsilon float64 - solveEpsilon float64 logger golog.Logger jump float64 + + // 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. + 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. -func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int, solveEpsilon float64) (*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 @@ -60,15 +71,16 @@ 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.exact = exact return ik, nil } // 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<- *Solution, seed []referenceframe.Input, - m StateMetric, + solveMetric StateMetric, rseed int, ) error { //nolint: gosec @@ -90,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. @@ -109,7 +122,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 +138,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 } @@ -134,13 +147,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), ) @@ -165,32 +178,52 @@ 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 err = multierr.Combine(err, nloptErr) } - if result < ik.epsilon*ik.epsilon { + if result < ik.epsilon || (solutionRaw != nil && !ik.exact) { select { case <-ctx.Done(): return err - case c <- referenceframe.FloatsToInputs(solutionRaw): + default: + } + solutionChan <- &Solution{ + Configuration: referenceframe.FloatsToInputs(solutionRaw), + Score: result, + Exact: result < ik.epsilon, } solutionsFound++ } @@ -266,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/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 9716e4e4d0f..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) + ik, err := CreateNloptIKSolver(m, logger, -1, false) test.That(t, err, test.ShouldBeNil) ik.id = 1 diff --git a/motionplan/inverseKinematics.go b/motionplan/inverseKinematics.go deleted file mode 100644 index d759acd3a2f..00000000000 --- a/motionplan/inverseKinematics.go +++ /dev/null @@ -1,23 +0,0 @@ -package motionplan - -import ( - "context" - - "go.viam.com/rdk/referenceframe" -) - -// 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 { - // Solve receives a context, the goal arm position, and current joint angles. - Solve(context.Context, chan<- []referenceframe.Input, []referenceframe.Input, StateMetric, int) error -} - -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/kinematic_test.go b/motionplan/kinematic_test.go index 726655755b2..2bc36add521 100644 --- a/motionplan/kinematic_test.go +++ b/motionplan/kinematic_test.go @@ -1,14 +1,10 @@ package motionplan 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" @@ -19,11 +15,6 @@ import ( "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) @@ -285,45 +276,6 @@ func TestComplicatedDynamicFrameSystem(t *testing.T) { 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) @@ -344,62 +296,6 @@ func TestSVAvsDH(t *testing.T) { } } -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 []frame.Input) - 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) - // 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 diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 6858f408f69..b3aa9105d25 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -5,6 +5,7 @@ import ( "context" "math/rand" "sort" + "sync" "time" "github.com/edaniels/golog" @@ -12,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" ) @@ -31,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) @@ -128,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 @@ -143,7 +147,7 @@ func motionPlanInternal(ctx context.Context, } type planner struct { - solver InverseKinematics + solver ik.InverseKinematics frame frame.Frame logger golog.Logger randseed *rand.Rand @@ -152,12 +156,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, @@ -167,7 +171,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, }) @@ -176,7 +180,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, @@ -186,14 +190,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 { @@ -265,11 +273,15 @@ 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 *ik.Solution, mp.planOpts.NumThreads*2) 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()) }) @@ -289,14 +301,15 @@ 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{ + 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, @@ -342,6 +355,13 @@ IK: // Cancel any ongoing processing within the IK solvers if we're done receiving solutions cancel() + for done := false; !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/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..2ff20bc46e3 100644 --- a/motionplan/nearestNeighbor.go +++ b/motionplan/nearestNeighbor.go @@ -6,6 +6,8 @@ import ( "sort" "go.viam.com/utils" + + "go.viam.com/rdk/motionplan/ik" ) const defaultNeighborsBeforeParallelization = 1000 @@ -32,7 +34,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 +81,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 +160,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 1c4fb925178..17e96229eaf 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -14,14 +14,16 @@ 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" ) 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. @@ -445,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 @@ -515,11 +517,8 @@ 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 = ik.NewSquaredNormSegmentMetric(defaultTPspaceOrientationScale) - // 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" } @@ -558,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/rrt.go b/motionplan/rrt.go index 158f71f2013..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} @@ -115,7 +116,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} } // fixedStepInterpolation returns inputs at qstep distance along the path from start to target @@ -204,7 +205,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 { @@ -226,8 +227,10 @@ 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 { @@ -237,3 +240,11 @@ func extractPath(startMap, goalMap map[node]node, pair *nodePair) []node { } return path } + +func sumCosts(path []node) float64 { + cost := 0. + for _, wp := range path { + cost += wp.Cost() + } + return cost +} diff --git a/motionplan/rrtStarConnect.go b/motionplan/rrtStarConnect.go index 6cd3e374fc6..56811915e28 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" ) @@ -115,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 @@ -143,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 { @@ -153,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 @@ -170,17 +171,17 @@ 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 { - 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} 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 5f214aa457f..c185903843a 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -8,37 +8,47 @@ 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" ) 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 + 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 // 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. - - // When getting the closest node to a pose, only look for nodes at least this far away. - defaultDuplicateNodeBuffer = 50. + defaultAddNodeEvery = 100. // 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 = 5. - // Default distance in mm to get within for tp-space trajectories. - defaultTPSpaceGoalDist = 10. + // 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 + // 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 { @@ -52,11 +62,26 @@ 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 + + // 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 + + // 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 + 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. @@ -68,9 +93,15 @@ type candidate struct { lastInTraj bool } +type nodeAndError struct { + node + error +} + // tpSpaceRRTMotionPlanner. type tpSpaceRRTMotionPlanner struct { *planner + mu sync.Mutex algOpts *tpspaceOptions tpFrame tpspace.PTGProvider } @@ -90,7 +121,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) @@ -101,6 +131,9 @@ func newTPSpaceMotionPlanner( tpFrame: tpFrame, } tpPlanner.setupTPSpaceOptions() + + tpPlanner.algOpts.ikSeed = []referenceframe.Input{{math.Pi / 2}, {tpFrame.PTGs()[0].MaxDistance() / 2}} + return tpPlanner, nil } @@ -114,7 +147,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{ @@ -172,185 +205,329 @@ func (mp *tpSpaceRRTMotionPlanner) planRunner( } } - dist := math.Sqrt(mp.planOpts.DistanceFunc(&Segment{StartPosition: startPose, EndPosition: goalPose})) - midPt := goalPose.Point().Sub(startPose.Point()) + 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})) + 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 - 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 + + utils.PanicCapturingGo(func() { + m1chan <- mp.attemptExtension(ctx, randPosNode, rrt.maps.startMap, false) + }) + utils.PanicCapturingGo(func() { + m2chan <- mp.attemptExtension(ctx, randPosNode, rrt.maps.goalMap, true) + }) + seedMapReached := <-m1chan + goalMapReached := <-m2chan + + seedMapNode := seedMapReached.node + goalMapNode := goalMapReached.node + err := multierr.Combine(seedMapReached.error, goalMapReached.error) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return } - randPosNode := &basicNode{q: nil, cost: 0, pose: randPos, corner: false} - successNode := mp.attemptExtension(ctx, nil, randPosNode, rrt) - // 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}) { - // If we've reached the goal, break out - path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: successNode}) + if seedMapNode != nil && goalMapNode != nil { + seedReached := mp.attemptExtension(ctx, goalMapNode, rrt.maps.startMap, false) + if seedReached.error != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: seedReached.error, maps: rrt.maps} + return + } + if seedReached.node == nil { + continue + } + goalReached := mp.attemptExtension(ctx, seedReached.node, rrt.maps.goalMap, true) + if goalReached.error != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: goalReached.error, maps: rrt.maps} + return + } + if goalReached.node == nil { + continue + } + 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) rrt.solutionChan <- &rrtPlanReturn{steps: path, maps: rrt.maps} return } } + 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) + if seedReached.error != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: seedReached.error, maps: rrt.maps} + return + } + if seedReached.node == nil { + continue + } + 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) + 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 := rectifyTPspacePath(bestPath, mp.frame) + if err != nil { + rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps} + return + } + rrt.solutionChan <- &rrtPlanReturn{steps: correctedPath, maps: rrt.maps} + 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")} } -// 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, ptgNum int, curPtg tpspace.PTG, - rrt *rrtParallelPlannerShared, + rrt rrtMap, nearest node, - planOpts *plannerOptions, // Need to pass this in explicitly for smoothing -) *candidate { - nm := &neighborManager{nCPU: mp.planOpts.NumThreads} + invert bool, +) (*candidate, error) { + nm := &neighborManager{nCPU: mp.planOpts.NumThreads / len(mp.tpFrame.PTGs())} nm.parallelNeighbors = 10 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 + 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 - relPose := spatialmath.Compose(spatialmath.PoseInverse(nearest.Pose()), randPosNode.Pose()) - relPosePt := relPose.Point() + var targetFunc ik.StateMetric + if invert { + 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()) + targetFunc = ik.NewSquaredNormMetric(relPose) + } + solutionChan := make(chan *ik.Solution, 1) + mp.mu.Lock() + rseed := mp.randseed.Int() + mp.mu.Unlock() + err := curPtg.Solve(context.Background(), solutionChan, mp.algOpts.ikSeed, targetFunc, rseed) - // Convert cartesian distance to tp-space using inverse curPtg, yielding TP-space coordinates goalK and goalD - nodes := curPtg.CToTP(relPosePt.X, relPosePt.Y) - bestNode, bestDist := mp.closestNode(relPose, nodes, mp.algOpts.dupeNodeBuffer) - if bestNode == nil { - return nil + var bestNode *ik.Solution + select { + case bestNode = <-solutionChan: + default: + } + if err != nil || bestNode == nil { + return nil, err } - goalK := bestNode.K - 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 := curPtg.Trajectory(goalK) - var lastNode *tpspace.TrajNode - var lastPose spatialmath.Pose + trajK, err := curPtg.Trajectory(goalAlpha, goalD) + if err != nil { + return nil, err + } + finalTrajNode := trajK[len(trajK)-1] + + arcStartPose := nearest.Pose() + if invert { + arcStartPose = spatialmath.Compose(arcStartPose, spatialmath.PoseInverse(finalTrajNode.Pose)) + } 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 { - if trajPt.Dist > goalD { - // After we've passed randD, no need to keep checking, just add to RRT tree - isLastNode = false - break + 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} - if sinceLastCollideCheck > planOpts.Resolution { - ok, _ := planOpts.CheckStateConstraints(trajState) + + 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 > mp.planOpts.Resolution { + ok, _ := mp.planOpts.CheckStateConstraints(trajState) if !ok { - return nil + return nil, errInvalidCandidate } sinceLastCollideCheck = 0. } - lastPose = trajState.Position - lastNode = trajPt - lastDist = lastNode.Dist + lastDist = trajPt.Dist } + + isLastNode := math.Abs(finalTrajNode.Dist-curPtg.MaxDistance()) < 0.1 + // add the last node in trajectory successNode = &basicNode{ - q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), float64(goalK), lastNode.Dist}), - cost: nearest.Cost() + lastNode.Dist, - pose: lastPose, + q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), goalAlpha, 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.maps.startMap) + nearest = nm.nearestNeighbor(ctx, mp.planOpts, successNode, rrt) if nearest != nil { - dist := planOpts.DistanceFunc(&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 { 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 // closest to that goal. func (mp *tpSpaceRRTMotionPlanner) attemptExtension( ctx context.Context, - seedNode, goalNode node, - rrt *rrtParallelPlannerShared, -) node { - for { + 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(): - return nil + return &nodeAndError{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) - if cand != nil { - if cand.err == nil { + ptgNumPar, curPtgPar := ptgNum, curPtg + utils.PanicCapturingGo(func() { + 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 + } + 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) } } } - reseedCandidate := mp.extendMap(ctx, candidates, rrt) + var err error + reseedCandidate, err = mp.extendMap(ctx, candidates, rrt, invert) + if err != nil && !errors.Is(err, errNoCandidates) { + return &nodeAndError{nil, err} + } if reseedCandidate == nil { - return nil + return &nodeAndError{nil, nil} } - dist := mp.planOpts.DistanceFunc(&Segment{StartPosition: reseedCandidate.newNode.Pose(), EndPosition: goalNode.Pose()}) - if dist < mp.planOpts.GoalThreshold || !reseedCandidate.lastInTraj { + 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 reseedCandidate.newNode + 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)) + } + // 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} } -// 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, - rrt *rrtParallelPlannerShared, -) *candidate { + rrt rrtMap, + invert bool, +) (*candidate, error) { if len(candidates) == 0 { - return 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 @@ -362,44 +539,66 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( bestDist = cand.dist } } - treeNode := bestCand.treeNode - newNode := bestCand.newNode + 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 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 + } + arcStartPose := treeNode.Pose() + if invert { + arcStartPose = spatialmath.Compose(arcStartPose, spatialmath.PoseInverse(trajK[len(trajK)-1].Pose)) + } 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 - } - 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), float64(randK), trajPt.Dist}), - cost: treeNode.Cost() + trajPt.Dist, - pose: trajState.Position, - corner: false, + var trajState *ik.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() } - rrt.maps.startMap[addedNode] = treeNode - sinceLastNode = 0. + 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) + } else { + mp.logger.Debugf("$REVTREE,%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 + // 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}), + cost: trajPt.Dist, + pose: trajState.Position, + corner: false, + } + rrt[addedNode] = treeNode + sinceLastNode = 0. + } + lastDist = trajPt.Dist + } + if mp.algOpts.pathdebug { + mp.logger.Debugf("$WPI,%f,%f\n", trajState.Position.Point().X, trajState.Position.Point().Y) } - lastDist = trajPt.Dist } - rrt.maps.startMap[newNode] = treeNode - return bestCand + rrt[newNode] = treeNode + return bestCand, nil } func (mp *tpSpaceRRTMotionPlanner) setupTPSpaceOptions() { @@ -407,62 +606,218 @@ func (mp *tpSpaceRRTMotionPlanner) setupTPSpaceOptions() { goalCheck: defaultGoalCheck, autoBB: defaultAutoBB, - addIntermediate: defaultAddInt, - addNodeEvery: defaultAddNodeEvery, + addIntermediate: defaultAddInt, + addNodeEvery: defaultAddNodeEvery, + attemptSolveEvery: defaultAttemptSolveEvery, + smoothScaleFactor: defaultSmoothScaleFactor, - dupeNodeBuffer: defaultDuplicateNodeBuffer, + poseSolveDist: defaultIdenticalNodeDistance, - distOptions: map[tpspace.PTG]*plannerOptions{}, + 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, false) + tpOpt.invertDistOptions[curPtg] = mp.make2DTPSpaceDistanceOptions(curPtg, true) } 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 { +func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTG, invert bool) *plannerOptions { opts := newBasicPlannerOptions(mp.frame) - - segMet := func(seg *Segment) float64 { + mp.mu.Lock() + //nolint: gosec + randSeed := rand.New(rand.NewSource(mp.randseed.Int63() + mp.randseed.Int63())) + mp.mu.Unlock() + + 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) } - relPose := spatialmath.Compose(spatialmath.PoseInverse(seg.StartPosition), seg.EndPosition) - relPosePt := relPose.Point() - nodes := ptg.CToTP(relPosePt.X, relPosePt.Y) - closeNode, _ := mp.closestNode(relPose, nodes, min) - if closeNode == nil { + var targetFunc ik.StateMetric + if invert { + 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) + targetFunc = ik.NewSquaredNormMetric(relPose) + } + solutionChan := make(chan *ik.Solution, 1) + err := ptg.Solve(context.Background(), solutionChan, mp.algOpts.ikSeed, targetFunc, randSeed.Int()) + + var closeNode *ik.Solution + select { + case closeNode = <-solutionChan: + default: + } + if err != nil || closeNode == nil { return math.Inf(1) } - return closeNode.Dist + pose, err := ptg.Transform(closeNode.Configuration) + if err != nil { + return math.Inf(1) + } + return targetFunc(&ik.State{Position: pose}) } - opts.DistanceFunc = segMet + 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") + 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 := tpspace.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(): + 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 + 2 + mp.randseed.Intn((len(path)-2)-firstEdge) + + newInputSteps, err := mp.attemptSmooth(ctx, path, firstEdge, secondEdge, smoothPlanner) + if err != nil || newInputSteps == nil { + continue + } + newCost := sumCosts(newInputSteps) + if newCost >= currCost { + continue + } + // Re-connect to the final goal + if newInputSteps[len(newInputSteps)-1] != path[len(path)-1] { + newInputSteps = append(newInputSteps, path[len(path)-1]) + } + + goalInputSteps, err := mp.attemptSmooth(ctx, newInputSteps, len(newInputSteps)-3, len(newInputSteps)-1, smoothPlanner) + if err != nil || goalInputSteps == nil { + continue + } + goalInputSteps = append(goalInputSteps, path[len(path)-1]) + path = goalInputSteps + currCost = sumCosts(path) + } return path } + +// 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, + firstEdge, secondEdge int, + smoother *tpSpaceRRTMotionPlanner, +) ([]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], {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)), + pose: spatialmath.Compose(parentPose, trajK[len(trajK)-1].Pose), + corner: false, + } + startMap[intNode] = parent + } + parent = pathNode + parentPose = parent.Pose() + } + // 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(&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") + } + + newInputSteps := extractPath(startMap, nil, &nodePair{a: reached.node, b: nil}, false) + + if secondEdge < len(path)-1 { + newInputSteps = append(newInputSteps, path[secondEdge+1:]...) + } + return rectifyTPspacePath(newInputSteps, mp.frame) +} + +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 +} + +// 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 +} diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 83140df52bf..0d86e7d0909 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -4,6 +4,7 @@ package motionplan import ( "context" + "math" "math/rand" "testing" @@ -11,20 +12,28 @@ 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" ) +var printPath = false + const testTurnRad = 0.3 func TestPtgRrt(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) geometries := []spatialmath.Geometry{roverGeom} - ackermanFrame, err := referenceframe.NewPTGFrameFromTurningRadius( + ctx := context.Background() + + ackermanFrame, err := tpspace.NewPTGFrameFromTurningRadius( "ackframe", + logger, 300., testTurnRad, 0, @@ -32,29 +41,72 @@ 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: 200, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 90}) opt := newBasicPlannerOptions(ackermanFrame) - opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) - opt.DistanceFunc = SquaredNormNoOrientSegmentMetric - opt.GoalThreshold = 10. + 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) + 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(context.Background(), goalPos, nil) + 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 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 { + tp.logger.Debugf("$WP,%f,%f\n", intPose.Point().X, intPose.Point().Y) + } + 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 + } + } + } + } } 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) geometries := []spatialmath.Geometry{roverGeom} - ackermanFrame, err := referenceframe.NewPTGFrameFromTurningRadius( + ackermanFrame, err := tpspace.NewPTGFrameFromTurningRadius( "ackframe", + logger, 300., testTurnRad, 0, @@ -64,19 +116,18 @@ 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()) opt := newBasicPlannerOptions(ackermanFrame) - opt.SetGoalMetric(NewPositionOnlyMetric(goalPos)) - opt.DistanceFunc = SquaredNormNoOrientSegmentMetric - opt.GoalThreshold = 30. + opt.DistanceFunc = ik.NewSquaredNormSegmentMetric(30.) + 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, 2000, 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) @@ -102,8 +153,153 @@ 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) - + 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) + test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThan, 2) + + allPtgs := ackermanFrame.(tpspace.PTGProvider).PTGs() + lastPose := spatialmath.NewZeroPose() + + 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 { + tp.logger.Debugf("$WP,%f,%f\n", intPose.Point().X, intPose.Point().Y) + } + 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 + } + } + } + } +} + +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) + geometries := []spatialmath.Geometry{roverGeom} + + ackermanFrame, err := tpspace.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(ackermanFrame) + 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) + 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) +} + +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 := tpspace.NewPTGFrameFromTurningRadius( + "ackframe", + logger, + 300., + testTurnRad, + 0, + geometries, + ) + test.That(t, err, test.ShouldBeNil) + + opt := newBasicPlannerOptions(ackermanFrame) + 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) + + // 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 = rectifyTPspacePath(plan, tp.frame) + 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 2ef1e86479a..4f40fb0ea8f 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -4,26 +4,26 @@ package tpspace import ( "math" - "github.com/golang/geo/r3" - + "go.viam.com/rdk/motionplan/ik" + "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. 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 + // 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 precomputed trajectory may travel - RefDistance() float64 + // MaxDistance returns the maximum distance that a single trajectory may travel + MaxDistance() 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, error) } // PTGProvider is something able to provide a set of PTGs associsated with it. For example, a frame which precomputes @@ -36,7 +36,8 @@ 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 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, @@ -46,18 +47,12 @@ 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 - - ptX float64 - ptY float64 } // 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() @@ -73,6 +68,60 @@ 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, 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 + distTravelled := math.Copysign(math.Abs(v)*diffT, dist) + + // Step through each time point for this alpha + for math.Abs(distTravelled) < math.Abs(dist) { + t += diffT + 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. + // 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) + 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}, {dist}}) + if err != nil { + return nil, err + } + 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) { + 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 + } + return &TrajNode{pose, atT, dist, alpha, v, w}, nil } diff --git a/motionplan/tpspace/ptgC.go b/motionplan/tpspace/ptgC.go index 4d0861b6181..f51f42a44b6 100644 --- a/motionplan/tpspace/ptgC.go +++ b/motionplan/tpspace/ptgC.go @@ -1,32 +1,80 @@ package tpspace import ( + "fmt" "math" + + "github.com/golang/geo/r3" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" ) // ptgDiffDriveC defines a PTG family composed of circular trajectories with an alpha-dependent radius. 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 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 + if dist == 0 { + return 0, 0, nil + } + k := math.Copysign(1.0, dist) + v := ptg.maxMMPS * k + w := (alpha / math.Pi) * ptg.maxRPS * 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) > 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 { + 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 / 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 + 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..a437e0ed337 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 @@ -10,22 +13,26 @@ 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).(*ptgDiffDriveC) + return &ptgDiffDriveCC{ maxMMPS: maxMMPS, maxRPS: maxRPS, - k: k, + 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 *ptgDiffDriveCC) PTGVelocities(alpha, t, 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 u := math.Abs(alpha) * 0.5 @@ -33,23 +40,45 @@ 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 { // l+ v = ptg.maxMMPS w = ptg.maxRPS } - // Turn in the opposite direction?? + // Turn in the opposite direction if alpha < 0 { w *= -1 } - v *= ptg.k - w *= ptg.k + v *= k + w *= k return v, w, nil } + +func (ptg *ptgDiffDriveCC) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + 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 + 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 + } + 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/motionplan/tpspace/ptgCCS.go b/motionplan/tpspace/ptgCCS.go index 1d3ffc5c6bc..0675ca3d830 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 @@ -9,34 +12,38 @@ 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).(*ptgDiffDriveC) + return &ptgDiffDriveCCS{ maxMMPS: maxMMPS, maxRPS: maxRPS, - k: k, + r: r, + 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 *ptgDiffDriveCCS) PTGVelocities(alpha, t, x, y, phi float64) (float64, float64, error) { +func (ptg *ptgDiffDriveCCS) PTGVelocities(alpha, dist float64) (float64, float64, error) { u := math.Abs(alpha) * 0.5 - - r := ptg.maxMMPS / ptg.maxRPS + k := math.Copysign(1.0, dist) v := ptg.maxMMPS w := 0. - if t < u*r/ptg.maxMMPS { + if dist < u*ptg.r { // l- v = -ptg.maxMMPS w = ptg.maxRPS - } else if t < (u+math.Pi/2)*r/ptg.maxMMPS { + } else if dist < (u+math.Pi/2)*ptg.r { // l+ pi/2 v = ptg.maxMMPS w = ptg.maxRPS @@ -47,8 +54,46 @@ 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 } + +func (ptg *ptgDiffDriveCCS) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + 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 + 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 + } + 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..0e9eaa9da37 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. @@ -9,32 +16,35 @@ import ( 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 } // 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 := turnStraightConst * r return &ptgDiffDriveCS{ - maxMMPS: maxMMPS, - maxRPS: maxRPS, - k: k, + maxMMPS: maxMMPS, + maxRPS: maxRPS, + r: r, + turnStraight: turnStraight, } } // 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) { - r := ptg.maxMMPS / ptg.maxRPS - +func (ptg *ptgDiffDriveCS) PTGVelocities(alpha, dist float64) (float64, float64, error) { // Magic number; rotate this much before going straight - // Bigger value = more rotation - turnStraight := 1.2 * math.Sqrt(math.Abs(alpha)) * r / ptg.maxMMPS + turnDist := math.Sqrt(math.Abs(alpha)) * ptg.turnStraight + k := math.Copysign(1.0, dist) v := ptg.maxMMPS w := 0. - if t < turnStraight { + if dist < turnDist { // l+ v = ptg.maxMMPS w = ptg.maxRPS * math.Min(1.0, 1.0-math.Exp(-1*alpha*alpha)) @@ -45,7 +55,35 @@ 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 } + +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 + direction := math.Copysign(1., dist) // forwards or backwards + 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 := 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/ptgGridSim.go b/motionplan/tpspace/ptgGridSim.go index e5b779ae039..cef65c82a75 100644 --- a/motionplan/tpspace/ptgGridSim.go +++ b/motionplan/tpspace/ptgGridSim.go @@ -1,57 +1,52 @@ package tpspace import ( + "context" "math" - "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/motionplan/ik" + "go.viam.com/rdk/referenceframe" ) const ( defaultMaxTime = 15. defaultDiffT = 0.005 - defaultAlphaCnt uint = 121 - - defaultSearchRadius = 10. - - defaultMaxHeadingChange = 1.95 * math.Pi + defaultAlphaCnt uint = 91 ) // 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 - // 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 + // 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 } // 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 } ptg := &ptgGridSim{ - refDist: simDist, - alphaCnt: arcs, - maxTime: defaultMaxTime, - diffT: defaultDiffT, - searchRad: defaultSearchRadius, - - trajNodeGrid: map[int]map[int][]*TrajNode{}, + refDist: simDist, + alphaCnt: arcs, + maxTime: defaultMaxTime, + 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 } @@ -60,41 +55,38 @@ func NewPTGGridSim(simPTG PrecomputePTG, arcs uint, simDist float64) (PTG, error return ptg, nil } -func (ptg *ptgGridSim) CToTP(x, y float64) []*TrajNode { - 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 - } - +func (ptg *ptgGridSim) Solve( + ctx context.Context, + solutionChan chan<- *ik.Solution, + 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 - 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 + if !ptg.endsOnly { + for k := 0; k < int(ptg.alphaCnt); k++ { + nMax := len(ptg.precomputeTraj[k]) - 1 + for n := 0; n <= nMax; n++ { + distToPoint := solveMetric(&ik.State{Position: ptg.precomputeTraj[k][n].Pose}) + if distToPoint < bestDist { + bestDist = distToPoint - bestNode = ptg.precomputeTraj[k][n] + bestNode = ptg.precomputeTraj[k][n] + } } } - } - if bestNode != nil { - return []*TrajNode{bestNode} + if bestNode != nil { + solutionChan <- &ik.Solution{ + Configuration: []referenceframe.Input{{bestNode.Alpha}, {bestNode.Dist}}, + Score: bestDist, + Exact: false, + } + return nil + } } // Given a point (x,y), compute the "k_closest" whose extrapolation @@ -102,9 +94,7 @@ func (ptg *ptgGridSim) CToTP(x, y float64) []*TrajNode { // 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) + distToPoint := solveMetric(&ik.State{Position: ptg.precomputeTraj[k][n].Pose}) if distToPoint < bestDist { bestDist = distToPoint @@ -112,98 +102,33 @@ func (ptg *ptgGridSim) CToTP(x, y float64) []*TrajNode { } } - return []*TrajNode{bestNode} + solutionChan <- &ik.Solution{ + Configuration: []referenceframe.Input{{bestNode.Alpha}, {bestNode.Dist}}, + Score: bestDist, + Exact: false, + } + return nil } -func (ptg *ptgGridSim) RefDistance() float64 { +func (ptg *ptgGridSim) MaxDistance() 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) Trajectory(alpha, dist float64) ([]*TrajNode, error) { + return ComputePTG(ptg, alpha, dist, defaultDiffT) } -func (ptg *ptgGridSim) simulateTrajectories(simPtg PrecomputePTG) ([][]*TrajNode, error) { - xMin := 500.0 - xMax := -500.0 - yMin := 500.0 - yMax := -500.0 - +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) - // 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{} + alphaTraj, err := ComputePTG(ptg, alpha, ptg.refDist, ptg.diffT) + if err != nil { + return nil, err } - 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/ptgGridSim_test.go b/motionplan/tpspace/ptgGridSim_test.go index 4f22427460b..6551103e68d 100644 --- a/motionplan/tpspace/ptgGridSim_test.go +++ b/motionplan/tpspace/ptgGridSim_test.go @@ -6,26 +6,26 @@ import ( "go.viam.com/test" ) -var defaultPTGs = []func(float64, float64, float64) PrecomputePTG{ - NewCirclePTG, - NewCCPTG, - NewCCSPTG, - NewCSPTG, - NewAlphaPTG, -} - var ( - defaultMps = 0.3 + defaultMMps = 300. turnRadMeters = 0.3 ) func TestSim(t *testing.T) { + simDist := 2500. + alphaCnt := uint(121) for _, ptg := range defaultPTGs { - radPS := defaultMps / turnRadMeters + radPS := defaultMMps / (turnRadMeters * 1000) - ptgGen := ptg(defaultMps, radPS, 1.) + ptgGen := ptg(defaultMMps, radPS) test.That(t, ptgGen, test.ShouldNotBeNil) - _, err := NewPTGGridSim(ptgGen, defaultAlphaCnt, 1000.) + grid, err := NewPTGGridSim(ptgGen, alphaCnt, simDist, false) test.That(t, err, test.ShouldBeNil) + + for i := uint(0); i < alphaCnt; i++ { + traj, err := grid.Trajectory(index2alpha(i, alphaCnt), simDist) + test.That(t, err, test.ShouldBeNil) + test.That(t, traj, test.ShouldNotBeNil) + } } } diff --git a/motionplan/tpspace/ptgGroupFrame.go b/motionplan/tpspace/ptgGroupFrame.go new file mode 100644 index 00000000000..61720ac379e --- /dev/null +++ b/motionplan/tpspace/ptgGroupFrame.go @@ -0,0 +1,208 @@ +package tpspace + +import ( + "errors" + "fmt" + "math" + + "github.com/edaniels/golog" + pb "go.viam.com/api/component/arm/v1" + + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" +) + +const ( + ptgIndex int = iota + trajectoryAlphaWithinPTG + distanceAlongTrajectoryIndex +) + +// 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{ + NewCirclePTG, + NewCCPTG, + NewCCSPTG, + NewCSPTG, + NewSideSPTG, + NewSideSOverturnPTG, +} + +type ptgGroupFrame struct { + name string + limits []referenceframe.Limit + geometries []spatialmath.Geometry + ptgs []PTG + velocityMMps float64 + turnRadMillimeters float64 + logger golog.Logger +} + +// 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 { + return nil, fmt.Errorf("cannot create ptg frame, refDist %f must be >=0", refDist) + } + + turnRadMillimeters := turnRadMeters * 1000 + + if refDist == 0 { + // Default to a distance of just over one half of a circle turning at max radius + refDist = turnRadMillimeters * math.Pi * refDistHalfCircles + logger.Debugf("refDist was zero, calculating default %f", refDist) + } + + // Get max angular velocity in radians per second + maxRPS := velocityMMps / turnRadMillimeters + pf := &ptgGroupFrame{name: name} + err := pf.initPTGs(logger, velocityMMps, maxRPS, refDist) + if err != nil { + return nil, err + } + + pf.geometries = geoms + pf.velocityMMps = velocityMMps + pf.turnRadMillimeters = turnRadMillimeters + + 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 +} + +// 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 { + return nil, fmt.Errorf("cannot create ptg frame, refDist %f must be >=0", refDist) + } + + if refDist <= 0 { + 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 / ptgFrame.turnRadMillimeters + pf := &ptgGroupFrame{name: ptgFrame.name} + err := pf.initPTGs(ptgFrame.logger, ptgFrame.velocityMMps, maxRPS, refDist) + 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: 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) { + if len(inputs) != len(pf.DoF()) { + return nil, referenceframe.NewIncorrectInputLengthError(len(inputs), len(pf.DoF())) + } + 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() []PTG { + return pf.ptgs +} + +func (pf *ptgGroupFrame) initPTGs(logger golog.Logger, maxMps, maxRPS, simDist float64) error { + ptgs := []PTG{} + for _, ptg := range defaultPTGs { + ptgGen := ptg(maxMps, maxRPS) + if ptgGen != nil { + newptg, err := NewPTGIK(ptgGen, logger, simDist, 2) + if err != nil { + return err + } + ptgs = append(ptgs, newptg) + } + } + pf.ptgs = ptgs + return nil +} diff --git a/motionplan/tpspace/ptgIK.go b/motionplan/tpspace/ptgIK.go new file mode 100644 index 00000000000..23c5d17c941 --- /dev/null +++ b/motionplan/tpspace/ptgIK.go @@ -0,0 +1,149 @@ +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 := newPTGIKFrame(simPTG, refDist) + + 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.Solution, + seed []referenceframe.Input, + solveMetric ik.StateMetric, + nloptSeed int, +) error { + internalSolutionGen := make(chan *ik.Solution, 1) + defer close(internalSolutionGen) + 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: + } + 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.Solution + 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) MaxDistance() 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 { + // Caching 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/tpspace/ptgIKFrame.go b/motionplan/tpspace/ptgIKFrame.go new file mode 100644 index 00000000000..57f6ac99c36 --- /dev/null +++ b/motionplan/tpspace/ptgIKFrame.go @@ -0,0 +1,60 @@ +package tpspace + +import ( + "errors" + "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 +} + +// 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 { + pf := &ptgIKFrame{PrecomputePTG: ptg} + + pf.limits = []referenceframe.Limit{ + {Min: -math.Pi, Max: math.Pi}, + {Min: 0, Max: dist}, + } + return pf +} + +func (pf *ptgIKFrame) DoF() []referenceframe.Limit { + return pf.limits +} + +func (pf *ptgIKFrame) Name() string { + return "" +} + +func (pf *ptgIKFrame) MarshalJSON() ([]byte, error) { + return nil, errors.New("marshal json not implemented for ptg IK frame") +} + +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, errors.New("geometries not implemented for ptg IK frame") +} diff --git a/motionplan/tpspace/ptgSideS.go b/motionplan/tpspace/ptgSideS.go new file mode 100644 index 00000000000..5c8ab838895 --- /dev/null +++ b/motionplan/tpspace/ptgSideS.go @@ -0,0 +1,106 @@ +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 // turning radius + countersteer float64 // scale the length of the second arc by this much + 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 +} diff --git a/motionplan/tpspace/ptg_test.go b/motionplan/tpspace/ptg_test.go new file mode 100644 index 00000000000..081d8339ade --- /dev/null +++ b/motionplan/tpspace/ptg_test.go @@ -0,0 +1,22 @@ +package tpspace + +import ( + "math" + "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) + } +} + +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 +} diff --git a/motionplan/tpspace/simPtgAlpha.go b/motionplan/tpspace/simPtgAlpha.go deleted file mode 100644 index e5899d02818..00000000000 --- a/motionplan/tpspace/simPtgAlpha.go +++ /dev/null @@ -1,38 +0,0 @@ -package tpspace - -import ( - "math" -) - -// 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 -} 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 diff --git a/referenceframe/ptgFrame.go b/referenceframe/ptgFrame.go deleted file mode 100644 index 093b2fc59fe..00000000000 --- a/referenceframe/ptgFrame.go +++ /dev/null @@ -1,158 +0,0 @@ -package referenceframe - -import ( - "fmt" - "math" - - pb "go.viam.com/api/component/arm/v1" - - "go.viam.com/rdk/motionplan/tpspace" - "go.viam.com/rdk/spatialmath" -) - -const ( - defaultSimDistMM = 2000. - defaultAlphaCnt uint = 121 -) - -const ( - ptgIndex int = iota - trajectoryIndexWithinPTG - distanceAlongTrajectoryIndex -) - -type ptgFactory func(float64, float64, float64) tpspace.PrecomputePTG - -var defaultPTGs = []ptgFactory{ - tpspace.NewCirclePTG, - tpspace.NewCCPTG, - tpspace.NewCCSPTG, - tpspace.NewCSPTG, - tpspace.NewAlphaPTG, -} - -type ptgGridSimFrame struct { - name string - limits []Limit - geometries []spatialmath.Geometry - ptgs []tpspace.PTG -} - -// NewPTGFrameFromTurningRadius will create a new Frame which is also a tpspace.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) { - 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 = []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() []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 []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) []Input { - n := make([]Input, len(jp.Values)) - for idx, d := range jp.Values { - n[idx] = Input{d} - } - return n -} - -func (pf *ptgGridSimFrame) ProtobufFromInput(input []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 []Input) (*GeometriesInFrame, error) { - if len(pf.geometries) == 0 { - return 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 NewGeometriesInFrame(pf.name, geoms), nil -} - -func (pf *ptgGridSimFrame) PTGs() []tpspace.PTG { - return pf.ptgs -} - -func (pf *ptgGridSimFrame) initPTGs(maxMps, maxRPS, simDist float64) error { - ptgs := []tpspace.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) - if err != nil { - return err - } - ptgs = append(ptgs, newptg) - } - } - } - pf.ptgs = ptgs - return nil -}