diff --git a/components/base/kinematicbase/ptgKinematics_test.go b/components/base/kinematicbase/ptgKinematics_test.go index a4fc72f852c..d610a303a3a 100644 --- a/components/base/kinematicbase/ptgKinematics_test.go +++ b/components/base/kinematicbase/ptgKinematics_test.go @@ -77,13 +77,14 @@ func TestPTGKinematicsNoGeom(t *testing.T) { fs.AddFrame(f, fs.World()) inputMap := referenceframe.StartPositions(fs) + startState := motionplan.NewPlanState(motionplan.PathState{f.Name(): referenceframe.NewZeroPoseInFrame(referenceframe.World)}, inputMap) + goalState := motionplan.NewPlanState(motionplan.PathState{f.Name(): dstPIF}, nil) + plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ - Logger: logger, - Goal: dstPIF, - Frame: f, - StartConfiguration: inputMap, - StartPose: spatialmath.NewZeroPose(), - FrameSystem: fs, + Logger: logger, + Goals: []*motionplan.PlanState{goalState}, + StartState: startState, + FrameSystem: fs, }) test.That(t, err, test.ShouldBeNil) test.That(t, plan, test.ShouldNotBeNil) @@ -164,14 +165,14 @@ func TestPTGKinematicsWithGeom(t *testing.T) { ) test.That(t, err, test.ShouldBeNil) + startState := motionplan.NewPlanState(motionplan.PathState{f.Name(): referenceframe.NewZeroPoseInFrame(referenceframe.World)}, inputMap) + goalState := motionplan.NewPlanState(motionplan.PathState{f.Name(): dstPIF}, nil) plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ - Logger: logger, - Goal: dstPIF, - Frame: f, - StartConfiguration: inputMap, - FrameSystem: fs, - WorldState: worldState, - StartPose: spatialmath.NewZeroPose(), + Logger: logger, + Goals: []*motionplan.PlanState{goalState}, + StartState: startState, + FrameSystem: fs, + WorldState: worldState, }) test.That(t, err, test.ShouldBeNil) test.That(t, plan, test.ShouldNotBeNil) diff --git a/motionplan/cBiRRT.go b/motionplan/cBiRRT.go index b9d04360bba..5e5672d1fc7 100644 --- a/motionplan/cBiRRT.go +++ b/motionplan/cBiRRT.go @@ -93,11 +93,17 @@ func newCBiRRTMotionPlanner( }, nil } -func (mp *cBiRRTMotionPlanner) plan(ctx context.Context, goal PathStep, seed map[string][]referenceframe.Input) ([]node, error) { - mp.planOpts.setGoal(goal) +func (mp *cBiRRTMotionPlanner) plan(ctx context.Context, seed, goal *PlanState) ([]node, error) { solutionChan := make(chan *rrtSolution, 1) + initMaps := initRRTSolutions(ctx, atomicWaypoint{mp: mp, startState: seed, goalState: goal}) + if initMaps.err != nil { + return nil, initMaps.err + } + if initMaps.steps != nil { + return initMaps.steps, nil + } utils.PanicCapturingGo(func() { - mp.rrtBackgroundRunner(ctx, seed, &rrtParallelPlannerShared{nil, nil, solutionChan}) + mp.rrtBackgroundRunner(ctx, &rrtParallelPlannerShared{initMaps.maps, nil, solutionChan}) }) solution := <-solutionChan if solution.err != nil { @@ -110,10 +116,10 @@ func (mp *cBiRRTMotionPlanner) plan(ctx context.Context, goal PathStep, seed map // Separating this allows other things to call rrtBackgroundRunner in parallel allowing the thread-agnostic Plan to be accessible. func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( ctx context.Context, - seed map[string][]referenceframe.Input, rrt *rrtParallelPlannerShared, ) { defer close(rrt.solutionChan) + mp.logger.CDebugf(ctx, "starting cbirrt with start map len %d and goal map len %d\n", len(rrt.maps.startMap), len(rrt.maps.goalMap)) // setup planner options if mp.planOpts == nil { @@ -128,13 +134,13 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( defer cancel() mp.start = time.Now() - if rrt.maps == nil || len(rrt.maps.goalMap) == 0 { - planSeed := initRRTSolutions(ctx, mp, seed) - if planSeed.err != nil || planSeed.steps != nil { - rrt.solutionChan <- planSeed - return + var seed map[string][]referenceframe.Input + // Pick a random (first in map) seed node to create the first interp node + for sNode, parent := range rrt.maps.startMap { + if parent == nil { + seed = sNode.Q() + break } - rrt.maps = planSeed.maps } mp.logger.CInfof(ctx, "goal node: %v\n", rrt.maps.optNode.Q()) interpConfig, err := referenceframe.InterpolateFS(mp.fs, seed, rrt.maps.optNode.Q(), 0.5) diff --git a/motionplan/cBiRRT_test.go b/motionplan/cBiRRT_test.go index 624270fc781..a294b0fa0cb 100644 --- a/motionplan/cBiRRT_test.go +++ b/motionplan/cBiRRT_test.go @@ -39,14 +39,13 @@ func TestSimpleLinearMotion(t *testing.T) { goalPos := spatialmath.NewPose(r3.Vector{X: 206, Y: 100, Z: 120.5}, &spatialmath.OrientationVectorDegrees{OY: -1}) opt := newBasicPlannerOptions() - opt.setGoal(PathStep{m.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos)}) + goalMetric := opt.getGoalMetric(PathState{m.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos)}) fs := referenceframe.NewEmptyFrameSystem("") fs.AddFrame(m, fs.World()) mp, err := newCBiRRTMotionPlanner(fs, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) cbirrt, _ := mp.(*cBiRRTMotionPlanner) - - solutions, err := mp.getSolutions(ctx, map[string][]referenceframe.Input{m.Name(): home7}) + solutions, err := mp.getSolutions(ctx, map[string][]referenceframe.Input{m.Name(): home7}, goalMetric) test.That(t, err, test.ShouldBeNil) near1 := &basicNode{q: map[string][]referenceframe.Input{m.Name(): home7}} diff --git a/motionplan/check.go b/motionplan/check.go index e08277ac546..7e5845f7188 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -45,9 +45,24 @@ func CheckPlan( if err != nil { return err } + + // Spot check plan for options + planOpts, err := sfPlanner.plannerSetupFromMoveRequest( + &PlanState{poses: plan.Path()[0]}, + &PlanState{poses: plan.Path()[len(plan.Path())-1]}, + plan.Trajectory()[0], + worldState, + nil, + nil, // no pb.Constraints + nil, // no plannOpts + ) + if err != nil { + return err + } + // This should be done for any plan whose configurations are specified in relative terms rather than absolute ones. // Currently this is only TP-space, so we check if the PTG length is >0. - if sfPlanner.useTPspace { + if planOpts.useTPspace { return checkPlanRelative(checkFrame, executionState, worldState, fs, lookAheadDistanceMM, sfPlanner) } return checkPlanAbsolute(checkFrame, executionState, worldState, fs, lookAheadDistanceMM, sfPlanner) @@ -80,8 +95,8 @@ func checkPlanRelative( // setup the planOpts. Poses should be in world frame. This allows us to know e.g. which obstacles may ephemerally collide. if sfPlanner.planOpts, err = sfPlanner.plannerSetupFromMoveRequest( - plan.Path()[0], - plan.Path()[len(plan.Path())-1], + &PlanState{poses: plan.Path()[0], configuration: plan.Trajectory()[0]}, + &PlanState{poses: plan.Path()[len(plan.Path())-1]}, plan.Trajectory()[0], worldState, nil, @@ -217,13 +232,13 @@ func checkPlanAbsolute( // setup the planOpts if sfPlanner.planOpts, err = sfPlanner.plannerSetupFromMoveRequest( - executionState.CurrentPoses(), - poses[len(poses)-1], + &PlanState{poses: executionState.CurrentPoses(), configuration: startingInputs}, + &PlanState{poses: poses[len(poses)-1]}, startingInputs, worldState, nil, - nil, // no pb.Constraints - nil, // no plannOpts + nil, // no Constraints + nil, // no planOpts ); err != nil { return err } diff --git a/motionplan/constraint.go b/motionplan/constraint.go index 0ef91a3eae1..d875b178c03 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -709,7 +709,7 @@ func (c *Constraints) GetCollisionSpecification() []CollisionSpecification { type fsPathConstraint struct { metricMap map[string]ik.StateMetric constraintMap map[string]StateConstraint - goalMap PathStep + goalMap PathState fs referenceframe.FrameSystem } @@ -755,7 +755,7 @@ func (fpc *fsPathConstraint) metric(state *ik.StateFS) float64 { func newFsPathConstraintSeparatedLinOrientTol( fs referenceframe.FrameSystem, startCfg map[string][]referenceframe.Input, - from, to PathStep, + from, to PathState, constructor func(spatial.Pose, spatial.Pose, float64, float64) (StateConstraint, ik.StateMetric), linTol, orientTol float64, ) (*fsPathConstraint, error) { @@ -787,7 +787,7 @@ func newFsPathConstraintSeparatedLinOrientTol( func newFsPathConstraintTol( fs referenceframe.FrameSystem, startCfg map[string][]referenceframe.Input, - from, to PathStep, + from, to PathState, constructor func(spatial.Pose, spatial.Pose, float64) (StateConstraint, ik.StateMetric), tolerance float64, ) (*fsPathConstraint, error) { @@ -822,7 +822,7 @@ func newFsPathConstraintTol( func CreateSlerpOrientationConstraintFS( fs referenceframe.FrameSystem, startCfg map[string][]referenceframe.Input, - from, to PathStep, + from, to PathState, tolerance float64, ) (StateFSConstraint, ik.StateFSMetric, error) { constraintInternal, err := newFsPathConstraintTol(fs, startCfg, from, to, NewSlerpOrientationConstraint, tolerance) @@ -838,7 +838,7 @@ func CreateSlerpOrientationConstraintFS( func CreateLineConstraintFS( fs referenceframe.FrameSystem, startCfg map[string][]referenceframe.Input, - from, to PathStep, + from, to PathState, tolerance float64, ) (StateFSConstraint, ik.StateFSMetric, error) { // Need to define a constructor here since NewLineConstraint takes r3.Vectors, not poses @@ -858,7 +858,7 @@ func CreateLineConstraintFS( func CreateAbsoluteLinearInterpolatingConstraintFS( fs referenceframe.FrameSystem, startCfg map[string][]referenceframe.Input, - from, to PathStep, + from, to PathState, linTol, orientTol float64, ) (StateFSConstraint, ik.StateFSMetric, error) { constraintInternal, err := newFsPathConstraintSeparatedLinOrientTol( @@ -882,7 +882,7 @@ func CreateAbsoluteLinearInterpolatingConstraintFS( func CreateProportionalLinearInterpolatingConstraintFS( fs referenceframe.FrameSystem, startCfg map[string][]referenceframe.Input, - from, to PathStep, + from, to PathState, linTol, orientTol float64, ) (StateFSConstraint, ik.StateFSMetric, error) { constraintInternal, err := newFsPathConstraintSeparatedLinOrientTol( diff --git a/motionplan/constraint_handler.go b/motionplan/constraint_handler.go index 814645599e7..b74baf4f8e7 100644 --- a/motionplan/constraint_handler.go +++ b/motionplan/constraint_handler.go @@ -113,7 +113,7 @@ func interpolateSegment(ci *ik.Segment, resolution float64) ([][]referenceframe. return nil, err } - steps := PathStepCount(ci.StartPosition, ci.EndPosition, resolution) + steps := PathStateCount(ci.StartPosition, ci.EndPosition, resolution) if steps < defaultMinStepCount { // Minimum step count ensures we are not missing anything steps = defaultMinStepCount @@ -163,7 +163,7 @@ func interpolateSegmentFS(ci *ik.SegmentFS, resolution float64) ([]map[string][] } // Calculate steps needed for this frame - steps := PathStepCount(startPos, endPos, resolution) + steps := PathStateCount(startPos, endPos, resolution) if steps > maxSteps { maxSteps = steps } diff --git a/motionplan/constraint_test.go b/motionplan/constraint_test.go index 9b6583633c2..e8aa664a8d7 100644 --- a/motionplan/constraint_test.go +++ b/motionplan/constraint_test.go @@ -29,15 +29,16 @@ func TestIKTolerances(t *testing.T) { test.That(t, err, test.ShouldBeNil) // Test inability to arrive at another position due to orientation - pos := PathStep{m.Name(): frame.NewPoseInFrame(frame.World, spatial.NewPoseFromProtobuf(&commonpb.Pose{ + goal := &PlanState{poses: PathState{m.Name(): frame.NewPoseInFrame(frame.World, spatial.NewPoseFromProtobuf(&commonpb.Pose{ X: -46, Y: 0, Z: 372, OX: -1.78, OY: -3.3, OZ: -1.11, - }))} - _, err = mp.plan(context.Background(), pos, map[string][]frame.Input{m.Name(): frame.FloatsToInputs(make([]float64, 6))}) + }))}} + seed := &PlanState{configuration: map[string][]frame.Input{m.Name(): frame.FloatsToInputs(make([]float64, 6))}} + _, err = mp.plan(context.Background(), seed, goal) test.That(t, err, test.ShouldNotBeNil) // Now verify that setting tolerances to zero allows the same arm to reach that position @@ -46,7 +47,7 @@ func TestIKTolerances(t *testing.T) { opt.SetMaxSolutions(50) mp, err = newCBiRRTMotionPlanner(fs, rand.New(rand.NewSource(1)), logger, opt) test.That(t, err, test.ShouldBeNil) - _, err = mp.plan(context.Background(), pos, map[string][]frame.Input{m.Name(): frame.FloatsToInputs(make([]float64, 6))}) + _, err = mp.plan(context.Background(), seed, goal) test.That(t, err, test.ShouldBeNil) } @@ -150,8 +151,8 @@ func TestLineFollow(t *testing.T) { opt := newBasicPlannerOptions() startCfg := map[string][]frame.Input{m.Name(): m.InputFromProtobuf(mp1)} - from := PathStep{markerFrame.Name(): frame.NewPoseInFrame(markerFrame.Name(), p1)} - to := PathStep{markerFrame.Name(): frame.NewPoseInFrame(goalFrame.Name(), p2)} + from := PathState{markerFrame.Name(): frame.NewPoseInFrame(markerFrame.Name(), p1)} + to := PathState{markerFrame.Name(): frame.NewPoseInFrame(goalFrame.Name(), p2)} validFunc, gradFunc, err := CreateLineConstraintFS(fs, startCfg, from, to, 0.001) test.That(t, err, test.ShouldBeNil) diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 50952897dd2..ce8efd27b16 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -26,13 +26,13 @@ import ( type motionPlanner interface { // Plan will take a context, a goal position, and an input start state and return a series of state waypoints which // should be visited in order to arrive at the goal while satisfying all constraints - plan(context.Context, PathStep, map[string][]referenceframe.Input) ([]node, error) + plan(ctx context.Context, seed, goal *PlanState) ([]node, error) // Everything below this point should be covered by anything that wraps the generic `planner` smoothPath(context.Context, []node) []node checkPath(map[string][]referenceframe.Input, map[string][]referenceframe.Input) bool checkInputs(map[string][]referenceframe.Input) bool - getSolutions(context.Context, map[string][]referenceframe.Input) ([]node, error) + getSolutions(context.Context, map[string][]referenceframe.Input, ik.StateFSMetric) ([]node, error) opt() *plannerOptions sample(node, int) (node, error) } @@ -41,17 +41,24 @@ type plannerConstructor func(referenceframe.FrameSystem, *rand.Rand, logging.Log // PlanRequest is a struct to store all the data necessary to make a call to PlanMotion. type PlanRequest struct { - Logger logging.Logger - Goal *referenceframe.PoseInFrame - Frame referenceframe.Frame - FrameSystem referenceframe.FrameSystem - StartPose spatialmath.Pose - StartConfiguration map[string][]referenceframe.Input - WorldState *referenceframe.WorldState - BoundingRegions []spatialmath.Geometry - Constraints *Constraints - Options map[string]interface{} - allGoals PathStep // TODO: this should replace Goal and Frame + Logger logging.Logger + // The planner will hit each Goal in order. Each goal may be a configuration or PathState for holonomic motion, or must be a + // PathState for non-holonomic motion. For holonomic motion, if both a configuration and PathState are given, an error is thrown. + // TODO: Perhaps we could do something where some components are enforced to arrive at a certain configuration, but others can have IK + // run to solve for poses. Doing this while enforcing configurations may be tricky. + Goals []*PlanState + FrameSystem referenceframe.FrameSystem + + // This must always have a configuration filled in, for geometry placement purposes. + // If poses are also filled in, the configuration will be used to determine geometry collisions, but the poses will be used + // in IK to generate plan start configurations. The given configuration will NOT automatically be added to the seed tree. + // The use case here is that if a particularly difficult path must be planned between two poses, that can be done first to ensure + // feasibility, and then other plans can be requested to connect to that returned plan's configurations. + StartState *PlanState + WorldState *referenceframe.WorldState + BoundingRegions []spatialmath.Geometry + Constraints *Constraints + Options map[string]interface{} } // validatePlanRequest ensures PlanRequests are not malformed. @@ -62,68 +69,92 @@ func (req *PlanRequest) validatePlanRequest() error { if req.Logger == nil { return errors.New("PlanRequest cannot have nil logger") } - if req.Frame == nil { - return errors.New("PlanRequest cannot have nil frame") - } if req.FrameSystem == nil { return errors.New("PlanRequest cannot have nil framesystem") - } else if req.FrameSystem.Frame(req.Frame.Name()) == nil { - return referenceframe.NewFrameMissingError(req.Frame.Name()) } - - if req.Goal == nil { - return errors.New("PlanRequest cannot have nil goal") + if req.StartState == nil { + return errors.New("PlanRequest cannot have nil StartState") } - - goalParentFrame := req.Goal.Parent() - if req.FrameSystem.Frame(goalParentFrame) == nil { - return referenceframe.NewParentFrameMissingError(req.Goal.Name(), goalParentFrame) + if req.StartState.configuration == nil { + return errors.New("PlanRequest cannot have nil StartState configuration") } - - if len(req.BoundingRegions) > 0 { - buffer, ok := req.Options["collision_buffer_mm"].(float64) - if !ok { - buffer = defaultCollisionBufferMM - } - // check that the request frame's geometries are within or in collision with the bounding regions - robotGifs, err := req.Frame.Geometries(make([]referenceframe.Input, len(req.Frame.DoF()))) + // If we have a start configuration, check for correctness. Reuse PathState compute function to provide error. + if len(req.StartState.configuration) > 0 { + _, err := ComputePathStateFromConfiguration(req.FrameSystem, req.StartState.configuration) if err != nil { return err } - var robotGeoms []spatialmath.Geometry - for _, geom := range robotGifs.Geometries() { - robotGeoms = append(robotGeoms, geom.Transform(req.StartPose)) + } + // if we have start poses, check we have valid frames + for fName, pif := range req.StartState.poses { + if req.FrameSystem.Frame(fName) == nil { + return referenceframe.NewFrameMissingError(fName) } - robotGeomBoundingRegionCheck := NewBoundingRegionConstraint(robotGeoms, req.BoundingRegions, buffer) - if !robotGeomBoundingRegionCheck(&ik.State{}) { - return fmt.Errorf("frame named %s is not within the provided bounding regions", req.Frame.Name()) - } - - // check that the destination is within or in collision with the bounding regions - destinationAsGeom := []spatialmath.Geometry{spatialmath.NewPoint(req.Goal.Pose().Point(), "")} - destinationBoundingRegionCheck := NewBoundingRegionConstraint(destinationAsGeom, req.BoundingRegions, buffer) - if !destinationBoundingRegionCheck(&ik.State{}) { - return errors.New("destination was not within the provided bounding regions") + if req.FrameSystem.Frame(pif.Parent()) == nil { + return referenceframe.NewParentFrameMissingError(fName, pif.Parent()) } } - frameDOF := len(req.Frame.DoF()) - seedMap, ok := req.StartConfiguration[req.Frame.Name()] - if frameDOF > 0 { - if !ok { - return errors.Errorf("%s does not have a start configuration", req.Frame.Name()) - } - if frameDOF != len(seedMap) { - return referenceframe.NewIncorrectDoFError(len(seedMap), len(req.Frame.DoF())) - } - } else if ok && frameDOF != len(seedMap) { - return referenceframe.NewIncorrectDoFError(len(seedMap), len(req.Frame.DoF())) + if len(req.Goals) == 0 { + return errors.New("PlanRequest must have at least one goal") } - // TODO: This should replace Frame and Goal - req.allGoals = map[string]*referenceframe.PoseInFrame{req.Frame.Name(): req.Goal} + // Validate the goals. Each goal with a pose must not alos have a configuration specified. The parent frame of the pose must exist. + for i, goalState := range req.Goals { + for fName, pif := range goalState.poses { + if len(goalState.configuration) > 0 { + return errors.New("individual goals cannot have both configuration and poses populated") + } + + goalParentFrame := pif.Parent() + if req.FrameSystem.Frame(goalParentFrame) == nil { + return referenceframe.NewParentFrameMissingError(fName, goalParentFrame) + } + + if len(req.BoundingRegions) > 0 { + // Check that robot components start within bounding regions. + // Bounding regions are for 2d planning, which requires a start pose + if len(goalState.poses) > 0 && len(req.StartState.poses) > 0 { + goalFrame := req.FrameSystem.Frame(fName) + if goalFrame == nil { + return referenceframe.NewFrameMissingError(fName) + } + buffer, ok := req.Options["collision_buffer_mm"].(float64) + if !ok { + buffer = defaultCollisionBufferMM + } + // check that the request frame's geometries are within or in collision with the bounding regions + robotGifs, err := goalFrame.Geometries(make([]referenceframe.Input, len(goalFrame.DoF()))) + if err != nil { + return err + } + if i == 0 { + // Only need to check start poses once + startPose, ok := req.StartState.poses[fName] + if !ok { + return fmt.Errorf("goal frame %s does not have a start pose", fName) + } + var robotGeoms []spatialmath.Geometry + for _, geom := range robotGifs.Geometries() { + robotGeoms = append(robotGeoms, geom.Transform(startPose.Pose())) + } + robotGeomBoundingRegionCheck := NewBoundingRegionConstraint(robotGeoms, req.BoundingRegions, buffer) + if !robotGeomBoundingRegionCheck(&ik.State{}) { + return fmt.Errorf("frame named %s is not within the provided bounding regions", fName) + } + } + // check that the destination is within or in collision with the bounding regions + destinationAsGeom := []spatialmath.Geometry{spatialmath.NewPoint(pif.Pose().Point(), "")} + destinationBoundingRegionCheck := NewBoundingRegionConstraint(destinationAsGeom, req.BoundingRegions, buffer) + if !destinationBoundingRegionCheck(&ik.State{}) { + return errors.New("destination was not within the provided bounding regions") + } + } + } + } + } return nil } @@ -149,13 +180,14 @@ func PlanFrameMotion(ctx context.Context, return nil, err } plan, err := PlanMotion(ctx, &PlanRequest{ - Logger: logger, - Goal: referenceframe.NewPoseInFrame(referenceframe.World, dst), - Frame: f, - StartConfiguration: map[string][]referenceframe.Input{f.Name(): seed}, - FrameSystem: fs, - Constraints: constraints, - Options: planningOpts, + Logger: logger, + Goals: []*PlanState{ + {poses: PathState{f.Name(): referenceframe.NewPoseInFrame(referenceframe.World, dst)}}, + }, + StartState: &PlanState{configuration: map[string][]referenceframe.Input{f.Name(): seed}}, + FrameSystem: fs, + Constraints: constraints, + Options: planningOpts, }) if err != nil { return nil, err @@ -182,7 +214,7 @@ func Replan(ctx context.Context, request *PlanRequest, currentPlan Plan, replanC return nil, err } - newPlan, err := sfPlanner.PlanSingleWaypoint(ctx, request, currentPlan) + newPlan, err := sfPlanner.planMultiWaypoint(ctx, request, currentPlan) if err != nil { return nil, err } @@ -343,14 +375,21 @@ func (mp *planner) smoothPath(ctx context.Context, path []node) []node { // getSolutions will initiate an IK solver for the given position and seed, collect solutions, and score them by constraints. // If maxSolutions is positive, once that many solutions have been collected, the solver will terminate and return that many solutions. // If minScore is positive, if a solution scoring below that amount is found, the solver will terminate and return that one solution. -func (mp *planner) getSolutions(ctx context.Context, seed map[string][]referenceframe.Input) ([]node, error) { +func (mp *planner) getSolutions(ctx context.Context, seed map[string][]referenceframe.Input, metric ik.StateFSMetric) ([]node, error) { // Linter doesn't properly handle loop labels nSolutions := mp.planOpts.MaxSolutions if nSolutions == 0 { nSolutions = defaultSolutionsToSeed } + if len(seed) == 0 { + seed = map[string][]referenceframe.Input{} + // If no seed is passed, generate one randomly + for _, frameName := range mp.fs.FrameNames() { + seed[frameName] = referenceframe.RandomFrameInputs(mp.fs.Frame(frameName), mp.randseed) + } + } - if mp.planOpts.goalMetric == nil { + if metric == nil { return nil, errors.New("metric is nil") } @@ -368,7 +407,7 @@ func (mp *planner) getSolutions(ctx context.Context, seed map[string][]reference return nil, err } - minFunc := mp.linearizeFSmetric(mp.planOpts.goalMetric) + minFunc := mp.linearizeFSmetric(metric) // Spawn the IK solver to generate solutions until done utils.PanicCapturingGo(func() { defer close(ikErr) diff --git a/motionplan/motionPlanner_test.go b/motionplan/motionPlanner_test.go index 348cee89388..f57d306f87f 100644 --- a/motionplan/motionPlanner_test.go +++ b/motionplan/motionPlanner_test.go @@ -32,8 +32,8 @@ var logger = logging.FromZapCompatible(zap.Must(zap.Config{ }.Build()).Sugar()) type planConfig struct { - Start map[string][]frame.Input - Goal PathStep + Start *PlanState + Goal *PlanState FS frame.FrameSystem Options *plannerOptions } @@ -139,8 +139,8 @@ func constrainedXArmMotion() (*planConfig, error) { opt.SetPathMetric(oFuncMet) opt.AddStateConstraint("orientation", orientConstraint) - start := map[string][]frame.Input{model.Name(): home7} - goal := PathStep{model.Name(): frame.NewPoseInFrame(frame.World, pos)} + start := &PlanState{configuration: map[string][]frame.Input{model.Name(): home7}} + goal := &PlanState{poses: PathState{model.Name(): frame.NewPoseInFrame(frame.World, pos)}} opt.fillMotionChains(fs, goal) return &planConfig{ @@ -166,11 +166,10 @@ func TestPlanningWithGripper(t *testing.T) { newPose := frame.NewPoseInFrame("gripper", spatialmath.NewPoseFromPoint(r3.Vector{100, 100, 0})) solutionMap, err := PlanMotion(context.Background(), &PlanRequest{ - Logger: logger, - Goal: newPose, - Frame: gripper, - StartConfiguration: zeroPos, - FrameSystem: fs, + Logger: logger, + Goals: []*PlanState{{poses: PathState{"gripper": newPose}}}, + StartState: &PlanState{configuration: zeroPos}, + FrameSystem: fs, }) test.That(t, err, test.ShouldBeNil) test.That(t, len(solutionMap.Trajectory()), test.ShouldBeGreaterThanOrEqualTo, 2) @@ -229,8 +228,7 @@ func simple2DMap() (*planConfig, error) { startInput := frame.StartPositions(fs) startInput[modelName] = frame.FloatsToInputs([]float64{-90., 90., 0}) goalPose := spatialmath.NewPoseFromPoint(r3.Vector{X: 90, Y: 90, Z: 0}) - goal := PathStep{modelName: frame.NewPoseInFrame(frame.World, goalPose)} - opt.setGoal(goal) + goal := &PlanState{poses: PathState{modelName: frame.NewPoseInFrame(frame.World, goalPose)}} seedMap := frame.StartPositions(fs) // create robot collision entities @@ -276,7 +274,7 @@ func simple2DMap() (*planConfig, error) { opt.fillMotionChains(fs, goal) return &planConfig{ - Start: startInput, + Start: &PlanState{configuration: startInput}, Goal: goal, FS: fs, Options: opt, @@ -296,14 +294,13 @@ func simpleXArmMotion() (*planConfig, error) { return nil, err } - goal := PathStep{ + goal := &PlanState{poses: PathState{ xarm.Name(): frame.NewPoseInFrame(frame.World, spatialmath.NewPoseFromProtobuf(&commonpb.Pose{X: 206, Y: 100, Z: 120, OZ: -1})), - } + }} // setup planner options opt := newBasicPlannerOptions() opt.SmoothIter = 20 - opt.setGoal(goal) // create robot collision entities movingGeometriesInFrame, err := xarm.Geometries(home7) @@ -345,7 +342,7 @@ func simpleXArmMotion() (*planConfig, error) { opt.fillMotionChains(fs, goal) return &planConfig{ - Start: start, + Start: &PlanState{configuration: start}, Goal: goal, FS: fs, Options: opt, @@ -364,14 +361,13 @@ func simpleUR5eMotion() (*planConfig, error) { return nil, err } - goal := PathStep{ + goal := &PlanState{poses: PathState{ ur5e.Name(): frame.NewPoseInFrame(frame.World, spatialmath.NewPoseFromProtobuf(&commonpb.Pose{X: -750, Y: -250, Z: 200, OX: -1})), - } + }} // setup planner options opt := newBasicPlannerOptions() opt.SmoothIter = 20 - opt.setGoal(goal) // create robot collision entities movingGeometriesInFrame, err := ur5e.Geometries(home6) @@ -413,7 +409,7 @@ func simpleUR5eMotion() (*planConfig, error) { opt.fillMotionChains(fs, goal) return &planConfig{ - Start: start, + Start: &PlanState{configuration: start}, Goal: goal, FS: fs, Options: opt, @@ -430,7 +426,8 @@ func testPlanner(t *testing.T, plannerFunc plannerConstructor, config planConfig test.That(t, err, test.ShouldBeNil) mp, err := plannerFunc(cfg.FS, rand.New(rand.NewSource(int64(seed))), logger, cfg.Options) test.That(t, err, test.ShouldBeNil) - nodes, err := mp.plan(context.Background(), cfg.Goal, cfg.Start) + + nodes, err := mp.plan(context.Background(), cfg.Start, cfg.Goal) test.That(t, err, test.ShouldBeNil) // test that path doesn't violate constraints @@ -493,11 +490,10 @@ func TestArmOOBSolve(t *testing.T) { // Set a goal unreachable by the UR due to sheer distance goal1 := spatialmath.NewPose(r3.Vector{X: 257, Y: 21000, Z: -300}, &spatialmath.OrientationVectorDegrees{OZ: -1}) _, err := PlanMotion(context.Background(), &PlanRequest{ - Logger: logger, - Goal: frame.NewPoseInFrame(frame.World, goal1), - Frame: fs.Frame("urCamera"), - StartConfiguration: positions, - FrameSystem: fs, + Logger: logger, + Goals: []*PlanState{{poses: PathState{"urCamera": frame.NewPoseInFrame(frame.World, goal1)}}}, + StartState: &PlanState{configuration: positions}, + FrameSystem: fs, }) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldEqual, errIKSolve.Error()) @@ -519,12 +515,11 @@ func TestArmObstacleSolve(t *testing.T) { // Set a goal unreachable by the UR goal1 := spatialmath.NewPose(r3.Vector{X: 257, Y: 210, Z: -300}, &spatialmath.OrientationVectorDegrees{OZ: -1}) _, err = PlanMotion(context.Background(), &PlanRequest{ - Logger: logger, - Goal: frame.NewPoseInFrame(frame.World, goal1), - Frame: fs.Frame("urCamera"), - StartConfiguration: positions, - FrameSystem: fs, - WorldState: worldState, + Logger: logger, + Goals: []*PlanState{{poses: PathState{"urCamera": frame.NewPoseInFrame(frame.World, goal1)}}}, + StartState: &PlanState{configuration: positions}, + FrameSystem: fs, + WorldState: worldState, }) test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldContainSubstring, errIKConstraint) @@ -546,12 +541,11 @@ func TestArmAndGantrySolve(t *testing.T) { // Set a goal such that the gantry and arm must both be used to solve goal1 := spatialmath.NewPose(r3.Vector{X: 257, Y: 2100, Z: -300}, &spatialmath.OrientationVectorDegrees{OZ: -1}) plan, err := PlanMotion(context.Background(), &PlanRequest{ - Logger: logger, - Goal: frame.NewPoseInFrame(frame.World, goal1), - Frame: fs.Frame("xArmVgripper"), - StartConfiguration: positions, - FrameSystem: fs, - Options: map[string]interface{}{"smooth_iter": 5}, + Logger: logger, + Goals: []*PlanState{{poses: PathState{"xArmVgripper": frame.NewPoseInFrame(frame.World, goal1)}}}, + StartState: &PlanState{configuration: positions}, + FrameSystem: fs, + Options: map[string]interface{}{"smooth_iter": 5}, }) test.That(t, err, test.ShouldBeNil) solvedPose, err := fs.Transform( @@ -569,12 +563,11 @@ func TestMultiArmSolve(t *testing.T) { // Solve such that the ur5 and xArm are pointing at each other, 40mm from gripper to camera goal2 := spatialmath.NewPose(r3.Vector{Z: 60}, &spatialmath.OrientationVectorDegrees{OZ: -1}) plan, err := PlanMotion(context.Background(), &PlanRequest{ - Logger: logger, - Goal: frame.NewPoseInFrame("urCamera", goal2), - Frame: fs.Frame("xArmVgripper"), - StartConfiguration: positions, - FrameSystem: fs, - Options: map[string]interface{}{"max_ik_solutions": 10, "timeout": 150.0, "smooth_iter": 5}, + Logger: logger, + Goals: []*PlanState{{poses: PathState{"xArmVgripper": frame.NewPoseInFrame("urCamera", goal2)}}}, + StartState: &PlanState{configuration: positions}, + FrameSystem: fs, + Options: map[string]interface{}{"max_ik_solutions": 10, "timeout": 150.0, "smooth_iter": 5}, }) test.That(t, err, test.ShouldBeNil) @@ -612,12 +605,11 @@ func TestReachOverArm(t *testing.T) { // plan to a location, it should interpolate to get there opts := map[string]interface{}{"timeout": 150.0} plan, err := PlanMotion(context.Background(), &PlanRequest{ - Logger: logger, - Goal: goal, - Frame: xarm, - StartConfiguration: frame.StartPositions(fs), - FrameSystem: fs, - Options: opts, + Logger: logger, + Goals: []*PlanState{{poses: PathState{xarm.Name(): goal}}}, + StartState: &PlanState{configuration: frame.StartPositions(fs)}, + FrameSystem: fs, + Options: opts, }) test.That(t, err, test.ShouldBeNil) @@ -631,12 +623,11 @@ func TestReachOverArm(t *testing.T) { // the plan should no longer be able to interpolate, but it should still be able to get there opts = map[string]interface{}{"timeout": 150.0, "smooth_iter": 5} plan, err = PlanMotion(context.Background(), &PlanRequest{ - Logger: logger, - Goal: goal, - Frame: xarm, - StartConfiguration: frame.StartPositions(fs), - FrameSystem: fs, - Options: opts, + Logger: logger, + Goals: []*PlanState{{poses: PathState{xarm.Name(): goal}}}, + StartState: &PlanState{configuration: frame.StartPositions(fs)}, + FrameSystem: fs, + Options: opts, }) test.That(t, err, test.ShouldBeNil) test.That(t, len(plan.Trajectory()), test.ShouldBeGreaterThan, 2) @@ -680,12 +671,11 @@ func TestPlanMapMotion(t *testing.T) { destination := frame.NewPoseInFrame(frame.World, dst) seedMap := map[string][]frame.Input{f.Name(): seed} plan, err := PlanMotion(ctx, &PlanRequest{ - Logger: logger, - Goal: destination, - Frame: f, - StartConfiguration: seedMap, - FrameSystem: fs, - WorldState: worldState, + Logger: logger, + Goals: []*PlanState{{poses: PathState{f.Name(): destination}}}, + StartState: &PlanState{configuration: seedMap}, + FrameSystem: fs, + WorldState: worldState, }) if err != nil { return nil, err @@ -723,13 +713,12 @@ func TestArmConstraintSpecificationSolve(t *testing.T) { checkReachable := func(worldState *frame.WorldState, constraints *Constraints) error { goal := spatialmath.NewPose(r3.Vector{X: 600, Y: 100, Z: 300}, &spatialmath.OrientationVectorDegrees{OX: 1}) _, err := PlanMotion(context.Background(), &PlanRequest{ - Logger: logger, - Goal: frame.NewPoseInFrame(frame.World, goal), - Frame: fs.Frame("xArmVgripper"), - FrameSystem: fs, - StartConfiguration: frame.StartPositions(fs), - WorldState: worldState, - Constraints: constraints, + Logger: logger, + Goals: []*PlanState{{poses: PathState{"xArmVgripper": frame.NewPoseInFrame(frame.World, goal)}}}, + StartState: &PlanState{configuration: frame.StartPositions(fs)}, + FrameSystem: fs, + WorldState: worldState, + Constraints: constraints, }) return err } @@ -806,12 +795,11 @@ func TestMovementWithGripper(t *testing.T) { "motion_profile": LinearMotionProfile, } request := &PlanRequest{ - Logger: logger, - StartConfiguration: startConfig, - Goal: frame.NewPoseInFrame(frame.World, goal), - Frame: fs.Frame("xArmVgripper"), - FrameSystem: fs, - Options: motionConfig, + Logger: logger, + Goals: []*PlanState{{poses: PathState{"xArmVgripper": frame.NewPoseInFrame(frame.World, goal)}}}, + StartState: &PlanState{configuration: startConfig}, + FrameSystem: fs, + Options: motionConfig, } solution, err := PlanMotion(context.Background(), request) test.That(t, err, test.ShouldBeNil) @@ -831,9 +819,8 @@ func TestMovementWithGripper(t *testing.T) { test.That(t, solution, test.ShouldNotBeNil) // plan with end of arm with gripper attached - this will fail - request.Frame = fs.Frame("xArm6") goal = spatialmath.NewPose(r3.Vector{500, 0, -100}, &spatialmath.OrientationVector{OZ: -1}) - request.Goal = frame.NewPoseInFrame(frame.World, goal) + request.Goals = []*PlanState{{poses: PathState{"xArm6": frame.NewPoseInFrame(frame.World, goal)}}} _, err = PlanMotion(context.Background(), request) test.That(t, err, test.ShouldNotBeNil) @@ -845,7 +832,6 @@ func TestMovementWithGripper(t *testing.T) { // remove gripper and try with linear constraint fs.RemoveFrame(fs.Frame("xArmVgripper")) - request.Frame = fs.Frame("xArm6") request.Options = motionConfig solution, err = PlanMotion(context.Background(), request) test.That(t, err, test.ShouldBeNil) @@ -910,13 +896,14 @@ func TestReplanValidations(t *testing.T) { for _, tc := range testCases { t.Run(tc.msg, func(t *testing.T) { _, err := Replan(ctx, &PlanRequest{ - Logger: logger, - Goal: frame.NewPoseInFrame(frame.World, goal), - StartPose: spatialmath.NewZeroPose(), - Frame: kinematicFrame, - FrameSystem: baseFS, - StartConfiguration: frame.StartPositions(baseFS), - Options: tc.extra, + Logger: logger, + Goals: []*PlanState{{poses: PathState{kinematicFrame.Name(): frame.NewPoseInFrame(frame.World, goal)}}}, + StartState: &PlanState{ + configuration: frame.StartPositions(baseFS), + poses: PathState{kinematicFrame.Name(): frame.NewZeroPoseInFrame(frame.World)}, + }, + FrameSystem: baseFS, + Options: tc.extra, }, nil, 0) if tc.err != nil { test.That(t, err, test.ShouldBeError, tc.err) @@ -954,13 +941,10 @@ func TestReplan(t *testing.T) { test.That(t, err, test.ShouldBeNil) planRequest := &PlanRequest{ - Logger: logger, - Goal: frame.NewPoseInFrame(frame.World, goal), - Frame: kinematicFrame, - FrameSystem: baseFS, - StartConfiguration: frame.StartPositions(baseFS), - WorldState: nil, - Options: nil, + Logger: logger, + Goals: []*PlanState{{poses: PathState{kinematicFrame.Name(): frame.NewPoseInFrame(frame.World, goal)}}}, + StartState: &PlanState{configuration: frame.StartPositions(baseFS)}, + FrameSystem: baseFS, } firstplan, err := PlanMotion(ctx, planRequest) @@ -968,7 +952,7 @@ func TestReplan(t *testing.T) { // Let's pretend we've moved towards the goal, so the goal is now closer goal = spatialmath.NewPoseFromPoint(r3.Vector{1000, 5000, 0}) - planRequest.Goal = frame.NewPoseInFrame(frame.World, goal) + planRequest.Goals = []*PlanState{{poses: PathState{kinematicFrame.Name(): frame.NewPoseInFrame(frame.World, goal)}}} // This should easily pass newPlan1, err := Replan(ctx, planRequest, firstplan, 1.0) @@ -1008,14 +992,15 @@ func TestPtgPosOnlyBidirectional(t *testing.T) { test.That(t, err, test.ShouldBeNil) planRequest := &PlanRequest{ - Logger: logger, - StartPose: spatialmath.NewZeroPose(), - Goal: frame.NewPoseInFrame(frame.World, goal), - Frame: kinematicFrame, - FrameSystem: baseFS, - StartConfiguration: frame.StartPositions(baseFS), - WorldState: nil, - Options: extra, + Logger: logger, + Goals: []*PlanState{{poses: PathState{kinematicFrame.Name(): frame.NewPoseInFrame(frame.World, goal)}}}, + FrameSystem: baseFS, + StartState: &PlanState{ + configuration: frame.StartPositions(baseFS), + poses: PathState{kinematicFrame.Name(): frame.NewZeroPoseInFrame(frame.World)}, + }, + WorldState: nil, + Options: extra, } bidirectionalPlanRaw, err := PlanMotion(ctx, planRequest) @@ -1049,8 +1034,8 @@ func TestValidatePlanRequest(t *testing.T) { err = fs.AddFrame(frame2, fs.World()) test.That(t, err, test.ShouldBeNil) - validGoal := frame.NewPoseInFrame("frame1", spatialmath.NewZeroPose()) - badGoal := frame.NewPoseInFrame("non-existent", spatialmath.NewZeroPose()) + validGoal := []*PlanState{{poses: PathState{"frame1": frame.NewPoseInFrame("frame1", spatialmath.NewZeroPose())}}} + badGoal := []*PlanState{{poses: PathState{"frame1": frame.NewPoseInFrame("non-existent", spatialmath.NewZeroPose())}}} testCases := []testCase{ { @@ -1058,82 +1043,64 @@ func TestValidatePlanRequest(t *testing.T) { request: PlanRequest{}, expectedErr: errors.New("PlanRequest cannot have nil logger"), }, - { - name: "nil frame - fail", - request: PlanRequest{ - Logger: logger, - }, - expectedErr: errors.New("PlanRequest cannot have nil frame"), - }, { name: "nil framesystem - fail", request: PlanRequest{ Logger: logger, - Frame: frame1, }, expectedErr: errors.New("PlanRequest cannot have nil framesystem"), }, { - name: "framesystem does not contain frame - fail", + name: "absent start state - fail", request: PlanRequest{ Logger: logger, - Frame: frame1, - FrameSystem: frame.NewEmptyFrameSystem("test"), + FrameSystem: fs, + Goals: validGoal, }, - expectedErr: errors.Errorf("frame with name %q not in frame system", frame1.Name()), + expectedErr: errors.New("PlanRequest cannot have nil StartState"), }, { name: "nil goal - fail", request: PlanRequest{ Logger: logger, - Frame: frame1, FrameSystem: fs, + StartState: &PlanState{configuration: map[string][]frame.Input{ + "frame1": {}, "frame2": {{0}}, + }}, }, - expectedErr: errors.New("PlanRequest cannot have nil goal"), + expectedErr: errors.New("PlanRequest must have at least one goal"), }, { name: "goal's parent not in frame system - fail", request: PlanRequest{ Logger: logger, - Frame: frame1, FrameSystem: fs, - Goal: badGoal, + Goals: badGoal, + StartState: &PlanState{configuration: map[string][]frame.Input{ + "frame1": {}, "frame2": {{0}}, + }}, }, - expectedErr: errors.New("part with name references non-existent parent non-existent"), + expectedErr: errors.New("part with name frame1 references non-existent parent non-existent"), }, { - name: "incorrect length StartConfiguration - fail", + name: "absent StartState Configuration - fail", request: PlanRequest{ Logger: logger, - Frame: frame1, FrameSystem: fs, - Goal: validGoal, - StartConfiguration: map[string][]frame.Input{ - "frame1": frame.FloatsToInputs([]float64{0}), - }, + Goals: validGoal, + StartState: &PlanState{}, }, - expectedErr: frame.NewIncorrectDoFError(1, 0), + expectedErr: errors.New("PlanRequest cannot have nil StartState configuration"), }, { name: "incorrect length StartConfiguration - fail", request: PlanRequest{ Logger: logger, - Frame: frame2, FrameSystem: fs, - Goal: validGoal, - }, - expectedErr: errors.New("frame2 does not have a start configuration"), - }, - { - name: "incorrect length StartConfiguration - fail", - request: PlanRequest{ - Logger: logger, - Frame: frame2, - FrameSystem: fs, - Goal: validGoal, - StartConfiguration: map[string][]frame.Input{ - "frame2": frame.FloatsToInputs([]float64{0, 0, 0, 0, 0}), - }, + Goals: validGoal, + StartState: &PlanState{configuration: map[string][]frame.Input{ + "frame1": {}, "frame2": frame.FloatsToInputs([]float64{0, 0, 0, 0, 0}), + }}, }, expectedErr: frame.NewIncorrectDoFError(5, 1), }, @@ -1141,12 +1108,11 @@ func TestValidatePlanRequest(t *testing.T) { name: "well formed PlanRequest", request: PlanRequest{ Logger: logger, - Frame: frame1, FrameSystem: fs, - Goal: validGoal, - StartConfiguration: map[string][]frame.Input{ - "frame1": {}, - }, + Goals: validGoal, + StartState: &PlanState{configuration: map[string][]frame.Input{ + "frame1": {}, "frame2": {{0}}, + }}, }, expectedErr: nil, }, @@ -1155,6 +1121,7 @@ func TestValidatePlanRequest(t *testing.T) { testFn := func(t *testing.T, tc testCase) { err := tc.request.validatePlanRequest() if tc.expectedErr != nil { + test.That(t, err, test.ShouldNotBeNil) test.That(t, err.Error(), test.ShouldEqual, tc.expectedErr.Error()) } else { test.That(t, err, test.ShouldBeNil) @@ -1198,11 +1165,10 @@ func TestArmGantryCheckPlan(t *testing.T) { f := fs.Frame("xArm6") planReq := PlanRequest{ - Logger: logger, - Goal: frame.NewPoseInFrame(frame.World, goal), - Frame: f, - FrameSystem: fs, - StartConfiguration: frame.StartPositions(fs), + Logger: logger, + Goals: []*PlanState{{poses: PathState{"xArm6": frame.NewPoseInFrame(frame.World, goal)}}}, + StartState: &PlanState{configuration: frame.StartPositions(fs)}, + FrameSystem: fs, } plan, err := PlanMotion(context.Background(), &planReq) diff --git a/motionplan/node.go b/motionplan/node.go new file mode 100644 index 00000000000..afb767297a5 --- /dev/null +++ b/motionplan/node.go @@ -0,0 +1,178 @@ +//go:build !no_cgo + +package motionplan + +import ( + "context" + "fmt" + "math" + + "go.viam.com/rdk/referenceframe" +) + +// fixedStepInterpolation returns inputs at qstep distance along the path from start to target +// if start and target have the same Input value, then no step increment is made. +func fixedStepInterpolation(start, target node, qstep map[string][]float64) map[string][]referenceframe.Input { + newNear := make(map[string][]referenceframe.Input) + + // Iterate through each frame's inputs + for frameName, startInputs := range start.Q() { + // As this is constructed in-algorithm from already-near nodes, this is guaranteed to always exist + targetInputs := target.Q()[frameName] + frameSteps := make([]referenceframe.Input, len(startInputs)) + + for j, nearInput := range startInputs { + if nearInput.Value == targetInputs[j].Value { + frameSteps[j] = nearInput + } else { + v1, v2 := nearInput.Value, targetInputs[j].Value + newVal := math.Min(qstep[frameName][j], math.Abs(v2-v1)) + // get correct sign + newVal *= (v2 - v1) / math.Abs(v2-v1) + frameSteps[j] = referenceframe.Input{Value: nearInput.Value + newVal} + } + } + newNear[frameName] = frameSteps + } + return newNear +} + +// node interface is used to wrap a configuration for planning purposes. +// TODO: This is somewhat redundant with a State. +type node interface { + // return the configuration associated with the node + Q() map[string][]referenceframe.Input + Cost() float64 + SetCost(float64) + Poses() PathState + Corner() bool + SetCorner(bool) +} + +type basicNode struct { + q map[string][]referenceframe.Input + cost float64 + poses PathState + corner bool +} + +// Special case constructors for nodes without costs to return NaN. +func newConfigurationNode(q map[string][]referenceframe.Input) node { + return &basicNode{ + q: q, + cost: math.NaN(), + corner: false, + } +} + +func (n *basicNode) Q() map[string][]referenceframe.Input { + return n.q +} + +func (n *basicNode) Cost() float64 { + return n.cost +} + +func (n *basicNode) SetCost(cost float64) { + n.cost = cost +} + +func (n *basicNode) Poses() PathState { + return n.poses +} + +func (n *basicNode) Corner() bool { + return n.corner +} + +func (n *basicNode) SetCorner(corner bool) { + n.corner = corner +} + +// nodePair groups together nodes in a tuple +// TODO(rb): in the future we might think about making this into a list of nodes. +type nodePair struct{ a, b node } + +func (np *nodePair) sumCosts() float64 { + aCost := np.a.Cost() + if math.IsNaN(aCost) { + return 0 + } + bCost := np.b.Cost() + if math.IsNaN(bCost) { + return 0 + } + return aCost + bCost +} + +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 { + startReached, goalReached = pair.a, pair.b + } else { + startReached, goalReached = pair.b, pair.a + } + + // extract the path to the seed + path := make([]node, 0) + for startReached != nil { + path = append(path, startReached) + startReached = startMap[startReached] + } + + // reverse the slice + for i, j := 0, len(path)-1; i < j; i, j = i+1, j-1 { + path[i], path[j] = path[j], path[i] + } + + if goalReached != nil { + 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 { + path = append(path, goalReached) + goalReached = goalMap[goalReached] + } + } + return path +} + +func sumCosts(path []node) float64 { + cost := 0. + for _, wp := range path { + cost += wp.Cost() + } + return cost +} + +// This function is the entrypoint to IK for all cases. Everything prior to here is poses or configurations as the user passed in, which +// here are converted to a list of nodes which are to be used as the from or to states for a motionPlanner. +func generateNodeListForPlanState( + ctx context.Context, + mp motionPlanner, + state *PlanState, + ikSeed map[string][]referenceframe.Input, +) ([]node, error) { + nodes := []node{} + if len(state.poses) != 0 { + // If we have goal state poses, add them to the goal state configurations + goalMetric := mp.opt().getGoalMetric(state.poses) + // get many potential end goals from IK solver + solutions, err := mp.getSolutions(ctx, ikSeed, goalMetric) + if err != nil { + return nil, err + } + nodes = append(nodes, solutions...) + } + if len(state.configuration) > 0 { + nodes = append(nodes, newConfigurationNode(state.configuration)) + } + if len(nodes) == 0 { + return nil, fmt.Errorf("could not create any nodes for state %v", state) + } + return nodes, nil +} diff --git a/motionplan/plan.go b/motionplan/plan.go index 974d69c1d2f..19eee448904 100644 --- a/motionplan/plan.go +++ b/motionplan/plan.go @@ -1,12 +1,14 @@ package motionplan import ( + "encoding/json" "errors" "fmt" "math" "github.com/golang/geo/r3" geo "github.com/kellydunn/golang-geo" + commonpb "go.viam.com/api/common/v1" pb "go.viam.com/api/service/motion/v1" "go.viam.com/rdk/motionplan/ik" @@ -49,9 +51,9 @@ func OffsetPlan(plan Plan, offset spatialmath.Pose) Plan { if path == nil { return NewSimplePlan(nil, plan.Trajectory()) } - newPath := make([]PathStep, 0, len(path)) + newPath := make([]PathState, 0, len(path)) for _, step := range path { - newStep := make(PathStep, len(step)) + newStep := make(PathState, len(step)) for frame, pose := range step { newStep[frame] = referenceframe.NewPoseInFrame(pose.Parent(), spatialmath.Compose(offset, pose.Pose())) } @@ -114,9 +116,9 @@ func (traj Trajectory) EvaluateCost(distFunc ik.SegmentFSMetric) float64 { return totalCost } -// Path is a slice of PathSteps describing a series of Poses for a robot to travel to in the course of following a Plan. -// The pose of the PathStep is the pose at the end of the corresponding set of inputs in the Trajectory. -type Path []PathStep +// Path is a slice of PathStates describing a series of Poses for a robot to travel to in the course of following a Plan. +// The pose of the PathState is the pose at the end of the corresponding set of inputs in the Trajectory. +type Path []PathState func newPath(solution []node, fs referenceframe.FrameSystem) (Path, error) { path := make(Path, 0, len(solution)) @@ -142,10 +144,10 @@ func newPathFromRelativePath(path Path) (Path, error) { if len(path) < 2 { return nil, errors.New("need to have at least 2 elements in path") } - newPath := make([]PathStep, 0, len(path)) + newPath := make([]PathState, 0, len(path)) newPath = append(newPath, path[0]) for i, step := range path[1:] { - newStep := make(PathStep, len(step)) + newStep := make(PathState, len(step)) for frame, pose := range step { lastPose := newPath[i][frame].Pose() newStep[frame] = referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.Compose(lastPose, pose.Pose())) @@ -179,11 +181,11 @@ func (path Path) String() string { return str } -// PathStep is a mapping of Frame names to PoseInFrames. -type PathStep map[string]*referenceframe.PoseInFrame +// PathState is a mapping of Frame names to PoseInFrames. +type PathState map[string]*referenceframe.PoseInFrame -// ToProto converts a PathStep to its representation in protobuf. -func (ps PathStep) ToProto() *pb.PlanStep { +// ToProto converts a PathState to its representation in protobuf. +func (ps PathState) ToProto() *pb.PlanStep { step := make(map[string]*pb.ComponentState) for name, pose := range ps { pbPose := spatialmath.PoseToProtobuf(pose.Pose()) @@ -192,13 +194,13 @@ func (ps PathStep) ToProto() *pb.PlanStep { return &pb.PlanStep{Step: step} } -// PathStepFromProto converts a *pb.PlanStep to a PlanStep. -func PathStepFromProto(ps *pb.PlanStep) (PathStep, error) { +// PathStateFromProto converts a *pb.PlanStep to a PlanStep. +func PathStateFromProto(ps *pb.PlanStep) (PathState, error) { if ps == nil { - return PathStep{}, errors.New("received nil *pb.PlanStep") + return PathState{}, errors.New("received nil *pb.PlanStep") } - step := make(PathStep, len(ps.Step)) + step := make(PathState, len(ps.Step)) for k, v := range ps.Step { step[k] = referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewPoseFromProtobuf(v.Pose)) } @@ -209,9 +211,9 @@ func PathStepFromProto(ps *pb.PlanStep) (PathStep, error) { // A Point with X as the longitude and Y as the latitude // An orientation using the heading as the theta in an OrientationVector with Z=1. func NewGeoPlan(plan Plan, pt *geo.Point) Plan { - newPath := make([]PathStep, 0, len(plan.Path())) + newPath := make([]PathState, 0, len(plan.Path())) for _, step := range plan.Path() { - newStep := make(PathStep) + newStep := make(PathState) for frame, pif := range step { pose := pif.Pose() geoPose := spatialmath.PoseToGeoPose(spatialmath.NewGeoPose(pt, 0), pose) @@ -261,7 +263,7 @@ type ExecutionState struct { currentInputs map[string][]referenceframe.Input // The current PoseInFrames of input-enabled elements described by this plan. - currentPose map[string]*referenceframe.PoseInFrame + currentPose PathState } // NewExecutionState will construct an ExecutionState struct. @@ -269,7 +271,7 @@ func NewExecutionState( plan Plan, index int, currentInputs map[string][]referenceframe.Input, - currentPose map[string]*referenceframe.PoseInFrame, + currentPose PathState, ) (ExecutionState, error) { if plan == nil { return ExecutionState{}, errors.New("cannot create new ExecutionState with nil plan") @@ -304,7 +306,7 @@ func (e *ExecutionState) CurrentInputs() map[string][]referenceframe.Input { } // CurrentPoses returns the current poses in frame of the components associated with the ExecutionState. -func (e *ExecutionState) CurrentPoses() map[string]*referenceframe.PoseInFrame { +func (e *ExecutionState) CurrentPoses() PathState { return e.currentPose } @@ -349,3 +351,118 @@ func CalculateFrameErrorState(e ExecutionState, executionFrame, localizationFram func newFrameNotFoundError(frameName string) error { return fmt.Errorf("could not find frame %s in ExecutionState", frameName) } + +// PlanState is a struct which holds both a PathState and a configuration. This is intended to be used as start or goal states for plans. +// Either field may be nil. +type PlanState struct { + poses PathState + configuration map[string][]referenceframe.Input +} + +// NewPlanState creates a PlanState from the given poses and configuration. Either or both may be nil. +func NewPlanState(poses PathState, configuration map[string][]referenceframe.Input) *PlanState { + return &PlanState{poses: poses, configuration: configuration} +} + +// Poses returns the poses of the PlanState. +func (p *PlanState) Poses() PathState { + return p.poses +} + +// Configuration returns the configuration of the PlanState. +func (p *PlanState) Configuration() map[string][]referenceframe.Input { + return p.configuration +} + +// ComputePoses returns the poses of a PlanState if they are populated, or computes them using the given FrameSystem if not. +func (p *PlanState) ComputePoses(fs referenceframe.FrameSystem) (PathState, error) { + if p.poses != nil { + return p.poses, nil + } + + if len(p.configuration) == 0 { + return nil, errors.New("cannot computes poses, neither poses nor configuration are populated") + } + + return ComputePathStateFromConfiguration(fs, p.configuration) +} + +// Serialize turns a PlanState into a map[string]interface suitable for being transmitted over proto. +func (p PlanState) Serialize() map[string]interface{} { + m := map[string]interface{}{} + poseMap := map[string]interface{}{} + confMap := map[string]interface{}{} + for fName, pif := range p.poses { + pifProto := referenceframe.PoseInFrameToProtobuf(pif) + poseMap[fName] = pifProto + } + for fName, conf := range p.configuration { + confMap[fName] = referenceframe.InputsToFloats(conf) + } + m["poses"] = poseMap + m["configuration"] = confMap + return m +} + +// ComputePathStateFromConfiguration computes the poses for each frame in a framesystem in frame of World, using the provided configuration. +func ComputePathStateFromConfiguration(fs referenceframe.FrameSystem, configuration map[string][]referenceframe.Input) (PathState, error) { + // Compute poses from configuration using the FrameSystem + computedPoses := make(PathState) + for _, frameName := range fs.FrameNames() { + pif, err := fs.Transform(configuration, referenceframe.NewZeroPoseInFrame(frameName), referenceframe.World) + if err != nil { + return nil, err + } + computedPoses[frameName] = pif.(*referenceframe.PoseInFrame) + } + return computedPoses, nil +} + +// DeserializePlanState turns a serialized PlanState back into a PlanState. +func DeserializePlanState(iface map[string]interface{}) (*PlanState, error) { + ps := &PlanState{ + poses: PathState{}, + configuration: map[string][]referenceframe.Input{}, + } + if posesIface, ok := iface["poses"]; ok { + if pathStateMap, ok := posesIface.(map[string]interface{}); ok { + for fName, pifIface := range pathStateMap { + pifJSON, err := json.Marshal(pifIface) + if err != nil { + return nil, err + } + pifPb := &commonpb.PoseInFrame{} + err = json.Unmarshal(pifJSON, pifPb) + if err != nil { + return nil, err + } + pif := referenceframe.ProtobufToPoseInFrame(pifPb) + ps.poses[fName] = pif + } + } else { + return nil, errors.New("could not decode contents of poses") + } + } + if confIface, ok := iface["configuration"]; ok { + if confMap, ok := confIface.(map[string]interface{}); ok { + for fName, inputsArrIface := range confMap { + if inputsArr, ok := inputsArrIface.([]interface{}); ok { + floats := make([]float64, 0, len(inputsArr)) + for _, inputIface := range inputsArr { + if val, ok := inputIface.(float64); ok { + floats = append(floats, val) + } else { + return nil, errors.New("configuration input array did not contain floats") + } + } + ps.configuration[fName] = referenceframe.FloatsToInputs(floats) + } else { + return nil, errors.New("configuration did not contain array of inputs") + } + } + } else { + return nil, errors.New("could not decode contents of configuration") + } + } + return ps, nil +} diff --git a/motionplan/planManager.go b/motionplan/planManager.go index b334c4c1d93..9433e060980 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -34,11 +34,8 @@ const ( // Intended information flow should be: // motionplan.PlanMotion() -> SolvableFrameSystem.SolveWaypointsWithOptions() -> planManager.planSingleWaypoint(). type planManager struct { - *planner + *planner // TODO: This should probably be removed activeBackgroundWorkers sync.WaitGroup - - useTPspace bool - ptgFrameName string } func newPlanManager( @@ -57,41 +54,24 @@ func newPlanManager( }, nil } -// PlanSingleWaypoint will solve the framesystem to the requested set of goals. +type atomicWaypoint struct { + mp motionPlanner + startState *PlanState // A list of starting states, any of which would be valid to start from + goalState *PlanState // A list of goal states, any of which would be valid to arrive at +} + +// planMultiWaypoint plans a motion through multiple waypoints, using identical constraints for each // Any constraints, etc, will be held for the entire motion. -func (pm *planManager) PlanSingleWaypoint(ctx context.Context, request *PlanRequest, seedPlan Plan) (Plan, error) { - if request.StartPose != nil { - request.Logger.Warn( - "plan request passed a start pose, but non-relative plans will use the pose from transforming StartConfiguration", - ) +func (pm *planManager) planMultiWaypoint(ctx context.Context, request *PlanRequest, seedPlan Plan) (Plan, error) { + startPoses, err := request.StartState.ComputePoses(request.FrameSystem) + if err != nil { + return nil, err } - startPoses := PathStep{} - // Use frame system to get configuration and transform - for frame, goal := range request.allGoals { - startPose, err := pm.fs.Transform( - request.StartConfiguration, - referenceframe.NewPoseInFrame(frame, spatialmath.NewZeroPose()), - goal.Parent(), - ) - if err != nil { - return nil, err - } - startPoses[frame] = startPose.(*referenceframe.PoseInFrame) - - request.Logger.CInfof(ctx, - "planning motion for frame %s\nGoal: %v\nStarting seed map %v\n, startPose %v\n, worldstate: %v\n", - request.Frame.Name(), - referenceframe.PoseInFrameToProtobuf(goal), - request.StartConfiguration, - referenceframe.PoseInFrameToProtobuf(startPose.(*referenceframe.PoseInFrame)), - request.WorldState.String(), - ) - } opt, err := pm.plannerSetupFromMoveRequest( - startPoses, - request.allGoals, - request.StartConfiguration, + request.StartState, + request.Goals[0], + request.StartState.configuration, // No way to this code without validating the request and ensuring this exists request.WorldState, request.BoundingRegions, request.Constraints, @@ -100,8 +80,14 @@ func (pm *planManager) PlanSingleWaypoint(ctx context.Context, request *PlanRequ if err != nil { return nil, err } - if pm.useTPspace { - return pm.planRelativeWaypoint(ctx, request, seedPlan) + if opt.useTPspace { + return pm.planRelativeWaypoint(ctx, request, seedPlan, opt) + } + // Theoretically, a plan could be made between two poses, by running IK on both the start and end poses to create sets of seed and + // goal configurations. However, the blocker here is the lack of a "known good" configuration used to determine which obstacles + // are allowed to collide with one another. + if request.StartState.configuration == nil { + return nil, errors.New("must populate start state configuration if not planning for 2d base/tpspace") } // set timeout for entire planning process if specified @@ -113,130 +99,40 @@ func (pm *planManager) PlanSingleWaypoint(ctx context.Context, request *PlanRequ defer cancel() } - // Transform goal poses into world frame if needed - alteredGoals := PathStep{} - for _, chain := range opt.motionChains { - if chain.worldRooted { - tf, err := pm.fs.Transform(request.StartConfiguration, request.Goal, referenceframe.World) - if err != nil { - return nil, err - } - alteredGoals[chain.solveFrameName] = tf.(*referenceframe.PoseInFrame) - } else { - alteredGoals[chain.solveFrameName] = request.allGoals[chain.solveFrameName] - } - } - - var goals []PathStep - var opts []*plannerOptions - - // TODO: This is a necessary optimization for linear motion to be performant - subWaypoints := false - - // linear motion profile has known intermediate points, so solving can be broken up and sped up - if profile, ok := request.Options["motion_profile"]; ok && profile == LinearMotionProfile { - subWaypoints = true - } - - if len(request.Constraints.GetLinearConstraint()) > 0 { - subWaypoints = true - } + waypoints := []atomicWaypoint{} - // If we are seeding off of a pre-existing plan, we don't need the speedup of subwaypoints - if seedPlan != nil { - subWaypoints = false - } - - if subWaypoints { - pathStepSize, ok := request.Options["path_step_size"].(float64) - if !ok { - pathStepSize = defaultPathStepSize - } - - numSteps := 0 - for frame, pif := range alteredGoals { - // Calculate steps needed for this frame - steps := PathStepCount(startPoses[frame].Pose(), pif.Pose(), pathStepSize) - if steps > numSteps { - numSteps = steps - } - } - - from := startPoses - for i := 1; i < numSteps; i++ { - by := float64(i) / float64(numSteps) - to := PathStep{} - for frame, pif := range alteredGoals { - toPose := spatialmath.Interpolate(startPoses[frame].Pose(), pif.Pose(), by) - to[frame] = referenceframe.NewPoseInFrame(pif.Parent(), toPose) - } - goals = append(goals, to) - wpOpt, err := pm.plannerSetupFromMoveRequest( - from, - to, - request.StartConfiguration, - request.WorldState, - request.BoundingRegions, - request.Constraints, - request.Options, - ) - if err != nil { - return nil, err - } - wpOpt.setGoal(to) - opts = append(opts, wpOpt) - - from = to - } - // Update opt to be just the last step - opt, err = pm.plannerSetupFromMoveRequest( - from, - request.allGoals, - request.StartConfiguration, - request.WorldState, - request.BoundingRegions, - request.Constraints, - request.Options, - ) + runningStart := startPoses + for i, goal := range request.Goals { + goalPoses, err := goal.ComputePoses(request.FrameSystem) if err != nil { return nil, err } - } - pm.planOpts = opt - opt.setGoal(alteredGoals) - opts = append(opts, opt) - goals = append(goals, alteredGoals) - planners := make([]motionPlanner, 0, len(opts)) - // Set up planners for later execution - for _, opt := range opts { - // Build planner - //nolint: gosec - pathPlanner, err := opt.PlannerConstructor( - pm.fs, - rand.New(rand.NewSource(int64(pm.randseed.Int()))), - pm.logger, - opt, - ) - if err != nil { - return nil, err + // Log each requested motion + // TODO: PlanRequest.String() could begin to exist + for frame, stepgoal := range goalPoses { + request.Logger.CInfof(ctx, + "setting up motion for frame %s\nGoal: %v\nstartPose %v\nworldstate: %v\n", + frame, + referenceframe.PoseInFrameToProtobuf(stepgoal), + referenceframe.PoseInFrameToProtobuf(runningStart[frame]), + request.WorldState.String(), + ) } - planners = append(planners, pathPlanner) - } + runningStart = goalPoses - // If we have multiple sub-waypoints, make sure the final goal is not unreachable. - if len(goals) > 1 { - // Viability check; ensure that the waypoint is not impossible to reach - _, err = planners[0].getSolutions(ctx, request.StartConfiguration) + // Solving highly constrained motions by breaking apart into small pieces is much more performant + goalWaypoints, err := pm.generateWaypoints(request, seedPlan, i) if err != nil { return nil, err } + waypoints = append(waypoints, goalWaypoints...) } - plan, err := pm.planAtomicWaypoints(ctx, goals, request.StartConfiguration, planners, seedPlan) + plan, err := pm.planAtomicWaypoints(ctx, waypoints, seedPlan) pm.activeBackgroundWorkers.Wait() if err != nil { - if len(goals) > 1 { + if len(waypoints) > 1 { err = fmt.Errorf("failed to plan path for valid goal: %w", err) } return nil, err @@ -249,9 +145,7 @@ func (pm *planManager) PlanSingleWaypoint(ctx context.Context, request *PlanRequ // intermediate waypoint. Waypoints here refer to points that the software has generated to. func (pm *planManager) planAtomicWaypoints( ctx context.Context, - goals []PathStep, - seed map[string][]referenceframe.Input, - planners []motionPlanner, + waypoints []atomicWaypoint, seedPlan Plan, ) (Plan, error) { var err error @@ -259,9 +153,10 @@ func (pm *planManager) planAtomicWaypoints( // Each atomic waypoint produces one result promise, all of which are resolved at the end, allowing multiple to be solved in parallel. resultPromises := []*resultPromise{} + var seed map[string][]referenceframe.Input + // try to solve each goal, one at a time - for i, goal := range goals { - pm.logger.Debug("start planning for ", goal.ToProto()) + for _, wp := range waypoints { // Check if ctx is done between each waypoint select { case <-ctx.Done(): @@ -269,17 +164,35 @@ func (pm *planManager) planAtomicWaypoints( default: } - pathPlanner := planners[i] - var maps *rrtMaps if seedPlan != nil { - maps, err = pm.planToRRTGoalMap(seedPlan, goal) + maps, err = pm.planToRRTGoalMap(seedPlan, wp) if err != nil { return nil, err } } + // If we don't pass in pre-made maps, initialize and seed with IK solutions here + // TPspace should fill in its maps in planRelativeWaypoint and then call planSingleAtomicWaypoint directly so no need to + // deal with that here. + // TODO: Once TPspace also supports multiple waypoints, this needs to be updated. + if !wp.mp.opt().useTPspace && maps == nil { + if seed != nil { + // If we have a seed, we are linking multiple waypoints, so the next one MUST start at the ending configuration of the last + wp.startState = &PlanState{configuration: seed} + } + planSeed := initRRTSolutions(ctx, wp) + if planSeed.err != nil { + return nil, planSeed.err + } + if planSeed.steps != nil { + resultPromises = append(resultPromises, &resultPromise{steps: planSeed.steps}) + seed = planSeed.steps[len(planSeed.steps)-1].Q() + continue + } + maps = planSeed.maps + } // Plan the single waypoint, and accumulate objects which will be used to constrauct the plan after all planning has finished - newseed, future, err := pm.planSingleAtomicWaypoint(ctx, goal, seed, pathPlanner, maps) + newseed, future, err := pm.planSingleAtomicWaypoint(ctx, wp, maps) if err != nil { return nil, err } @@ -294,32 +207,39 @@ func (pm *planManager) planAtomicWaypoints( if err != nil { return nil, err } - pm.logger.Debug("completed planning for ", goals[i].ToProto()) - resultSlices = append(resultSlices, steps...) + pm.logger.Debugf("completed planning for subwaypoint %d", i) + if i > 0 { + // Prevent doubled steps. The first step of each plan is the last step of the prior plan. + resultSlices = append(resultSlices, steps[1:]...) + } else { + resultSlices = append(resultSlices, steps...) + } } - return newRRTPlan(resultSlices, pm.fs, pm.useTPspace, nil) + // // TODO: Once TPspace also supports multiple waypoints, this needs to be updated. For now it can be false. + return newRRTPlan(resultSlices, pm.fs, false, nil) } // planSingleAtomicWaypoint attempts to plan a single waypoint. It may optionally be pre-seeded with rrt maps; these will be passed to the // planner if supported, or ignored if not. func (pm *planManager) planSingleAtomicWaypoint( ctx context.Context, - goal PathStep, - seed map[string][]referenceframe.Input, - pathPlanner motionPlanner, + wp atomicWaypoint, maps *rrtMaps, ) (map[string][]referenceframe.Input, *resultPromise, error) { - if parPlan, ok := pathPlanner.(rrtParallelPlanner); ok { + pm.logger.Debug("start planning for ", wp.goalState.configuration, wp.goalState.poses) + + if _, ok := wp.mp.(rrtParallelPlanner); ok { // rrtParallelPlanner supports solution look-ahead for parallel waypoint solving // This will set that up, and if we get a result on `endpointPreview`, then the next iteration will be started, and the steps // for this solve will be rectified at the end. + endpointPreview := make(chan node, 1) solutionChan := make(chan *rrtSolution, 1) pm.activeBackgroundWorkers.Add(1) utils.PanicCapturingGo(func() { defer pm.activeBackgroundWorkers.Done() - pm.planParallelRRTMotion(ctx, goal, seed, parPlan, endpointPreview, solutionChan, maps) + pm.planParallelRRTMotion(ctx, wp, endpointPreview, solutionChan, maps) }) // We don't want to check context here; context cancellation will be handled by planParallelRRTMotion. // Instead, if a timeout occurs while we are smoothing, we want to return the best plan we have so far, rather than nothing at all. @@ -337,17 +257,17 @@ func (pm *planManager) planSingleAtomicWaypoint( } else { // This ctx is used exclusively for the running of the new planner and timing it out. It may be different from the main `ctx` // timeout due to planner fallbacks. - plannerctx, cancel := context.WithTimeout(ctx, time.Duration(pathPlanner.opt().Timeout*float64(time.Second))) + plannerctx, cancel := context.WithTimeout(ctx, time.Duration(wp.mp.opt().Timeout*float64(time.Second))) defer cancel() - nodes, err := pathPlanner.plan(plannerctx, goal, seed) + plan, err := wp.mp.plan(plannerctx, wp.startState, wp.goalState) if err != nil { return nil, nil, err } - smoothedPath := pathPlanner.smoothPath(ctx, nodes) + smoothedPath := wp.mp.smoothPath(ctx, plan) // Update seed for the next waypoint to be the final configuration of this waypoint - seed = smoothedPath[len(smoothedPath)-1].Q() + seed := smoothedPath[len(smoothedPath)-1].Q() return seed, &resultPromise{steps: smoothedPath}, nil } } @@ -356,23 +276,17 @@ func (pm *planManager) planSingleAtomicWaypoint( // as necessary. func (pm *planManager) planParallelRRTMotion( ctx context.Context, - goal PathStep, - seed map[string][]referenceframe.Input, - pathPlanner rrtParallelPlanner, + wp atomicWaypoint, endpointPreview chan node, solutionChan chan *rrtSolution, maps *rrtMaps, ) { + pathPlanner := wp.mp.(rrtParallelPlanner) var rrtBackground sync.WaitGroup var err error - // If we don't pass in pre-made maps, initialize and seed with IK solutions here - if !pm.useTPspace && maps == nil { - planSeed := initRRTSolutions(ctx, pathPlanner, seed) - if planSeed.err != nil || planSeed.steps != nil { - solutionChan <- planSeed - return - } - maps = planSeed.maps + if maps == nil { + solutionChan <- &rrtSolution{err: errors.New("nil maps")} + return } // publish endpoint of plan if it is known @@ -389,7 +303,7 @@ func (pm *planManager) planParallelRRTMotion( // This ctx is used exclusively for the running of the new planner and timing it out. It may be different from the main `ctx` timeout // due to planner fallbacks. - plannerctx, cancel := context.WithTimeout(ctx, time.Duration(pathPlanner.opt().Timeout*float64(time.Second))) + plannerctx, cancel := context.WithTimeout(ctx, time.Duration(wp.mp.opt().Timeout*float64(time.Second))) defer cancel() plannerChan := make(chan *rrtSolution, 1) @@ -398,7 +312,7 @@ func (pm *planManager) planParallelRRTMotion( rrtBackground.Add(1) utils.PanicCapturingGo(func() { defer rrtBackground.Done() - pathPlanner.rrtBackgroundRunner(plannerctx, seed, &rrtParallelPlannerShared{maps, endpointPreview, plannerChan}) + pathPlanner.rrtBackgroundRunner(plannerctx, &rrtParallelPlannerShared{maps, endpointPreview, plannerChan}) }) // Wait for results from the planner. This will also handle calling the fallback if needed, and will ultimately return the best path @@ -436,7 +350,7 @@ func (pm *planManager) planParallelRRTMotion( // If there *was* an error, then either the fallback will not error and will replace it, or the error will be returned if finalSteps.err == nil { if fallbackPlanner != nil { - if ok, score := pm.goodPlan(finalSteps, pm.opt()); ok { + if ok, score := pm.goodPlan(finalSteps, pathPlanner.opt()); ok { pm.logger.CDebugf(ctx, "got path with score %f, close enough to optimal %f", score, maps.optNode.Cost()) fallbackPlanner = nil } else { @@ -460,13 +374,9 @@ func (pm *planManager) planParallelRRTMotion( // Run fallback only if we don't have a very good path if fallbackPlanner != nil { - _, alternateFuture, err = pm.planSingleAtomicWaypoint( - ctx, - goal, - seed, - fallbackPlanner, - mapSeed, - ) + fallbackWP := wp + fallbackWP.mp = fallbackPlanner + _, alternateFuture, err = pm.planSingleAtomicWaypoint(ctx, fallbackWP, mapSeed) if err != nil { alternateFuture = nil } @@ -477,7 +387,7 @@ func (pm *planManager) planParallelRRTMotion( score := math.Inf(1) if finalSteps.steps != nil { - score = nodesToTrajectory(finalSteps.steps).EvaluateCost(pm.opt().scoreFunc) + score = nodesToTrajectory(finalSteps.steps).EvaluateCost(pathPlanner.opt().scoreFunc) } // If we ran a fallback, retrieve the result and compare to the smoothed path @@ -486,7 +396,7 @@ func (pm *planManager) planParallelRRTMotion( if err == nil { // If the fallback successfully found a path, check if it is better than our smoothed previous path. // The fallback should emerge pre-smoothed, so that should be a non-issue - altCost := nodesToTrajectory(alternate).EvaluateCost(pm.opt().scoreFunc) + altCost := nodesToTrajectory(alternate).EvaluateCost(pathPlanner.opt().scoreFunc) if altCost < score { pm.logger.CDebugf(ctx, "replacing path with score %f with better score %f", score, altCost) finalSteps = &rrtSolution{steps: alternate} @@ -508,13 +418,14 @@ func (pm *planManager) planParallelRRTMotion( // This is where the map[string]interface{} passed in via `extra` is used to decide how planning happens. func (pm *planManager) plannerSetupFromMoveRequest( - from, to PathStep, - seedMap map[string][]referenceframe.Input, + from, to *PlanState, + seedMap map[string][]referenceframe.Input, // A known good configuration to set up collsiion constraints. Not necessarily `from`. worldState *referenceframe.WorldState, boundingRegions []spatialmath.Geometry, constraints *Constraints, planningOpts map[string]interface{}, ) (*plannerOptions, error) { + var err error if constraints == nil { // Constraints may be nil, but if a motion profile is set in planningOpts we need it to be a valid pointer to an empty struct. constraints = &Constraints{} @@ -524,7 +435,15 @@ func (pm *planManager) plannerSetupFromMoveRequest( // Start with normal options opt := newBasicPlannerOptions() opt.extra = planningOpts - opt.startPoses = from + + startPoses, err := from.ComputePoses(pm.fs) + if err != nil { + return nil, err + } + goalPoses, err := to.ComputePoses(pm.fs) + if err != nil { + return nil, err + } collisionBufferMM := defaultCollisionBufferMM collisionBufferMMRaw, ok := planningOpts["collision_buffer_mm"] @@ -538,7 +457,7 @@ func (pm *planManager) plannerSetupFromMoveRequest( } } - err := opt.fillMotionChains(pm.fs, to) + err = opt.fillMotionChains(pm.fs, to) if err != nil { return nil, err } @@ -548,26 +467,26 @@ func (pm *planManager) plannerSetupFromMoveRequest( // create motion chains for each goal, and error check for PTG frames // TODO: currently, if any motion chain has a PTG frame, that must be the only motion chain and that frame must be the only // frame in the chain with nonzero DoF. Eventually this need not be the case. - pm.useTPspace = false for _, chain := range opt.motionChains { for _, movingFrame := range chain.frames { if _, isPTGframe := movingFrame.(tpspace.PTGProvider); isPTGframe { - if pm.useTPspace { + if opt.useTPspace { return nil, errors.New("only one PTG frame can be planned for at a time") } if len(opt.motionChains) > 1 { return nil, errMixedFrameTypes } - pm.useTPspace = true - pm.ptgFrameName = movingFrame.Name() + opt.useTPspace = true + opt.ptgFrameName = movingFrame.Name() + chain.worldRooted = true } else if len(movingFrame.DoF()) > 0 { - if pm.useTPspace { + if opt.useTPspace { return nil, errMixedFrameTypes } } } } - if pm.useTPspace { + if opt.useTPspace { opt.Resolution = defaultPTGCollisionResolution } @@ -642,7 +561,6 @@ func (pm *planManager) plannerSetupFromMoveRequest( } opt.profile = FreeMotionProfile - switch motionProfile { case LinearMotionProfile: opt.profile = LinearMotionProfile @@ -676,12 +594,12 @@ func (pm *planManager) plannerSetupFromMoveRequest( constraints.AddOrientationConstraint(OrientationConstraint{tolerance}) case PositionOnlyMotionProfile: opt.profile = PositionOnlyMotionProfile - if !pm.useTPspace || opt.PositionSeeds <= 0 { + if !opt.useTPspace || opt.PositionSeeds <= 0 { opt.goalMetricConstructor = ik.NewPositionOnlyMetric } } - hasTopoConstraint, err := opt.addTopoConstraints(pm.fs, seedMap, from, to, constraints) + hasTopoConstraint, err := opt.addTopoConstraints(pm.fs, seedMap, startPoses, goalPoses, constraints) if err != nil { return nil, err } @@ -702,7 +620,7 @@ func (pm *planManager) plannerSetupFromMoveRequest( return nil, errors.New("could not interpret planning_alg field as string") } } - if pm.useTPspace && planAlg != "" { + if opt.useTPspace && planAlg != "" { return nil, fmt.Errorf("cannot specify a planning_alg when planning for a TP-space frame. alg specified was %s", planAlg) } if hasTopoConstraint { @@ -722,7 +640,7 @@ func (pm *planManager) plannerSetupFromMoveRequest( default: // use default, already set } - if pm.useTPspace { + if opt.useTPspace { // overwrite default with TP space opt.PlannerConstructor = newTPSpaceMotionPlanner @@ -730,10 +648,9 @@ func (pm *planManager) plannerSetupFromMoveRequest( opt.poseDistanceFunc = ik.NewSquaredNormSegmentMetric(defaultTPspaceOrientationScale) // If we have PTGs, then we calculate distances using the PTG-specific distance function. // Otherwise we just use squared norm on inputs. - opt.scoreFunc = tpspace.NewPTGDistanceMetric([]string{pm.ptgFrameName}) + opt.scoreFunc = tpspace.NewPTGDistanceMetric([]string{opt.ptgFrameName}) planAlg = "tpspace" - opt.relativeInputs = true } if opt.profile == FreeMotionProfile || opt.profile == PositionOnlyMotionProfile { @@ -758,6 +675,139 @@ func (pm *planManager) plannerSetupFromMoveRequest( return opt, nil } +// generateWaypoints will return the list of atomic waypoints that correspond to a specific goal in a plan request. +func (pm *planManager) generateWaypoints(request *PlanRequest, seedPlan Plan, wpi int) ([]atomicWaypoint, error) { + wpGoals := request.Goals[wpi] + startState := request.StartState + if wpi > 0 { + startState = request.Goals[wpi-1] + } + + startPoses, err := startState.ComputePoses(pm.fs) + if err != nil { + return nil, err + } + goalPoses, err := wpGoals.ComputePoses(pm.fs) + if err != nil { + return nil, err + } + + subWaypoints := useSubWaypoints(request, seedPlan, wpi) + opt, err := pm.plannerSetupFromMoveRequest( + startState, + wpGoals, + request.StartState.configuration, + request.WorldState, + request.BoundingRegions, + request.Constraints, + request.Options, + ) + if err != nil { + return nil, err + } + if wpGoals.poses != nil { + // Transform goal poses into world frame if needed. This is used for e.g. when a component's goal is given in terms of itself. + alteredGoals, err := alterGoals(opt.motionChains, pm.fs, request.StartState.configuration, wpGoals) + if err != nil { + return nil, err + } + wpGoals = alteredGoals + // Regenerate opts since our metrics will have changed + opt, err = pm.plannerSetupFromMoveRequest( + startState, + wpGoals, + request.StartState.configuration, + request.WorldState, + request.BoundingRegions, + request.Constraints, + request.Options, + ) + if err != nil { + return nil, err + } + } + + // TPspace should never use subwaypoints + if !subWaypoints || opt.useTPspace { + //nolint: gosec + pathPlanner, err := opt.PlannerConstructor( + pm.fs, + rand.New(rand.NewSource(int64(pm.randseed.Int()))), + pm.logger, + opt, + ) + if err != nil { + return nil, err + } + return []atomicWaypoint{{mp: pathPlanner, startState: request.StartState, goalState: wpGoals}}, nil + } + + pathStateSize, ok := request.Options["path_step_size"].(float64) + if !ok { + pathStateSize = defaultPathStateSize + } + + numSteps := 0 + for frame, pif := range goalPoses { + // Calculate steps needed for this frame + steps := PathStateCount(startPoses[frame].Pose(), pif.Pose(), pathStateSize) + if steps > numSteps { + numSteps = steps + } + } + + from := startState + waypoints := []atomicWaypoint{} + for i := 1; i <= numSteps; i++ { + by := float64(i) / float64(numSteps) + to := &PlanState{PathState{}, map[string][]referenceframe.Input{}} + if wpGoals.poses != nil { + for frameName, pif := range wpGoals.poses { + toPose := spatialmath.Interpolate(startPoses[frameName].Pose(), pif.Pose(), by) + to.poses[frameName] = referenceframe.NewPoseInFrame(pif.Parent(), toPose) + } + } + if wpGoals.configuration != nil { + for frameName, inputs := range wpGoals.configuration { + frame := pm.fs.Frame(frameName) + // If subWaypoints was true, then StartState had a configuration, and if our goal does, so will `from` + toInputs, err := frame.Interpolate(from.configuration[frameName], inputs, by) + if err != nil { + return nil, err + } + to.configuration[frameName] = toInputs + } + } + wpOpt, err := pm.plannerSetupFromMoveRequest( + from, + to, + request.StartState.configuration, + request.WorldState, + request.BoundingRegions, + request.Constraints, + request.Options, + ) + if err != nil { + return nil, err + } + //nolint: gosec + pathPlanner, err := wpOpt.PlannerConstructor( + pm.fs, + rand.New(rand.NewSource(int64(pm.randseed.Int()))), + pm.logger, + wpOpt, + ) + if err != nil { + return nil, err + } + waypoints = append(waypoints, atomicWaypoint{mp: pathPlanner, startState: from, goalState: to}) + + from = to + } + + return waypoints, nil +} + // check whether the solution is within some amount of the optimal. func (pm *planManager) goodPlan(pr *rrtSolution, opt *plannerOptions) (bool, float64) { solutionCost := math.Inf(1) @@ -774,7 +824,7 @@ func (pm *planManager) goodPlan(pr *rrtSolution, opt *plannerOptions) (bool, flo return false, solutionCost } -func (pm *planManager) planToRRTGoalMap(plan Plan, goal PathStep) (*rrtMaps, error) { +func (pm *planManager) planToRRTGoalMap(plan Plan, goal atomicWaypoint) (*rrtMaps, error) { traj := plan.Trajectory() path := plan.Path() if len(traj) != len(path) { @@ -785,17 +835,17 @@ func (pm *planManager) planToRRTGoalMap(plan Plan, goal PathStep) (*rrtMaps, err planNodes = append(planNodes, &basicNode{q: tConf, poses: path[i]}) } - if pm.useTPspace { + if goal.mp.opt().useTPspace { // Fill in positions from the old origin to where the goal was during the last run - planNodesOld, err := rectifyTPspacePath(planNodes, pm.fs.Frame(pm.ptgFrameName), spatialmath.NewZeroPose()) + planNodesOld, err := rectifyTPspacePath(planNodes, pm.fs.Frame(goal.mp.opt().ptgFrameName), spatialmath.NewZeroPose()) if err != nil { return nil, err } // Figure out where our new starting point is relative to our last one, and re-rectify using the new adjusted location - oldGoal := planNodesOld[len(planNodesOld)-1].Poses()[pm.ptgFrameName].Pose() - pathDiff := spatialmath.PoseBetween(oldGoal, goal[pm.ptgFrameName].Pose()) - planNodes, err = rectifyTPspacePath(planNodes, pm.fs.Frame(pm.ptgFrameName), pathDiff) + oldGoal := planNodesOld[len(planNodesOld)-1].Poses()[goal.mp.opt().ptgFrameName].Pose() + pathDiff := spatialmath.PoseBetween(oldGoal, goal.goalState.poses[goal.mp.opt().ptgFrameName].Pose()) + planNodes, err = rectifyTPspacePath(planNodes, pm.fs.Frame(goal.mp.opt().ptgFrameName), pathDiff) if err != nil { return nil, err } @@ -806,7 +856,7 @@ func (pm *planManager) planToRRTGoalMap(plan Plan, goal PathStep) (*rrtMaps, err for i := len(planNodes) - 1; i >= 0; i-- { if i != 0 { // Fill in costs - cost := pm.opt().configurationDistanceFunc(&ik.SegmentFS{ + cost := goal.mp.opt().configurationDistanceFunc(&ik.SegmentFS{ StartConfiguration: planNodes[i-1].Q(), EndConfiguration: planNodes[i].Q(), FS: pm.fs, @@ -827,16 +877,20 @@ func (pm *planManager) planToRRTGoalMap(plan Plan, goal PathStep) (*rrtMaps, err // planRelativeWaypoint will solve the PTG frame to one individual pose. This is used for frames whose inputs are relative, that // is, the pose returned by `Transform` is a transformation rather than an absolute position. -func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRequest, seedPlan Plan) (Plan, error) { - if request.StartPose == nil { +func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRequest, seedPlan Plan, opt *plannerOptions) (Plan, error) { + if request.StartState.poses == nil { return nil, errors.New("must provide a startPose if solving for PTGs") } - startPose := request.StartPose + if len(request.Goals) != 1 { + return nil, errors.New("can only provide one goal if solving for PTGs") + } + startPose := request.StartState.poses[opt.ptgFrameName].Pose() + goalPif := request.Goals[0].poses[opt.ptgFrameName] request.Logger.CInfof(ctx, "planning relative motion for frame %s\nGoal: %v\nstartPose %v\n, worldstate: %v\n", - request.Frame.Name(), - referenceframe.PoseInFrameToProtobuf(request.Goal), + opt.ptgFrameName, + referenceframe.PoseInFrameToProtobuf(goalPif), spatialmath.PoseToProtobuf(startPose), request.WorldState.String(), ) @@ -844,8 +898,8 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe if pathdebug { pm.logger.Debug("$type,X,Y") pm.logger.Debugf("$SG,%f,%f", startPose.Point().X, startPose.Point().Y) - pm.logger.Debugf("$SG,%f,%f", request.Goal.Pose().Point().X, request.Goal.Pose().Point().Y) - gifs, err := request.WorldState.ObstaclesInWorldFrame(pm.fs, request.StartConfiguration) + pm.logger.Debugf("$SG,%f,%f", goalPif.Pose().Point().X, goalPif.Pose().Point().Y) + gifs, err := request.WorldState.ObstaclesInWorldFrame(pm.fs, request.StartState.configuration) if err == nil { for _, geom := range gifs.Geometries() { pts := geom.ToPoints(1.) @@ -867,76 +921,51 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe defer cancel() } - startMap := map[string]*referenceframe.PoseInFrame{pm.ptgFrameName: referenceframe.NewPoseInFrame(referenceframe.World, startPose)} - - // Use frame name instead of Frame object - tf, err := pm.fs.Transform(request.StartConfiguration, request.Goal, referenceframe.World) - if err != nil { - return nil, err - } - goalPose := tf.(*referenceframe.PoseInFrame).Pose() - goalMap := map[string]*referenceframe.PoseInFrame{pm.ptgFrameName: referenceframe.NewPoseInFrame(referenceframe.World, goalPose)} - opt, err := pm.plannerSetupFromMoveRequest( - startMap, - goalMap, - request.StartConfiguration, - request.WorldState, - request.BoundingRegions, - request.Constraints, - request.Options, - ) - if err != nil { - return nil, err - } - // Create frame system subset using frame name - relativeOnlyFS, err := pm.fs.FrameSystemSubset(pm.fs.Frame(request.Frame.Name())) + relativeOnlyFS, err := pm.fs.FrameSystemSubset(pm.fs.Frame(opt.ptgFrameName)) if err != nil { return nil, err } pm.fs = relativeOnlyFS - pm.planOpts = opt - - opt.setGoal(goalMap) - // Build planner - //nolint: gosec - pathPlanner, err := opt.PlannerConstructor( - pm.fs, - rand.New(rand.NewSource(int64(pm.randseed.Int()))), - pm.logger, - opt, - ) + wps, err := pm.generateWaypoints(request, seedPlan, 0) if err != nil { return nil, err } + // Should never happen, but checking to guard against future changes breaking this + if len(wps) != 1 { + return nil, fmt.Errorf("tpspace should only generate exactly one atomic waypoint, but got %d", len(wps)) + } + wp := wps[0] + zeroInputs := map[string][]referenceframe.Input{} - zeroInputs[pm.ptgFrameName] = make([]referenceframe.Input, len(pm.fs.Frame(pm.ptgFrameName).DoF())) + zeroInputs[opt.ptgFrameName] = make([]referenceframe.Input, len(pm.fs.Frame(opt.ptgFrameName).DoF())) maps := &rrtMaps{} if seedPlan != nil { // TODO: This probably needs to be flipped? Check if these paths are ever used. - maps, err = pm.planToRRTGoalMap(seedPlan, goalMap) + maps, err = pm.planToRRTGoalMap(seedPlan, wp) if err != nil { return nil, err } } - if pm.opt().PositionSeeds > 0 && pm.opt().profile == PositionOnlyMotionProfile { - err = maps.fillPosOnlyGoal(goalMap, pm.opt().PositionSeeds) + if opt.PositionSeeds > 0 && opt.profile == PositionOnlyMotionProfile { + err = maps.fillPosOnlyGoal(wp.goalState.poses, opt.PositionSeeds) if err != nil { return nil, err } } else { + goalPose := wp.goalState.poses[opt.ptgFrameName].Pose() goalMapFlip := map[string]*referenceframe.PoseInFrame{ - pm.ptgFrameName: referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.Compose(goalPose, flipPose)), + opt.ptgFrameName: referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.Compose(goalPose, flipPose)), } goalNode := &basicNode{q: zeroInputs, poses: goalMapFlip} maps.goalMap = map[node]node{goalNode: nil} } - startNode := &basicNode{q: zeroInputs, poses: startMap} + startNode := &basicNode{q: zeroInputs, poses: request.StartState.poses} maps.startMap = map[node]node{startNode: nil} // Plan the single waypoint, and accumulate objects which will be used to constrauct the plan after all planning has finished - _, future, err := pm.planSingleAtomicWaypoint(ctx, goalMap, zeroInputs, pathPlanner, maps) + _, future, err := pm.planSingleAtomicWaypoint(ctx, wp, maps) if err != nil { return nil, err } @@ -945,7 +974,7 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe return nil, err } - return newRRTPlan(steps, pm.fs, pm.useTPspace, startPose) + return newRRTPlan(steps, pm.fs, opt.useTPspace, startPose) } // Copy any atomic values. @@ -964,3 +993,34 @@ func nodesToTrajectory(nodes []node) Trajectory { } return traj } + +// Determines whether to break a motion down into sub-waypoints if all intermediate points are known. +func useSubWaypoints(request *PlanRequest, seedPlan Plan, wpi int) bool { + // If we are seeding off of a pre-existing plan, we don't need the speedup of subwaypoints + if seedPlan != nil { + return false + } + // If goal has a configuration, do not use subwaypoints *unless* the start state is also a configuration. + // We can interpolate from a pose or configuration to a pose, or a configuration to a configuration, but not from a pose to a + // configuration. + // TODO: If we run planning backwards, we could remove this restriction. + if request.Goals[wpi].configuration != nil { + startState := request.StartState + if wpi > 0 { + startState = request.Goals[wpi-1] + } + if startState.configuration == nil { + return false + } + } + + // linear motion profile has known intermediate points, so solving can be broken up and sped up + if profile, ok := request.Options["motion_profile"]; ok && profile == LinearMotionProfile { + return true + } + + if len(request.Constraints.GetLinearConstraint()) > 0 { + return true + } + return false +} diff --git a/motionplan/plan_test.go b/motionplan/plan_test.go index 45bf0b693cc..7c10b8a589e 100644 --- a/motionplan/plan_test.go +++ b/motionplan/plan_test.go @@ -60,16 +60,16 @@ func TestPlanStep(t *testing.T) { baseNameB: {Pose: spatialmath.PoseToProtobuf(poseB)}, }, } - stepAB := PathStep{ + stepAB := PathState{ baseNameA: referenceframe.NewPoseInFrame(referenceframe.World, poseA), baseNameB: referenceframe.NewPoseInFrame(referenceframe.World, poseB), } - t.Run("pathStepFromProto", func(t *testing.T) { + t.Run("pathStateFromProto", func(t *testing.T) { type testCase struct { description string input *pb.PlanStep - result PathStep + result PathState err error } @@ -77,23 +77,23 @@ func TestPlanStep(t *testing.T) { { description: "nil pointer returns an error", input: nil, - result: PathStep{}, + result: PathState{}, err: errors.New("received nil *pb.PlanStep"), }, { - description: "an empty *pb.PlanStep returns an empty PathStep{}", + description: "an empty *pb.PlanStep returns an empty PathState{}", input: &pb.PlanStep{}, - result: PathStep{}, + result: PathState{}, }, { - description: "a full *pb.PlanStep returns an full PathStep{}", + description: "a full *pb.PlanStep returns an full PathState{}", input: protoAB, result: stepAB, }, } for _, tc := range testCases { t.Run(tc.description, func(t *testing.T) { - res, err := PathStepFromProto(tc.input) + res, err := PathStateFromProto(tc.input) if tc.err != nil { test.That(t, err, test.ShouldBeError, tc.err) } else { @@ -107,23 +107,23 @@ func TestPlanStep(t *testing.T) { t.Run("ToProto()", func(t *testing.T) { type testCase struct { description string - input PathStep + input PathState result *pb.PlanStep } testCases := []testCase{ { - description: "an nil PathStep returns an empty *pb.PlanStep", + description: "an nil PathState returns an empty *pb.PlanStep", input: nil, result: &pb.PlanStep{}, }, { - description: "an empty PathStep returns an empty *pb.PlanStep", - input: PathStep{}, + description: "an empty PathState returns an empty *pb.PlanStep", + input: PathState{}, result: &pb.PlanStep{}, }, { - description: "a full PathStep{} returns an full *pb.PlanStep", + description: "a full PathState{} returns an full *pb.PlanStep", input: stepAB, result: protoAB, }, @@ -151,12 +151,13 @@ func TestNewGeoPlan(t *testing.T) { goal := spatialmath.NewPoseFromPoint(r3.Vector{X: 1000, Y: 8000, Z: 0}) plan, err := Replan(context.Background(), &PlanRequest{ - Logger: logging.NewTestLogger(t), - StartPose: spatialmath.NewZeroPose(), - Goal: referenceframe.NewPoseInFrame(referenceframe.World, goal), - Frame: kinematicFrame, - FrameSystem: baseFS, - StartConfiguration: referenceframe.StartPositions(baseFS), + Logger: logging.NewTestLogger(t), + StartState: &PlanState{ + poses: PathState{kinematicFrame.Name(): referenceframe.NewZeroPoseInFrame(referenceframe.World)}, + configuration: referenceframe.StartPositions(baseFS), + }, + Goals: []*PlanState{{poses: PathState{kinematicFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goal)}}}, + FrameSystem: baseFS, }, nil, math.NaN()) test.That(t, err, test.ShouldBeNil) diff --git a/motionplan/plannerOptions.go b/motionplan/plannerOptions.go index 1f9e21518ba..342b3f892fe 100644 --- a/motionplan/plannerOptions.go +++ b/motionplan/plannerOptions.go @@ -64,7 +64,7 @@ const ( defaultRobotCollisionConstraintDesc = "Collision between a robot component that is moving and one that is stationary" // When breaking down a path into smaller waypoints, add a waypoint every this many mm of movement. - defaultPathStepSize = 10 + defaultPathStateSize = 10 // This is commented out due to Go compiler bug. See comment in newBasicPlannerOptions for explanation. // var defaultPlanner = newCBiRRTMotionPlanner. @@ -91,7 +91,6 @@ func newBasicPlannerOptions() *plannerOptions { opt.nodeDistanceFunc = nodeConfigurationDistanceFunc opt.scoreFunc = ik.FSConfigurationL2Distance opt.pathMetric = ik.NewZeroFSMetric() // 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. // TODO: RSDK-6079 this should be properly used, and deduplicated with defaultEpsilon, InputIdentDist, etc. opt.GoalThreshold = 0.1 @@ -122,11 +121,14 @@ func newBasicPlannerOptions() *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 - motionChains []*motionChain + motionChains []*motionChain + + // This is used to create functions which are passed to IK for solving. This may be used to turn starting or ending state poses into + // configurations for nodes. goalMetricConstructor func(spatialmath.Pose) ik.StateMetric - goalMetric ik.StateFSMetric // Distance function which converges to the final goal position - pathMetric ik.StateFSMetric // Distance function which converges on the valid manifold of intermediate path states - nodeDistanceFunc func(node, node) float64 // Node distance function used for nearest neighbor + + pathMetric ik.StateFSMetric // Distance function which converges on the valid manifold of intermediate path states + nodeDistanceFunc func(node, node) float64 // Node distance function used for nearest neighbor extra map[string]interface{} @@ -170,8 +172,6 @@ type plannerOptions struct { // Number of seeds to pre-generate for bidirectional position-only solving. PositionSeeds int `json:"position_seeds"` - startPoses PathStep // The starting poses of the plan. Useful when planning for frames with relative inputs. - // poseDistanceFunc is the function that the planner will use to measure the degree of "closeness" between two poses poseDistanceFunc ik.SegmentMetric @@ -188,18 +188,17 @@ type plannerOptions struct { Fallback *plannerOptions - // relativeInputs is a flag that is set by the planning algorithm describing if the solutions it generates are - // relative as in each step in the solution builds off a previous one, as opposed to being asolute with respect to some reference frame. - relativeInputs bool + useTPspace bool + ptgFrameName string } -// setGoal sets the distance metric for the solver. -func (p *plannerOptions) setGoal(goal PathStep) { +// getGoalMetric creates the distance metric for the solver using the configured options. +func (p *plannerOptions) getGoalMetric(goal PathState) ik.StateFSMetric { metrics := map[string]ik.StateMetric{} for frame, goalInFrame := range goal { metrics[frame] = p.goalMetricConstructor(goalInFrame.Pose()) } - goalMetricFS := func(state *ik.StateFS) float64 { + return func(state *ik.StateFS) float64 { score := 0. for frame, goalMetric := range metrics { poseParent := goal[frame].Parent() @@ -215,7 +214,6 @@ func (p *plannerOptions) setGoal(goal PathStep) { } return score } - p.goalMetric = goalMetricFS } // SetPathDist sets the distance metric for the solver to move a constraint-violating point into a valid manifold. @@ -238,7 +236,7 @@ func (p *plannerOptions) SetMinScore(minScore float64) { func (p *plannerOptions) addTopoConstraints( fs referenceframe.FrameSystem, startCfg map[string][]referenceframe.Input, - from, to PathStep, + from, to PathState, constraints *Constraints, ) (bool, error) { topoConstraints := false @@ -273,7 +271,7 @@ func (p *plannerOptions) addTopoConstraints( func (p *plannerOptions) addLinearConstraints( fs referenceframe.FrameSystem, startCfg map[string][]referenceframe.Input, - from, to PathStep, + from, to PathState, linConstraint LinearConstraint, ) error { // Linear constraints @@ -299,7 +297,7 @@ func (p *plannerOptions) addLinearConstraints( func (p *plannerOptions) addPseudolinearConstraints( fs referenceframe.FrameSystem, startCfg map[string][]referenceframe.Input, - from, to PathStep, + from, to PathState, plinConstraint PseudolinearConstraint, ) error { // Linear constraints @@ -325,7 +323,7 @@ func (p *plannerOptions) addPseudolinearConstraints( func (p *plannerOptions) addOrientationConstraints( fs referenceframe.FrameSystem, startCfg map[string][]referenceframe.Input, - from, to PathStep, + from, to PathState, orientConstraint OrientationConstraint, ) error { orientTol := orientConstraint.OrientationToleranceDegs @@ -341,11 +339,18 @@ func (p *plannerOptions) addOrientationConstraints( return nil } -func (p *plannerOptions) fillMotionChains(fs referenceframe.FrameSystem, to PathStep) error { - motionChains := make([]*motionChain, 0, len(to)) +func (p *plannerOptions) fillMotionChains(fs referenceframe.FrameSystem, to *PlanState) error { + motionChains := make([]*motionChain, 0, len(to.poses)+len(to.configuration)) - for frame, goal := range to { - chain, err := motionChainFromGoal(fs, frame, goal) + for frame, pif := range to.poses { + chain, err := motionChainFromGoal(fs, frame, pif.Parent()) + if err != nil { + return err + } + motionChains = append(motionChains, chain) + } + for frame := range to.configuration { + chain, err := motionChainFromGoal(fs, frame, frame) if err != nil { return err } diff --git a/motionplan/rrt.go b/motionplan/rrt.go index ddb519c509b..e8feb083112 100644 --- a/motionplan/rrt.go +++ b/motionplan/rrt.go @@ -5,7 +5,6 @@ package motionplan import ( "context" "errors" - "math" "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" @@ -28,7 +27,7 @@ const ( type rrtParallelPlanner interface { motionPlanner - rrtBackgroundRunner(context.Context, map[string][]referenceframe.Input, *rrtParallelPlannerShared) + rrtBackgroundRunner(context.Context, *rrtParallelPlannerShared) } type rrtParallelPlannerShared struct { @@ -51,7 +50,7 @@ type rrtMaps struct { optNode node // The highest quality IK solution } -func (maps *rrtMaps) fillPosOnlyGoal(goal PathStep, posSeeds int) error { +func (maps *rrtMaps) fillPosOnlyGoal(goal PathState, posSeeds int) error { thetaStep := 360. / float64(posSeeds) if maps == nil { return errors.New("cannot call method fillPosOnlyGoal on nil maps") @@ -60,7 +59,7 @@ func (maps *rrtMaps) fillPosOnlyGoal(goal PathStep, posSeeds int) error { maps.goalMap = map[node]node{} } for i := 0; i < posSeeds; i++ { - newMap := PathStep{} + newMap := PathState{} for frame, goal := range goal { newMap[frame] = referenceframe.NewPoseInFrame( frame, @@ -79,44 +78,53 @@ func (maps *rrtMaps) fillPosOnlyGoal(goal PathStep, posSeeds int) error { // initRRTsolutions will create the maps to be used by a RRT-based algorithm. It will generate IK solutions to pre-populate the goal // map, and will check if any of those goals are able to be directly interpolated to. -func initRRTSolutions(ctx context.Context, mp motionPlanner, seed map[string][]referenceframe.Input) *rrtSolution { +// If the waypoint specifies poses for start or goal, IK will be run to create configurations. +func initRRTSolutions(ctx context.Context, wp atomicWaypoint) *rrtSolution { rrt := &rrtSolution{ maps: &rrtMaps{ startMap: map[node]node{}, goalMap: map[node]node{}, }, } - seedNode := &basicNode{q: seed, cost: 0} - rrt.maps.startMap[seedNode] = nil - // get many potential end goals from IK solver - solutions, err := mp.getSolutions(ctx, seed) + startNodes, err := generateNodeListForPlanState(ctx, wp.mp, wp.startState, wp.goalState.configuration) + if err != nil { + rrt.err = err + return rrt + } + goalNodes, err := generateNodeListForPlanState(ctx, wp.mp, wp.goalState, wp.startState.configuration) if err != nil { rrt.err = err return rrt } // the smallest interpolated distance between the start and end input represents a lower bound on cost - optimalCost := mp.opt().configurationDistanceFunc(&ik.SegmentFS{StartConfiguration: seed, EndConfiguration: solutions[0].Q()}) - rrt.maps.optNode = &basicNode{q: solutions[0].Q(), cost: optimalCost} + optimalCost := wp.mp.opt().configurationDistanceFunc(&ik.SegmentFS{ + StartConfiguration: startNodes[0].Q(), + EndConfiguration: goalNodes[0].Q(), + }) + rrt.maps.optNode = &basicNode{q: goalNodes[0].Q(), cost: optimalCost} // Check for direct interpolation for the subset of IK solutions within some multiple of optimal // Since solutions are returned ordered, we check until one is out of bounds, then skip remaining checks canInterp := true // initialize maps and check whether direct interpolation is an option - for _, solution := range solutions { - if canInterp { - cost := mp.opt().configurationDistanceFunc(&ik.SegmentFS{StartConfiguration: seed, EndConfiguration: solution.Q()}) - if cost < optimalCost*defaultOptimalityMultiple { - if mp.checkPath(seed, solution.Q()) { - rrt.steps = []node{seedNode, solution} - return rrt + for _, seed := range startNodes { + for _, solution := range goalNodes { + if canInterp { + cost := wp.mp.opt().configurationDistanceFunc(&ik.SegmentFS{StartConfiguration: seed.Q(), EndConfiguration: solution.Q()}) + if cost < optimalCost*defaultOptimalityMultiple { + if wp.mp.checkPath(seed.Q(), solution.Q()) { + rrt.steps = []node{seed, solution} + return rrt + } + } else { + canInterp = false } - } else { - canInterp = false } + rrt.maps.goalMap[&basicNode{q: solution.Q(), cost: 0}] = nil } - rrt.maps.goalMap[&basicNode{q: solution.Q(), cost: 0}] = nil + rrt.maps.startMap[&basicNode{q: seed.Q(), cost: 0}] = nil } return rrt } @@ -136,145 +144,6 @@ func shortestPath(maps *rrtMaps, nodePairs []*nodePair) *rrtSolution { return &rrtSolution{steps: extractPath(maps.startMap, maps.goalMap, nodePairs[minIdx], true), maps: maps} } -// fixedStepInterpolation returns inputs at qstep distance along the path from start to target -// if start and target have the same Input value, then no step increment is made. -func fixedStepInterpolation(start, target node, qstep map[string][]float64) map[string][]referenceframe.Input { - newNear := make(map[string][]referenceframe.Input) - - // Iterate through each frame's inputs - for frameName, startInputs := range start.Q() { - // As this is constructed in-algorithm from already-near nodes, this is guaranteed to always exist - targetInputs := target.Q()[frameName] - frameSteps := make([]referenceframe.Input, len(startInputs)) - - for j, nearInput := range startInputs { - if nearInput.Value == targetInputs[j].Value { - frameSteps[j] = nearInput - } else { - v1, v2 := nearInput.Value, targetInputs[j].Value - newVal := math.Min(qstep[frameName][j], math.Abs(v2-v1)) - // get correct sign - newVal *= (v2 - v1) / math.Abs(v2-v1) - frameSteps[j] = referenceframe.Input{Value: nearInput.Value + newVal} - } - } - newNear[frameName] = frameSteps - } - return newNear -} - -// node interface is used to wrap a configuration for planning purposes. -// TODO: This is somewhat redundant with a State. -type node interface { - // return the configuration associated with the node - Q() map[string][]referenceframe.Input - Cost() float64 - SetCost(float64) - Poses() PathStep - Corner() bool - SetCorner(bool) -} - -type basicNode struct { - q map[string][]referenceframe.Input - cost float64 - poses PathStep - corner bool -} - -// Special case constructors for nodes without costs to return NaN. -func newConfigurationNode(q map[string][]referenceframe.Input) node { - return &basicNode{ - q: q, - cost: math.NaN(), - corner: false, - } -} - -func (n *basicNode) Q() map[string][]referenceframe.Input { - return n.q -} - -func (n *basicNode) Cost() float64 { - return n.cost -} - -func (n *basicNode) SetCost(cost float64) { - n.cost = cost -} - -func (n *basicNode) Poses() PathStep { - return n.poses -} - -func (n *basicNode) Corner() bool { - return n.corner -} - -func (n *basicNode) SetCorner(corner bool) { - n.corner = corner -} - -// nodePair groups together nodes in a tuple -// TODO(rb): in the future we might think about making this into a list of nodes. -type nodePair struct{ a, b node } - -func (np *nodePair) sumCosts() float64 { - aCost := np.a.Cost() - if math.IsNaN(aCost) { - return 0 - } - bCost := np.b.Cost() - if math.IsNaN(bCost) { - return 0 - } - return aCost + bCost -} - -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 { - startReached, goalReached = pair.a, pair.b - } else { - startReached, goalReached = pair.b, pair.a - } - - // extract the path to the seed - path := make([]node, 0) - for startReached != nil { - path = append(path, startReached) - startReached = startMap[startReached] - } - - // reverse the slice - for i, j := 0, len(path)-1; i < j; i, j = i+1, j-1 { - path[i], path[j] = path[j], path[i] - } - - if goalReached != nil { - 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 { - path = append(path, goalReached) - goalReached = goalMap[goalReached] - } - } - return path -} - -func sumCosts(path []node) float64 { - cost := 0. - for _, wp := range path { - cost += wp.Cost() - } - return cost -} - type rrtPlan struct { SimplePlan diff --git a/motionplan/rrtStarConnect.go b/motionplan/rrtStarConnect.go index 0fdbd799a36..5681f8b5f62 100644 --- a/motionplan/rrtStarConnect.go +++ b/motionplan/rrtStarConnect.go @@ -5,6 +5,7 @@ package motionplan import ( "context" "encoding/json" + "errors" "math/rand" "time" @@ -83,27 +84,32 @@ func newRRTStarConnectMotionPlanner( return &rrtStarConnectMotionPlanner{mp, algOpts}, nil } -func (mp *rrtStarConnectMotionPlanner) plan(ctx context.Context, goal PathStep, seed map[string][]referenceframe.Input) ([]node, error) { - mp.planOpts.setGoal(goal) +func (mp *rrtStarConnectMotionPlanner) plan(ctx context.Context, seed, goal *PlanState) ([]node, error) { solutionChan := make(chan *rrtSolution, 1) + initMaps := initRRTSolutions(ctx, atomicWaypoint{mp: mp, startState: seed, goalState: goal}) + if initMaps.err != nil { + return nil, initMaps.err + } + if initMaps.steps != nil { + return initMaps.steps, nil + } utils.PanicCapturingGo(func() { - mp.rrtBackgroundRunner(ctx, seed, &rrtParallelPlannerShared{nil, nil, solutionChan}) + mp.rrtBackgroundRunner(ctx, &rrtParallelPlannerShared{ + initMaps.maps, + nil, + solutionChan, + }) }) - select { - case <-ctx.Done(): - return nil, ctx.Err() - case solution := <-solutionChan: - if solution.err != nil { - return nil, solution.err - } - return solution.steps, nil + solution := <-solutionChan + if solution.err != nil { + return nil, solution.err } + return solution.steps, nil } // rrtBackgroundRunner will execute the plan. Plan() will call rrtBackgroundRunner in a separate thread and wait for results. // Separating this allows other things to call rrtBackgroundRunner in parallel allowing the thread-agnostic Plan to be accessible. func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, - seed map[string][]referenceframe.Input, rrt *rrtParallelPlannerShared, ) { mp.logger.CDebug(ctx, "Starting RRT*") @@ -117,13 +123,15 @@ func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, mp.start = time.Now() - if rrt.maps == nil || len(rrt.maps.goalMap) == 0 { - planSeed := initRRTSolutions(ctx, mp, seed) - if planSeed.err != nil || planSeed.steps != nil { - rrt.solutionChan <- planSeed - return + if rrt.maps == nil || len(rrt.maps.goalMap) == 0 || len(rrt.maps.startMap) == 0 { + rrt.solutionChan <- &rrtSolution{err: errors.New("cannot run RRT background runner without prepopulated maps")} + } + var seed map[string][]referenceframe.Input + // Pick a random seed node to create the first interp node + for sNode, parent := range rrt.maps.startMap { + if parent == nil { + seed = sNode.Q() } - rrt.maps = planSeed.maps } targetConf, err := referenceframe.InterpolateFS(mp.fs, seed, rrt.maps.optNode.Q(), 0.5) if err != nil { diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index b69d08f4846..0dbfeecd43e 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -156,41 +156,27 @@ func newTPSpaceMotionPlanner( return tpPlanner, nil } -// TODO: seed is not immediately useful for TP-space. -func (mp *tpSpaceRRTMotionPlanner) plan(ctx context.Context, goal PathStep, seed map[string][]referenceframe.Input) ([]node, error) { - mp.planOpts.setGoal(goal) +func (mp *tpSpaceRRTMotionPlanner) plan(ctx context.Context, seed, goal *PlanState) ([]node, error) { + zeroInputs := map[string][]referenceframe.Input{} + zeroInputs[mp.tpFrame.Name()] = make([]referenceframe.Input, len(mp.tpFrame.DoF())) solutionChan := make(chan *rrtSolution, 1) - seedPos := mp.opt().startPoses + maps := &rrtMaps{startMap: map[node]node{}, goalMap: map[node]node{}} - startNode := &basicNode{q: make(map[string][]referenceframe.Input, len(mp.lfs.dof)), cost: 0, poses: seedPos, corner: false} - maps := &rrtMaps{startMap: map[node]node{startNode: nil}} - if mp.opt().PositionSeeds > 0 && mp.opt().profile == PositionOnlyMotionProfile { - err := maps.fillPosOnlyGoal(goal, mp.opt().PositionSeeds) - if err != nil { - return nil, err - } - } else { - goalNode := &basicNode{ - q: make(map[string][]referenceframe.Input), - cost: 0, - poses: PathStep{ - mp.tpFrame.Name(): referenceframe.NewPoseInFrame( - referenceframe.World, - spatialmath.Compose(goal[mp.tpFrame.Name()].Pose(), flipPose), - ), - }, - corner: false, - } - maps.goalMap = map[node]node{goalNode: nil} + startNode := &basicNode{q: zeroInputs, poses: PathState{mp.tpFrame.Name(): referenceframe.NewZeroPoseInFrame(referenceframe.World)}} + if seed != nil { + startNode = &basicNode{q: zeroInputs, poses: seed.poses} } + maps.startMap = map[node]node{startNode: nil} + goalNode := &basicNode{q: zeroInputs, poses: goal.poses} + maps.goalMap = map[node]node{flipNodePoses(goalNode): nil} var planRunners sync.WaitGroup planRunners.Add(1) utils.PanicCapturingGo(func() { defer planRunners.Done() - mp.rrtBackgroundRunner(ctx, seed, &rrtParallelPlannerShared{maps, nil, solutionChan}) + mp.rrtBackgroundRunner(ctx, &rrtParallelPlannerShared{maps, nil, solutionChan}) }) select { case <-ctx.Done(): @@ -204,11 +190,11 @@ func (mp *tpSpaceRRTMotionPlanner) plan(ctx context.Context, goal PathStep, seed } } -func (mp *tpSpaceRRTMotionPlanner) poseToPathStep(pose spatialmath.Pose) PathStep { - return PathStep{mp.tpFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, pose)} +func (mp *tpSpaceRRTMotionPlanner) poseToPathState(pose spatialmath.Pose) PathState { + return PathState{mp.tpFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, pose)} } -func (mp *tpSpaceRRTMotionPlanner) pathStepToPose(step PathStep) spatialmath.Pose { +func (mp *tpSpaceRRTMotionPlanner) pathStateToPose(step PathState) spatialmath.Pose { return step[mp.tpFrame.Name()].Pose() } @@ -216,12 +202,11 @@ func (mp *tpSpaceRRTMotionPlanner) pathStepToPose(step PathStep) spatialmath.Pos // Separating this allows other things to call planRunner in parallel allowing the thread-agnostic Plan to be accessible. func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner( ctx context.Context, - _ map[string][]referenceframe.Input, // TODO: this may be needed for smoothing rrt *rrtParallelPlannerShared, ) { defer close(rrt.solutionChan) // get start and goal poses - var startPoses PathStep + var startPoses PathState var goalPose spatialmath.Pose var goalNode node @@ -319,12 +304,12 @@ func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner( midPtNormalized := midPt.Sub(startPose.Point()) midOrient := &spatialmath.OrientationVector{OZ: 1, Theta: math.Atan2(-midPtNormalized.X, midPtNormalized.Y)} - midptNode := &basicNode{poses: mp.poseToPathStep(spatialmath.NewPose(midPt, midOrient)), cost: midPt.Sub(startPose.Point()).Norm()} + midptNode := &basicNode{poses: mp.poseToPathState(spatialmath.NewPose(midPt, midOrient)), cost: midPt.Sub(startPose.Point()).Norm()} var randPosNode node = midptNode for iter := 0; iter < mp.planOpts.PlanIter; iter++ { if pathdebug { - randPose := mp.pathStepToPose(randPosNode.Poses()) + randPose := mp.pathStateToPose(randPosNode.Poses()) mp.logger.Debugf("$RRTGOAL,%f,%f", randPose.Point().X, randPose.Point().Y) } mp.logger.CDebugf(ctx, "TP Space RRT iteration %d", iter) @@ -352,8 +337,8 @@ func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner( if seedReached.node != nil && goalReached.node != nil { // Flip the orientation of the goal node for distance calculation and seed extension reachedDelta = mp.planOpts.poseDistanceFunc(&ik.Segment{ - StartPosition: mp.pathStepToPose(seedReached.node.Poses()), - EndPosition: mp.pathStepToPose(flipNodePoses(goalReached.node).Poses()), + StartPosition: mp.pathStateToPose(seedReached.node.Poses()), + EndPosition: mp.pathStateToPose(flipNodePoses(goalReached.node).Poses()), }) if reachedDelta > mp.planOpts.GoalThreshold { // If both maps extended, but did not reach the same point, then attempt to extend them towards each other @@ -364,8 +349,8 @@ func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner( } if seedReached.node != nil { reachedDelta = mp.planOpts.poseDistanceFunc(&ik.Segment{ - StartPosition: mp.pathStepToPose(seedReached.node.Poses()), - EndPosition: mp.pathStepToPose(flipNodePoses(goalReached.node).Poses()), + StartPosition: mp.pathStateToPose(seedReached.node.Poses()), + EndPosition: mp.pathStateToPose(flipNodePoses(goalReached.node).Poses()), }) if reachedDelta > mp.planOpts.GoalThreshold { goalReached = mp.attemptExtension(ctx, flipNodePoses(seedReached.node), rrt.maps.goalMap, true) @@ -376,8 +361,8 @@ func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner( } if goalReached.node != nil { reachedDelta = mp.planOpts.poseDistanceFunc(&ik.Segment{ - StartPosition: mp.pathStepToPose(seedReached.node.Poses()), - EndPosition: mp.pathStepToPose(flipNodePoses(goalReached.node).Poses()), + StartPosition: mp.pathStateToPose(seedReached.node.Poses()), + EndPosition: mp.pathStateToPose(flipNodePoses(goalReached.node).Poses()), }) } } @@ -430,8 +415,8 @@ func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner( continue } reachedDelta = mp.planOpts.poseDistanceFunc(&ik.Segment{ - StartPosition: mp.pathStepToPose(seedReached.node.Poses()), - EndPosition: mp.pathStepToPose(flipNodePoses(goalMapNode).Poses()), + StartPosition: mp.pathStateToPose(seedReached.node.Poses()), + EndPosition: mp.pathStateToPose(flipNodePoses(goalMapNode).Poses()), }) if reachedDelta <= mp.planOpts.GoalThreshold { // If we've reached the goal, extract the path from the RRT trees and return @@ -496,7 +481,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( return nil, errNoNeighbors } - rawVal, ok := distMap.Load(mp.pathStepToPose(nearest.Poses())) + rawVal, ok := distMap.Load(mp.pathStateToPose(nearest.Poses())) if !ok { mp.logger.Error("nearest neighbor failed to find nearest pose in distMap") return nil, errNoNeighbors @@ -508,14 +493,14 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( return nil, errNoNeighbors } } else { - solution, err = mp.ptgSolution(curPtg, mp.pathStepToPose(nearest.Poses()), mp.pathStepToPose(randPosNode.Poses())) + solution, err = mp.ptgSolution(curPtg, mp.pathStateToPose(nearest.Poses()), mp.pathStateToPose(randPosNode.Poses())) if err != nil || solution == nil { return nil, err } } // Get cartesian distance from NN to rand - arcStartPose := mp.pathStepToPose(nearest.Poses()) + arcStartPose := mp.pathStateToPose(nearest.Poses()) successNodes := []node{} arcPose := spatialmath.NewZeroPose() // This will be the relative pose that is the delta from one end of the combined traj to the other. @@ -549,17 +534,17 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( } } - arcPose = spatialmath.Compose(arcPose, mp.pathStepToPose(goodNode.Poses())) + arcPose = spatialmath.Compose(arcPose, mp.pathStateToPose(goodNode.Poses())) // add the last node in trajectory - arcStartPose = spatialmath.Compose(arcStartPose, mp.pathStepToPose(goodNode.Poses())) + arcStartPose = spatialmath.Compose(arcStartPose, mp.pathStateToPose(goodNode.Poses())) successNode = &basicNode{ q: map[string][]referenceframe.Input{ mp.tpFrame.Name(): {{float64(ptgNum)}, goodNode.Q()[mp.tpFrame.Name()][0], {0}, goodNode.Q()[mp.tpFrame.Name()][1]}, }, cost: goodNode.Cost(), - poses: mp.poseToPathStep(arcStartPose), + poses: mp.poseToPathState(arcStartPose), corner: false, } successNodes = append(successNodes, successNode) @@ -572,7 +557,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( return nil, errInvalidCandidate } - targetFunc := defaultGoalMetricConstructor(spatialmath.PoseBetween(arcStartPose, mp.pathStepToPose(randPosNode.Poses()))) + targetFunc := defaultGoalMetricConstructor(spatialmath.PoseBetween(arcStartPose, mp.pathStateToPose(randPosNode.Poses()))) bestDist := targetFunc(&ik.State{Position: arcPose}) cand := &candidate{dist: bestDist, treeNode: nearest, newNodes: successNodes} @@ -582,8 +567,8 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( nearest = nm.nearestNeighbor(ctx, mp.planOpts, successNode, rrt) if nearest != nil { dist := mp.planOpts.poseDistanceFunc(&ik.Segment{ - StartPosition: mp.pathStepToPose(successNode.Poses()), - EndPosition: mp.pathStepToPose(nearest.Poses()), + StartPosition: mp.pathStateToPose(successNode.Poses()), + EndPosition: mp.pathStateToPose(nearest.Poses()), }) // If too close, don't add a new node if dist < mp.algOpts.identicalNodeDistance { @@ -626,7 +611,7 @@ func (mp *tpSpaceRRTMotionPlanner) checkTraj(trajK []*tpspace.TrajNode, arcStart mp.tpFrame.Name(): {{trajPt.Alpha}, {trajPt.Dist}}, }, cost: trajPt.Dist, - poses: mp.poseToPathStep(trajPt.Pose), + poses: mp.poseToPathState(trajPt.Pose), corner: false, } passed = append(passed, okNode) @@ -717,7 +702,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( for _, newNode := range reseedCandidate.newNodes { distTravelledByCandidate += math.Abs(newNode.Q()[mp.tpFrame.Name()][3].Value - newNode.Q()[mp.tpFrame.Name()][2].Value) } - distToGoal := mp.pathStepToPose(endNode.Poses()).Point().Distance(mp.pathStepToPose(goalNode.Poses()).Point()) + distToGoal := mp.pathStateToPose(endNode.Poses()).Point().Distance(mp.pathStateToPose(goalNode.Poses()).Point()) if distToGoal < mp.planOpts.GoalThreshold || lastIteration { // Reached the goal position, or otherwise failed to fully extend to the end of a trajectory return &nodeAndError{endNode, nil} @@ -782,7 +767,7 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( if err != nil { return nil, err } - arcStartPose := mp.pathStepToPose(treeNode.Poses()) + arcStartPose := mp.pathStateToPose(treeNode.Poses()) lastDist := 0. sinceLastNode := 0. @@ -813,7 +798,7 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( mp.tpFrame.Name(): referenceframe.FloatsToInputs([]float64{float64(ptgNum), randAlpha, 0, trajPt.Dist}), }, cost: trajPt.Dist, - poses: mp.poseToPathStep(trajState.Position), + poses: mp.poseToPathState(trajState.Position), corner: false, } rrt[addedNode] = treeNode @@ -894,8 +879,8 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTGS opts.poseDistanceFunc = segMetric opts.nodeDistanceFunc = func(node1, node2 node) float64 { return segMetric(&ik.Segment{ - StartPosition: mp.pathStepToPose(node1.Poses()), - EndPosition: mp.pathStepToPose(node2.Poses()), + StartPosition: mp.pathStateToPose(node1.Poses()), + EndPosition: mp.pathStateToPose(node2.Poses()), }) } opts.Resolution = defaultPTGCollisionResolution @@ -950,7 +935,7 @@ func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) if pathdebug { allPtgs := mp.solvers - lastPose := mp.pathStepToPose(path[0].Poses()) + lastPose := mp.pathStateToPose(path[0].Poses()) for _, mynode := range path { trajPts, err := allPtgs[int(mynode.Q()[mp.tpFrame.Name()][0].Value)].Trajectory( mynode.Q()[mp.tpFrame.Name()][1].Value, @@ -987,7 +972,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( ) ([]node, error) { startMap := map[node]node{} var parent node - parentPose := mp.pathStepToPose(path[0].Poses()) + parentPose := mp.pathStateToPose(path[0].Poses()) for j := 0; j <= firstEdge; j++ { pathNode := path[j] @@ -1011,13 +996,13 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( intNode := &basicNode{ q: newQ, cost: pathNode.Cost() - math.Abs(adj), - poses: mp.poseToPathStep(spatialmath.Compose(parentPose, trajK[len(trajK)-1].Pose)), + poses: mp.poseToPathState(spatialmath.Compose(parentPose, trajK[len(trajK)-1].Pose)), corner: false, } startMap[intNode] = parent } parent = pathNode - parentPose = mp.pathStepToPose(parent.Poses()) + parentPose = mp.pathStateToPose(parent.Poses()) } // TODO: everything below this point can become an invocation of `smoother.planRunner` reached := smoother.attemptExtension(ctx, path[secondEdge], startMap, false) @@ -1026,8 +1011,8 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( } reachedDelta := mp.planOpts.poseDistanceFunc(&ik.Segment{ - StartPosition: mp.pathStepToPose(reached.node.Poses()), - EndPosition: mp.pathStepToPose(path[secondEdge].Poses()), + StartPosition: mp.pathStateToPose(reached.node.Poses()), + EndPosition: mp.pathStateToPose(path[secondEdge].Poses()), }) // If we tried the goal and have a close-enough XY location, check if the node is good enough to be a final goal @@ -1047,7 +1032,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( // so this step will replace it. newInputSteps = append(newInputSteps, path[len(path)-1]) } - return rectifyTPspacePath(newInputSteps, mp.tpFrame, mp.pathStepToPose(path[0].Poses())) + return rectifyTPspacePath(newInputSteps, mp.tpFrame, mp.pathStateToPose(path[0].Poses())) } func (mp *tpSpaceRRTMotionPlanner) sample(rSeed node, iter int) (node, error) { @@ -1061,13 +1046,13 @@ func (mp *tpSpaceRRTMotionPlanner) sample(rSeed node, iter int) (node, error) { randPosTheta := math.Pi * (mp.randseed.Float64() - 0.5) randPos := spatialmath.NewPose( r3.Vector{ - mp.pathStepToPose(rSeed.Poses()).Point().X + (randPosX - rDist/2.), - mp.pathStepToPose(rSeed.Poses()).Point().Y + (randPosY - rDist/2.), + mp.pathStateToPose(rSeed.Poses()).Point().X + (randPosX - rDist/2.), + mp.pathStateToPose(rSeed.Poses()).Point().Y + (randPosY - rDist/2.), 0, }, &spatialmath.OrientationVector{OZ: 1, Theta: randPosTheta}, ) - return &basicNode{poses: mp.poseToPathStep(randPos)}, nil + return &basicNode{poses: mp.poseToPathState(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 @@ -1088,7 +1073,7 @@ func rectifyTPspacePath(path []node, frame referenceframe.Frame, startPose spati thisNode := &basicNode{ q: wp.Q(), cost: wp.Cost(), - poses: PathStep{frame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, runningPose)}, + poses: PathState{frame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, runningPose)}, corner: wp.Corner(), } correctedPath = append(correctedPath, thisNode) @@ -1140,7 +1125,7 @@ func extractTPspacePath(fName string, startMap, goalMap map[node]node, pair *nod fName: {{0}, {0}, {0}, {0}}, }, cost: goalReached.Cost(), - poses: PathStep{fName: referenceframe.NewPoseInFrame(goalPiF.Parent(), spatialmath.Compose(goalPiF.Pose(), flipPose))}, + poses: PathState{fName: referenceframe.NewPoseInFrame(goalPiF.Parent(), spatialmath.Compose(goalPiF.Pose(), flipPose))}, corner: goalReached.Corner(), } } else { @@ -1154,7 +1139,7 @@ func extractTPspacePath(fName string, startMap, goalMap map[node]node, pair *nod }, }, cost: goalReached.Cost(), - poses: PathStep{fName: referenceframe.NewPoseInFrame(goalPiF.Parent(), spatialmath.Compose(goalPiF.Pose(), flipPose))}, + poses: PathState{fName: referenceframe.NewPoseInFrame(goalPiF.Parent(), spatialmath.Compose(goalPiF.Pose(), flipPose))}, corner: goalReached.Corner(), } } @@ -1167,7 +1152,7 @@ func extractTPspacePath(fName string, startMap, goalMap map[node]node, pair *nod // Returns a new node whose orientation is flipped 180 degrees from the provided node. // It does NOT flip the configurations/inputs. func flipNodePoses(n node) node { - flippedPoses := PathStep{} + flippedPoses := PathState{} for f, pif := range n.Poses() { flippedPoses[f] = referenceframe.NewPoseInFrame(pif.Parent(), spatialmath.Compose(pif.Pose(), flipPose)) } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 2c82d71e6c9..1981033ca29 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -44,20 +44,19 @@ func TestPtgRrtBidirectional(t *testing.T) { fs.AddFrame(ackermanFrame, fs.World()) goalPos := spatialmath.NewPose(r3.Vector{X: 200, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 90}) - goalMap := PathStep{ + goal := &PlanState{poses: PathState{ ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos), - } + }} + start := &PlanState{poses: PathState{ + ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose()), + }} opt := newBasicPlannerOptions() opt.poseDistanceFunc = ik.NewSquaredNormSegmentMetric(30.) opt.scoreFunc = tpspace.NewPTGDistanceMetric([]string{ackermanFrame.Name()}) opt.PlannerConstructor = newTPSpaceMotionPlanner - opt.relativeInputs = true - opt.startPoses = PathStep{ - ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose()), - } - opt.setGoal(goalMap) - opt.fillMotionChains(fs, goalMap) + + opt.fillMotionChains(fs, goal) mp, err := newTPSpaceMotionPlanner(fs, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) @@ -70,7 +69,7 @@ func TestPtgRrtBidirectional(t *testing.T) { tp.logger.Debugf("$SG,%f,%f", goalPos.Point().X, goalPos.Point().Y) } - plan, err := tp.plan(ctx, goalMap, nil) + plan, err := tp.plan(ctx, start, goal) test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThanOrEqualTo, 2) } @@ -102,12 +101,12 @@ func TestPtgWithObstacle(t *testing.T) { startPose := spatialmath.NewPoseFromPoint(r3.Vector{0, -1000, 0}) goalPos := spatialmath.NewPoseFromPoint(r3.Vector{X: 6500, Y: 0, Z: 0}) - startMap := map[string]*referenceframe.PoseInFrame{ + start := &PlanState{poses: map[string]*referenceframe.PoseInFrame{ ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - } - goalMap := map[string]*referenceframe.PoseInFrame{ + }} + goal := &PlanState{poses: map[string]*referenceframe.PoseInFrame{ ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos), - } + }} // Create obstacles obstacle1, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{3300, -500, 0}), r3.Vector{180, 1800, 1}, "") @@ -137,11 +136,8 @@ func TestPtgWithObstacle(t *testing.T) { opt.poseDistanceFunc = ik.NewSquaredNormSegmentMetric(30.) opt.GoalThreshold = 5 opt.PlannerConstructor = newTPSpaceMotionPlanner - opt.relativeInputs = true - opt.startPoses = startMap - opt.setGoal(goalMap) opt.scoreFunc = tpspace.NewPTGDistanceMetric([]string{ackermanFrame.Name()}) - opt.fillMotionChains(fs, goalMap) + opt.fillMotionChains(fs, goal) // Create collision constraints worldGeometries, err := worldState.ObstaclesInWorldFrame(fs, nil) @@ -181,7 +177,7 @@ func TestPtgWithObstacle(t *testing.T) { } // Plan and verify results - plan, err := tp.plan(ctx, goalMap, nil) + plan, err := tp.plan(ctx, start, goal) test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThan, 2) @@ -231,15 +227,14 @@ func TestTPsmoothing(t *testing.T) { opt.poseDistanceFunc = ik.NewSquaredNormSegmentMetric(30.) opt.scoreFunc = tpspace.NewPTGDistanceMetric([]string{ackermanFrame.Name()}) opt.PlannerConstructor = newTPSpaceMotionPlanner - opt.relativeInputs = true // Needed to determine motion chains goalPos := spatialmath.NewPoseFromPoint(r3.Vector{X: 6500, Y: 0, Z: 0}) - goalMap := map[string]*referenceframe.PoseInFrame{ + goal := &PlanState{poses: map[string]*referenceframe.PoseInFrame{ ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos), - } + }} - opt.fillMotionChains(fs, goalMap) + opt.fillMotionChains(fs, goal) // Create and initialize planner mp, err := newTPSpaceMotionPlanner(fs, rand.New(rand.NewSource(42)), logger, opt) diff --git a/motionplan/utils.go b/motionplan/utils.go index c927c2cda88..1ed72f151e4 100644 --- a/motionplan/utils.go +++ b/motionplan/utils.go @@ -13,10 +13,10 @@ import ( "go.viam.com/rdk/utils" ) -// PathStepCount will determine the number of steps which should be used to get from the seed to the goal. +// PathStateCount will determine the number of steps which should be used to get from the seed to the goal. // The returned value is guaranteed to be at least 1. // stepSize represents both the max mm movement per step, and max R4AA degrees per step. -func PathStepCount(seedPos, goalPos spatialmath.Pose, stepSize float64) int { +func PathStateCount(seedPos, goalPos spatialmath.Pose, stepSize float64) int { // use a default size of 1 if zero is passed in to avoid divide-by-zero if stepSize == 0 { stepSize = 1. @@ -126,11 +126,11 @@ type motionChain struct { worldRooted bool } -func motionChainFromGoal(fs referenceframe.FrameSystem, moveFrame string, goal *referenceframe.PoseInFrame) (*motionChain, error) { +func motionChainFromGoal(fs referenceframe.FrameSystem, moveFrame, goalFrameName string) (*motionChain, error) { // get goal frame - goalFrame := fs.Frame(goal.Parent()) + goalFrame := fs.Frame(goalFrameName) if goalFrame == nil { - return nil, referenceframe.NewFrameMissingError(goal.Parent()) + return nil, referenceframe.NewFrameMissingError(goalFrameName) } goalFrameList, err := fs.TracebackFrame(goalFrame) if err != nil { @@ -287,3 +287,32 @@ func uniqInPlaceSlice(s []referenceframe.Frame) []referenceframe.Frame { func nodeConfigurationDistanceFunc(node1, node2 node) float64 { return ik.FSConfigurationL2Distance(&ik.SegmentFS{StartConfiguration: node1.Q(), EndConfiguration: node2.Q()}) } + +// If a motion chain is worldrooted, then goals are translated to their position in `World` before solving. +// This is useful when e.g. moving a gripper relative to a point seen by a camera built into that gripper. +func alterGoals( + chains []*motionChain, + fs referenceframe.FrameSystem, + start map[string][]referenceframe.Input, + goal *PlanState, +) (*PlanState, error) { + alteredGoals := PathState{} + if goal.poses != nil { + for _, chain := range chains { + // chain solve frame may only be in the goal configuration, in which case we skip as the configuration will be passed through + if goalPif, ok := goal.poses[chain.solveFrameName]; ok { + if chain.worldRooted { + tf, err := fs.Transform(start, goalPif, referenceframe.World) + if err != nil { + return nil, err + } + alteredGoals[chain.solveFrameName] = tf.(*referenceframe.PoseInFrame) + } else { + alteredGoals[chain.solveFrameName] = goalPif + } + } + } + return &PlanState{poses: alteredGoals, configuration: goal.configuration}, nil + } + return goal, nil +} diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index e399bea4fbb..447a2db3722 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -393,10 +393,6 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m } func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Plan, error) { - if req.Destination == nil { - return nil, errors.New("cannot specify a nil destination in motion.MoveReq") - } - frameSys, err := ms.fsService.FrameSystem(ctx, req.WorldState.Transforms()) if err != nil { return nil, err @@ -414,24 +410,47 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Pla return nil, fmt.Errorf("component named %s not found in robot frame system", req.ComponentName.ShortName()) } - // re-evaluate goalPose to be in the frame of World - solvingFrame := referenceframe.World // TODO(erh): this should really be the parent of rootName - tf, err := frameSys.Transform(fsInputs, req.Destination, solvingFrame) + startState, waypoints, err := waypointsFromRequest(req, fsInputs) if err != nil { return nil, err } - goalPose, _ := tf.(*referenceframe.PoseInFrame) + if len(waypoints) == 0 { + return nil, errors.New("could not find any waypoints to plan for in MoveRequest. Fill in Destination or goal_state") + } + + // re-evaluate goal poses to be in the frame of World + // TODO (RSDK-8847) : this is a workaround to help account for us not yet being able to properly synchronize simultaneous motion across + // multiple components. If we are moving component1, mounted on arm2, to a goal in frame of component2, which is mounted on arm2, then + // passing that raw poseInFrame will certainly result in a plan which moves arm1 and arm2. We cannot guarantee that this plan is + // collision-free until RSDK-8847 is complete. By transforming goals to world, only one arm should move for such a plan. + worldWaypoints := []*motionplan.PlanState{} + solvingFrame := referenceframe.World + for _, wp := range waypoints { + if wp.Poses() != nil { + step := motionplan.PathState{} + for fName, destination := range wp.Poses() { + tf, err := frameSys.Transform(fsInputs, destination, solvingFrame) + if err != nil { + return nil, err + } + goalPose, _ := tf.(*referenceframe.PoseInFrame) + step[fName] = goalPose + } + worldWaypoints = append(worldWaypoints, motionplan.NewPlanState(step, wp.Configuration())) + } else { + worldWaypoints = append(worldWaypoints, wp) + } + } // the goal is to move the component to goalPose which is specified in coordinates of goalFrameName return motionplan.PlanMotion(ctx, &motionplan.PlanRequest{ - Logger: ms.logger, - Goal: goalPose, - Frame: movingFrame, - StartConfiguration: fsInputs, - FrameSystem: frameSys, - WorldState: req.WorldState, - Constraints: req.Constraints, - Options: req.Extra, + Logger: ms.logger, + Goals: worldWaypoints, + StartState: startState, + FrameSystem: frameSys, + WorldState: req.WorldState, + Constraints: req.Constraints, + Options: req.Extra, }) } @@ -521,3 +540,63 @@ func (ms *builtIn) execute(ctx context.Context, trajectory motionplan.Trajectory } return nil } + +func waypointsFromRequest( + req motion.MoveReq, + fsInputs map[string][]referenceframe.Input, +) (*motionplan.PlanState, []*motionplan.PlanState, error) { + var startState *motionplan.PlanState + var waypoints []*motionplan.PlanState + var err error + + if startStateIface, ok := req.Extra["start_state"]; ok { + if startStateMap, ok := startStateIface.(map[string]interface{}); ok { + startState, err = motionplan.DeserializePlanState(startStateMap) + if err != nil { + return nil, nil, err + } + } else { + return nil, nil, errors.New("extras start_state could not be interpreted as map[string]interface{}") + } + if startState.Configuration() == nil { + startState = motionplan.NewPlanState(startState.Poses(), fsInputs) + } + } else { + startState = motionplan.NewPlanState(nil, fsInputs) + } + + if waypointsIface, ok := req.Extra["waypoints"]; ok { + if waypointsIfaceList, ok := waypointsIface.([]interface{}); ok { + for _, wpIface := range waypointsIfaceList { + if wpMap, ok := wpIface.(map[string]interface{}); ok { + wp, err := motionplan.DeserializePlanState(wpMap) + if err != nil { + return nil, nil, err + } + waypoints = append(waypoints, wp) + } else { + return nil, nil, errors.New("element in extras waypoints could not be interpreted as map[string]interface{}") + } + } + } else { + return nil, nil, errors.New("Invalid 'waypoints' extra type. Expected an array") + } + } + + // If goal state is specified, it overrides the request goal + if goalStateIface, ok := req.Extra["goal_state"]; ok { + if goalStateMap, ok := goalStateIface.(map[string]interface{}); ok { + goalState, err := motionplan.DeserializePlanState(goalStateMap) + if err != nil { + return nil, nil, err + } + waypoints = append(waypoints, goalState) + } else { + return nil, nil, errors.New("extras goal_state could not be interpreted as map[string]interface{}") + } + } else if req.Destination != nil { + goalState := motionplan.NewPlanState(motionplan.PathState{req.ComponentName.ShortName(): req.Destination}, nil) + waypoints = append(waypoints, goalState) + } + return startState, waypoints, nil +} diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 14246f2680e..fa2e290672d 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -1334,3 +1334,186 @@ func TestDoCommand(t *testing.T) { test.That(t, err, test.ShouldResemble, motionplan.NewAlgAndConstraintMismatchErr("rrtstar")) }) } + +func TestMultiWaypointPlanning(t *testing.T) { + ms, teardown := setupMotionServiceFromConfig(t, "../data/moving_arm.json") + defer teardown() + ctx := context.Background() + + // Helper function to extract plan from Move call using DoCommand + getPlanFromMove := func(t *testing.T, req motion.MoveReq) motionplan.Trajectory { + t.Helper() + // Convert MoveReq to proto format for DoCommand + moveReqProto, err := req.ToProto("") + test.That(t, err, test.ShouldBeNil) + bytes, err := protojson.Marshal(moveReqProto) + test.That(t, err, test.ShouldBeNil) + + resp, err := ms.DoCommand(ctx, map[string]interface{}{ + DoPlan: string(bytes), + }) + test.That(t, err, test.ShouldBeNil) + + plan, ok := resp[DoPlan].(motionplan.Trajectory) + test.That(t, ok, test.ShouldBeTrue) + return plan + } + + t.Run("plan through multiple pose waypoints", func(t *testing.T) { + // Define waypoints as poses relative to world frame + waypoint1 := referenceframe.NewPoseInFrame("world", spatialmath.NewPoseFromPoint(r3.Vector{X: -800, Y: -180, Z: 30})) + waypoint2 := referenceframe.NewPoseInFrame("world", spatialmath.NewPoseFromPoint(r3.Vector{X: -800, Y: -190, Z: 30})) + finalPose := referenceframe.NewPoseInFrame("world", spatialmath.NewPoseFromPoint(r3.Vector{X: -800, Y: -200, Z: 30})) + + wp1State := motionplan.NewPlanState(motionplan.PathState{"pieceGripper": waypoint1}, nil) + wp2State := motionplan.NewPlanState(motionplan.PathState{"pieceGripper": waypoint2}, nil) + + moveReq := motion.MoveReq{ + ComponentName: gripper.Named("pieceGripper"), + Destination: finalPose, + Extra: map[string]interface{}{ + "waypoints": []interface{}{wp1State.Serialize(), wp2State.Serialize()}, + "smooth_iter": 5, + }, + } + + plan := getPlanFromMove(t, moveReq) + test.That(t, len(plan), test.ShouldBeGreaterThan, 0) + + // Verify start configuration matches current robot state + fsInputs, _, err := ms.(*builtIn).fsService.CurrentInputs(ctx) + test.That(t, err, test.ShouldBeNil) + test.That(t, plan[0], test.ShouldResemble, fsInputs) + + // Verify final pose + frameSys, err := ms.(*builtIn).fsService.FrameSystem(ctx, nil) + test.That(t, err, test.ShouldBeNil) + + finalConfig := plan[len(plan)-1] + finalPoseInWorld, err := frameSys.Transform(finalConfig, + referenceframe.NewPoseInFrame("pieceGripper", spatialmath.NewZeroPose()), + "world") + test.That(t, err, test.ShouldBeNil) + plannedPose := finalPoseInWorld.(*referenceframe.PoseInFrame).Pose() + test.That(t, spatialmath.PoseAlmostEqualEps(plannedPose, finalPose.Pose(), 1e-3), test.ShouldBeTrue) + }) + + t.Run("plan through mixed pose and configuration waypoints", func(t *testing.T) { + // Define specific arm configuration for first waypoint + armConfig := []float64{0.2, 0.3, 0.4, 0.5, 0.6, 0.7} + wp1State := motionplan.NewPlanState(nil, map[string][]referenceframe.Input{ + "pieceArm": referenceframe.FloatsToInputs(armConfig), + }) + + // Define pose for second waypoint + intermediatePose := spatialmath.NewPoseFromPoint(r3.Vector{X: -800, Y: -190, Z: 30}) + wp2State := motionplan.NewPlanState( + motionplan.PathState{"pieceGripper": referenceframe.NewPoseInFrame("world", intermediatePose)}, + nil, + ) + + finalPose := referenceframe.NewPoseInFrame("world", spatialmath.NewPoseFromPoint(r3.Vector{X: -800, Y: -180, Z: 34})) + + moveReq := motion.MoveReq{ + ComponentName: gripper.Named("pieceGripper"), + Destination: finalPose, + Extra: map[string]interface{}{ + "waypoints": []interface{}{wp1State.Serialize(), wp2State.Serialize()}, + "smooth_iter": 5, + }, + } + + plan := getPlanFromMove(t, moveReq) + test.That(t, len(plan), test.ShouldBeGreaterThan, 0) + + // Find configuration closest to first waypoint + foundMatchingConfig := false + for _, config := range plan { + if armInputs, ok := config["pieceArm"]; ok { + // Check if this configuration matches our waypoint within some epsilon + matches := true + for i, val := range armInputs { + if math.Abs(val.Value-armConfig[i]) > 1e-3 { + matches = false + break + } + } + if matches { + foundMatchingConfig = true + break + } + } + } + test.That(t, foundMatchingConfig, test.ShouldBeTrue) + + // Verify final pose + frameSys, err := ms.(*builtIn).fsService.FrameSystem(ctx, nil) + test.That(t, err, test.ShouldBeNil) + + finalConfig := plan[len(plan)-1] + finalPoseInWorld, err := frameSys.Transform(finalConfig, + referenceframe.NewPoseInFrame("pieceGripper", spatialmath.NewZeroPose()), + "world") + test.That(t, err, test.ShouldBeNil) + plannedPose := finalPoseInWorld.(*referenceframe.PoseInFrame).Pose() + test.That(t, spatialmath.PoseAlmostEqualEps(plannedPose, finalPose.Pose(), 1e-3), test.ShouldBeTrue) + }) + + t.Run("plan with custom start state", func(t *testing.T) { + startConfig := []float64{0.1, 0.2, 0.3, 0.4, 0.5, 0.6} + startState := motionplan.NewPlanState(nil, map[string][]referenceframe.Input{ + "pieceArm": referenceframe.FloatsToInputs(startConfig), + }) + + finalPose := referenceframe.NewPoseInFrame("world", spatialmath.NewPoseFromPoint(r3.Vector{X: -800, Y: -180, Z: 34})) + + moveReq := motion.MoveReq{ + ComponentName: gripper.Named("pieceGripper"), + Destination: finalPose, + Extra: map[string]interface{}{ + "start_state": startState.Serialize(), + "smooth_iter": 5, + }, + } + + plan := getPlanFromMove(t, moveReq) + test.That(t, len(plan), test.ShouldBeGreaterThan, 0) + + // Verify start configuration matches specified start state + startArmConfig := plan[0]["pieceArm"] + test.That(t, startArmConfig, test.ShouldResemble, referenceframe.FloatsToInputs(startConfig)) + + // Verify final pose + frameSys, err := ms.(*builtIn).fsService.FrameSystem(ctx, nil) + test.That(t, err, test.ShouldBeNil) + + finalConfig := plan[len(plan)-1] + finalPoseInWorld, err := frameSys.Transform(finalConfig, + referenceframe.NewPoseInFrame("pieceGripper", spatialmath.NewZeroPose()), + "world") + test.That(t, err, test.ShouldBeNil) + plannedPose := finalPoseInWorld.(*referenceframe.PoseInFrame).Pose() + test.That(t, spatialmath.PoseAlmostEqualEps(plannedPose, finalPose.Pose(), 1e-3), test.ShouldBeTrue) + }) + + t.Run("plan with explicit goal state configuration", func(t *testing.T) { + goalConfig := []float64{0.7, 0.6, 0.5, 0.4, 0.3, 0.2} + + goalState := motionplan.NewPlanState(nil, map[string][]referenceframe.Input{"pieceArm": referenceframe.FloatsToInputs(goalConfig)}) + + moveReq := motion.MoveReq{ + ComponentName: gripper.Named("pieceGripper"), + Extra: map[string]interface{}{ + "goal_state": goalState.Serialize(), + "smooth_iter": 5, + }, + } + + plan := getPlanFromMove(t, moveReq) + test.That(t, len(plan), test.ShouldBeGreaterThan, 0) + + // Verify final configuration matches goal state + finalArmConfig := plan[len(plan)-1]["pieceArm"] + test.That(t, finalArmConfig, test.ShouldResemble, referenceframe.FloatsToInputs(goalConfig)) + }) +} diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 4623659d185..62a2aa0cab4 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -95,10 +95,11 @@ func (mr *moveRequest) Plan(ctx context.Context) (motionplan.Plan, error) { if len(mr.kinematicBase.Kinematics().DoF()) == 2 { inputs = inputs[:2] } - mr.planRequest.StartConfiguration = map[string][]referenceframe.Input{mr.kinematicBase.Kinematics().Name(): inputs} + startConf := map[string][]referenceframe.Input{mr.kinematicBase.Kinematics().Name(): inputs} + mr.planRequest.StartState = motionplan.NewPlanState(mr.planRequest.StartState.Poses(), startConf) // get existing elements of the worldstate - existingGifs, err := mr.planRequest.WorldState.ObstaclesInWorldFrame(mr.planRequest.FrameSystem, mr.planRequest.StartConfiguration) + existingGifs, err := mr.planRequest.WorldState.ObstaclesInWorldFrame(mr.planRequest.FrameSystem, startConf) if err != nil { return nil, err } @@ -147,7 +148,7 @@ func (mr *moveRequest) execute(ctx context.Context, plan motionplan.Plan) (state // If our motion profile is position_only then, we only check against our current & desired position // Conversely if our motion profile is anything else, then we also need to check again our // current & desired orientation - if resp := mr.atGoalCheck(mr.planRequest.StartPose); resp { + if resp := mr.atGoalCheck(mr.planRequest.StartState.Poses()[mr.kinematicBase.Name().ShortName()].Pose()); resp { mr.logger.Info("no need to move, already within planDeviationMM of the goal") return state.ExecuteResponse{Replan: false}, nil } @@ -284,7 +285,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( // we need the original input to place that thing in its original position // hence, cached CurrentInputs from the start are used i.e. mr.planRequest.StartConfiguration existingGifs, err := mr.planRequest.WorldState.ObstaclesInWorldFrame( - mr.planRequest.FrameSystem, mr.planRequest.StartConfiguration, + mr.planRequest.FrameSystem, mr.planRequest.StartState.Configuration(), ) if err != nil { return state.ExecuteResponse{}, err @@ -386,7 +387,7 @@ func (mr *moveRequest) augmentBaseExecutionState( // The exception to this is if we are at the index we are currently executing, then // we will use the base's reported current position. - currPathStep := existingPlan.Path()[idx] + currPathState := existingPlan.Path()[idx] kbTraj := currTraj[mr.kinematicBase.Name().Name] // determine which pose should be used as the origin of a ptg input @@ -394,7 +395,7 @@ func (mr *moveRequest) augmentBaseExecutionState( if idx == baseExecutionState.Index() { prevPathPose = baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose() } else { - kbPose := currPathStep[mr.kinematicBase.Kinematics().Name()] + kbPose := currPathState[mr.kinematicBase.Kinematics().Name()] trajPose, err := mr.kinematicBase.Kinematics().Transform(kbTraj) if err != nil { return baseExecutionState, err @@ -913,19 +914,22 @@ func (ms *builtIn) createBaseMoveRequest( } var backgroundWorkers sync.WaitGroup + startState := motionplan.NewPlanState( + motionplan.PathState{kinematicFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose)}, + currentInputs, + ) + goals := []*motionplan.PlanState{motionplan.NewPlanState(motionplan.PathState{kinematicFrame.Name(): goal}, nil)} mr := &moveRequest{ config: motionCfg, logger: ms.logger, planRequest: &motionplan.PlanRequest{ - Logger: logger, - Goal: goal, - Frame: kinematicFrame, - FrameSystem: planningFS, - StartConfiguration: currentInputs, - StartPose: startPose, - WorldState: worldState, - Options: valExtra.extra, + Logger: logger, + Goals: goals, + FrameSystem: planningFS, + StartState: startState, + WorldState: worldState, + Options: valExtra.extra, }, kinematicBase: kb, replanCostFactor: valExtra.replanCostFactor, diff --git a/services/motion/builtin/state/state_test.go b/services/motion/builtin/state/state_test.go index 05add9df2e5..905262e5cb4 100644 --- a/services/motion/builtin/state/state_test.go +++ b/services/motion/builtin/state/state_test.go @@ -584,17 +584,17 @@ func TestState(t *testing.T) { planFunc: func(ctx context.Context) (motionplan.Plan, error) { // first plan succeeds if replanCount == 0 { - pbc := motionplan.PathStep{ + pbc := motionplan.PathState{ req.ComponentName.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose()), } - return motionplan.NewSimplePlan([]motionplan.PathStep{pbc}, nil), nil + return motionplan.NewSimplePlan([]motionplan.PathState{pbc}, nil), nil } // first replan succeeds if replanCount == 1 { - pbc := motionplan.PathStep{ + pbc := motionplan.PathState{ req.ComponentName.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose()), } - return motionplan.NewSimplePlan([]motionplan.PathStep{pbc, pbc}, nil), nil + return motionplan.NewSimplePlan([]motionplan.PathState{pbc, pbc}, nil), nil } // second replan fails return nil, replanFailReason @@ -951,7 +951,7 @@ func TestState(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, len(ps6), test.ShouldEqual, 0) - pbc := motionplan.PathStep{ + pbc := motionplan.PathState{ req.ComponentName.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose()), } @@ -967,11 +967,11 @@ func TestState(t *testing.T) { planFunc: func(ctx context.Context) (motionplan.Plan, error) { // first replan succeeds if replanCount == 0 || replanCount == 1 { - return motionplan.NewSimplePlan([]motionplan.PathStep{pbc, pbc}, nil), nil + return motionplan.NewSimplePlan([]motionplan.PathState{pbc, pbc}, nil), nil } // second replan fails - return motionplan.NewSimplePlan([]motionplan.PathStep{}, nil), replanFailReason + return motionplan.NewSimplePlan([]motionplan.PathState{}, nil), replanFailReason }, executeFunc: func(ctx context.Context, plan motionplan.Plan) (state.ExecuteResponse, error) { if replanCount == 0 { diff --git a/services/motion/client_test.go b/services/motion/client_test.go index 47dbc4975e8..7ac79892867 100644 --- a/services/motion/client_test.go +++ b/services/motion/client_test.go @@ -82,6 +82,11 @@ func TestClient(t *testing.T) { t.Run("motion client 1", func(t *testing.T) { conn, err := viamgrpc.Dial(context.Background(), listener1.Addr().String(), logger) + testPose := spatialmath.NewPose( + r3.Vector{X: 1., Y: 2., Z: 3.}, + &spatialmath.R4AA{Theta: math.Pi / 2, RX: 0., RY: 1., RZ: 0.}, + ) + test.That(t, err, test.ShouldBeNil) client, err := motion.NewClientFromConn(context.Background(), conn, "", testMotionServiceName, logger) @@ -112,10 +117,6 @@ func TestClient(t *testing.T) { test.That(t, result, test.ShouldEqual, success) // GetPose - testPose := spatialmath.NewPose( - r3.Vector{X: 1., Y: 2., Z: 3.}, - &spatialmath.R4AA{Theta: math.Pi / 2, RX: 0., RY: 1., RZ: 0.}, - ) transforms := []*referenceframe.LinkInFrame{ referenceframe.NewLinkInFrame("arm1", testPose, "frame1", nil), referenceframe.NewLinkInFrame("frame1", testPose, "frame2", nil), @@ -487,7 +488,7 @@ func TestClient(t *testing.T) { }) t.Run("otherwise returns a slice of PlanWithStatus", func(t *testing.T) { - steps := []motionplan.PathStep{{"mybase": zeroPoseInFrame}} + steps := []motionplan.PathState{{"mybase": zeroPoseInFrame}} reason := "some reason" id := uuid.New() executionID := uuid.New() @@ -517,7 +518,7 @@ func TestClient(t *testing.T) { }) t.Run("supports returning a slice of PlanWithStatus with more than one plan", func(t *testing.T) { - steps := []motionplan.PathStep{{"mybase": zeroPoseInFrame}} + steps := []motionplan.PathState{{"mybase": zeroPoseInFrame}} reason := "some reason" idA := uuid.New() diff --git a/services/motion/motion_test.go b/services/motion/motion_test.go index cd3c8493a39..44a6a2f4872 100644 --- a/services/motion/motion_test.go +++ b/services/motion/motion_test.go @@ -49,7 +49,7 @@ func TestPlanWithStatus(t *testing.T) { ExecutionID: executionID, ComponentName: baseName, Plan: motionplan.NewSimplePlan( - []motionplan.PathStep{ + []motionplan.PathState{ {baseName.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, poseA)}, {baseName.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, poseB)}, }, @@ -585,7 +585,7 @@ func TestPlan(t *testing.T) { ExecutionID: executionID, ComponentName: baseName, Plan: motionplan.NewSimplePlan( - []motionplan.PathStep{ + []motionplan.PathState{ {baseName.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, poseA)}, {baseName.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, poseB)}, }, diff --git a/services/motion/pbhelpers.go b/services/motion/pbhelpers.go index 5c1a0b7bcd2..da4a402360d 100644 --- a/services/motion/pbhelpers.go +++ b/services/motion/pbhelpers.go @@ -27,14 +27,19 @@ func (r MoveReq) ToProto(name string) (*pb.MoveRequest, error) { if err != nil { return nil, err } - return &pb.MoveRequest{ + reqPB := &pb.MoveRequest{ Name: name, ComponentName: rprotoutils.ResourceNameToProto(r.ComponentName), - Destination: referenceframe.PoseInFrameToProtobuf(r.Destination), WorldState: worldStateMsg, Constraints: r.Constraints.ToProtobuf(), Extra: ext, - }, nil + } + if r.Destination != nil { + // Destination is not needed if goal_state present. Validation on receiving end. + reqPB.Destination = referenceframe.PoseInFrameToProtobuf(r.Destination) + } + + return reqPB, nil } // MoveReqFromProto converts a pb.MoveRequest to a MoveReq struct. @@ -43,9 +48,16 @@ func MoveReqFromProto(req *pb.MoveRequest) (MoveReq, error) { if err != nil { return MoveReq{}, err } + dst := req.GetDestination() + var destination *referenceframe.PoseInFrame + if dst != nil { + // Destination is not needed if goal_state present. Validation on receiving end. + destination = referenceframe.ProtobufToPoseInFrame(req.GetDestination()) + } + return MoveReq{ rprotoutils.ResourceNameFromProto(req.GetComponentName()), - referenceframe.ProtobufToPoseInFrame(req.GetDestination()), + destination, worldState, motionplan.ConstraintsFromProtobuf(req.GetConstraints()), req.Extra.AsMap(), @@ -161,7 +173,7 @@ func planFromProto(p *pb.Plan) (PlanWithMetadata, error) { steps := motionplan.Path{} for _, s := range p.Steps { - step, err := motionplan.PathStepFromProto(s) + step, err := motionplan.PathStateFromProto(s) if err != nil { return PlanWithMetadata{}, err } diff --git a/services/motion/server_test.go b/services/motion/server_test.go index 1c7072d3127..86e77260a75 100644 --- a/services/motion/server_test.go +++ b/services/motion/server_test.go @@ -672,7 +672,7 @@ func TestServerGetPlan(t *testing.T) { planID2 := uuid.New() base1 := base.Named("base1") - steps := []motionplan.PathStep{{base1.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose())}} + steps := []motionplan.PathState{{base1.ShortName(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose())}} plan1 := motion.PlanWithMetadata{ ID: planID1, diff --git a/services/navigation/builtin/builtin_test.go b/services/navigation/builtin/builtin_test.go index 403efad6f14..7e1a9375b48 100644 --- a/services/navigation/builtin/builtin_test.go +++ b/services/navigation/builtin/builtin_test.go @@ -738,7 +738,7 @@ func TestPaths(t *testing.T) { Plan: motion.PlanWithMetadata{ ExecutionID: executionID, Plan: motionplan.NewSimplePlan( - []motionplan.PathStep{{s.base.Name().ShortName(): referenceframe.NewPoseInFrame( + []motionplan.PathState{{s.base.Name().ShortName(): referenceframe.NewPoseInFrame( referenceframe.World, spatialmath.NewPose(r3.Vector{X: expectedLng, Y: expectedLat}, nil), )}},