diff --git a/components/base/kinematicbase/differentialDrive.go b/components/base/kinematicbase/differentialDrive.go index 9b77428e462..8f8ff7231e4 100644 --- a/components/base/kinematicbase/differentialDrive.go +++ b/components/base/kinematicbase/differentialDrive.go @@ -71,14 +71,17 @@ func wrapWithDifferentialDriveKinematics( boundingSphere, err = spatialmath.BoundingSphere(geometry) } if boundingSphere == nil || err != nil { - logger.CWarn(ctx, "base %s not configured with a geometry, will be considered a 300mm sphere for collision detection purposes.") + logger.CWarnf( + ctx, "base %s not configured with a geometry, will be considered a 300mm sphere for collision detection purposes.", + b.Name().Name, + ) boundingSphere, err = spatialmath.NewSphere(spatialmath.NewZeroPose(), 150., b.Name().ShortName()) if err != nil { return nil, err } } - ddk.executionFrame, err = referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits, boundingSphere) + ddk.localizationFrame, err = referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits, boundingSphere) if err != nil { return nil, err } @@ -89,7 +92,7 @@ func wrapWithDifferentialDriveKinematics( return nil, err } } else { - ddk.planningFrame = ddk.executionFrame + ddk.planningFrame = ddk.localizationFrame } ddk.noLocalizerCacheInputs = originInputs @@ -99,19 +102,23 @@ func wrapWithDifferentialDriveKinematics( type differentialDriveKinematics struct { base.Base motion.Localizer - logger logging.Logger - planningFrame, executionFrame referenceframe.Model - options Options - noLocalizerCacheInputs []referenceframe.Input - currentTrajectory [][]referenceframe.Input - currentIdx int - mutex sync.RWMutex + logger logging.Logger + planningFrame, localizationFrame referenceframe.Frame + options Options + noLocalizerCacheInputs []referenceframe.Input + currentTrajectory [][]referenceframe.Input + currentIdx int + mutex sync.RWMutex } func (ddk *differentialDriveKinematics) Kinematics() referenceframe.Frame { return ddk.planningFrame } +func (ddk *differentialDriveKinematics) LocalizationFrame() referenceframe.Frame { + return ddk.localizationFrame +} + func (ddk *differentialDriveKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { // If no localizer is present, CurrentInputs returns the expected position of the robot assuming after // each part of move command was completed accurately. @@ -310,7 +317,7 @@ func (ddk *differentialDriveKinematics) inputDiff(current, desired []referencefr ) // transform the goal pose such that it is in the base frame - currentPose, err := ddk.executionFrame.Transform(current) + currentPose, err := ddk.localizationFrame.Transform(current) if err != nil { return 0, 0, err } diff --git a/components/base/kinematicbase/differentialDrive_test.go b/components/base/kinematicbase/differentialDrive_test.go index 0be3993655d..50ed15a22df 100644 --- a/components/base/kinematicbase/differentialDrive_test.go +++ b/components/base/kinematicbase/differentialDrive_test.go @@ -68,12 +68,12 @@ func TestWrapWithDifferentialDriveKinematics(t *testing.T) { if err != nil { return } - limits := ddk.executionFrame.DoF() + limits := ddk.localizationFrame.DoF() test.That(t, limits[0].Min, test.ShouldBeLessThan, 0) test.That(t, limits[1].Min, test.ShouldBeLessThan, 0) test.That(t, limits[0].Max, test.ShouldBeGreaterThan, 0) test.That(t, limits[1].Max, test.ShouldBeGreaterThan, 0) - geometry, err := ddk.executionFrame.(*referenceframe.SimpleModel).Geometries(make([]referenceframe.Input, len(limits))) + geometry, err := ddk.localizationFrame.(*referenceframe.SimpleModel).Geometries(make([]referenceframe.Input, len(limits))) test.That(t, err, test.ShouldBeNil) equivalent := spatialmath.GeometriesAlmostEqual( geometry.GeometryByName(testCfg.Name+":"+testCfg.Frame.Geometry.Label), diff --git a/components/base/kinematicbase/execution.go b/components/base/kinematicbase/execution.go index a6febacd883..81363fbb2fb 100644 --- a/components/base/kinematicbase/execution.go +++ b/components/base/kinematicbase/execution.go @@ -354,7 +354,7 @@ func (ptgk *ptgBaseKinematics) courseCorrect( return nil, err } // trajPose is the pose we should have nominally reached along the currently executing arc from the start position. - trajPose, err := ptgk.frame.Transform(currentInputs) + trajPose, err := ptgk.Kinematics().Transform(currentInputs) if err != nil { return nil, err } diff --git a/components/base/kinematicbase/kinematics.go b/components/base/kinematicbase/kinematics.go index d37da50ae0e..fef8c62357b 100644 --- a/components/base/kinematicbase/kinematics.go +++ b/components/base/kinematicbase/kinematics.go @@ -23,6 +23,7 @@ type KinematicBase interface { referenceframe.InputEnabled Kinematics() referenceframe.Frame + LocalizationFrame() referenceframe.Frame // ExecutionState returns the state of execution of the base, returning the plan (with any edits) that it is executing, the point // along that plan where it currently is, the inputs representing its current state, and its current position. diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index f034b3e6737..d9b121d9a59 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -31,14 +31,14 @@ const ( type ptgBaseKinematics struct { base.Base motion.Localizer - logger logging.Logger - frame referenceframe.Frame - ptgs []tpspace.PTGSolver - opts Options - courseCorrectionIdx int - linVelocityMMPerSecond float64 - angVelocityDegsPerSecond float64 - nonzeroBaseTurningRadiusMeters float64 + logger logging.Logger + planningFrame, localizationFrame referenceframe.Frame + ptgs []tpspace.PTGSolver + opts Options + courseCorrectionIdx int + linVelocityMMPerSecond float64 + angVelocityDegsPerSecond float64 + nonzeroBaseTurningRadiusMeters float64 // All changeable state of the base is here inputLock sync.RWMutex @@ -98,7 +98,10 @@ func wrapWithPTGKinematics( geometries, err := b.Geometries(ctx, nil) if len(geometries) == 0 || err != nil { - logger.CWarn(ctx, "base %s not configured with a geometry, will be considered a 300mm sphere for collision detection purposes.") + logger.CWarnf( + ctx, "base %s not configured with a geometry, will be considered a 300mm sphere for collision detection purposes.", + b.Name().Name, + ) sphere, err := spatialmath.NewSphere(spatialmath.NewZeroPose(), 150., b.Name().Name) if err != nil { return nil, err @@ -107,7 +110,7 @@ func wrapWithPTGKinematics( } nonzeroBaseTurningRadiusMeters := (linVelocityMMPerSecond / rdkutils.DegToRad(angVelocityDegsPerSecond)) / 1000. - frame, err := tpspace.NewPTGFrameFromKinematicOptions( + planningFrame, err := tpspace.NewPTGFrameFromKinematicOptions( b.Name().ShortName(), logger, nonzeroBaseTurningRadiusMeters, @@ -119,14 +122,14 @@ func wrapWithPTGKinematics( if err != nil { return nil, err } - ptgProv, err := rdkutils.AssertType[tpspace.PTGProvider](frame) + ptgProv, err := rdkutils.AssertType[tpspace.PTGProvider](planningFrame) if err != nil { return nil, err } ptgs := ptgProv.PTGSolvers() origin := spatialmath.NewZeroPose() - ptgCourseCorrection, err := rdkutils.AssertType[tpspace.PTGCourseCorrection](frame) + ptgCourseCorrection, err := rdkutils.AssertType[tpspace.PTGCourseCorrection](planningFrame) if err != nil { return nil, err } @@ -141,11 +144,17 @@ func wrapWithPTGKinematics( } startingState := baseState{currentInputs: zeroInput} + localizationFrame, err := referenceframe.NewPoseFrame(b.Name().ShortName()+"_LocalizationFrame", nil) + if err != nil { + return nil, err + } + return &ptgBaseKinematics{ Base: b, Localizer: localizer, logger: logger, - frame: frame, + planningFrame: planningFrame, + localizationFrame: localizationFrame, opts: options, ptgs: ptgs, courseCorrectionIdx: courseCorrectionIdx, @@ -159,7 +168,11 @@ func wrapWithPTGKinematics( } func (ptgk *ptgBaseKinematics) Kinematics() referenceframe.Frame { - return ptgk.frame + return ptgk.planningFrame +} + +func (ptgk *ptgBaseKinematics) LocalizationFrame() referenceframe.Frame { + return ptgk.localizationFrame } // For a ptgBaseKinematics, `CurrentInputs` returns inputs which reflect what the base is currently doing. @@ -169,6 +182,7 @@ func (ptgk *ptgBaseKinematics) Kinematics() referenceframe.Frame { func (ptgk *ptgBaseKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { ptgk.inputLock.RLock() defer ptgk.inputLock.RUnlock() + return ptgk.currentState.currentInputs, nil } @@ -177,7 +191,7 @@ func (ptgk *ptgBaseKinematics) ExecutionState(ctx context.Context) (motionplan.E return motionplan.ExecutionState{}, errors.New("cannot call ExecutionState on a base without a localizer") } - actualPIF, err := ptgk.CurrentPosition(ctx) + actualPIF, err := ptgk.Localizer.CurrentPosition(ctx) if err != nil { return motionplan.ExecutionState{}, err } @@ -193,7 +207,7 @@ func (ptgk *ptgBaseKinematics) ExecutionState(ctx context.Context) (motionplan.E currentPlan, currentIdx, map[string][]referenceframe.Input{ptgk.Kinematics().Name(): currentInputs}, - map[string]*referenceframe.PoseInFrame{ptgk.Kinematics().Name(): actualPIF}, + map[string]*referenceframe.PoseInFrame{ptgk.LocalizationFrame().Name(): actualPIF}, ) } diff --git a/components/base/kinematicbase/ptgKinematics_test.go b/components/base/kinematicbase/ptgKinematics_test.go index 45e50a599df..b481c4e8163 100644 --- a/components/base/kinematicbase/ptgKinematics_test.go +++ b/components/base/kinematicbase/ptgKinematics_test.go @@ -239,7 +239,7 @@ func TestPTGKinematicsWithGeom(t *testing.T) { t.Run("ErrorState", func(t *testing.T) { executionState, err := kb.ExecutionState(ctx) test.That(t, err, test.ShouldBeNil) - errorState, err := motionplan.CalculateFrameErrorState(executionState, kb.Kinematics()) + errorState, err := motionplan.CalculateFrameErrorState(executionState, kb.Kinematics(), kb.LocalizationFrame()) test.That(t, err, test.ShouldBeNil) // Error State should be computed based on current inputs, current executing steps, and the localizer's position function @@ -281,7 +281,7 @@ func TestPTGKinematicsWithGeom(t *testing.T) { // After course correction, error state should always be zero executionState, err := kb.ExecutionState(ctx) test.That(t, err, test.ShouldBeNil) - errorState, err := motionplan.CalculateFrameErrorState(executionState, kb.Kinematics()) + errorState, err := motionplan.CalculateFrameErrorState(executionState, kb.Kinematics(), kb.LocalizationFrame()) test.That(t, err, test.ShouldBeNil) test.That(t, spatialmath.PoseAlmostEqualEps(errorState, spatialmath.NewZeroPose(), 1e-5), test.ShouldBeTrue) }) diff --git a/motionplan/check.go b/motionplan/check.go index 0a5ac1210fe..bcdf703b0b1 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -14,6 +14,10 @@ import ( "go.viam.com/rdk/spatialmath" ) +var errCheckFrameNotInPath = errors.New("checkFrame given not in plan.Path() map") + +const relativePlanOptsResolution = 30 + // CheckPlan checks if obstacles intersect the trajectory of the frame following the plan. If one is // detected, the interpolated position of the rover when a collision is detected is returned along // with an error with additional collision details. @@ -66,58 +70,7 @@ func checkPlanRelative( lookAheadDistanceMM float64, sfPlanner *planManager, ) error { - plan := executionState.Plan() - startingInputs := plan.Trajectory()[0] - currentInputs := executionState.CurrentInputs() - currentPoses := executionState.CurrentPoses() - if currentPoses == nil { - return errors.New("executionState had nil return from CurrentPoses") - } - currentPoseIF, ok := currentPoses[checkFrame.Name()] - if !ok { - return errors.New("checkFrame not found in current pose map") - } - wayPointIdx := executionState.Index() - sf := sfPlanner.frame - - // Validate the given PoseInFrame of the relative frame. Relative frame poses cannot be given in their own frame, or the frame of - // any of their children. - // TODO(RSDK-7421): there will need to be checks once there is a real possibility of multiple, hierarchical relative frames, or - // something expressly forbidding it. - validateRelPiF := func(pif *referenceframe.PoseInFrame) error { - observingFrame := fs.Frame(pif.Parent()) - // Ensure the frame of the pose-in-frame is in the frame system - if observingFrame == nil { - sfPlanner.logger.Errorf( - "pose of %s was given in frame of %s, but no frame with that name was found in the frame system", - checkFrame.Name(), - pif.Parent(), - ) - return nil - } - // Ensure nothing between the PiF's frame and World is the relative frame - observingParentage, err := fs.TracebackFrame(observingFrame) - if err != nil { - return err - } - for _, parent := range observingParentage { - if parent.Name() == checkFrame.Name() { - return fmt.Errorf( - "pose of %s was given in frame of %s, but current pose of checked frame must not be observed by self or child", - checkFrame.Name(), - pif.Parent(), - ) - } - } - return nil - } - toWorld := func(pif *referenceframe.PoseInFrame, inputs map[string][]referenceframe.Input) (*referenceframe.PoseInFrame, error) { - // Check our current position is valid - err := validateRelPiF(pif) - if err != nil { - return nil, err - } transformable, err := fs.Transform(inputs, pif, referenceframe.World) if err != nil { return nil, err @@ -130,88 +83,101 @@ func checkPlanRelative( return poseInWorld, nil } - // Check out path pose is valid - stepEndPiF, ok := plan.Path()[wayPointIdx][checkFrame.Name()] - if !ok { - return errors.New("check frame given not in plan Path map") - } - expectedArcEndInWorld, err := toWorld(stepEndPiF, plan.Trajectory()[wayPointIdx]) + plan := executionState.Plan() + zeroPosePIF := referenceframe.NewPoseInFrame(checkFrame.Name(), spatialmath.NewZeroPose()) + + // determine plan's starting pose + planStartPoseWorld, err := toWorld(zeroPosePIF, plan.Trajectory()[0]) if err != nil { return err } - currentPoseInWorld, err := toWorld(currentPoseIF, currentInputs) + // determine plan's ending pose + planEndPoseWorld, err := toWorld(zeroPosePIF, plan.Trajectory()[len(plan.Path())-1]) if err != nil { return err } - arcInputs, ok := plan.Trajectory()[wayPointIdx][checkFrame.Name()] - if !ok { - return errors.New("given checkFrame had no inputs in trajectory map at current index") - } - fullArcPose, err := checkFrame.Transform(arcInputs) - if err != nil { + // 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( + planStartPoseWorld.Pose(), + planEndPoseWorld.Pose(), + plan.Trajectory()[0], + worldState, + nil, + nil, // no pb.Constraints + nil, // no plannOpts + ); err != nil { return err } + // change from 60mm to 30mm so we have finer interpolation along segments + sfPlanner.planOpts.Resolution = relativePlanOptsResolution - // Relative current inputs will give us the arc the base has executed. Calculating that transform and subtracting it from the - // arc end position (that is, the same-index node in plan.Path()) gives us our expected location. - frameCurrentInputs, ok := currentInputs[checkFrame.Name()] - if !ok { - return errors.New("given checkFrame had no inputs in CurrentInputs map") - } - poseThroughArc, err := checkFrame.Transform(frameCurrentInputs) + currentInputs := executionState.CurrentInputs() + wayPointIdx := executionState.Index() + sf := sfPlanner.frame + + // construct first segment's start configuration + // get checkFrame's currentInputs + // *currently* it is guaranteed that a relative frame will constitute 100% of a solver frame's dof + checkFrameCurrentInputs, err := sf.mapToSlice(currentInputs) if err != nil { return err } - remainingArcPose := spatialmath.PoseBetween(poseThroughArc, fullArcPose) - expectedCurrentPose := spatialmath.PoseBetweenInverse(remainingArcPose, expectedArcEndInWorld.Pose()) - errorState := spatialmath.PoseBetween(expectedCurrentPose, currentPoseInWorld.Pose()) - planStartPiF, ok := plan.Path()[0][checkFrame.Name()] - if !ok { - return errors.New("check frame given not in plan Path map") - } - planStartPoseWorld, err := toWorld(planStartPiF, startingInputs) + // construct first segment's end configuration + arcEndInputs, err := sf.mapToSlice(plan.Trajectory()[wayPointIdx]) if err != nil { return err } - planEndPiF, ok := plan.Path()[len(plan.Path())-1][checkFrame.Name()] + + // construct first segment's startPosition + currentPoses := executionState.CurrentPoses() + if currentPoses == nil { + return errors.New("executionState had nil return from CurrentPoses") + } + + currentPoseIF, ok := currentPoses[checkFrame.Name()] if !ok { - return errors.New("check frame given not in plan Path map") + return errors.New("checkFrame not found in current pose map") } - planEndPoseWorld, err := toWorld(planEndPiF, plan.Trajectory()[len(plan.Path())-1]) + currentPoseInWorld, err := toWorld(currentPoseIF, currentInputs) if err != nil { return err } - // 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( - planStartPoseWorld.Pose(), - planEndPoseWorld.Pose(), - startingInputs, - worldState, - nil, - nil, // no pb.Constraints - nil, // no plannOpts - ); err != nil { + // construct first segment's endPosition + expectedArcEndInWorld, err := toWorld(zeroPosePIF, plan.Trajectory()[wayPointIdx]) + if err != nil { return err } - // create a list of segments to iterate through - segments := make([]*ik.Segment, 0, len(plan.Path())-wayPointIdx) - // get checkFrame's currentInputs - // *currently* it is guaranteed that a relative frame will constitute 100% of a solver frame's dof - checkFrameCurrentInputs, err := sf.mapToSlice(currentInputs) + arcInputs, ok := plan.Trajectory()[wayPointIdx][checkFrame.Name()] + if !ok { + return errCheckFrameNotInPath + } + fullArcPose, err := checkFrame.Transform(arcInputs) if err != nil { return err } - arcEndInputs, err := sf.mapToSlice(plan.Trajectory()[wayPointIdx]) + + // Relative current inputs will give us the arc the base has executed. Calculating that transform and subtracting it from the + // arc end position (that is, the same-index node in plan.Path()) gives us our expected location. + frameCurrentInputs, ok := currentInputs[checkFrame.Name()] + if !ok { + return errors.New("given checkFrame had no inputs in CurrentInputs map") + } + + poseThroughArc, err := checkFrame.Transform(frameCurrentInputs) if err != nil { return err } - + remainingArcPose := spatialmath.PoseBetween(poseThroughArc, fullArcPose) + expectedCurrentPose := spatialmath.PoseBetweenInverse(remainingArcPose, expectedArcEndInWorld.Pose()) + errorState := spatialmath.PoseBetween(expectedCurrentPose, currentPoseInWorld.Pose()) currentArcEndPose := spatialmath.Compose(expectedArcEndInWorld.Pose(), errorState) + + segments := make([]*ik.Segment, 0, len(plan.Path())-wayPointIdx) // pre-pend to segments so we can connect to the input we have not finished actuating yet segments = append(segments, &ik.Segment{ StartPosition: currentPoseInWorld.Pose(), @@ -222,34 +188,25 @@ func checkPlanRelative( }) lastArcEndPose := currentArcEndPose + // iterate through remaining plan and append remaining segments to check for i := wayPointIdx + 1; i <= len(plan.Path())-1; i++ { - thisArcEndPoseTf, ok := plan.Path()[i][checkFrame.Name()] - if !ok { - return errors.New("check frame given not in plan Path map") - } - thisArcEndPoseInWorld, err := toWorld(thisArcEndPoseTf, plan.Trajectory()[i]) + thisArcEndPoseInWorld, err := toWorld(zeroPosePIF, plan.Trajectory()[i]) if err != nil { return err } thisArcEndPose := spatialmath.Compose(thisArcEndPoseInWorld.Pose(), errorState) - // Starting inputs for relative frames should be all-zero - startInputs := map[string][]referenceframe.Input{} - for k, v := range plan.Trajectory()[i] { - if k == checkFrame.Name() { - startInputs[k] = make([]referenceframe.Input, len(checkFrame.DoF())) - } else { - startInputs[k] = v - } - } - segment, err := createSegment(sf, lastArcEndPose, thisArcEndPose, startInputs, plan.Trajectory()[i]) + startInputs := plan.Trajectory()[i-1] + nextInputs := plan.Trajectory()[i] + segment, err := createSegment(sf, lastArcEndPose, thisArcEndPose, startInputs, nextInputs) if err != nil { return err } lastArcEndPose = thisArcEndPose segments = append(segments, segment) } - return checkSegments(sfPlanner, segments, true, lookAheadDistanceMM) + + return checkSegments(sfPlanner, segments, lookAheadDistanceMM) } func checkPlanAbsolute( @@ -317,48 +274,7 @@ func checkPlanAbsolute( segments = append(segments, segment) } - // go through segments and check that we satisfy constraints - // TODO(RSDK-5007): If we can make interpolate a method on Frame the need to write this out will be lessened and we should be - // able to call CheckStateConstraintsAcrossSegment directly. - var totalTravelDistanceMM float64 - for _, segment := range segments { - interpolatedConfigurations, err := interpolateSegment(segment, sfPlanner.planOpts.Resolution) - if err != nil { - return err - } - for _, interpConfig := range interpolatedConfigurations { - poseInPath, err := sf.Transform(interpConfig) - if err != nil { - return err - } - - // Check if look ahead distance has been reached - currentTravelDistanceMM := totalTravelDistanceMM + poseInPath.Point().Distance(segment.StartPosition.Point()) - if currentTravelDistanceMM > lookAheadDistanceMM { - return nil - } - - // If we are working with a PTG plan the returned value for poseInPath will only - // tell us how far along the arc we have traveled. Since this is only the relative position, - // i.e. relative to where the robot started executing the arc, - // we must compose poseInPath with segment.StartPosition to get the absolute position. - interpolatedState := &ik.State{Configuration: interpConfig, Frame: sf} - - // Checks for collision along the interpolated route and returns a the first interpolated pose where a collision is detected. - if isValid, err := sfPlanner.planOpts.CheckStateConstraints(interpolatedState); !isValid { - return fmt.Errorf("found error between positions %v and %v: %s", - segment.StartPosition.Point(), - segment.EndPosition.Point(), - err, - ) - } - } - - // Update total traveled distance after segment has been checked - totalTravelDistanceMM += segment.EndPosition.Point().Distance(segment.StartPosition.Point()) - } - - return checkSegments(sfPlanner, segments, false, lookAheadDistanceMM) + return checkSegments(sfPlanner, segments, lookAheadDistanceMM) } // createSegment is a function to ease segment creation for solver frames. @@ -391,7 +307,7 @@ func createSegment( return segment, nil } -func checkSegments(sfPlanner *planManager, segments []*ik.Segment, relative bool, lookAheadDistanceMM float64) error { +func checkSegments(sfPlanner *planManager, segments []*ik.Segment, lookAheadDistanceMM float64) error { // go through segments and check that we satisfy constraints // TODO(RSDK-5007): If we can make interpolate a method on Frame the need to write this out will be lessened and we should be // able to call CheckStateConstraintsAcrossSegment directly. @@ -413,15 +329,11 @@ func checkSegments(sfPlanner *planManager, segments []*ik.Segment, relative bool return nil } - // If we are working with a PTG plan the returned value for poseInPath will only - // tell us how far along the arc we have traveled. Since this is only the relative position, - // i.e. relative to where the robot started executing the arc, - // we must compose poseInPath with segment.StartPosition to get the absolute position. - interpolatedState := &ik.State{Frame: sfPlanner.frame} - if relative { - interpolatedState.Position = spatialmath.Compose(segment.StartPosition, poseInPath) - } else { - interpolatedState.Configuration = interpConfig + // define State which only houses inputs, pose information not needed since we cannot get arcs from + // an interpolating poses, this would only yield a straight line. + interpolatedState := &ik.State{ + Frame: sfPlanner.frame, + Configuration: interpConfig, } // Checks for collision along the interpolated route and returns a the first interpolated pose where a collision is detected. @@ -429,7 +341,7 @@ func checkSegments(sfPlanner *planManager, segments []*ik.Segment, relative bool return fmt.Errorf("found constraint violation or collision in segment between %v and %v at %v: %s", segment.StartPosition.Point(), segment.EndPosition.Point(), - interpolatedState.Position.Point(), + poseInPath.Point(), err, ) } diff --git a/motionplan/ik/metrics.go b/motionplan/ik/metrics.go index 402ccec7f55..a430d9bb6e1 100644 --- a/motionplan/ik/metrics.go +++ b/motionplan/ik/metrics.go @@ -23,7 +23,8 @@ type Segment struct { } func (s *Segment) String() string { - return fmt.Sprintf("Segment: StartPosition: %v,\n\t EndPosition: %v,\n\t StartConfiguration:%v,\n\t EndConfiguration:%v,\n\t Frame: %v", + return fmt.Sprintf( + "Segment: \n\t StartPosition: %v,\n\t EndPosition: %v,\n\t StartConfiguration:%v,\n\t EndConfiguration:%v,\n\t Frame: %v", spatial.PoseToProtobuf(s.StartPosition), spatial.PoseToProtobuf(s.EndPosition), s.StartConfiguration, diff --git a/motionplan/plan.go b/motionplan/plan.go index 591f941e8d4..8efbbb87d8f 100644 --- a/motionplan/plan.go +++ b/motionplan/plan.go @@ -173,7 +173,7 @@ func (path Path) String() string { for _, step := range path { str += "\n" for frame, pose := range step { - str += fmt.Sprintf("%s: %v %v\t", frame, pose.Pose().Point(), pose.Pose().Orientation().OrientationVectorDegrees()) + str += fmt.Sprintf("%s: %v %v\t", frame, pose.Pose().Point(), *pose.Pose().Orientation().OrientationVectorDegrees()) } } return str @@ -310,16 +310,16 @@ func (e *ExecutionState) CurrentPoses() map[string]*referenceframe.PoseInFrame { // CalculateFrameErrorState takes an ExecutionState and a Frame and calculates the error between the Frame's expected // and actual positions. -func CalculateFrameErrorState(e ExecutionState, frame referenceframe.Frame) (spatialmath.Pose, error) { - currentInputs, ok := e.CurrentInputs()[frame.Name()] +func CalculateFrameErrorState(e ExecutionState, executionFrame, localizationFrame referenceframe.Frame) (spatialmath.Pose, error) { + currentInputs, ok := e.CurrentInputs()[executionFrame.Name()] if !ok { - return nil, fmt.Errorf("could not find frame %s in ExecutionState", frame.Name()) + return nil, newFrameNotFoundError(executionFrame.Name()) } - currentPose, ok := e.CurrentPoses()[frame.Name()] + currentPose, ok := e.CurrentPoses()[localizationFrame.Name()] if !ok { - return nil, fmt.Errorf("could not find frame %s in ExecutionState", frame.Name()) + return nil, newFrameNotFoundError(localizationFrame.Name()) } - currPoseInArc, err := frame.Transform(currentInputs) + currPoseInArc, err := executionFrame.Transform(currentInputs) if err != nil { return nil, err } @@ -334,9 +334,9 @@ func CalculateFrameErrorState(e ExecutionState, frame referenceframe.Frame) (spa if index < 0 || index >= len(path) { return nil, fmt.Errorf("index %d out of bounds for Path of length %d", index, len(path)) } - pose, ok := path[index][frame.Name()] + pose, ok := path[index][executionFrame.Name()] if !ok { - return nil, fmt.Errorf("could not find frame %s in ExecutionState", frame.Name()) + return nil, newFrameNotFoundError(executionFrame.Name()) } if pose.Parent() != currentPose.Parent() { return nil, errors.New("cannot compose two PoseInFrames with different parents") @@ -344,3 +344,8 @@ func CalculateFrameErrorState(e ExecutionState, frame referenceframe.Frame) (spa nominalPose := spatialmath.Compose(pose.Pose(), currPoseInArc) return spatialmath.PoseBetween(nominalPose, currentPose.Pose()), nil } + +// newFrameNotFoundError returns an error indicating that a given frame was not found in the given ExecutionState. +func newFrameNotFoundError(frameName string) error { + return fmt.Errorf("could not find frame %s in ExecutionState", frameName) +} diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 43914509b8d..8b85f650a9a 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -508,18 +508,6 @@ func (pm *planManager) plannerSetupFromMoveRequest( return nil, err // no geometries defined for frame } movingRobotGeometries := movingGeometriesInFrame.Geometries() // solver frame returns geoms in frame World - if pm.useTPspace { - // If we are starting a ptg plan at a different place than the origin, then that translation must be represented in the geometries, - // all of which need to be in the "correct" position when transformed to the world frame. - // At this point in the plan manager, a ptg plan is moving to a goal in the world frame, and the start pose is the base's location - // relative to world. Since nothing providing a geometry knows anything about where the base is relative to world, any geometries - // need to be transformed by the start position to place them correctly in world. - startGeoms := make([]spatialmath.Geometry, 0, len(movingRobotGeometries)) - for _, geometry := range movingRobotGeometries { - startGeoms = append(startGeoms, geometry.Transform(from)) - } - movingRobotGeometries = startGeoms - } // find all geometries that are not moving but are in the frame system staticRobotGeometries := make([]spatialmath.Geometry, 0) @@ -762,6 +750,18 @@ func (pm *planManager) planToRRTGoalMap(plan Plan, goal spatialmath.Pose) (*rrtM // planRelativeWaypoint will solve the solver frame to one individual pose. This is used for solverframes 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) { + anyNonzero := false // Whether non-PTG frames exist + for _, movingFrame := range pm.frame.frames { + if _, isPTGframe := movingFrame.(tpspace.PTGProvider); isPTGframe { + continue + } else if len(movingFrame.DoF()) > 0 { + anyNonzero = true + } + if anyNonzero { + return nil, errors.New("cannot combine ptg with other nonzero DOF frames in a single planning call") + } + } + if request.StartPose == nil { return nil, errors.New("must provide a startPose if solving for PTGs") } @@ -813,7 +813,15 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe if err != nil { return nil, err } + + // re-root the frame system on the relative frame + relativeOnlyFS, err := pm.frame.fss.FrameSystemSubset(request.Frame) + if err != nil { + return nil, err + } + pm.frame.fss = relativeOnlyFS pm.planOpts = opt + opt.SetGoal(goalPos) // Build planner diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index 227ab782621..e6c438d5b06 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -148,8 +148,7 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, } var ptgs []tpspace.PTGSolver - anyPTG := false // Whether PTG frames have been observed - anyNonzero := false // Whether non-PTG frames + anyPTG := false // Whether PTG frames have been observed for _, movingFrame := range frames { if ptgFrame, isPTGframe := movingFrame.(tpspace.PTGProvider); isPTGframe { if anyPTG { @@ -157,11 +156,6 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, } anyPTG = true ptgs = ptgFrame.PTGSolvers() - } else if len(movingFrame.DoF()) > 0 { - anyNonzero = true - } - if anyNonzero && anyPTG { - return nil, errors.New("cannot combine ptg with other nonzero DOF frames in a single planning call") } } @@ -210,13 +204,13 @@ func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame. } interp := make([]frame.Input, 0, len(to)) posIdx := 0 - for _, frame := range sf.frames { - dof := len(frame.DoF()) + posIdx + for _, currFrame := range sf.frames { + dof := len(currFrame.DoF()) + posIdx fromSubset := from[posIdx:dof] toSubset := to[posIdx:dof] posIdx = dof - interpSub, err := frame.Interpolate(fromSubset, toSubset, by) + interpSub, err := currFrame.Interpolate(fromSubset, toSubset, by) if err != nil { return nil, err } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 744e9f6019e..0218fcce0fc 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -257,208 +257,6 @@ func TestTPsmoothing(t *testing.T) { test.That(t, smoothcost, test.ShouldBeLessThan, oldcost) } -func TestPtgCheckPlan(t *testing.T) { - logger := logging.NewTestLogger(t) - roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") - test.That(t, err, test.ShouldBeNil) - geometries := []spatialmath.Geometry{roverGeom} - ackermanFrame, err := tpspace.NewPTGFrameFromKinematicOptions( - "ackframe", - logger, - testTurnRad, - 0, - geometries, - false, - false, - ) - test.That(t, err, test.ShouldBeNil) - - goalPos := spatialmath.NewPoseFromPoint(r3.Vector{X: 5000, Y: 0, Z: 0}) - - fs := referenceframe.NewEmptyFrameSystem("test") - fs.AddFrame(ackermanFrame, fs.World()) - - opt := newBasicPlannerOptions(ackermanFrame) - opt.StartPose = spatialmath.NewZeroPose() - opt.DistanceFunc = ik.NewSquaredNormSegmentMetric(30.) - opt.GoalThreshold = 30. - - test.That(t, err, test.ShouldBeNil) - sf, err := newSolverFrame(fs, ackermanFrame.Name(), referenceframe.World, nil) - test.That(t, err, test.ShouldBeNil) - - mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) - test.That(t, err, test.ShouldBeNil) - tp, _ := mp.(*tpSpaceRRTMotionPlanner) - - nodes, err := tp.plan(context.Background(), goalPos, nil) - test.That(t, err, test.ShouldBeNil) - plan, err := newRRTPlan(nodes, sf, true) - test.That(t, err, test.ShouldBeNil) - - startPose := spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}) - inputs := plan.Trajectory()[0] - - t.Run("base case - validate plan without obstacles", func(t *testing.T) { - executionState := ExecutionState{ - plan: plan, - index: 0, - currentInputs: inputs, - currentPose: map[string]*referenceframe.PoseInFrame{ - ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - }, - } - err = CheckPlan(ackermanFrame, executionState, nil, fs, math.Inf(1), logger) - test.That(t, err, test.ShouldBeNil) - }) - - t.Run("obstacles blocking path", func(t *testing.T) { - obstacle, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2000, 0, 0}), r3.Vector{20, 2000, 1}, "") - test.That(t, err, test.ShouldBeNil) - - geoms := []spatialmath.Geometry{obstacle} - gifs := []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, geoms)} - - worldState, err := referenceframe.NewWorldState(gifs, nil) - test.That(t, err, test.ShouldBeNil) - - executionState := ExecutionState{ - plan: plan, - index: 0, - currentInputs: inputs, - currentPose: map[string]*referenceframe.PoseInFrame{ - ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - }, - } - err = CheckPlan(ackermanFrame, executionState, worldState, fs, math.Inf(1), logger) - test.That(t, err, test.ShouldNotBeNil) - }) - - // create camera_origin frame - cameraOriginFrame, err := referenceframe.NewStaticFrame("camera-origin", spatialmath.NewPoseFromPoint(r3.Vector{0, -20, 0})) - test.That(t, err, test.ShouldBeNil) - err = fs.AddFrame(cameraOriginFrame, ackermanFrame) - test.That(t, err, test.ShouldBeNil) - - // create camera geometry - cameraGeom, err := spatialmath.NewBox( - spatialmath.NewZeroPose(), - r3.Vector{1, 1, 1}, "camera", - ) - test.That(t, err, test.ShouldBeNil) - - // create cameraFrame and add to framesystem - cameraFrame, err := referenceframe.NewStaticFrameWithGeometry( - "camera-frame", spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}), cameraGeom, - ) - test.That(t, err, test.ShouldBeNil) - err = fs.AddFrame(cameraFrame, cameraOriginFrame) - test.That(t, err, test.ShouldBeNil) - - t.Run("obstacles NOT in world frame - no collision - integration test", func(t *testing.T) { - obstacle, err := spatialmath.NewBox( - spatialmath.NewPoseFromPoint(r3.Vector{25000, -40, 0}), - r3.Vector{10, 10, 1}, "obstacle", - ) - test.That(t, err, test.ShouldBeNil) - geoms := []spatialmath.Geometry{obstacle} - gifs := []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(cameraFrame.Name(), geoms)} - - worldState, err := referenceframe.NewWorldState(gifs, nil) - test.That(t, err, test.ShouldBeNil) - executionState := ExecutionState{ - plan: plan, - index: 1, - currentInputs: inputs, - currentPose: map[string]*referenceframe.PoseInFrame{ - ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - }, - } - err = CheckPlan(ackermanFrame, executionState, worldState, fs, math.Inf(1), logger) - test.That(t, err, test.ShouldBeNil) - }) - t.Run("obstacles NOT in world frame cause collision - integration test", func(t *testing.T) { - obstacle, err := spatialmath.NewBox( - spatialmath.NewPoseFromPoint(r3.Vector{2500, 20, 0}), - r3.Vector{10, 2000, 1}, "obstacle", - ) - test.That(t, err, test.ShouldBeNil) - geoms := []spatialmath.Geometry{obstacle} - gifs := []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(cameraFrame.Name(), geoms)} - - worldState, err := referenceframe.NewWorldState(gifs, nil) - test.That(t, err, test.ShouldBeNil) - - executionState := ExecutionState{ - plan: plan, - index: 1, - currentInputs: inputs, - currentPose: map[string]*referenceframe.PoseInFrame{ - ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - }, - } - err = CheckPlan(ackermanFrame, executionState, worldState, fs, math.Inf(1), logger) - test.That(t, err, test.ShouldNotBeNil) - }) - t.Run("checking from partial-plan, ensure success with obstacles - integration test", func(t *testing.T) { - // create obstacle behind where we are - obstacle, err := spatialmath.NewBox( - spatialmath.NewPoseFromPoint(r3.Vector{0, 20, 0}), - r3.Vector{10, 200, 1}, "obstacle", - ) - test.That(t, err, test.ShouldBeNil) - geoms := []spatialmath.Geometry{obstacle} - gifs := []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, geoms)} - - worldState, err := referenceframe.NewWorldState(gifs, nil) - test.That(t, err, test.ShouldBeNil) - - ov := spatialmath.NewOrientationVector().Degrees() - ov.OZ = 1.0000000000000004 - ov.Theta = -101.42430306111874 - vector := r3.Vector{669.0803080526971, 234.2834571597409, 0} - - startPose := spatialmath.NewPose(vector, ov) - - executionState := ExecutionState{ - plan: plan, - index: 2, - currentInputs: inputs, - currentPose: map[string]*referenceframe.PoseInFrame{ - ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - }, - } - err = CheckPlan(ackermanFrame, executionState, worldState, fs, math.Inf(1), logger) - test.That(t, err, test.ShouldBeNil) - }) - t.Run("verify partial plan with non-nil errorState and obstacle", func(t *testing.T) { - // create obstacle which is behind where the robot already is, but is on the path it has already traveled - box, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 1}, "obstacle") - test.That(t, err, test.ShouldBeNil) - gifs := []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, []spatialmath.Geometry{box})} - - worldState, err := referenceframe.NewWorldState(gifs, nil) - test.That(t, err, test.ShouldBeNil) - - remainingPlan, err := RemainingPlan(plan, 2) - test.That(t, err, test.ShouldBeNil) - - pathPose := remainingPlan.Path()[0][ackermanFrame.Name()].Pose() - startPose := spatialmath.NewPose(r3.Vector{0, 1000, 0}, pathPose.Orientation()) - - executionState := ExecutionState{ - plan: plan, - index: 2, - currentInputs: inputs, - currentPose: map[string]*referenceframe.PoseInFrame{ - ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - }, - } - err = CheckPlan(ackermanFrame, executionState, worldState, fs, math.Inf(1), logger) - test.That(t, err, test.ShouldBeNil) - }) -} - func planToTpspaceRec(plan Plan, f referenceframe.Frame) ([]node, error) { nodes := []node{} for _, inp := range plan.Trajectory() { diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 0fe4d65e9a3..4bbf36726e7 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -445,3 +445,122 @@ func (rf rotationalFrame) MarshalJSON() ([]byte, error) { return json.Marshal(temp) } + +type poseFrame struct { + *baseFrame + geometries []spatial.Geometry +} + +// NewPoseFrame creates an orientation vector frame, i.e a frame with +// 7 degrees of freedom: X, Y, Z, OX, OY, OZ, and Theta in radians. +func NewPoseFrame(name string, geometry []spatial.Geometry) (Frame, error) { + limits := []Limit{ + {Min: math.Inf(-1), Max: math.Inf(1)}, // X + {Min: math.Inf(-1), Max: math.Inf(1)}, // Y + {Min: math.Inf(-1), Max: math.Inf(1)}, // Z + {Min: math.Inf(-1), Max: math.Inf(1)}, // OX + {Min: math.Inf(-1), Max: math.Inf(1)}, // OY + {Min: math.Inf(-1), Max: math.Inf(1)}, // OZ + {Min: math.Inf(-1), Max: math.Inf(1)}, // Theta + } + return &poseFrame{ + &baseFrame{name, limits}, + geometry, + }, nil +} + +// Transform on the poseFrame acts as the identity function. Whatever inputs are given are directly translated +// in a 7DoF pose. We note that theta should be in radians. +func (pf *poseFrame) Transform(inputs []Input) (spatial.Pose, error) { + if err := pf.baseFrame.validInputs(inputs); err != nil { + return nil, err + } + return spatial.NewPose( + r3.Vector{X: inputs[0].Value, Y: inputs[1].Value, Z: inputs[2].Value}, + &spatial.OrientationVector{ + OX: inputs[3].Value, + OY: inputs[4].Value, + OZ: inputs[5].Value, + Theta: inputs[6].Value, + }, + ), nil +} + +// Interpolate interpolates the given amount between the two sets of inputs. +func (pf *poseFrame) Interpolate(from, to []Input, by float64) ([]Input, error) { + if err := pf.baseFrame.validInputs(from); err != nil { + return nil, NewIncorrectInputLengthError(len(from), len(pf.DoF())) + } + if err := pf.baseFrame.validInputs(to); err != nil { + return nil, NewIncorrectInputLengthError(len(to), len(pf.DoF())) + } + fromPose, err := pf.Transform(from) + if err != nil { + return nil, err + } + toPose, err := pf.Transform(to) + if err != nil { + return nil, err + } + interpolatedPose := spatial.Interpolate(fromPose, toPose, by) + return PoseToInputs(interpolatedPose), nil +} + +// Geometries returns an object representing the 3D space associeted with the staticFrame. +func (pf *poseFrame) Geometries(inputs []Input) (*GeometriesInFrame, error) { + transformByPose, err := pf.Transform(inputs) + if err != nil { + return nil, err + } + if len(pf.geometries) == 0 { + return NewGeometriesInFrame(pf.name, []spatial.Geometry{}), nil + } + transformedGeometries := []spatial.Geometry{} + for _, geom := range pf.geometries { + transformedGeometries = append(transformedGeometries, geom.Transform(transformByPose)) + } + return NewGeometriesInFrame(pf.name, transformedGeometries), nil +} + +// DoF returns the number of degrees of freedom within a model. +func (pf *poseFrame) DoF() []Limit { + return pf.limits +} + +// MarshalJSON serializes a Model. +func (pf *poseFrame) MarshalJSON() ([]byte, error) { + return nil, errors.New("serializing a poseFrame is currently not supported") +} + +// InputFromProtobuf converts pb.JointPosition to inputs. +func (pf *poseFrame) InputFromProtobuf(jp *pb.JointPositions) []Input { + n := make([]Input, len(jp.Values)) + for idx, d := range jp.Values[:len(jp.Values)-1] { + n[idx] = Input{d} + } + n[len(jp.Values)-1] = Input{utils.DegToRad(jp.Values[len(jp.Values)-1])} + return n +} + +// ProtobufFromInput converts inputs to pb.JointPosition. +func (pf *poseFrame) ProtobufFromInput(input []Input) *pb.JointPositions { + n := make([]float64, len(input)) + for idx, a := range input[:len(input)-1] { + n[idx] = a.Value + } + n[len(input)-1] = utils.RadToDeg(input[len(input)-1].Value) + return &pb.JointPositions{Values: n} +} + +// PoseToInputs is a convenience method for turning a pose into a slice of inputs +// in the form [X, Y, Z, OX, OY, OZ, Theta (in radians)] +// This is the format that is expected by the poseFrame type and should not be used with other frames. +func PoseToInputs(p spatial.Pose) []Input { + return FloatsToInputs([]float64{ + p.Point().X, p.Point().Y, p.Point().Z, + p.Orientation().OrientationVectorRadians().OX, + p.Orientation().OrientationVectorRadians().OY, + p.Orientation().OrientationVectorRadians().OZ, + p.Orientation().OrientationVectorRadians().Theta, + }) +} diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 5ccf2cc072f..eeee2272c1a 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -4,6 +4,7 @@ import ( "context" "fmt" "math" + "strings" "testing" "time" @@ -23,6 +24,7 @@ import ( _ "go.viam.com/rdk/components/register" "go.viam.com/rdk/config" "go.viam.com/rdk/logging" + "go.viam.com/rdk/motionplan" "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" @@ -780,8 +782,7 @@ func TestStoppableMoveFunctions(t *testing.T) { }) } -func TestGetTransientDetections(t *testing.T) { - t.Parallel() +func TestGetTransientDetectionsSlam(t *testing.T) { ctx := context.Background() _, ms, closeFunc := CreateMoveOnMapTestEnvironment( @@ -828,50 +829,142 @@ func TestGetTransientDetections(t *testing.T) { type testCase struct { name string - f spatialmath.Pose detectionPose spatialmath.Pose } testCases := []testCase{ { name: "relative - SLAM/base theta does not matter", - f: spatialmath.NewZeroPose(), detectionPose: spatialmath.NewPose(r3.Vector{4, 10, -8}, &spatialmath.OrientationVectorDegrees{OY: 1, Theta: -90}), }, + } + + testFn := func(t *testing.T, tc testCase) { + t.Helper() + transformedGeoms, err := mr.getTransientDetections(ctx, injectedVis, camera.Named("test-camera")) + test.That(t, err, test.ShouldBeNil) + test.That(t, transformedGeoms.Parent(), test.ShouldEqual, referenceframe.World) + test.That(t, len(transformedGeoms.Geometries()), test.ShouldEqual, 1) + test.That(t, spatialmath.PoseAlmostEqual(transformedGeoms.Geometries()[0].Pose(), tc.detectionPose), test.ShouldBeTrue) + } + + for _, tc := range testCases { + c := tc // needed to workaround loop variable not being captured by func literals + t.Run(c.name, func(t *testing.T) { + testFn(t, c) + }) + } +} + +func TestGetTransientDetectionsMath(t *testing.T) { + ctx := context.Background() + + _, ms, closeFunc := CreateMoveOnGlobeTestEnvironment( + ctx, t, geo.NewPoint(0, 0), 300, spatialmath.NewZeroPose(), + ) + t.Cleanup(func() { closeFunc(ctx) }) + + destinationPose := spatialmath.NewPoseFromPoint(r3.Vector{0, 3000, 0}) + geoPoseOrigin := spatialmath.NewGeoPose(geo.NewPoint(0, 0), 0) + destinationGeoPose := spatialmath.PoseToGeoPose(geoPoseOrigin, destinationPose) + + moveSensor, ok := ms.(*builtIn).movementSensors[resource.NewName(movementsensor.API, moveSensorName)] + test.That(t, ok, test.ShouldBeTrue) + + // construct move request + moveReq := motion.MoveOnGlobeReq{ + ComponentName: base.Named("test-base"), + Destination: destinationGeoPose.Location(), + MovementSensorName: moveSensor.Name(), + MotionCfg: &motion.MotionConfiguration{ + PlanDeviationMM: 1, + ObstacleDetectors: []motion.ObstacleDetectorName{ + {VisionServiceName: vision.Named("injectedVisionSvc"), CameraName: camera.Named("test-camera")}, + }, + }, + } + + planExecutor, err := ms.(*builtIn).newMoveOnGlobeRequest(ctx, moveReq, nil, 0) + test.That(t, err, test.ShouldBeNil) + + mr, ok := planExecutor.(*moveRequest) + test.That(t, ok, test.ShouldBeTrue) + + getTransientDetectionMock := func(currentPose, obstaclePose spatialmath.Pose) []spatialmath.Geometry { + inputMap, _, err := mr.fsService.CurrentInputs(ctx) + test.That(t, err, test.ShouldBeNil) + kbInputs := make([]referenceframe.Input, len(mr.kinematicBase.Kinematics().DoF())) + kbInputs = append(kbInputs, referenceframe.PoseToInputs(currentPose)...) + inputMap[mr.kinematicBase.Name().ShortName()] = kbInputs + + fakeDetectionGeometry, err := spatialmath.NewBox( + obstaclePose, r3.Vector{X: 8, Y: 4, Z: 2}, "box", + ) + test.That(t, err, test.ShouldBeNil) + + cam, ok := ms.(*builtIn).components[resource.NewName(camera.API, "injectedCamera")] + test.That(t, ok, test.ShouldBeTrue) + + tf, err := mr.localizingFS.Transform( + inputMap, + referenceframe.NewGeometriesInFrame( + cam.Name().ShortName(), []spatialmath.Geometry{fakeDetectionGeometry}, + ), + referenceframe.World, + ) + test.That(t, err, test.ShouldBeNil) + + worldGifs, ok := tf.(*referenceframe.GeometriesInFrame) + test.That(t, ok, test.ShouldBeTrue) + return worldGifs.Geometries() + } + + type testCase struct { + name string + basePose, obstaclePose, expectedGeomPose spatialmath.Pose + } + + testCases := []testCase{ { - name: "absolute - SLAM theta: 0, base theta: -90 == 270", - f: spatialmath.NewPose(r3.Vector{-4, -10, 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: -90}), - detectionPose: spatialmath.NewPose(r3.Vector{6, -14, -8}, &spatialmath.OrientationVectorDegrees{OX: 1, Theta: -90}), + name: "base at zero pose", + basePose: spatialmath.NewPose(r3.Vector{X: 0, Y: 0, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1}), + obstaclePose: spatialmath.NewPoseFromPoint(r3.Vector{X: 10, Y: -30, Z: 1500}), + expectedGeomPose: spatialmath.NewPose(r3.Vector{X: 10, Y: 1500, Z: 30}, &spatialmath.OrientationVectorDegrees{OY: 1, Theta: -90}), }, { - name: "absolute - SLAM theta: 90, base theta: 0", - f: spatialmath.NewPose(r3.Vector{-4, -10, 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 0}), - detectionPose: spatialmath.NewPose(r3.Vector{0, 0, -8}, &spatialmath.OrientationVectorDegrees{OY: 1, Theta: -90}), + name: "base slightly from origin with zero theta", + basePose: spatialmath.NewPose(r3.Vector{X: 3, Y: 9, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1}), + obstaclePose: spatialmath.NewPoseFromPoint(r3.Vector{X: 10, Y: -30, Z: 1500}), + expectedGeomPose: spatialmath.NewPose(r3.Vector{X: 13, Y: 1509, Z: 30}, &spatialmath.OrientationVectorDegrees{OY: 1, Theta: -90}), }, { - name: "absolute - SLAM theta: 180, base theta: 90", - f: spatialmath.NewPose(r3.Vector{-4, -10, 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 90}), - detectionPose: spatialmath.NewPose(r3.Vector{-14, -6, -8}, &spatialmath.OrientationVectorDegrees{OX: -1, Theta: -90}), + name: "base at origin with non-zero theta", + basePose: spatialmath.NewPose(r3.Vector{X: 0, Y: 0, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: -45}), + obstaclePose: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: math.Sqrt2}), + expectedGeomPose: spatialmath.NewPose( + r3.Vector{X: 1, Y: 1, Z: 0}, &spatialmath.OrientationVectorDegrees{OX: 0.7071067811865478, OY: 0.7071067811865474, Theta: -90}, + ), }, { - name: "absolute - SLAM theta: 270, base theta: 180", - f: spatialmath.NewPose(r3.Vector{-4, -10, 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 180}), - detectionPose: spatialmath.NewPose(r3.Vector{-8, -20, -8}, &spatialmath.OrientationVectorDegrees{OY: -1, Theta: -90}), + name: "base slightly from origin with non-zero theta", + basePose: spatialmath.NewPose(r3.Vector{X: 3, Y: 9, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: -45}), + obstaclePose: spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: math.Sqrt2}), + expectedGeomPose: spatialmath.NewPose( + r3.Vector{X: 4, Y: 10, Z: 0}, &spatialmath.OrientationVectorDegrees{OX: 0.7071067811865478, OY: 0.7071067811865474, Theta: -90}, + ), }, } testFn := func(t *testing.T, tc testCase) { t.Helper() - transformedGeoms, err := mr.getTransientDetections(ctx, injectedVis, camera.Named("test-camera"), tc.f) - test.That(t, err, test.ShouldBeNil) - test.That(t, transformedGeoms.Parent(), test.ShouldEqual, referenceframe.World) - test.That(t, len(transformedGeoms.Geometries()), test.ShouldEqual, 1) - test.That(t, spatialmath.PoseAlmostEqual(transformedGeoms.Geometries()[0].Pose(), tc.detectionPose), test.ShouldBeTrue) + transformedGeoms := getTransientDetectionMock(tc.basePose, tc.obstaclePose) + test.That(t, len(transformedGeoms), test.ShouldEqual, 1) + + test.That(t, spatialmath.PoseAlmostEqual(transformedGeoms[0].Pose(), tc.expectedGeomPose), test.ShouldBeTrue) } for _, tc := range testCases { c := tc // needed to workaround loop variable not being captured by func literals t.Run(c.name, func(t *testing.T) { - t.Parallel() testFn(t, c) }) } @@ -933,3 +1026,210 @@ func TestBaseInputs(t *testing.T) { err := kb.GoToInputs(ctx, []referenceframe.Input{{0}, {0.001 + math.Pi/2}, {0}, {91}}) test.That(t, err, test.ShouldBeNil) } + +func TestCheckPlan(t *testing.T) { + ctx := context.Background() + logger := logging.NewTestLogger(t) + origin := geo.NewPoint(0, 0) + + localizer, ms, closeFunc := CreateMoveOnGlobeTestEnvironment(ctx, t, origin, 30, spatialmath.NewZeroPose()) + defer closeFunc(ctx) + + dst := geo.NewPoint(origin.Lat(), origin.Lng()+5e-5) + // Note: spatialmath.GeoPointToPoint(dst, origin) produces r3.Vector{5559.746, 0, 0} + + movementSensor, ok := localizer.(movementsensor.MovementSensor) + test.That(t, ok, test.ShouldBeTrue) + + fakeBase, ok := ms.(*builtIn).components[baseResource] + test.That(t, ok, test.ShouldBeTrue) + + req := motion.MoveOnGlobeReq{ + ComponentName: fakeBase.Name(), + Destination: dst, + MovementSensorName: movementSensor.Name(), + } + + // construct move request + planExecutor, err := ms.(*builtIn).newMoveOnGlobeRequest(ctx, req, nil, 0) + test.That(t, err, test.ShouldBeNil) + mr, ok := planExecutor.(*moveRequest) + test.That(t, ok, test.ShouldBeTrue) + + // construct plan + plan, err := mr.Plan(ctx) + test.That(t, err, test.ShouldBeNil) + test.That(t, len(plan.Path()), test.ShouldBeGreaterThan, 2) + + wrapperFrame := mr.localizingFS.Frame(mr.kinematicBase.Name().Name) + + currentInputs := map[string][]referenceframe.Input{ + mr.kinematicBase.Kinematics().Name(): { + {Value: 0}, // ptg index + {Value: 0}, // trajectory alpha within ptg + {Value: 0}, // start distance along trajectory index + {Value: 0}, // end distace along trajectory index + }, + mr.kinematicBase.LocalizationFrame().Name(): { + {Value: 0}, // X + {Value: 0}, // Y + {Value: 0}, // Z + {Value: 0}, // OX + {Value: 0}, // OY + {Value: 1}, // OZ + {Value: 0}, // Theta + }, + } + + baseExecutionState, err := motionplan.NewExecutionState( + plan, 0, currentInputs, + map[string]*referenceframe.PoseInFrame{ + mr.kinematicBase.LocalizationFrame().Name(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewPose( + r3.Vector{X: 0, Y: 0, Z: 0}, + &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 0}, + )), + }, + ) + test.That(t, err, test.ShouldBeNil) + + augmentedBaseExecutionState, err := mr.augmentBaseExecutionState(baseExecutionState) + test.That(t, err, test.ShouldBeNil) + + t.Run("base case - validate plan without obstacles", func(t *testing.T) { + err = motionplan.CheckPlan(wrapperFrame, augmentedBaseExecutionState, nil, mr.localizingFS, math.Inf(1), logger) + test.That(t, err, test.ShouldBeNil) + }) + + t.Run("obstacles blocking path", func(t *testing.T) { + obstacle, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{2000, 0, 0}), r3.Vector{20, 2000, 1}, "") + test.That(t, err, test.ShouldBeNil) + + geoms := []spatialmath.Geometry{obstacle} + gifs := []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, geoms)} + + worldState, err := referenceframe.NewWorldState(gifs, nil) + test.That(t, err, test.ShouldBeNil) + + err = motionplan.CheckPlan(wrapperFrame, augmentedBaseExecutionState, worldState, mr.localizingFS, math.Inf(1), logger) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, strings.Contains(err.Error(), "found constraint violation or collision in segment between"), test.ShouldBeTrue) + }) + + // create camera_origin frame + cameraOriginFrame, err := referenceframe.NewStaticFrame("camera-origin", spatialmath.NewPoseFromPoint(r3.Vector{0, -20, 0})) + test.That(t, err, test.ShouldBeNil) + err = mr.localizingFS.AddFrame(cameraOriginFrame, wrapperFrame) + test.That(t, err, test.ShouldBeNil) + + // create camera geometry + cameraGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{1, 1, 1}, "camera") + test.That(t, err, test.ShouldBeNil) + + // create cameraFrame and add to framesystem + cameraFrame, err := referenceframe.NewStaticFrameWithGeometry( + "camera-frame", spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}), cameraGeom, + ) + test.That(t, err, test.ShouldBeNil) + err = mr.localizingFS.AddFrame(cameraFrame, cameraOriginFrame) + test.That(t, err, test.ShouldBeNil) + inputs := augmentedBaseExecutionState.CurrentInputs() + inputs[cameraFrame.Name()] = referenceframe.FloatsToInputs(make([]float64, len(cameraFrame.DoF()))) + executionStateWithCamera, err := motionplan.NewExecutionState( + augmentedBaseExecutionState.Plan(), augmentedBaseExecutionState.Index(), + inputs, augmentedBaseExecutionState.CurrentPoses(), + ) + test.That(t, err, test.ShouldBeNil) + + t.Run("obstacles NOT in world frame - no collision - integration test", func(t *testing.T) { + obstacle, err := spatialmath.NewBox( + spatialmath.NewPoseFromPoint(r3.Vector{25000, -40, 0}), + r3.Vector{10, 10, 1}, "obstacle", + ) + test.That(t, err, test.ShouldBeNil) + geoms := []spatialmath.Geometry{obstacle} + gifs := []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(cameraFrame.Name(), geoms)} + + worldState, err := referenceframe.NewWorldState(gifs, nil) + test.That(t, err, test.ShouldBeNil) + + err = motionplan.CheckPlan(wrapperFrame, executionStateWithCamera, worldState, mr.localizingFS, math.Inf(1), logger) + test.That(t, err, test.ShouldBeNil) + }) + + t.Run("obstacles NOT in world frame cause collision - integration test", func(t *testing.T) { + obstacle, err := spatialmath.NewBox( + spatialmath.NewPoseFromPoint(r3.Vector{2500, 20, 0}), + r3.Vector{10, 2000, 1}, "obstacle", + ) + test.That(t, err, test.ShouldBeNil) + geoms := []spatialmath.Geometry{obstacle} + gifs := []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(cameraFrame.Name(), geoms)} + + worldState, err := referenceframe.NewWorldState(gifs, nil) + test.That(t, err, test.ShouldBeNil) + + err = motionplan.CheckPlan(wrapperFrame, executionStateWithCamera, worldState, mr.localizingFS, math.Inf(1), logger) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, strings.Contains(err.Error(), "found constraint violation or collision in segment between"), test.ShouldBeTrue) + }) + + currentInputs = map[string][]referenceframe.Input{ + mr.kinematicBase.Kinematics().Name(): { + {Value: 0}, // ptg index + {Value: 0}, // trajectory alpha within ptg + {Value: 0}, // start distance along trajectory index + {Value: 0}, // end distace along trajectory index + }, + mr.kinematicBase.LocalizationFrame().Name(): { + {Value: 2779.937}, // X + {Value: 0}, // Y + {Value: 0}, // Z + {Value: 0}, // OX + {Value: 0}, // OY + {Value: 1}, // OZ + {Value: -math.Pi}, // Theta + }, + cameraFrame.Name(): referenceframe.FloatsToInputs(make([]float64, len(cameraFrame.DoF()))), + } + currentPoses := map[string]*referenceframe.PoseInFrame{ + mr.kinematicBase.LocalizationFrame().Name(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewPose( + r3.Vector{X: 2779.937, Y: 0, Z: 0}, + &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: -90}, + )), + } + + newExecutionState, err := motionplan.NewExecutionState(plan, 2, currentInputs, currentPoses) + test.That(t, err, test.ShouldBeNil) + updatedExecutionState, err := mr.augmentBaseExecutionState(newExecutionState) + test.That(t, err, test.ShouldBeNil) + + t.Run("checking from partial-plan, ensure success with obstacles - integration test", func(t *testing.T) { + // create obstacle behind where we are + obstacle, err := spatialmath.NewBox( + spatialmath.NewPoseFromPoint(r3.Vector{0, 20, 0}), + r3.Vector{10, 200, 1}, "obstacle", + ) + test.That(t, err, test.ShouldBeNil) + geoms := []spatialmath.Geometry{obstacle} + gifs := []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, geoms)} + + worldState, err := referenceframe.NewWorldState(gifs, nil) + test.That(t, err, test.ShouldBeNil) + + err = motionplan.CheckPlan(wrapperFrame, updatedExecutionState, worldState, mr.localizingFS, math.Inf(1), logger) + test.That(t, err, test.ShouldBeNil) + }) + + t.Run("verify partial plan with non-nil errorState and obstacle", func(t *testing.T) { + // create obstacle which is behind where the robot already is, but is on the path it has already traveled + box, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 1}, "obstacle") + test.That(t, err, test.ShouldBeNil) + gifs := []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, []spatialmath.Geometry{box})} + + worldState, err := referenceframe.NewWorldState(gifs, nil) + test.That(t, err, test.ShouldBeNil) + + err = motionplan.CheckPlan(wrapperFrame, updatedExecutionState, worldState, mr.localizingFS, math.Inf(1), logger) + test.That(t, err, test.ShouldBeNil) + }) +} diff --git a/services/motion/builtin/move_on_globe_test.go b/services/motion/builtin/move_on_globe_test.go index 57a93e6b2c0..f3675779f89 100644 --- a/services/motion/builtin/move_on_globe_test.go +++ b/services/motion/builtin/move_on_globe_test.go @@ -313,8 +313,10 @@ func TestObstacleReplanningGlobe(t *testing.T) { // The camera is parented to the base. Thus, this will always see an obstacle 300mm in front of where the base is. // Note: for CreateMoveOnGlobeTestEnvironment, the camera is given an orientation such that it is pointing left, not // forwards. Thus, an obstacle in front of the base will be seen as being in +X. - obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: 300, Y: 0, Z: 0}) - box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 20, Y: 20, Z: 10}, caseName) + // NOTE: the orientation of the camera is OY:1, Theta: -90 degrees, this influences both the position and dimensions of + // the observed obstacle in this test case. + obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 300}) + box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 40, Y: 10, Z: 40}, caseName) test.That(t, err, test.ShouldBeNil) detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) @@ -330,10 +332,10 @@ func TestObstacleReplanningGlobe(t *testing.T) { name: "ensure replan reaching goal", getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { caseName := "test-case-3" - // This base will always see an obstacle 800mm in front of it, triggering several replans. + // This base will always see an obstacle 900mm in front of it, triggering several replans. // However, enough replans should eventually get it to its goal. - obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: 900, Y: 0, Z: 0}) - box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 1, Y: 1, Z: 10}, caseName) + obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 900}) + box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 1, Y: 10, Z: 1}, caseName) test.That(t, err, test.ShouldBeNil) detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) @@ -362,7 +364,7 @@ func TestObstacleReplanningGlobe(t *testing.T) { ctx, t, gpsOrigin, - 1, + 2, nil, ) defer closeFunc(ctx) diff --git a/services/motion/builtin/move_on_map_test.go b/services/motion/builtin/move_on_map_test.go index df188de01b1..35e777616f8 100644 --- a/services/motion/builtin/move_on_map_test.go +++ b/services/motion/builtin/move_on_map_test.go @@ -2,6 +2,7 @@ package builtin import ( "context" + "strings" "testing" "time" @@ -265,7 +266,6 @@ func TestMoveOnMapStaticObs(t *testing.T) { // place obstacle in opposte position and show that the generate path // collides with obstacleLeft - wrldSt, err := referenceframe.NewWorldState( []*referenceframe.GeometriesInFrame{ referenceframe.NewGeometriesInFrame( @@ -275,27 +275,52 @@ func TestMoveOnMapStaticObs(t *testing.T) { }, nil, ) test.That(t, err, test.ShouldBeNil) - executionState, err := motionplan.NewExecutionState( - plan, - 1, - referenceframe.StartPositions(mr.planRequest.FrameSystem), + + currentInputs := map[string][]referenceframe.Input{ + mr.kinematicBase.Kinematics().Name(): { + {Value: 0}, // ptg index + {Value: 0}, // trajectory alpha within ptg + {Value: 0}, // start distance along trajectory index + {Value: 0}, // end distace along trajectory index + }, + mr.kinematicBase.LocalizationFrame().Name(): { + {Value: 587}, // X + {Value: -808}, // Y + {Value: 0}, // Z + {Value: 0}, // OX + {Value: 0}, // OY + {Value: 1}, // OZ + {Value: 0}, // Theta + }, + } + + baseExecutionState, err := motionplan.NewExecutionState( + plan, 1, currentInputs, map[string]*referenceframe.PoseInFrame{ - mr.planRequest.Frame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewPose( + mr.kinematicBase.LocalizationFrame().Name(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewPose( r3.Vector{X: 0.58772e3, Y: -0.80826e3, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 0}, )), }, ) + test.That(t, err, test.ShouldBeNil) + + augmentedBaseExecutionState, err := mr.augmentBaseExecutionState(baseExecutionState) + test.That(t, err, test.ShouldBeNil) + + wrapperFrame := mr.localizingFS.Frame(mr.kinematicBase.Name().Name) + test.That(t, err, test.ShouldBeNil) err = motionplan.CheckPlan( - mr.planRequest.Frame, - executionState, + wrapperFrame, + augmentedBaseExecutionState, wrldSt, - mr.planRequest.FrameSystem, + mr.localizingFS, lookAheadDistanceMM, logger, ) test.That(t, err, test.ShouldNotBeNil) + test.That(t, strings.Contains(err.Error(), "found constraint violation or collision in segment between"), test.ShouldBeTrue) }) t.Run("fail due to obstacles enclosing goals", func(t *testing.T) { diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index f6ea872db5a..a1193cc3410 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -16,6 +16,7 @@ import ( "go.viam.com/rdk/components/base/kinematicbase" "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan" + "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" @@ -58,7 +59,6 @@ type moveRequest struct { requestType requestType // geoPoseOrigin is only set if requestType == requestTypeMoveOnGlobe geoPoseOrigin *spatialmath.GeoPose - poseOrigin spatialmath.Pose logger logging.Logger config *validatedMotionConfiguration planRequest *motionplan.PlanRequest @@ -67,6 +67,13 @@ type moveRequest struct { obstacleDetectors map[vision.Service][]resource.Name replanCostFactor float64 fsService framesystem.Service + // localizingFS is used for placing observed transient obstacles into their absolute world position when + // they are observed. It is also used by CheckPlan to perform collision checking. + // The localizingFS combines a kinematic bases localization and kinematics(aka execution) frames into a + // singulare frame moniker wrapperFrame. The wrapperFrame allows us to position the geometries of the base + // using only provided referenceframe.Input values and we do not have to compose with a separate pose + /// to absolutely position ourselves in the world frame. + localizingFS referenceframe.FrameSystem executeBackgroundWorkers *sync.WaitGroup responseChan chan moveResponse @@ -97,7 +104,7 @@ func (mr *moveRequest) Plan(ctx context.Context) (motionplan.Plan, error) { gifs := []*referenceframe.GeometriesInFrame{} for visSrvc, cameraNames := range mr.obstacleDetectors { for _, camName := range cameraNames { - transientGifs, err := mr.getTransientDetections(ctx, visSrvc, camName, mr.poseOrigin) + transientGifs, err := mr.getTransientDetections(ctx, visSrvc, camName) if err != nil { return nil, err } @@ -159,7 +166,7 @@ func (mr *moveRequest) deviatedFromPlan(ctx context.Context, plan motionplan.Pla if err != nil { return state.ExecuteResponse{}, err } - errorState, err := motionplan.CalculateFrameErrorState(executionState, mr.kinematicBase.Kinematics()) + errorState, err := motionplan.CalculateFrameErrorState(executionState, mr.kinematicBase.Kinematics(), mr.kinematicBase.LocalizationFrame()) if err != nil { return state.ExecuteResponse{}, err } @@ -180,7 +187,6 @@ func (mr *moveRequest) getTransientDetections( ctx context.Context, visSrvc vision.Service, camName resource.Name, - transformBy spatialmath.Pose, ) (*referenceframe.GeometriesInFrame, error) { mr.logger.CDebugf(ctx, "proceeding to get detections from vision service: %s with camera: %s", @@ -188,23 +194,30 @@ func (mr *moveRequest) getTransientDetections( camName.ShortName(), ) - detections, err := visSrvc.GetObjectPointClouds(ctx, camName.Name, nil) + baseExecutionState, err := mr.kinematicBase.ExecutionState(ctx) + if err != nil { + return nil, err + } + // the inputMap informs where we are in the world + // the inputMap will be used downstream to transform the observed geometry from the camera frame + // into the world frame + inputMap, _, err := mr.fsService.CurrentInputs(ctx) if err != nil { return nil, err } + kbInputs := make([]referenceframe.Input, len(mr.kinematicBase.Kinematics().DoF())) + kbInputs = append(kbInputs, referenceframe.PoseToInputs( + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose(), + )...) + inputMap[mr.kinematicBase.Name().ShortName()] = kbInputs - cameraOrigin := referenceframe.NewPoseInFrame(camName.ShortName(), spatialmath.NewZeroPose()) - cameraPoseInBaseFrame, err := mr.fsService.TransformPose(ctx, cameraOrigin, mr.kinematicBase.Name().ShortName(), nil) + detections, err := visSrvc.GetObjectPointClouds(ctx, camName.Name, nil) if err != nil { - mr.logger.CDebugf(ctx, - "we assume the base named: %s is coincident with the camera named: %s due to err: %v", - mr.kinematicBase.Name().ShortName(), camName.ShortName(), err.Error(), - ) - cameraPoseInBaseFrame = cameraOrigin + return nil, err } // transformed detections - transformedGeoms := []spatialmath.Geometry{} + transientGeoms := []spatialmath.Geometry{} for i, detection := range detections { geometry := detection.Geometry // update the label of the geometry so we know it is transient @@ -214,14 +227,24 @@ func (mr *moveRequest) getTransientDetections( } geometry.SetLabel(label) - // transform the geometry to be relative to the base frame which is +Y forwards - relativeGeom := geometry.Transform(cameraPoseInBaseFrame.Pose()) - - // apply any transformation on the geometry defined a priori by the caller - transformedGeom := relativeGeom.Transform(transformBy) - transformedGeoms = append(transformedGeoms, transformedGeom) + // the geometry is originally in the frame of the camera that observed it + // here we use a framesystem which has the wrapper frame to position the geometry + // in the world frame + tf, err := mr.localizingFS.Transform( + inputMap, + referenceframe.NewGeometriesInFrame(camName.ShortName(), []spatialmath.Geometry{geometry}), + referenceframe.World, + ) + if err != nil { + return nil, err + } + worldGifs, ok := tf.(*referenceframe.GeometriesInFrame) + if !ok { + return nil, errors.New("unable to assert referenceframe.Transformable into *referenceframe.GeometriesInFrame") + } + transientGeoms = append(transientGeoms, worldGifs.Geometries()...) } - return referenceframe.NewGeometriesInFrame(referenceframe.World, transformedGeoms), nil + return referenceframe.NewGeometriesInFrame(referenceframe.World, transientGeoms), nil } // obstaclesIntersectPlan takes a list of waypoints and an index of a waypoint on that Plan and reports an error indicating @@ -244,23 +267,17 @@ func (mr *moveRequest) obstaclesIntersectPlan( return state.ExecuteResponse{}, err } - // get the current position of the base - currentPosition, err := mr.kinematicBase.CurrentPosition(ctx) - if err != nil { - return state.ExecuteResponse{}, err - } - for visSrvc, cameraNames := range mr.obstacleDetectors { for _, camName := range cameraNames { // Note: detections are initially observed from the camera frame but must be transformed to be in // world frame. We cannot use the inputs of the base to transform the detections since they are relative. - gifs, err := mr.getTransientDetections(ctx, visSrvc, camName, currentPosition.Pose()) + gifs, err := mr.getTransientDetections(ctx, visSrvc, camName) if err != nil { return state.ExecuteResponse{}, err } if len(gifs.Geometries()) == 0 { - mr.logger.CDebug(ctx, "will not check if obstacles intersect path since nothing was detected") - return state.ExecuteResponse{}, nil + mr.logger.CDebug(ctx, "no obstacles detected") + continue } // construct new worldstate @@ -278,29 +295,25 @@ func (mr *moveRequest) obstaclesIntersectPlan( // build representation of frame system's inputs // TODO(pl): in the case where we have e.g. an arm (not moving) mounted on a base, we should be passing its current // configuration rather than the zero inputs - inputMap := referenceframe.StartPositions(mr.planRequest.FrameSystem) - inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().ShortName()] - executionState, err := motionplan.NewExecutionState( - baseExecutionState.Plan(), - baseExecutionState.Index(), - inputMap, - baseExecutionState.CurrentPoses(), - ) - if err != nil { - return state.ExecuteResponse{}, err + updatedBaseExecutionState := baseExecutionState + _, ok := mr.kinematicBase.Kinematics().(tpspace.PTGProvider) + if ok { + updatedBaseExecutionState, err = mr.augmentBaseExecutionState(baseExecutionState) + if err != nil { + return state.ExecuteResponse{}, err + } } mr.logger.CDebugf(ctx, "CheckPlan inputs: \n currentPosition: %v\n currentInputs: %v\n worldstate: %s", - spatialmath.PoseToProtobuf(executionState.CurrentPoses()[mr.kinematicBase.Name().ShortName()].Pose()), - inputMap, + spatialmath.PoseToProtobuf(updatedBaseExecutionState.CurrentPoses()[mr.kinematicBase.Kinematics().Name()].Pose()), + updatedBaseExecutionState.CurrentInputs(), worldState.String(), ) - if err := motionplan.CheckPlan( - mr.kinematicBase.Kinematics(), // frame we wish to check for collisions - executionState, + mr.localizingFS.Frame(mr.kinematicBase.Kinematics().Name()), // frame we wish to check for collisions + updatedBaseExecutionState, worldState, // detected obstacles by this instance of camera + service - mr.planRequest.FrameSystem, + mr.localizingFS, lookAheadDistanceMM, mr.planRequest.Logger, ); err != nil { @@ -312,6 +325,96 @@ func (mr *moveRequest) obstaclesIntersectPlan( return state.ExecuteResponse{}, nil } +// In order for the localizingFS to work as intended when working with PTGs we must update the baseExecutionState. +// The original baseExecutionState passed into this method contains a plan, currentPoses, and currentInputs. +// We update the plan object such that the trajectory which is of form referenceframe.Input also houses information +// about where we are in the world. This changes the plans trajectory from being of length 4 into being of length +// 11. This is because we add 7 values corresponding the X, Y, Z, OX, OY, OZ, Theta(radians). +// We update the currentInputs information so that when the wrapperFrame transforms off its inputs, the resulting +// pose is its position in world and we do not need to compose the resultant value with another pose. +// We update the currentPoses so that it is now in the name of the wrapperFrame which shares its name with +// mr.kinematicBase.Kinematics(). +func (mr *moveRequest) augmentBaseExecutionState( + baseExecutionState motionplan.ExecutionState, +) (motionplan.ExecutionState, error) { + // update plan + existingPlan := baseExecutionState.Plan() + newTrajectory := make(motionplan.Trajectory, 0, len(existingPlan.Trajectory())) + for idx, currTraj := range existingPlan.Trajectory() { + // Suppose we have some plan of the following form: + // Path = [s, p1, p2, g, g] + // Traj = [ [0,0,0,0], [i1, a1, ds1, de1], + // [i2, a2, ds2, de2], + // [i3, a3, ds3, de3], [0,0,0,0] ] + // To properly interpolate across segments we will create in CheckPlan + // we want the inputs of the pose frame to be the start position of that arc, + // so we must look at the prior path step for the starting point. + // To put this into an example: + // [0,0,0,0] is tied to s, here we start at s and end at s + // [i1, a1, ds1, de1] is tied to s, here we start at s and end at p1 + // [i2, a2, ds2, de2] is tied to p1, here we start at p1 and end at p2 + // [i3, a3, ds3, de3] is tied to p2, here we start at p2 and end at g + // [0,0,0,0] is tied to g, here we start at g and end at g. + + // To accomplish this, we use PoseBetweenInverse on the transform of the + // trajectory and the path pose for our current index to calculate our path + // pose in the previous step. + + // 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] + kbTraj := currTraj[mr.kinematicBase.Name().Name] + + // determine which pose should be used as the origin of a ptg input + var prevPathPose spatialmath.Pose + if idx == baseExecutionState.Index() { + prevPathPose = baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose() + } else { + kbPose := currPathStep[mr.kinematicBase.Kinematics().Name()] + trajPose, err := mr.kinematicBase.Kinematics().Transform(kbTraj) + if err != nil { + return baseExecutionState, err + } + prevPathPose = spatialmath.PoseBetweenInverse(trajPose, kbPose.Pose()) + } + + updatedTraj := kbTraj + updatedTraj = append(updatedTraj, referenceframe.PoseToInputs(prevPathPose)...) + newTrajectory = append( + newTrajectory, map[string][]referenceframe.Input{mr.kinematicBase.Kinematics().Name(): updatedTraj}, + ) + } + augmentedPlan := motionplan.NewSimplePlan(existingPlan.Path(), newTrajectory) + + // update currentInputs + allCurrentInputsFromBaseExecutionState := baseExecutionState.CurrentInputs() + kinematicBaseCurrentInputs := allCurrentInputsFromBaseExecutionState[mr.kinematicBase.Kinematics().Name()] + // The order of inputs here matters as we construct the inputs for our poseFrame. + // The poseFrame has DoF = 11. + // The first four inputs correspond to the executionFrame's (ptgFrame) inputs which are: + // alpha, index, start distance, end distance + // The last seven inputs correspond to the localizationFrame's inputs which are: + // X, Y, Z, OX, OY, OZ, Theta (in radians) + kinematicBaseCurrentInputs = append( + kinematicBaseCurrentInputs, + referenceframe.PoseToInputs(baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose())..., + ) + allCurrentInputsFromBaseExecutionState[mr.kinematicBase.Kinematics().Name()] = kinematicBaseCurrentInputs + + // originally currenPoses are in the name of the localization frame + // here to transfer them to be in the name of the kinematics frame + // the kinematic frame's name is the same as the wrapper frame + existingCurrentPoses := baseExecutionState.CurrentPoses() + localizationFramePose := existingCurrentPoses[mr.kinematicBase.LocalizationFrame().Name()] + existingCurrentPoses[mr.kinematicBase.Name().Name] = localizationFramePose + delete(existingCurrentPoses, mr.kinematicBase.LocalizationFrame().Name()) + + return motionplan.NewExecutionState( + augmentedPlan, baseExecutionState.Index(), allCurrentInputsFromBaseExecutionState, existingCurrentPoses, + ) +} + func kbOptionsFromCfg(motionCfg *validatedMotionConfiguration, validatedExtra validatedExtra) kinematicbase.Options { kinematicsOptions := kinematicbase.NewKinematicBaseOptions() @@ -469,6 +572,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( if !ok { return nil, resource.DependencyNotFoundError(req.MovementSensorName) } + origin, _, err := movementSensor.Position(ctx, nil) if err != nil { return nil, err @@ -513,6 +617,9 @@ func (ms *builtIn) newMoveOnGlobeRequest( if straightlineDistance > maxTravelDistanceMM { return nil, fmt.Errorf("cannot move more than %d kilometers", int(maxTravelDistanceMM*1e-6)) } + + // Set the limits for a base if we are using diffential drive. + // If we are using PTG kineamtics these limits will be ignored. limits := []referenceframe.Limit{ {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, @@ -666,6 +773,12 @@ func (ms *builtIn) createBaseMoveRequest( worldObstacles []spatialmath.Geometry, valExtra validatedExtra, ) (*moveRequest, error) { + startPoseIF, err := kb.CurrentPosition(ctx) + if err != nil { + return nil, err + } + startPose := startPoseIF.Pose() + // replace original base frame with one that knows how to move itself and allow planning for kinematicFrame := kb.Kinematics() if err := fs.ReplaceFrame(kinematicFrame); err != nil { @@ -682,11 +795,46 @@ func (ms *builtIn) createBaseMoveRequest( return nil, err } - startPoseIF, err := kb.CurrentPosition(ctx) + planningFS := baseOnlyFS + _, ok := kinematicFrame.(tpspace.PTGProvider) + if ok { + // If we are not working with a differential drive base then we need to create a separate framesystem + // used for planning. + // This is because we rely on our starting configuration to place ourselves in our absolute position in the world. + // If we are not working with a differential drive base then our base frame + // is relative aka a PTG frame and therefore is not able to accurately place itself in its absolute position. + // For this reason, we add a static transform which allows us to place the base in its absolute position in the + // world frame. + planningFS = referenceframe.NewEmptyFrameSystem("store-starting point") + transformFrame, err := referenceframe.NewStaticFrame(kb.LocalizationFrame().Name(), startPose) + if err != nil { + return nil, err + } + err = planningFS.AddFrame(transformFrame, planningFS.World()) + if err != nil { + return nil, err + } + err = planningFS.MergeFrameSystem(baseOnlyFS, transformFrame) + if err != nil { + return nil, err + } + } + + // We create a wrapperFrame so that we may place observed transient obstacles in + // their absolute position in the world frame. The collision framesystem is used + // by CheckPlan so that we may solely rely on referenceframe.Input information + // to position ourselves correctly in the world. + executionFrame := kb.Kinematics() + localizationFrame := kb.LocalizationFrame() + wf, err := newWrapperFrame(localizationFrame, executionFrame) + if err != nil { + return nil, err + } + collisionFS := baseOnlyFS + err = collisionFS.ReplaceFrame(wf) if err != nil { return nil, err } - startPose := startPoseIF.Pose() goal := referenceframe.NewPoseInFrame(referenceframe.World, goalPoseInWorld) @@ -754,17 +902,17 @@ func (ms *builtIn) createBaseMoveRequest( Logger: logger, Goal: goal, Frame: kinematicFrame, - FrameSystem: baseOnlyFS, + FrameSystem: planningFS, StartConfiguration: currentInputs, StartPose: startPose, WorldState: worldState, Options: valExtra.extra, }, - poseOrigin: startPose, kinematicBase: kb, replanCostFactor: valExtra.replanCostFactor, obstacleDetectors: obstacleDetectors, fsService: ms.fsService, + localizingFS: collisionFS, executeBackgroundWorkers: &backgroundWorkers, diff --git a/services/motion/builtin/testutils.go b/services/motion/builtin/testutils.go index 7179f803ec5..1207acba3d6 100644 --- a/services/motion/builtin/testutils.go +++ b/services/motion/builtin/testutils.go @@ -406,7 +406,9 @@ func CreateMoveOnGlobeTestEnvironment(ctx context.Context, t *testing.T, origin injectedCamera := inject.NewCamera("injectedCamera") cameraLink := referenceframe.NewLinkInFrame( baseLink.Name(), - spatialmath.NewPose(r3.Vector{X: 1}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 90}), + // we recreate an intel real sense orientation placed along the +Y axis of the base's coordinate frame. + // i.e. the camera is pointed along the axis in which the base moves forward + spatialmath.NewPose(r3.Vector{}, &spatialmath.OrientationVectorDegrees{OY: 1, Theta: -90}), "injectedCamera", cameraGeom, ) diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go new file mode 100644 index 00000000000..5f1eb3a2a49 --- /dev/null +++ b/services/motion/builtin/wrapperFrame.go @@ -0,0 +1,154 @@ +package builtin + +import ( + "github.com/pkg/errors" + pb "go.viam.com/api/component/arm/v1" + + "go.viam.com/rdk/motionplan/tpspace" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils" +) + +// WrapperFrame is a frame which merges the planning and localization frames of a PTG base. +type wrapperFrame struct { + name string + localizationFrame referenceframe.Frame + executionFrame referenceframe.Frame + ptgSolvers []tpspace.PTGSolver +} + +func newWrapperFrame(localizationFrame, executionFrame referenceframe.Frame) (referenceframe.Frame, error) { + ptgFrame, err := utils.AssertType[tpspace.PTGProvider](executionFrame) + if err != nil { + return nil, err + } + return &wrapperFrame{ + name: executionFrame.Name(), + localizationFrame: localizationFrame, + executionFrame: executionFrame, + ptgSolvers: ptgFrame.PTGSolvers(), + }, nil +} + +// Name returns the name of the wrapper frame's execution frame's name. +func (wf *wrapperFrame) Name() string { + return wf.name +} + +// Transform returns the associated pose given a list of inputs. +func (wf *wrapperFrame) Transform(inputs []referenceframe.Input) (spatialmath.Pose, error) { + if len(inputs) != len(wf.DoF()) { + return nil, referenceframe.NewIncorrectInputLengthError(len(inputs), len(wf.DoF())) + } + // executionFramePose is our pose in a given set of plan.Trajectory inputs + executionFramePose, err := wf.executionFrame.Transform(inputs[:len(wf.executionFrame.DoF())]) + if err != nil { + return nil, err + } + // localizationFramePose is where the executionFramePose is supposed to be relative to + // since the executionFramePose is always first with respect to the origin + localizationFramePose, err := wf.localizationFrame.Transform(inputs[len(wf.executionFrame.DoF()):]) + if err != nil { + return nil, err + } + return spatialmath.Compose(localizationFramePose, executionFramePose), nil +} + +// Interpolate interpolates the given amount between the two sets of inputs. +func (wf *wrapperFrame) Interpolate(from, to []referenceframe.Input, by float64) ([]referenceframe.Input, error) { + if len(from) != len(wf.DoF()) { + return nil, referenceframe.NewIncorrectInputLengthError(len(from), len(wf.DoF())) + } + if len(to) != len(wf.DoF()) { + return nil, referenceframe.NewIncorrectInputLengthError(len(to), len(wf.DoF())) + } + interp := make([]referenceframe.Input, 0, len(to)) + + // both from and to are lists with length = 11 + // the first four values correspond to ptg(execution) frame values we want the interpolate + // the latter seven values correspond to pose(localization) frame values we want the interpolate + + // Note: since we are working with a ptg frame all interpolations of any inputs list + // will always begin at the origin, i.e. the zero pose + + // executionFrame interpolation + executionFrameFromSubset := make([]referenceframe.Input, len(wf.executionFrame.DoF())) + executionFrameToSubset := to[:len(wf.executionFrame.DoF())] + interpSub, err := wf.executionFrame.Interpolate(executionFrameFromSubset, executionFrameToSubset, by) + if err != nil { + return nil, err + } + interp = append(interp, interpSub...) + + // interpolating the localization frame is a special case + // we do not need to interpolate the values of the localization frame at all + // the localization frame informs where the execution frame's interpolations + // are supposed to begin, i.e. be with respect to in case the pose the + // localizationFrameSubset produces is not the origin + + // execution(localization) frame interpolation + localizationFrameSubset := to[len(wf.executionFrame.DoF()):] + interp = append(interp, localizationFrameSubset...) + + return interp, nil +} + +// Geometries returns an object representing the 3D space associated with the executionFrame's geometry. +func (wf *wrapperFrame) Geometries(inputs []referenceframe.Input) (*referenceframe.GeometriesInFrame, error) { + if len(inputs) != len(wf.DoF()) { + return nil, referenceframe.NewIncorrectInputLengthError(len(inputs), len(wf.DoF())) + } + sfGeometries := []spatialmath.Geometry{} + gf, err := wf.executionFrame.Geometries(make([]referenceframe.Input, len(wf.executionFrame.DoF()))) + if err != nil { + return nil, err + } + transformBy, err := wf.Transform(inputs) + if err != nil { + return nil, err + } + for _, g := range gf.Geometries() { + sfGeometries = append(sfGeometries, g.Transform(transformBy)) + } + + return referenceframe.NewGeometriesInFrame(wf.name, sfGeometries), nil +} + +// DoF returns the number of degrees of freedom within a given frame. +func (wf *wrapperFrame) DoF() []referenceframe.Limit { + return append(wf.executionFrame.DoF(), wf.localizationFrame.DoF()...) +} + +// MarshalJSON serializes a given frame. +func (wf *wrapperFrame) MarshalJSON() ([]byte, error) { + return nil, errors.New("serializing a poseFrame is currently not supported") +} + +// InputFromProtobuf converts pb.JointPosition to inputs. +func (wf *wrapperFrame) InputFromProtobuf(jp *pb.JointPositions) []referenceframe.Input { + jpValues := jp.Values + + executionFrameSubset := jpValues[:len(wf.executionFrame.DoF())] + executionFrameInputs := wf.executionFrame.InputFromProtobuf(&pb.JointPositions{Values: executionFrameSubset}) + + localizationFrameSubset := jpValues[len(wf.executionFrame.DoF()):] + localizationFrameInputs := wf.localizationFrame.InputFromProtobuf(&pb.JointPositions{Values: localizationFrameSubset}) + + return append(executionFrameInputs, localizationFrameInputs...) +} + +// ProtobufFromInput converts inputs to pb.JointPosition. +func (wf *wrapperFrame) ProtobufFromInput(input []referenceframe.Input) *pb.JointPositions { + executionFrameSubset := input[:len(wf.executionFrame.DoF())] + executionFrameJP := wf.executionFrame.ProtobufFromInput(executionFrameSubset) + + localizationFrameSubset := input[len(wf.executionFrame.DoF()):] + localizationFrameJP := wf.localizationFrame.ProtobufFromInput(localizationFrameSubset) + + return &pb.JointPositions{Values: append(executionFrameJP.Values, localizationFrameJP.Values...)} +} + +func (wf *wrapperFrame) PTGSolvers() []tpspace.PTGSolver { + return wf.ptgSolvers +} diff --git a/services/motion/explore/explore.go b/services/motion/explore/explore.go index 78a32496ff1..03a89a393ae 100644 --- a/services/motion/explore/explore.go +++ b/services/motion/explore/explore.go @@ -385,7 +385,7 @@ func (ms *explore) checkForObstacles( ms.logger, ) if err != nil { - if strings.Contains(err.Error(), "found error between positions") { + if strings.Contains(err.Error(), "found constraint violation or collision") { ms.logger.CDebug(ctx, "collision found in given range") ms.obstacleResponseChan <- moveResponse{success: true} return